常见的姿态表示方法之间的转换
本文介绍了常见的机器人姿态表示方法之间的转换方程。有关位姿表示的更多信息，请查看 位置、方向和坐标变换。
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''， rol l \(\phi\)，pitc h \(\theta\) 和ya w \(\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\)， pitc h \(\theta\), 和ya w \(\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是第三个。
旋转矢量到轴-角
一个旋转向 量 \(\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}\]
轴-角到四元数
Axis \(\boldsymbol{u}\) and angle \(\theta\) can be converted to a unit quaternion \(\boldsymbol{q}\) as follows:
\[\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}\]
旋转矩阵到四元数
旋转矩 阵 \(\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}\]
四元数到轴-角
单位四元 数 \(\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}\]
轴-角到旋转矢量
Axis \(\boldsymbol{u}\) and angle \(\theta\) can be converted to a rotation vector \(\boldsymbol{r}\) as follows:
\[\begin{split}\boldsymbol{r} = \begin{bmatrix} u_x \theta & u_y \theta & u_z \theta \\ \end{bmatrix}\end{split}\]
旋转矩阵到Roll-Pitch-Yaw
从旋转矩阵确定滚动、俯仰、偏航角并不简单。可以有多个，有时甚至是无数个解。这就需要一种算法，该算法可以根据某些标准从多个解中选择出其中一个解。