旋转矩阵、变换矩阵、旋转向量、欧拉角、四元数

视觉SLAM十四讲(三)——三维空间刚体运动(上)

• 三维空间刚体运动的描述方法有：旋转矩阵、变换矩阵、旋转向量、欧拉角和四元数，接下来将逐一介绍它们

一、旋转矩阵

1. 点、向量、坐标系
   * 点——存在于三维空间之中，点和点组成向量，点本身由原点指向它的向量所描述
   * 向量——带指向性的箭头，可以进行加法减法等运算，定义坐标系后，向量可以由 R3 $R^3$当中的三个数表示， 如何理解这句话呢。如下图所示：
   
   在代数中，我们用一组基底和向量 a $a$ 在每个坐标轴上的投影来表示一个向量，对于 a $a$，通过某种线性组合，可以表示为 a=axe1+aye2+aze3 $a=a_x{e}_1+a_y{e}_2+a_z{e}_3$
   而上面那句话的意思是在矩阵运算中， a $a$ 可以表示为 ⎡⎣⎢axayaz⎤⎦⎥ $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$，因为 (e1,e2,e3)⎡⎣⎢axayaz⎤⎦⎥=axe1+aye2+aze3 $(e_1,e_2,e_3)\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=a_x{e}_1+a_y{e}_2+a_z{e}_3$
   * 坐标系——三个正交的轴，构成线性空间的一组基，分为左手系和右手系
   * 向量的运算可以由坐标运算来表达：加减法，内积，外积
2. 问题的出现——一个最简单的情况，机器人从某一点直线运动到另一点，假设机器人是质点，并且和目标点处于同一平面上，分别以机器人和目标点建立坐标系，在移动过程中机器人的坐标系位置一直在变，要计算与目标点的距离，就需要描述坐标系之间如何变化
   * 进而——如何计算同一个向量在不同坐标系里的坐标
   * 如果刚才的机器人不是直线运动，而会有拐弯，这时坐标系就会旋转，因此描述整个运动过程就是三个轴的旋转和原点间的平移，这就是所谓的欧式变换，保证同一个向量在各个坐标系下的长度和夹角都不会发生变化，通过旋转和平移两部分组成

3. 问题解决
   * 平移是一个向量
   * 旋转

   • 设某坐标系(用三个方向上的单位向量表示) (e1,e2,e3) $(e_1,e_2,e_3)$ 发生了一次旋转，变成了 (e′1,e′2,e′3) $(e_1^{{}^{\prime }},e_2^{{}^{\prime }},e_3^{{}^{\prime }})$
   • 对于某个固定的向量 a $a$(向量不随坐标系旋转)，它的坐标怎么变化，其中 ⎡⎣⎢a1a2a3⎤⎦⎥ $\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$ 是 a $a$ 在第一个坐标系中的坐标， ⎡⎣⎢⎢a′1a′2a′3⎤⎦⎥⎥ $\left[\begin{array}{c}{a}_1^{{}^{\prime }}\\ a_2^{{}^{\prime }}\\ a_3^{{}^{\prime }}\end{array}\right]$ 是 a $a$ 在另一个坐标系中的坐标，如图，P为向量 a $a$
     
   • 坐标关系 [e1,e2,e3]⎡⎣⎢a1a2a3⎤⎦⎥=[e′1,e′2,e′3]⎡⎣⎢⎢a′1a′2a′3⎤⎦⎥⎥  $[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=[e_1^{{}^{\prime }},e_2^{{}^{\prime }},e_3^{{}^{\prime }}]\left[\begin{array}{c}{a}_1^{{}^{\prime }}\\ a_2^{{}^{\prime }}\\ a_3^{{}^{\prime }}\end{array}\right]$，乘出来的就是向量本身
   • 左乘 ⎡⎣⎢⎢eT1eT2eT3⎤⎦⎥⎥ $\left[\begin{array}{c}{e}_1^T\\ e_2^T\\ e_3^T\end{array}\right]$,得： ⎡⎣⎢a1a2a3⎤⎦⎥=⎡⎣⎢⎢eT1e′1eT2e′1eT3e′1eT1e′2eT2e′2eT3e′2eT1e′3eT2e′3eT3e′3⎤⎦⎥⎥⎡⎣⎢⎢a′1a′2a′3⎤⎦⎥⎥Δ=Ra′ $\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]\begin{array}{c}\mathrm{\Delta }\\ =\end{array}R{a}^{{}^{\prime }}$
   • 中间的矩阵 R $R$ 称为旋转矩阵
   • 根据定义可以验证
     * R $R$ 是一个正交矩阵(矩阵的逆是其转置)
     * R $R$ 的行列式为 +1 $+1$
   • 满足这两个性质的矩阵称为旋转矩阵

   *旋转矩阵描述了两个坐标的变换关系，ex： a1=R12a2 $a_1=R_{12}{a}_2$，反之 a2=R21a1 $a_2=R_{21}{a}_1$，于是 R21=R−112=RT12 $R_{21}=R_{12}^{-1}=R_{12}^T$， 进一步，三个坐标系亦有 a3=R32a2=R32R21a1=R31a1 $a_3=R_{32}{a}_2=R_{32}{R}_{21}{a}_1=R_{31}{a}_1$
   * 加上平移 a′a3=R32a2=R32R21a1=R31a1 $a_3=R_{32}{a}_2=R_{32}{R}_{21}{a}_1=R_{31}{a}_1$
   * 加上平移 a′=Ra+t