You're viewing an old version of the documentation. Click here to see the latest release.

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

本文介绍了常见的机器人姿态表示方法之间的转换方程。有关位姿表示的更多信息，请查看 位置、方向和坐标变换。

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 $\varphi$，pitc h $\theta$ 和ya w $\psi$ 角度可以转换为下面的旋转矩 阵 $R$ ：

$$\begin{array}{rl}{\mathit{R}}_{x-y-z}=& {\mathit{R}}_{z-y^{{}^{\prime }}-x^{{}^{″}}}={\mathit{R}}_{z,\varphi }{\mathit{R}}_{y,\theta }{\mathit{R}}_{x,\psi }=\\ \left[\begin{array}{ccc}cos\varphi & -sin\varphi & 0\\ sin\varphi & cos\varphi & 0\\ 0& 0& 1\end{array}\right]& \left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\psi & -sin\psi \\ 0& sin\psi & cos\psi \end{array}\right]\end{array}$$

对 于 z-y-x 或者 x-y'-z''， rol l $\varphi$， pitc h $\theta$, 和ya w $\psi$ 角度可以转换为下面的旋转矩 阵 $R$ ：

$$\begin{array}{rl}{\mathit{R}}_{z-y-x}=& {\mathit{R}}_{x-y^{{}^{\prime }}-z^{{}^{″}}}={\mathit{R}}_{x,\varphi }{\mathit{R}}_{y,\theta }{\mathit{R}}_{z,\psi }=\\ \left[\begin{array}{ccc}1& 0& 0\\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi & cos\varphi \end{array}\right]& \left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]\left[\begin{array}{ccc}cos\psi & -sin\psi & 0\\ sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right]\end{array}$$

备注

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

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

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

旋转矢量到轴-角

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

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

$$\begin{array}{r}\mathit{u}=\left[\begin{array}{ccc}\frac{r_x}{\theta }& \frac{r_y}{\theta }& \frac{r_z}{\theta }\end{array}\right]\end{array}$$

轴-角到四元数

Axis $\mathit{u}$ and angle $\theta$ can be converted to a unit quaternion $\mathit{q}$ as follows:

$$\begin{array}{r}\mathit{q}=\left[\begin{array}{cccc}\cos \frac{\theta }{2}& u_x\sin \frac{\theta }{2}& u_y\sin \frac{\theta }{2}& u_z\sin \frac{\theta }{2}\end{array}\right]\end{array}$$

四元数到旋转矩阵

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

$$\begin{array}{r}\mathit{R}=\left[\begin{array}{ccc}1-2{q_y}^2-2{q_z}^2& 2q_x{q}_y-2q_z{q}_w& 2q_x{q}_z-2q_y{q}_w\\ 2q_x{q}_y-2q_z{q}_w& 1-2{q_x}^2-2{q_z}^2& 2q_y{q}_z-2q_x{q}_w\\ 2q_x{q}_z-2q_y{q}_w& 2q_y{q}_z-2q_x{q}_w& 1-2{q_x}^2-2{q_y}^2\end{array}\right]\end{array}$$

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

$$n=\sqrt{{q_w}^2+{q_c}^2+{q_y}^2+{q_z}^2}$$

$$\begin{array}{r}\mathit{q}=\left[\begin{array}{cccc}\frac{q_w}{n}& \frac{q_x}{n}& \frac{q_y}{n}& \frac{q_z}{n}\end{array}\right]\end{array}$$

旋转矩阵到四元数

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

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

$$\begin{array}{r}\mathit{q}=\left[\begin{array}{cccc}{q}_w& \frac{r_{32}-r_{23}}{4q_w}& \frac{r_{13}-r_{31}}{4q_w}& \frac{r_{21}-r_{12}}{4q_w}\end{array}\right]\end{array}$$

四元数到轴-角

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

$$\theta =2{\cos }^{-1}{q}_w$$

$$\begin{array}{r}\mathit{u}=\left[\begin{array}{ccc}\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{array}\right]\end{array}$$

轴-角到旋转矢量

Axis $\mathit{u}$ and angle $\theta$ can be converted to a rotation vector $\mathit{r}$ as follows:

$$\begin{array}{r}\mathit{r}=\left[\begin{array}{ccc}{u}_x\theta & u_y\theta & u_z\theta \end{array}\right]\end{array}$$

旋转矩阵到Roll-Pitch-Yaw

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