三维刚体运动

Rigid body motion in three dimensions

目录

• Rigid body motion in three dimensions
  • 1.点与坐标系
  • 2. 旋转矩阵
  • 3. 旋转向量与欧拉角
    • 3.1 旋转向量
    • 3.2 欧拉角
  • 4.四元数

1.点与坐标系

• 2D 的情况下：用两个坐标加旋转角表达，即 $(x,y,\theta )$。

• 3D 的情况下：则使用 $(x,y,z)$ 来表平移，而旋转则有很多其他的表示方式，例如欧拉角，旋转向量，四元数，旋转矩阵等，后面将分别介绍。

• 向量的运算

  • 内积

    $$a\cdot b=a^{T}b=\sum _{i=1}^3=|a||b|cos⟨a,b⟩$$

  • 外积

    $$a\times b=\left[\begin{array}{ccc}i& j& k\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right]=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b\triangleq a^{\wedge }b$$

    其中 $a^{\wedge }$ 成为向量 $a$ 的反对陈矩阵。

• 在 SLAM 中，世界坐标系是固定，而和机器人坐标系则是移动的，同时还有很多传感器坐标系。那么如何描述坐标系之间的一个变化呢，直观上来说分为两个部分

  1. 原点之间的平移
  2. 三个轴的旋转

2. 旋转矩阵

考虑一次旋转：

• 坐标系 $(e_1,e_2,e_3)$ 发生了旋转，变成了 $(e_1^{‘}),(e_2^{‘}),(e_3^{‘})$，但是向量 $a$ 不动，那么他们的坐标系如何变化。具体的变化过程用数学的方式展现如下所示：
  $$\begin{gathered} a=\left[\begin{array}{ccc}{e}_1& e_2& e_3\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^{\prime }& e_2^{\prime }& e_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right] \\ \Downarrow \\ a=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^T{e}_1^{\prime }& e_1^T{e}_2^{\prime }& e_1^T{e}_3^{\prime }\\ e_2^T{e}_1^{\prime }& e_2^T{e}_2^{\prime }& e_2^T{e}_3^{\prime }\\ e_3^T{e}_1^{\prime }& e_3^T{e}_2^{\prime }& e_3^T{e}_3^{\prime }\end{array}\right]=\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\triangleq R{a}^{\prime } \end{gathered}$$

​ 无论坐标系如何变化，其实世界坐标系也只是相对的，一个真实世界中的障碍物，无论在任何一个坐标系下的障碍物，其映射到真实世界中其实都应当是一个。

​ 上式中从上到下的一个变换是等式左右两边同时左乘了矩阵：

$$\left[\begin{array}{c}{e}_1^T\\ e_2^T\\ e_3^T\end{array}\right]$$

​ $[e_1{e}_2{e}_3]$ 乘该矩阵后为单位矩阵，则左边还是 $a$.

​ 其中 $R$ 成为旋转矩阵，可以验证：

• $R$ 是一个正交矩阵 -> $R^T\cdot R=I$ $R^T=R^{-1}$

• $R$ 的行列式为 $1$ -> $det(R)=1$

满足这两个性质的矩阵称为旋转矩阵,可以扩展到 $N$ 维空间:

$$SO(n)=\{R\in {\mathbb{R}}^{n\times n}|R{R}^T=I,det(R)=1\}$$

其中 $SO(n)$ 即我们通常所叫的特殊正交群 $(SpecialOrthogonalGroup)$

于是，1到2的旋转可表达为：

$$a_1=R_{12}{a}_2$$

反之：

$$a_2=R_{21}{a}_1$$

矩阵关系：

$$R_{21}=R_{12}^{-1}=R_{12}^T$$

其中 $R_{cw}$ 表示的则是从 $c$ 系到 $w$ 系的旋转矩阵。

旋转加平移可以表示为：

$$a^{\prime }=Ra+t$$

欧拉定理（$Eule{r}^{\prime }srotationtheorem$）：刚体在三维空间里的一般运动，可分解为刚体上方某一点的平移，以及绕经过此点的旋转轴的转动。

在实际的使用过程中我们一般使用的是齐次坐标的形式进行表示，其将二维空间中的坐标为度扩展到三维，将三维空间中的坐标扩展到四维，增加的一个维度一般用 $1$ 来代替。则上式的齐次形式可以表示为：

$$\left[\begin{array}{c}a\prime \\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]\triangleq T\left[\begin{array}{c}a\\ 1\end{array}\right]$$

其中，$R$的维度为 $3\times 3$ , $t$ 的维度为 $3\times 1$ ，$0$ 的维度为 $1\times 3$，$1$的维度为 $1\times 1$

变换矩阵的集合称为特殊欧氏群$SE(3)$ （$SpecialEuclideanGroup$）

$$SE(3)=\{\mathit{T}=\left[\begin{array}{cc}\mathit{R}& \mathit{t}\\ {\mathbf{0}}^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathit{R}\in SO(3),\mathit{t}\in {\mathbb{R}}^3\}$$

逆形式：

$${\mathit{T}}^{-1}=\left[\begin{array}{cc}{\mathit{R}}^T& -{\mathit{R}}^{T}t\\ {\mathbf{0}}^T& 1\end{array}\right]$$

3. 旋转向量与欧拉角

​ 除了旋转矩阵/变换矩阵之外，还存在其他的表示方式，旋转矩阵 R 有九个元素，但仅有三个自由度，能否以更少的元素表达旋转？答案是可以的。

3.1 旋转向量

• 方向为旋转轴，长度为转过的角度

• 称为角轴/轴角（$AngleAxis$）或旋转向量（$RotationVector$）

• 旋转向量与矩阵的不同：

  • 仅有三个量
  • 无约束
  • 更直观

• 旋转向量与旋转矩阵之间的变化关系可以通过罗格里德斯公式给出，在已知旋转向量的情况下得到旋转矩阵：

  $$\mathit{R}=\cos \theta \mathit{I}+(1-\cos \theta )\mathit{n}{\mathit{n}}^T+\sin \theta {\mathit{n}}^{\wedge }$$

• 旋转矩阵转向量：

  $$\begin{gathered} \theta =\arccos \left(\frac{\mathrm{tr}(\mathit{R})-1}{2}\right) \\ Rn=n \end{gathered}$$

  第二个公式则是因为在旋转矩阵为正交矩阵，且 $n$ 本身为旋转轴，旋转之后 $n$ 是不变的。

3.2 欧拉角

将旋转分解为三个方向上的转动例，按 $Z-Y-X$ 顺序转动轴可以是定轴或动轴，顺序亦可不同常见的有 $yaw-pitch-roll$ ，东北天不同领域的习惯有所不同。

1. 绕物体的Z轴旋转，得到偏航角yaw；
2. 绕旋转之后的Y轴旋转，得到俯仰角pitch；
3. 绕旋转之后的X轴旋转，得到滚转角roll；

4.四元数

四元数其实才是当今 SLAM 中使用最多的旋转表示方式，其只是殿太过于庞大，笔者也是花得了大量时间查阅证明进行理解，这里放出笔者认为目前网络上最好的一个全部证明，供大家参考：

• https://graphics.stanford.edu/courses/cs348a-17-winter/Papers/quaternion.pdf
• https://krasjet.github.io/quaternion/quaternion.pdf