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

This article explains how to convert between orientation representations commonly used in robotics. It includes the necessary mathematical equations as well as code examples showing how to integrate these conversions into your own applications.

If you want to utilize these transformations interactively, try our Pose Conversion GUI tool.

For a more in-depth explanation of pose representations, check out 位置、方向和坐标变换.

These conversions only change the rotation (orientation) part of the pose; the translation part is carried over unchanged. The Zivid transformation matrix expects the translation in millimeters (mm), because Zivid point clouds are in millimeters. Most robot controllers report translation in meters, so multiply the translation by 1000 when converting a robot pose to the Zivid matrix.

Roll-Pitch-Yaw到旋转矩阵

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

• 这些角度是绕哪个轴的旋转角度？

• 这些轴是固定的还是移动的？

• 旋转定义的顺序是什么（有多种可能性）？

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

下面是两个不同的例子。

对于 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++

source

rollPitchYawListToRotationMatrix(rpyList);

C＃

source

RollPitchYawListToRotationMatrix(rpyList);

Python

source

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}\]

假设四元数已归一化 \((q_w + q_x + q_y + q_z = 1)\) 。如果不是，则应在使用以下公式进行转换之前将其归一化：

\[n = \sqrt{{q_w}^2+{q_c}^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++

source

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

C＃

source

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

Python

source

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++

source

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

C＃

source

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

Python

source

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++

source

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

C＃

source

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

Python

source

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++

source

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

C＃

source

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

Python

source

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

旋转矩阵到Roll-Pitch-Yaw

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

C++

source

const auto rpyList = rotationMatrixToRollPitchYawList(rotationMatrix);

C＃

source

var rpyList = RotationMatrixToRollPitchYawList(rotationMatrix);

Python

source

rpy_list = rotation_matrix_to_roll_pitch_yaw(rotation_matrix)

Robot-Specific Pose Conventions

The hardest part of converting a robot pose to a transformation matrix is identifying which orientation representation the robot controller uses. The mathematical conversions above are unambiguous once you know the representation, but robot vendors rarely state it clearly, and the same term (for example roll-pitch-yaw) can mean different rotation orders on different robots.

This section collects the conventions we have seen on specific robots, so you can pick the right conversion above without rediscovering it.

警告

This information is gathered from experience with specific robot models and is not exhaustively tested across firmware versions, models, or vendors. Always verify the result of a conversion before relying on it, for example by comparing the assembled transformation matrix against the pose shown on the robot teach pendant.

Universal Robots

Universal Robots reports the TCP pose as a six-element vector [x, y, z, rx, ry, rz]. The first three elements are the translation, and the last three are the orientation as a rotation vector (axis-angle scaled by the angle, also called the Rodrigues representation), not roll-pitch-yaw. The script and RTDE interfaces (for example get_actual_tcp_pose) report the translation in meters, while the teach pendant displays it in millimeters.

To build a 4x4 transformation matrix:

1. Ensure the translation is in millimeters, converting from meters if you read the pose from the script or RTDE interface.

2. Convert the rotation vector to a rotation matrix using the conversions above.

3. Place the rotation matrix and translation vector into a 4x4 transformation matrix.

Yaskawa

Yaskawa controllers report the pose as a translation followed by three Euler angles (Rx, Ry, Rz) in xyz extrinsic order.

To build a 4x4 transformation matrix:

1. Ensure the translation is in millimeters.

2. Convert the Euler angles to a rotation matrix using the Roll-Pitch-Yaw to Rotation Matrix conversion above, with the rotation order from the table.

3. Place the rotation matrix and translation vector into a 4x4 transformation matrix.

Known Euler-Angle Orders

The following table lists the Euler-angle orders we have seen per robot vendor. The steps are the same as for Yaskawa above; only the Euler-angle order changes from one vendor to the next. Use it as a starting point, but confirm against your own robot, as the order can differ between models and firmware from the same vendor.

Robot vendor	Euler angles order	Notes
Universal Robots	Rotation vector (not Euler)	Axis-angle / Rodrigues
Fanuc	xyz extrinsic	Reported as W-P-R
Yaskawa	xyz extrinsic
Kawasaki	zyz intrinsic
KUKA	zyx intrinsic	Reported as A-B-C
Doosan	zyz intrinsic
ABB	zyx intrinsic	Controller also exposes a quaternion
Hyundai	xyz intrinsic
Robostar	xyz extrinsic
Nachi	zyx intrinsic