SLAM LECTURE 3：三维空间刚体运动

L3 三维空间刚体运动

§3.1 旋转矩阵

§3.1.1 内外积Recap

1. 内积：对于 $\mathit{a},\mathit{b}\in {\mathbb{R}}^3$ ,

$$\begin{array}{}\text{(3.1)}& \mathit{a}\cdot \mathit{b}={\mathit{a}}^{\mathrm{T}}\mathit{b}=\sum _{i=1}^3{a}_i{b}_i=\left|\mathit{a}\right|\left|\mathit{b}\right|\cos ⟨\mathit{a},\mathit{b}⟩.\end{array}$$

2. 外积：

$$\begin{array}{}\text{(3.2)}& \mathit{a}\times \mathit{b}=\Vert \begin{array}{ccc}{\mathit{e}}_1& {\mathit{e}}_2& {\mathit{e}}_3\\ \\ a_1& a_2& a_3\\ \\ b_1& b_2& b_3\end{array}\Vert =\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ \\ a_3{b}_1-a_2{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]\mathit{b}\stackrel{\mathrm{def}}{=}{\mathit{a}}^{\wedge }\mathit{b}.\end{array}$$

其中 ${}^{\wedge }$ 符号将 $\mathit{a}$ 一一映射成一个反对称矩阵 ${\mathit{a}}^{\wedge }$ ，后文将经常用到

§3.1.2 坐标系间的欧式变换

旋转中，设某个单位正交基 $({\mathit{e}}_1,{\mathit{e}}_2,{\mathit{e}}_3)$ 经过一次旋转变成了 $({\mathit{e}}_1^{\prime },{\mathit{e}}_2^{\prime },{\mathit{e}}_3^{\prime })$ ，根据坐标的定义有，

$$\begin{array}{}\text{(3.3)}& [{\mathit{e}}_1,{\mathit{e}}_2,{\mathit{e}}_3]\left[\begin{array}{c}{a}_1\\ \\ a_2\\ \\ a_3\end{array}\right]=[{\mathit{e}}_1^{\prime },{\mathit{e}}_2^{\prime },{\mathit{e}}_3^{\prime }]\left[\begin{array}{c}{a}_1^{\prime }\\ \\ a_2^{\prime }\\ \\ a_3^{\prime }\end{array}\right].\end{array}$$

两边同时左乘 $[{\mathit{e}}_1,{\mathit{e}}_2,{\mathit{e}}_3{]}^{\mathrm{\top }}$ ，得到，

$$\begin{array}{}\text{(3.4)}& \left[\begin{array}{c}{a}_1\\ \\ a_2\\ \\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^{\mathrm{T}}{e}_1^{\prime }& e_1^{\mathrm{T}}{e}_2^{\prime }& e_1^{\mathrm{T}}{e}_3^{\prime }\\ \\ e_2^{\mathrm{T}}{e}_1^{\prime }& e_2^{\mathrm{T}}{e}_2^{\prime }& e_2^{\mathrm{T}}{e}_3^{\prime }\\ \\ e_3^{\mathrm{T}}{e}_1^{\prime }& e_3^{\mathrm{T}}{e}_2^{\prime }& e_3^{\mathrm{T}}{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}}{=}\mathit{R}{\mathit{a}}^{\prime }.\end{array}$$

其中 $\mathit{R}$ 成为旋转矩阵（Rotation Matix），且是一个行列式为1的正交矩阵。

n为旋转矩阵的集合定义如下：

$$\begin{array}{}\text{(3.5)}& \mathrm{SO}(n)=\{\mathit{R}\in {\mathbb{R}}^{n\times n}|{\mathit{R}}^{\mathrm{T}}\mathit{R}=\mathit{I},det(\mathit{R})=1\}\end{array}$$

其中SO(n)是特殊正交群（Special Orthogonal Group）的意思。

平移中，直接相加即可，记作 $\mathit{t}$ ，称为平移向量。

因此，把旋转和平移合到一起，有

$$\begin{array}{}\text{(3.6)}& {\mathit{a}}^{\prime }=\mathit{R}\mathit{a}+\mathit{t}\end{array}$$

完整地描述了一个欧氏空间的坐标变换关系。

实际上，对于坐标系1、坐标系2，你们向量 $\mathit{a}$ 在两个坐标系下的坐标为 ${\mathit{a}}_1$ ， ${\mathit{a}}_2$ ，它们之间的关系应该是：

$$\begin{array}{}\text{(3.7)}& {\mathit{a}}_1={\mathit{R}}_{12}{\mathit{a}}_2+{\mathit{t}}_{12}\end{array}$$

其中， ${\mathit{R}}_{12}$ 是指“把坐标系2的向量变换到坐标系1”中， ${\mathit{t}}_{12}$ 对应的是坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标，下标需尤其注意。

§3.1.3 变换矩阵与齐次坐标

考虑到变换关系非线性，多次变换后数学式会很复杂，因此引入齐次坐标和变换矩阵，重写式(3.6)：

$$\begin{array}{}\text{(3.8)}& \left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}\right]\left[\begin{array}{c}a\\ \\ 1\end{array}\right]\stackrel{\mathrm{def}}{=}\mathit{T}\left[\begin{array}{c}a\\ \\ 1\end{array}\right].\end{array}$$

在三维向量的末尾添加1，变成了四维向量，成为齐次坐标，从而将变换变成线性关系，矩阵 $\mathit{T}$ 称为变换矩阵(Transform Matrix)，后续在不引起歧义的情况下，写成 $\mathit{b}=\mathit{T}\mathit{a}$ 时，默认进行了齐次坐标的变换。

关于变换矩阵 $\mathit{T}$ ，左上角为旋转矩阵，右上角为平移向量，左下角为 $\mathbf{0}$ ，右下角为1，这种矩阵又称为特殊欧式群(Special Euclidean Group):

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

该矩阵的逆表示一个反向的变换：

$$\begin{array}{}\text{(3.10)}& {\mathit{T}}^{-1}=\left[\begin{array}{cc}{\mathit{R}}^{\mathrm{T}}& -{\mathit{R}}^{\mathrm{T}}\mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}\right].\end{array}$$

同样，我们用 ${\mathit{T}}_{12}$ 来表示从2到1的变换。

§3.3 旋转向量和欧拉角

§3.3.1 旋转向量

旋转矩阵用9个参数和一些限制条件来表示仅有3个自由度的旋转，不够紧凑，因而提出使用一个三维向量来表示旋转，其方向与旋转轴一致，而长度等于旋转角，这种向量被成为旋转向量（Axis-Angle）

考虑某个旋转对应的旋转矩阵为 $\mathit{R}$ ，其旋转轴为一个单位长度的向量 $\mathit{n}$ ，旋转角为 $\theta$ ，那么向量 $\theta \mathit{n}$ 即是描述这个旋转的旋转向量，下面给出两种表达方式之间的转换关系。

从旋转向量到旋转矩阵的转换过程由罗德里格斯公式(Rodrigues’s Formula)表明，公式如下：

$$\begin{array}{}\text{(3.11)}& \mathit{R}=\cos \theta \mathit{I}+(1-\cos \theta )\mathit{n}{\mathit{n}}^{\mathrm{T}}+\sin \theta {\mathit{n}}^{\wedge }.\end{array}$$

从一个旋转矩阵到旋转向量的计算过程分两步走。对于转角 $\theta$ ，取两边的trace，得到：

$$\begin{array}{}\text{(3.12)}& \begin{array}{rl}\mathrm{tr}\left(R\right)& =\cos \theta \mathrm{tr}\left(\mathit{I}\right)+(1-\cos \theta )\mathrm{tr}\left(\mathit{n}{\mathit{n}}^{\mathrm{T}}\right)+\sin \theta \mathrm{tr}({\mathit{n}}^{\wedge })\\ & =3\cos \theta +(1-\cos \theta )\\ & =1+2\cos \theta \end{array}\end{array}$$

因此：

$$\begin{array}{}\text{(3.13)}& \theta =\arccos \frac{\mathrm{tr}(\mathit{R})-1}{2}.\end{array}$$

关于转轴 $\mathit{n}$ ，旋转轴上的向量在旋转后不发生变化，说明：

$$\begin{array}{}\text{(3.14)}& \mathit{R}\mathit{n}=\mathit{n}.\end{array}$$

因此， $\mathit{n}$ 是矩阵 $\mathit{R}$ 特征值1对应的特征向量，求解后再归一化得到转轴。

tip：旋转向量具有奇异性， $\theta ={90}^{\circ }$ 时旋转轴不唯一，丢失自由度

§3.3.2 欧拉角

以ZYX为例，将任意旋转分解成以下3个轴的转角：

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

此时可以使用 $[r,p,y{]}^{\mathrm{T}}$ 来描述仍以旋转。

但是存在一个重大缺点，会出现万向锁问题（Gimbal Lock），即当俯仰角pitch = ±90°时第一次旋转与第三次旋转将使用同一个轴，从而丢失了一个自由度，这被称为奇异性问题。

§3.4 四元数

§3.4.1 定义

一个四元数 $\mathit{q}$ 拥有一个实部和三个虚部，

$$\begin{array}{}\text{(3.15)}& \mathit{q}=q_0+q_1\mathrm{i}+q_2\mathrm{j}+q_3\mathrm{k}\end{array}$$

其中 $\mathrm{i}$ , $\mathrm{j}$ , $\mathrm{k}$ 为三个虚部，且满足关系式：

$$\begin{array}{}\text{(3.16)}& \{\begin{array}{l}{\mathrm{i}}^2={\mathrm{j}}^2={\mathrm{k}}^2=-1\\ \mathrm{i}\mathrm{j}=\mathrm{k},\mathrm{j}\mathrm{i}=-\mathrm{k}\\ \mathrm{j}\mathrm{k}=\mathrm{i},\mathrm{k}\mathrm{j}=-\mathrm{i}\\ \mathrm{k}\mathrm{i}=\mathrm{j},\mathrm{i}\mathrm{k}=-\mathrm{j}& \end{array}\end{array}$$

有时，四元数也可以用一个标量和一个向量来表示，

$$\begin{array}{}\text{(3.17)}& \mathit{q}={[s,\mathit{v}]}^{\mathrm{T}},s=q_0\in \mathbb{R},\mathit{v}={[q_1,q_2,q_3]}^{\mathrm{T}}\in {\mathbb{R}}^3.\end{array}$$

其中， $s$ 成为四元数的实部，而 $\mathit{v}$ 称为虚部。可以用单位四元数来表示三维空间中任意一个旋转。

§3.4.2 运算

现有两个四元数 ${\mathit{q}}_a$ ， ${\mathit{q}}_b$ ，它们的向量表示为 $[s_a,{\mathit{v}}_a{]}^{\mathrm{T}}$ ， $[s_b,{\mathit{v}}_b{]}^{\mathrm{T}}$ ，运算可表示如下。

1. 加减法（数乘类似）

$$\begin{array}{}\text{(3.18)}& {\mathit{q}}_a\pm {\mathit{q}}_b={[s_a\pm s_b,{\mathit{v}}_a\pm {\mathit{v}}_b]}^{\mathrm{T}}.\end{array}$$

2. 乘法：每项相乘相加，整理可得

$$\begin{array}{}\text{(3.19)}& {\mathit{q}}_a{\mathit{q}}_b={\left[\begin{array}{c}{s}_a{s}_b-{\mathit{v}}_a^{\mathrm{T}}{\mathit{v}}_b,s_a{\mathit{v}}_b+s_b{\mathit{v}}_a+{\mathit{v}}_a\times {\mathit{v}}_b\end{array}\right]}^{\mathrm{T}}.\end{array}$$

需注意，不满足交换律

3. 模长

$$\begin{array}{}\text{(3.20)}& \parallel q_a\parallel =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}.\end{array}$$

且满足

$$\begin{array}{}\text{(3.21)}& \parallel {\mathit{q}}_a{\mathit{q}}_b\parallel =\parallel {\mathit{q}}_a\parallel \parallel {\mathit{q}}_b\parallel .\end{array}$$

4. 共轭

$$\begin{array}{}\text{(3.22)}& {\mathit{q}}_a^{\ast }=[{\mathrm{s}}_{\mathrm{a}},-{\mathrm{v}}_{\mathrm{a}}{]}^{\mathrm{T}}.\end{array}$$

四元数共轭与自身相乘，会得到一个实四元数，其实部为模长的平方：

$$\begin{array}{}\text{(3.23)}& {\mathit{q}}^{\ast }\mathit{q}=\mathit{q}{\mathit{q}}^{\ast }={[s_a^2+{\mathit{v}}^{\mathrm{T}}\mathit{v},\mathbf{0}]}^{\mathrm{T}}.\end{array}$$

5. 逆

$${\mathit{q}}^{-1}={\mathit{q}}^{\ast }/\parallel \mathit{q}{\parallel }^2.$$

因此，四元数和自己的逆的成绩为实四元数 $\mathbf{1}$

$$\begin{array}{}\text{(3.24)}& {\mathit{q}}^{-1}\mathit{q}=\mathit{q}{\mathit{q}}^{-1}=\mathbf{1}.\end{array}$$

对于单位四元数，其逆和共轭是一个量。

同时，乘积的逆具有和矩阵的相似性质：

$$\begin{array}{}\text{(3.25)}& {\left({\mathit{q}}_a{\mathit{q}}_b\right)}^{-1}={\mathit{q}}_b^{-1}{\mathit{q}}_a^{-1}.\end{array}$$

§3.4.3 用四元数表示旋转

我们可以用四元数表达对一个点的旋转。假设有一个空间三维点 $\mathit{p}=[x,y,z]\in {\mathbb{R}}^3$ ，以及一个单位四元数 $\mathit{q}$ 来指定将 $\mathit{p}$ 旋转至 ${\mathit{p}}^{\prime }$ 。

首先，把三维空间点用一个虚四元数来描述：

$$\begin{array}{}\text{(3.26)}& \mathit{p}=[0,x,y,z{]}^{\mathrm{T}}=[0,\mathit{v}{]}^{\mathrm{T}}.\end{array}$$

则旋转后的 ${\mathit{p}}^{\prime }$ 可以表示为：

$$\begin{array}{}\text{(3.27)}& {\mathit{p}}^{\prime }=\mathit{q}\mathit{p}{\mathit{q}}^{-1}.\end{array}$$

§3.4.4 四元数到其他旋转表示的转换

设 $\mathit{q}=[s,\mathit{v}{]}^{\mathrm{T}}$ ，定义如下符号：

$$\begin{array}{}\text{(3.28)}& {\mathit{q}}^{+}=\left[\begin{array}{cc}s& -{\mathit{v}}^{\mathrm{T}}\\ \\ \mathit{v}& s\mathit{I}+{\mathit{v}}^{\wedge }\end{array}\right],{\mathit{q}}^{\oplus }=\left[\begin{array}{cc}s& -{\mathit{v}}^{\mathrm{T}}\\ \\ \mathit{v}& s\mathit{I}-{\mathit{v}}^{\wedge }\end{array}\right].\end{array}$$

可证明

$$\begin{array}{}\text{(3.29)}& {\mathit{q}}_1{\mathit{q}}_2={\mathit{q}}_1^{+}{\mathit{q}}_2={\mathit{q}}_2^{\oplus }{\mathit{q}}_1.\end{array}$$

于是可以得到

$$\begin{array}{}\text{(3.30)}& \begin{array}{rl}{\mathit{p}}^{\prime }& =\mathit{q}\mathit{p}{\mathit{q}}^{-1}={\mathit{q}}^{+}{\mathit{p}}^{+}{\mathit{q}}^{-1}\\ & ={\mathit{q}}^{+}{\mathit{q}}^{-1^{\oplus }}\mathit{p}.\end{array}.\end{array}$$

代入矩阵可得

$$\begin{array}{}\text{(3.31)}& {\mathit{q}}^{+}({\mathit{q}}^{-1}{)}^{\oplus }=\left[\begin{array}{cc}1& \mathbf{0}\\ {\mathbf{0}}^{\mathrm{T}}& \mathit{v}{\mathit{v}}^{\mathrm{T}}+s^2\mathit{I}+2s{\mathit{v}}^{\wedge }+{\left({\mathit{v}}^{\wedge }\right)}^2\end{array}\right].\end{array}$$

矩阵的右下角即给出了从四元数到旋转矩阵的关系：

$$\begin{array}{}\text{(3.32)}& \mathit{R}=\mathit{v}{\mathit{v}}^{\mathrm{T}}+s^2\mathit{I}+2s{\mathit{v}}^{\wedge }+{\left({\mathit{v}}^{\wedge }\right)}^2.\end{array}$$

要求四元数到旋转向量的转换公式，类似地可求得：

$$\begin{array}{}\text{(3.33)}& \{\begin{array}{l}\theta =2\arccos q_0\\ [n_x,n_y,n_z{]}^{\mathrm{T}}=[q_1,q_2,q_3{]}^{\mathrm{T}}/\sin \frac{\theta }{2}\end{array}\end{array}$$

§3.5 相似、仿射、射影变换

欧式变换保持了向量的长度和夹角，而下面几种变换会改变外形。

§3.5.1 相似变换

比欧式变换多了一个自由度，允许物体进行缩放，其矩阵表示为

$$\begin{array}{}\text{(3.34)}& {\mathit{T}}_S=\left[\begin{array}{cc}s\mathit{R}& \mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}\right].\end{array}$$

旋转部分多了一个缩放因子 $s$ ，三维相似变换的集合叫做相似变换群，记作 $\mathrm{Sim}(3)$

§3.5.2 仿射变换

$$\begin{array}{}\text{(3.34)}& {\mathit{T}}_A=\left[\begin{array}{cc}\mathit{A}& \mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}\right].\end{array}$$

仿射变换只要求 $\mathit{A}$ 是一个可逆矩阵，而不必是正交矩阵。仿射变换也叫正交投影，经过仿射变换，直线仍为直线，保持平行关系。

三维的仿射变换有12个自由度： $3$ 个平移， $3\times 3=9$ 个线性变换

§3.5.3 射影变换

$$\begin{array}{}\text{(3.35)}& T_P=\left[\begin{array}{cc}\mathit{A}& \mathit{t}\\ \\ {\mathit{a}}^{\mathrm{T}}& v\end{array}\right].\end{array}$$

左上角 $\mathit{A}$ 是一个可逆矩阵，左下角为缩放 ${\mathit{a}}^{\mathrm{T}}$ ，是最一般的变换。由于采用了齐次坐标，矩阵乘以任何非零常数，代表的变换是完全一样的，因此二维的射影变换自由度为8，三维的自由度为15。