常见的位姿表示方法之间的转换

本文介绍了机器人学中常用的方向表示方法之间的转换。文章包含了必要的数学公式以及代码示例，并展示了如何将这些转换集成到您自己的应用中。

如果您想以交互方式使用这些变换，请尝试我们的 Pose Conversion GUI 工具。

有关位姿表示的更详细介绍，请查看位置、方向和坐标变换。

这些转换仅改变位姿的旋转（方向）部分；平移部分保持不变。Zivid 变换矩阵要求平移量以毫米（mm）为单位，因为 Zivid 点云以毫米为单位。大多数机器人控制器以米为单位报告平移量，因此在将机器人位姿转换为 Zivid 矩阵时，需要将平移量乘以 1000。

Roll-Pitch-Yaw 到旋转矩阵

Roll-pitch-yaw 是表示方向的常用术语。每一个代表了围绕单个轴的旋转角度，它们组合起来描述了完整的旋转角度。但是，它们所表示的确切意义并不清楚。有以下困惑：

这些角度是绕哪个轴的旋转角度？
这些轴是固定的还是移动的？
旋转定义的顺序是什么（有多种可能性）？

旋转顺序通常表示为 x-y-z 或者 z-y'-x'' 。这里的 x ， y 和 z 表示它围绕其旋转的轴。 ' 用于指示轴是否是固定的。绕固定轴旋转称为 extrinsic rotation 。围绕移动轴的旋转称为 intrinsic rotation 。

The two examples below show the conversion for the extrinsic x-y-z order and the extrinsic z-y-x order.

对于 x-y-z 或者 z-y'-x'' ， roll \(\phi\) ，pitch \(\theta\) 和 yaw \(\psi\) 角度可以转换为下面的旋转矩阵 \(R\) ：

\[\begin{split}\boldsymbol{R}_{x-y-z} = &\boldsymbol{R}_{z-{y}^{'}-{x}^{''}} =\boldsymbol{R}_{z,\phi}\boldsymbol{R}_{y,\theta}\boldsymbol{R}_{x,\psi} = \\ \begin{bmatrix} cos{\phi} & -sin{\phi} & 0 \\ sin{\phi} & cos{\phi} & 0\\ 0 & 0 & 1\end{bmatrix}& \begin{bmatrix} cos{\theta} & 0 & sin{\theta} \\ 0 & 1 & 0 \\ -sin{\theta} & 0 & cos{\theta} \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\\ 0 & cos{\psi} & -sin{\psi} \\ 0 & sin{\psi} & cos{\psi} \end{bmatrix}\end{split}\]

对于 z-y-x 或者 x-y'-z'' ， rol l \(\phi\) ， pitch \(\theta\) , 和 yaw \(\psi\) 角度可以转换为下面的旋转矩阵 \(R\) ：

\[\begin{split}\boldsymbol{R}_{z-y-x} = &\boldsymbol{R}_{x-{y}^{'}-{z}^{''}} =\boldsymbol{R}_{x,\phi}\boldsymbol{R}_{y,\theta}\boldsymbol{R}_{z,\psi} = \\ \begin{bmatrix} 1 & 0 & 0\\ 0 & cos{\phi} & -sin{\phi} \\ 0 & sin{\phi} & cos{\phi} \end{bmatrix}& \begin{bmatrix} cos{\theta} & 0 & sin{\theta} \\ 0 & 1 & 0 \\ -sin{\theta} & 0 & cos{\theta} \end{bmatrix} \begin{bmatrix} cos{\psi} & -sin{\psi} & 0 \\ sin{\psi} & cos{\psi} & 0\\ 0 & 0 & 1\end{bmatrix}\end{split}\]

备注

即使两个示例中的角度是相同的，它们也不代表相同的最终旋转矩阵。

即使最终旋转矩阵是相同的，两个示例之间的角度也不相同。

这里引入了一个定义：roll 分配给绕运动轴的第一个旋转，pitch 是第二个，yaw 是第三个。

C++

rollPitchYawListToRotationMatrix(rpyList);

C＃

RollPitchYawListToRotationMatrix(rpyList);

Python

roll_pitch_yaw_to_rotation_matrix(rpy_list)

旋转矢量到轴-角

一个旋转向量 \(\boldsymbol{r}\) 可以转换成轴 \(\boldsymbol{u}\) 和角度 \(\theta\) ，如下所示：

\[\theta = \sqrt{{r_x}^2+{r_y}^2+{r_z}^2}\]

\[\begin{split}\boldsymbol{u} = \begin{bmatrix} \frac{r_x}{\theta} & \frac{r_y}{\theta} & \frac{r_z}{\theta} \\ \end{bmatrix}\end{split}\]

轴-角到四元数

轴 \(\boldsymbol{u}\) 和角度 \(\theta\) 可以转换为单位四元数 \(\boldsymbol{q}\) ，如下所示：

\[\begin{split}\boldsymbol{q} = \begin{bmatrix} \cos{\frac{\theta}{2}} & u_x \sin{\frac{\theta}{2}} & u_y \sin{\frac{\theta}{2}} & u_z \sin{\frac{\theta}{2}} \\ \end{bmatrix}\end{split}\]

四元数到旋转矩阵

单位四元数 \(\boldsymbol{q}\) 可以转换为旋转矩阵 \(\boldsymbol{R}\) ，如下所示：

\[\begin{split}\boldsymbol{R} = \begin{bmatrix} 1 - 2 {q_{y}}^2 - 2 {q_{z}}^2 & 2 q_{x} q_{y} - 2 q_{z} q_{w} & 2 q_{x} q_{z} - 2 q_{y} q_{w} \\ 2 q_{x} q_{y} - 2 q_{z} q_{w} & 1 - 2 {q_{x}}^2 - 2 {q_{z}}^2 & 2 q_{y} q_{z} - 2 q_{x} q_{w} \\ 2 q_{x} q_{z} - 2 q_{y} q_{w} & 2 q_{y} q_{z} - 2 q_{x} q_{w} & 1 - 2 {q_{x}}^2 - 2 {q_{y}}^2\\ \end{bmatrix}\end{split}\]

It is assumed that the quaternion is normalized \(({q_w}^2 + {q_x}^2 + {q_y}^2 + {q_z}^2 = 1)\). If not, it should be normalized before doing the conversion using this equation:

\[n = \sqrt{{q_w}^2+{q_x}^2+{q_y}^2+{q_z}^2}\]

\[\begin{split}\boldsymbol{q} = \begin{bmatrix} \frac{q_w}{n} & \frac{q_x }{n} & \frac{q_y }{n} & \frac{q_z }{ n}\\ \end{bmatrix}\end{split}\]

C++

const Eigen::Matrix3f rotationMatrixFromQuaternion = quaternion.toRotationMatrix();
std::cout << "Rotation Matrix from Quaternion:\n"
          << rotationMatrixFromQuaternion.format(matrixFormatRules) << std::endl;

C＃

var rotationMatrixFromQuaternion = QuaternionToRotationMatrix(quaternion);
Console.WriteLine("Rotation Matrix from Quaternion:\n" + MatrixToString(rotationMatrixFromQuaternion));

Python

rotation_matrix_from_quaternion = quaternion_to_rotation_matrix(quaternion)
print(f"Rotation Matrix from Quaternion:\n{rotation_matrix_from_quaternion}")

旋转矩阵到四元数

旋转矩阵 \(\boldsymbol{R}\) 可以转换为单位四元数 \(\boldsymbol{q}\) ，如下所示：

\[q_w = \frac{\sqrt{1 + r_{11} + r_{22}+ r_{33}}}{2}\]

\[\begin{split}\boldsymbol{q} = \begin{bmatrix} q_w & \frac{r_{32} - r_{23}}{4 q_w} & \frac{r_{13} - r_{31}}{4 q_w} & \frac{r_{21} - r_{12}}{4 q_w} \\ \end{bmatrix}\end{split}\]

C++

const Eigen::Quaternionf quaternion(rotationMatrix);
std::cout << "Quaternion:\n" << quaternion.coeffs().format(vectorFormatRules) << std::endl;

C＃

var quaternion = RotationMatrixToQuaternion(rotationMatrix);
Console.WriteLine("Quaternion:\n" + MatrixToString(quaternion.Transpose()));

Python

quaternion = rotation_matrix_to_quaternion(rotation_matrix)
print(f"Quaternion:\n{quaternion}")

四元数到轴-角

单位四元数 \(\boldsymbol{q}\) 可以转换成轴 \(\boldsymbol{u}\) 和角度 \(\theta\) ，如下所示：

\[\theta = 2 \cos^{-1}{q_w}\]

\[\begin{split}\boldsymbol{u} = \begin{bmatrix} \frac{q_x}{\sqrt{1-{q_w}^2}} & \frac{q_y}{\sqrt{1-{q_w}^2}} & \frac{q_z}{\sqrt{1-{q_w}^2}} \\ \end{bmatrix}\end{split}\]

这对于从旋转矩阵转换为轴-角很有用。请参阅下面的代码示例实现。

C++

const Eigen::AngleAxisf axisAngle(rotationMatrix);
std::cout << "AxisAngle:\n"
          << axisAngle.axis().format(vectorFormatRules) << ", " << axisAngle.angle() << std::endl;

C＃

var axisAngle = RotationMatrixToAxisAngle(rotationMatrix);
Console.WriteLine("AxisAngle:\n" + MatrixToString(axisAngle.Axis.Transpose()) + ", " + String.Format(" {0:G4} ", axisAngle.Angle));

Python

axis_angle = rotation_matrix_to_axis_angle(rotation_matrix)
print(f"AxisAngle:\n{axis_angle.axis}, {axis_angle.angle:.4f}")

轴-角到旋转矢量

轴 \(\boldsymbol{u}\) 和角度 \(\theta\) 可以转换为旋转向量 \(\boldsymbol{r}\) ，如下所示：

\[\begin{split}\boldsymbol{r} = \begin{bmatrix} u_x \theta & u_y \theta & u_z \theta \\ \end{bmatrix}\end{split}\]

这对于将旋转矩阵转换为旋转向量很有用。请参阅下面的代码示例实现。

C++

const Eigen::Vector3f rotationVector = rotationMatrixToRotationVector(rotationMatrix);
std::cout << "Rotation Vector:\n" << rotationVector.format(vectorFormatRules) << std::endl;

C＃

var rotationVector = axisAngle.Axis * axisAngle.Angle;
Console.WriteLine("Rotation Vector:\n" + MatrixToString(rotationVector.Transpose()));

Python

rotation_vector = rotation_matrix_to_rotation_vector(rotation_matrix)
print(f"Rotation Vector:\n{rotation_vector}")

旋转矩阵到 Roll-Pitch-Yaw

从旋转矩阵确定滚动、俯仰、偏航角并不简单。可以有多个，有时甚至是无数个解。这就需要一种算法，该算法可以根据某些标准从多个解中选择出其中一个解。

C++

const auto rpyList = rotationMatrixToRollPitchYawList(rotationMatrix);

C＃

var rpyList = RotationMatrixToRollPitchYawList(rotationMatrix);

Python

rpy_list = rotation_matrix_to_roll_pitch_yaw(rotation_matrix)

特定机器人的位姿约定

将机器人位姿转换为变换矩阵时，最困难的部分是确定机器人控制器使用的方向表示方法。一旦知道了具体的表示方法，上述数学转换就是明确无误的，但机器人厂商很少明确说明这一点，而且同一术语（例如 roll-pitch-yaw）在不同机器人上可能表示不同的旋转顺序。

本节整理了我们在特定机器人上所观察到的约定，以便您无需重新摸索即可从上文中选择正确的转换方法。

警告

这些信息是根据特定机器人型号的使用经验汇总而来的，并未针对所有固件版本、型号或厂商进行全面测试。在依赖转换结果之前，请务必先进行验证，例如将组装好的变换矩阵与机器人示教器上显示的位姿进行比较。

Universal Robots

Universal Robots 将 TCP 位姿报告为六元素向量 [x, y, z, rx, ry, rz] 。前三个元素是平移量，后三个元素是以 rotation vector （按角度缩放的轴-角表示，也称为罗德里格斯表示）形式表示的方向，而非 roll-pitch-yaw。脚本接口和 RTDE 接口（例如 get_actual_tcp_pose ）以米为单位报告平移量，而示教器则以毫米显示。

构建 4x4 变换矩阵：

确保平移量以毫米为单位；如果从脚本或 RTDE 接口读取位姿，需要从米转换为毫米。
Convert the rotation vector to a rotation matrix: rotation vector to axis-angle, then axis-angle to quaternion, then quaternion to rotation matrix.
将旋转矩阵和平移向量放入 4x4 变换矩阵中。

Yaskawa

Yaskawa 控制器以平移量后跟三个欧拉角（ Rx 、 Ry 、 Rz ）的形式报告位姿，旋转顺序为 xyz 外旋。

构建 4x4 变换矩阵：

确保平移量以毫米为单位。
Convert the Euler angles to a rotation matrix using the Roll-Pitch-Yaw to Rotation Matrix conversion, with the rotation order from the table.
将旋转矩阵和平移向量放入 4x4 变换矩阵中。

已知的欧拉角顺序

下表列出了我们在各机器人厂商中观察到的欧拉角顺序。转换步骤与上文 Yaskawa 的示例相同；不同厂商之间只是欧拉角顺序不同。请将其作为参考起点，并结合自己的机器人进行确认，因为同一厂商的不同型号和固件之间顺序也可能不同。

机器人厂商	欧拉角顺序	备注
Universal Robots	旋转向量（非欧拉角）	轴-角 / 罗德里格斯
Fanuc	xyz extrinsic	以 W-P-R 形式报告
Yaskawa	xyz extrinsic
Kawasaki	zyz intrinsic
KUKA	zyx intrinsic	以 A-B-C 形式报告
Doosan	zyz intrinsic
ABB	zyx intrinsic	控制器还提供四元数
Hyundai	xyz intrinsic
Robostar	xyz extrinsic
Nachi	zyx intrinsic