Rigid body motion in three dimensions

Rigid body motion in three dimensions

1.点与坐标系

2D 的情况下：用两个坐标加旋转角表达，即 $(x,y,\theta)$。
3D 的情况下：则使用 $(x,y,z)$ 来表平移，而旋转则有很多其他的表示方式，例如欧拉角，旋转向量，四元数，旋转矩阵等，后面将分别介绍。
向量的运算
- 内积
  
  \[a\cdot{}b=a^{T}b=\sum_{i=1}^{3}=\lvert a \rvert \ \lvert b \rvert \ cos\left \langle a,b \right \rangle
  \]
- 外积
  
  \[a \times b =
  \begin{bmatrix}
  i & j & k \\
  a_{1} & a_{2} & a_{3}\\
  b_{1} & b_{2} &b_{3}
  \end{bmatrix} = \begin{bmatrix}
  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{bmatrix} =
  \begin{bmatrix}
  0 & -a_{3} &a_{2} \\
  a_{3}& 0 & -a_{1}\\
  -a_{2}& a_{1}&0
  \end{bmatrix} \ 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$ 不动，那么他们的坐标系如何变化。具体的变化过程用数学的方式展现如下所示：

\[a = \begin{bmatrix}
e_1&e_2 &e_3
\end{bmatrix} \
\begin{bmatrix}
a_1\\
a_2\\
a_3
\end{bmatrix} =
\begin{bmatrix}
e_1^{\prime}&e_2^{\prime} &e_3^{\prime}
\end{bmatrix}
\begin{bmatrix}
a_1^{\prime}\\
a_2^{\prime}\\
a_3^{\prime}
\end{bmatrix} \\ \Downarrow \\
a =
\begin{bmatrix}
a_1\\
a_2\\
a_3
\end{bmatrix}=
\begin{bmatrix}
e_1^Te_1^{\prime}& e_1^Te_2^{\prime}& e_1^Te_3^{\prime} \\
e_2^Te_1^{\prime}& e_2^Te_2^{\prime}& e_2^Te_3^{\prime} \\
e_3^Te_1^{\prime}& e_3^Te_2^{\prime}& e_3^Te_3^{\prime}
\end{bmatrix} =
\begin{bmatrix}
a_1^{\prime}\\
a_2^{\prime}\\
a_3^{\prime}
\end{bmatrix}
\triangleq Ra^{\prime}
\]

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

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

\[\begin{bmatrix}
e_1^{T} \\ e_2^{T} \\ e_3^{T}
\end{bmatrix}
\]

$ [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)=\left \{
R\in \mathbb{R}^{n\times n} | R \ R^T = I ,\ det (R)=1
\right \}
\]

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

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

\[a_1=R_{12}a_2 \\
\]

反之：

\[a_2=R_{21}a_1
\]

矩阵关系：

\[R_{21}=R^{-1}_{12}=R_{12}^{T}
\]

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

旋转加平移可以表示为：

\[a^{\prime}=Ra+t
\]

欧拉定理（$Euler’s rotation theorem$）：刚体在三维空间里的一般运动，可分解为刚体上方某一点的平移，以及绕经过此点的旋转轴的转动。

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

\[\begin{bmatrix}
a\prime \\
1
\end{bmatrix}=
\begin{bmatrix}
R& t\\
0^T&1
\end{bmatrix}\begin{bmatrix}
a\\
1
\end{bmatrix} \triangleq
T\begin{bmatrix}
a\\
1
\end{bmatrix}
\]

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

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

\[S E(3)=\left\{\boldsymbol{T}=\left[\begin{array}{cc}
\boldsymbol{R} & \boldsymbol{t} \\
\mathbf{0}^{T} & 1
\end{array}\right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in S O(3), \boldsymbol{t} \in \mathbb{R}^{3}\right\}
\]

逆形式：

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

3. 旋转向量与欧拉角

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

3.1 旋转向量

方向为旋转轴，长度为转过的角度
称为角轴/轴角（$Angle Axis$）或旋转向量（$Rotation Vector$）
旋转向量与矩阵的不同：
- 仅有三个量
- 无约束
- 更直观
旋转向量与旋转矩阵之间的变化关系可以通过罗格里德斯公式给出，在已知旋转向量的情况下得到旋转矩阵：

\[\boldsymbol{R}=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{T}+\sin \theta \boldsymbol{n}^{\wedge}
\]
旋转矩阵转向量：

\[\theta=\arccos \left(\frac{\operatorname{tr}(\boldsymbol{R})-1}{2}\right) \\
Rn=n
\]

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