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

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 位置、方向和坐标变换.

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

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

假设四元数已归一化 \((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++

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)