视觉SLAM十四讲：第3讲 三维空间刚体运动

第3讲：三维空间刚体运动

三维空间中刚体运动的描述方式：旋转矩阵、变换矩阵、四元数和欧拉角

3.1 旋转矩阵

3.1.1 点和向量，坐标系

三维空间中，给定线性空间基$({\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3)$，则向量$\mathbf{a},\mathbf{b}\in {\mathbb{R}}^3$，

$$\mathbf{a}=\left[\begin{array}{ccc}{\mathbf{e}}_1& {\mathbf{e}}_2& {\mathbf{e}}_3\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{\mathbf{e}}_1+a_2{\mathbf{e}}_2+a_3{\mathbf{e}}_3\text{\hspace{1em}(3-1)}$$

内积：

$$\mathbf{a}\cdot \mathbf{b}={\mathbf{a}}^{\text{T}}\mathbf{b}=\sum _{i=1}^3{a}_i{b}_i=|\mathbf{a}||\mathbf{b}|\cos ⟨\mathbf{a},\mathbf{b}⟩\text{\hspace{1em}(3-2)}$$

内积描述向量间的投影关系。外积为：

$$\mathbf{a}\times \mathbf{b}=\left[\begin{array}{ccc}\mathbf{i}& \mathbf{j}& \mathbf{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]\mathbf{b}\triangleq {\mathbf{a}}^{\wedge }\mathbf{b}\text{\hspace{1em}(3-3)}$$

外积的方向垂直于这两个向量，大小为$|\mathbf{a}||\mathbf{b}|⟨\mathbf{a},\mathbf{b}⟩$，表示两个向量张成的四边形的有向面积。符号$\wedge$表示反对称符号，将$\mathbf{a}$改写成反对称矩阵（skew-symmetric）形式，其作用为将外积$\mathbf{a}\times \mathbf{b}$写成矩阵与向量的乘积${\mathbf{a}}^{\wedge }\mathbf{b}$，从而将外积变成线性运算。外积只对三维向量存在定义，外积可以表示向量的旋转。

在右手法则下，用右手的四指从$\mathbf{a}$转向$\mathbf{b}$，其拇指方向就是旋转向量的方向，即$\mathbf{a}\times \mathbf{b}$的方向，其大小由 $\mathbf{a}$和$\mathbf{b}$的夹角决定。该方式构造了从$\mathbf{a}$到$\mathbf{b}$的旋转向量，该向量同样位于三维空间中，在此坐标系下，可以用三个实数描述。

3.1.2 坐标系间的欧氏变换

两个坐标系间的变换关系：旋转、平移。机器人在运动过程中，通常设定一个惯性坐标系（世界坐标系），认为其固定不动，如图中$x_W,y_W,z_W$；相机或机器人则是一个移动坐标系，如$x_C,y_C,z_C$。相机视野中某个向量$\mathbf{p}$，其坐标为${\mathbf{p}}_c$，在世界坐标系中，其坐标为${\mathbf{p}}_w$。

刚体运动：同一个向量在各个坐标系下的长度和夹角不会改变，这种变换称为欧氏变换。

旋转：假设一组单位正交基$({\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3)$经一次旋转，变成$({\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime })$，对于同一个向量$\mathbf{a}$（该向量并没有随坐标系旋转而运动），其在两个坐标系下的坐标分别为$[a_1,a_2,a_3{]}^{\text{T}}$和$[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^{\text{T}}$，则：

[ e 1 e 2 e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ e 2 ′ e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] (3-4) \begin{bmatrix} \mathbf{e}_{1} & \mathbf{e}_{2} & \mathbf{e}_{3} \end{bmatrix} \begin{bmatrix} a_{1} \\ a_{2} \\ a_{3} \end{bmatrix} = \begin{bmatrix} \mathbf{e}_{1}^{\prime} & \mathbf{e}_{2}^{\prime} & \mathbf{e}_{3}^{\prime} \end{bmatrix} \begin{bmatrix} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{bmatrix} \tag {3-4} [e1​​e2​​e3​​]⎣⎡​a1​a2​a3​​⎦⎤​=[e1′​​e2′​​e3′​​]⎣⎡​a1′​a2′​a3′</