【SLAM】01_三维空间刚体运动

摘要：介绍如何描述刚体在三维空间中的运动

1. 旋转矩阵

1.1 点、向量和坐标系

向量 $a$ 在三维空间的坐标表示为：
$$a=[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{e}_1+a_2{e}_2+a_3{e}_3$$
该空间的基为$(e_1,e_2,e_3)$，这里的$(a_1,a_2,a_3{)}^T$为向量$a$在此基下的坐标。

坐标的运算方式为：

• 加减法：各自坐标相加
• 内积（点乘）：
  $a\cdot b=a^{T}b={\sum }_{i=1}^3{a}_i\times b_i=|a||b|\cos <a,b>$, 且 $a\cdot b=b\cdot a$
• 外积（叉乘）：
  

即$a\times b=|a||b|\sin <a,b>$，方向垂直于向量$a$、$b$组成的平面，遵循右手定则。
性质：$a\times b=-b\times a$，记为下列公式，这是一个反对称矩阵，满足$A^T=-A$，$\wedge$记作反对称符号。
$$a^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$$

1.2 坐标系间的欧氏变换

(1)
刚体运动：两个坐标系之间的运动由一个旋转加上一个平移组成
对于同一个向量$p$,它在世界坐标系下的坐标$p_w$和在相机坐标系下的坐标$p_c$是不同的。这个变换关系由变换矩阵$T$来描述

(2)
对于坐标系旋转变换，假设原坐标系和现坐标系单位正交基底分别为 $[e_1,e_2,e_3]$ 和 $[e_1^{\prime },e_2^{\prime },e_3^{\prime }]$ , 坐标分别为 $[a_1,a_2,a_3{]}^T$ 和 $[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T$ , 由于向量本身的绝对位置没有改变，则
$$[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3][\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}]=[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$$
将上式两端分别左乘$\left[\begin{array}{c}{e}_1^T\\ e_2^T\\ e_3^T\end{array}\right]$得到，
$$[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}]=\left[\begin{array}{ccc}{\mathbf{e}}_1^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\stackrel{\mathrm{def}}{=}\mathbf{R}{\mathbf{a}}^{\prime }$$
矩阵$\mathbf{R}$即为旋转矩阵 (Rotation Matrix)
性质：

将满足此性质的矩阵集合定义为特殊正交群：
$$\mathrm{SO}(n)=\left\{\mathbf{R}\in {\mathbb{R}}^{n\times n}\mid \mathbf{R}{\mathbf{R}}^{\mathrm{T}}=\mathbf{I},\det (\mathbf{R})=1\right\}$$

(3)旋转加平移

可以将三维空间的刚体运动分解为平移(平移向量$t$)以及旋转运动，$a_1={\mathbf{R}}_{12}{\mathbf{a}}_2+{\mathbf{t}}_{12}$

这里的${\mathbf{R}}_{12}$ 是指“把坐标系 2 的向量变换到坐标系 1” 中,下标从右往左读。

1.3 变换矩阵与齐次坐标

多次变换之后，引用齐次坐标和变换矩阵
$a$到$c$的变换为：
$$a^{\prime }=Ra+t\to \left[\begin{array}{c}{a}^{\prime }\\ \\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ & \\ 0^{\mathrm{T}}& 1\end{array}\right]\left[\begin{array}{c}a\\ \\ 1\end{array}\right]\stackrel{\mathrm{def}}{=}T\left[\begin{array}{c}a\\ \\ 1\end{array}\right]$$
三维向量尾部添加1，称为齐次坐标。
$\mathbf{T}$为变换矩阵(Transform Matrix)，旋转与平移都在这个矩阵中，左上角为旋转矩阵，右侧为平移向量，左下角为 $0$ 向量，右下角为 $1$。这种矩阵又称为特殊欧氏群(Special Euclidean Group):
$$\mathrm{SE}(3)=\{\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|\mathbf{R}\in \mathrm{SO}(3),\mathbf{t}\in {\mathbb{R}}^3\}$$
其逆方向变换为：
$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathrm{T}}& -{\mathbf{R}}^{\mathrm{T}}\mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]$$

齐次坐标简介：

2. 旋转向量与欧拉角

2.1 旋转向量

三维空间中刚体运动有六个自由度 (旋转和平移各三个)。显然无论是上述的旋转矩阵 (9个量)还是变换矩阵 (16个量),都有很大的冗余；并且，矩阵中的元素相互关联，这不利于后续的非线性优化计算。因此我们希望找到一种更为紧凑的表达方式。

任意旋转都可以同一个旋转轴和一个旋转角来刻画，
则使用旋转向量来表达：即向量方向为旋转轴，其模长为旋转角。

罗德里格斯公式描述了旋转矩阵和旋转向量之间的关系：
$$\mathbf{R}=\cos (\theta \mathbf{I})+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^T+\sin (\theta {\mathbf{n}}^{\wedge })$$
反之：
$$\theta =\arccos \frac{\mathfrak{tr}(\mathbf{R})-1}{2}$$
其中，$tr(\mathbf{R})$表示求迹，即矩阵主对角线元素之和。

由于旋转轴在旋转过程中是不动的，则有
$$\mathbf{Rn}=\mathbf{n}$$
这说明转轴$\mathbf{n}$是矩阵$\mathbf{R}$特征值为 1 对应的特征向量，由此可以求出向量$\mathbf{n}$的值。

2.2 欧拉角

欧拉角直观的将旋转分解为X\Y\Z三个轴上的转动，常用和顺序为ZYX旋转，等价于“偏航-俯仰-滚转”(yaw-pitch-roll)：

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

此时可以使用向量$[y,p,r{]}^T$描述任意旋转

万向锁问题：即当俯仰角为$\pm 90°$时，第三次旋转轴和第一次旋转轴重合，使系统丢失了一个自由度，这被称为奇异性问题。因此欧拉角在SLAM较少使用。

旋转向量也存在奇异性，当0超过2π时会产生周期性，这时将其限定在2π范围内便可以避免。

3. 四元数

3.1 四元数定义

紧凑又没有奇异性的三维空间旋转表达方式，但是不够直观运算稍微复杂些。

与复数类比，四元数是一种扩展的复数。

二维空间下，在复数平面空间，结合欧拉公式 $e^{i\theta }=\cos \theta +i\sin \theta$，如下图，向量$z$旋转$90^{\circ }$，相当于乘上$i$

三维空间中，采用四元数描述旋转。
四元数具有一个实部，三个虚部（分别对应z,y,z轴）:
$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$$
写成向量形式:
$$q=[s,\mathbf{v}],s=q_0\in \mathbb{R},\mathbf{v}=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$$

并且，虚部之间满足:
$$\{\begin{array}{c}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$

当四元数的实部为0时，称为虚四元数（分别对应三维坐标），此时，可以用来表示三维空间中的点。

3.2 四元数的计算

现有两个四元数 $q_a,q_b$，它们的向量表示为 $[s_a,{\mathbf{v}}_a{]}^{\mathrm{T}},[s_b,{\mathbf{v}}_b{]}^{\mathrm{T}}$, 或者原始四元数表示为：
$$q_a=s_a+x_{a}i+y_{a}j+z_{a}k,q_b=s_b+x_{b}i+y_{b}j+z_{b}k$$

1. 加法：对应部分相加乘法：每一项相乘，最后相加
2. 模长：各项系数平方和再开方；并且两个四元数乘积的模等于各自模的乘积，即 $\Vert q_1{q}_2\Vert =\Vert q_1\Vert \cdot \Vert q_2\Vert$ ，模长为1的四元数称为单位四元数
3. 共轭：实部相等，虚部互为相反数：$q_a^{\ast }=[s_a,-v_a{]}^T$。并且，$q^{\ast }q=q{q}^{\ast }=[s^2+{\mathbf{v}}^T\mathbf{v}$,$0{]}^T$
4. 逆：$q^{-1}=q^{\ast }/||q|{|}^2$，则$q^{-1}q=q{q}^{-1}=1$，同时，$(q_a{q}_b{)}^{-1}=q_a^{-1}{q}_b^{-1}$
5. 数乘：$k\mathbf{q}=[ks,k\mathbf{v}{]}^T$

3.3 四元数表示旋转

三维空间任意一点均可用一个纯虚四元数来表示：$p=[0,v{]}^T$，
用单位四元数$q$表示三维空间中任意一个旋转，旋转以后得到$p^{\prime }$，则：
$$p^{\prime }=qp{q}^{-1}$$
$p^{\prime }$的虚部为旋转后的点的坐标。

3.4 四元数与其他旋转表示法的转换

角轴（旋转向量）到四元数：
$$\mathbf{q}=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}]$$

四元数到角轴（旋转向量）：
$$\{\begin{array}{l}\theta =2arccos{q}_0\\ [n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}\end{array}$$

四元数到旋转矩阵：
$$\{\begin{array}{l}\mathbf{R}=\mathbf{v}{\mathbf{v}}^{\mathrm{T}}+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+{\left({\mathbf{v}}^{\wedge }\right)}^2\\ \theta =2\arccos s\end{array}$$

3.5 相似、仿射、射影变换