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

视觉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⎤⎦⎥ [ a x a y a z ] ，因为 (e1,e2,e3)⎡⎣⎢axayaz⎤⎦⎥=axe1+aye2+aze3 ( e 1 , e 2 , e 3 ) [ a x a y a z ] = 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 ′ , e 2 ′ , e 3 ′ )
   • 对于某个固定的向量 a a (向量不随坐标系旋转)，它的坐标怎么变化，其中 ⎡⎣⎢a1a2a3⎤⎦⎥ [ a 1 a 2 a 3 ] 是 a a 在第一个坐标系中的坐标， ⎡⎣⎢⎢a′1a′2a′3⎤⎦⎥⎥ [ a 1 ′ a 2 ′ a 3 ′ ] 是 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 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ]   ，乘出来的就是向量本身
   • 左乘 ⎡⎣⎢⎢eT1eT2eT3⎤⎦⎥⎥ [ e 1 T e 2 T e 3 T ] ,得： ⎡⎣⎢a1a2a3⎤⎦⎥=⎡⎣⎢⎢eT1e′1eT2e′1eT3e′1eT1e′2eT2e′2eT3e′2eT1e′3eT2e′3eT3e′3⎤⎦⎥⎥⎡⎣⎢⎢a′1a′2a′3⎤⎦⎥⎥Δ=Ra′ [ a 1 a 2 a 3 ] = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] Δ = R a ′
   • 中间的矩阵 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′=Ra+t a ′ = R a + t ，因此两个坐标系的刚体运动可以由 R，t R ， t 完全描述

二、变换矩阵

• 变换矩阵的引入
  * 用旋转和平移方式有一点不便之处，比如发生了两次变换 b=R1a+t1，c=R2b+t2 b = R 1 a + t 1 ， c = R 2 b + t 2
  * 这时 c=R2(R1a+t1)+t2 c = R 2 ( R 1 a + t 1 ) + t 2 ，叠加起来过于复杂
  * 改变形式，写成 [a′1]=[R0Tt1][a1]Δ=T[a1] [ a ′ 1 ] = [ R t 0 T 1 ] [ a 1 ] Δ = T [ a 1 ]
  * 例如 b=T1a，c=T2b， b = T 1 a ， c = T 2 b ， 得出 c=T2T1a c = T 2 T 1 a
• 这种用四个数表达三维向量的做法称为齐次坐标，这样旋转和平移可以放入同一个矩阵，称为变换矩阵，即 [R0Tt1] [ R t 0 T 1 ] ,其反向变换为 [RT0T−RTt1] [ R T − R T t 0 T 1 ] ，即矩阵的逆
• 在 SLAM 中，通常定义世界坐标系 TW T W 与 机器人坐标系 TR T R ，一个点的世界坐标为 pW p W ，机器人坐标系下为 pR p R ，那么满足关系： pR=TRWpW p R = T R W p W ，反之亦然，实际编程中可以使用 TRW T R W 或 TWR T W R 来描述机器人位姿

三、旋转向量和欧拉角

1. 旋转向量
   * 旋转矩阵和变换矩阵固然可以表示旋转，但是要求太多：每次旋转只需要三个自由度，也就是x，y，z，但是旋转矩阵用9个量表达了3个自由度，变换矩阵用16个量表达了6个自由度，这种表达方式是冗余的，而且旋转矩阵和变换矩阵都必须是正交矩阵，自身约束会在实际求解中增加困难
   * 任意旋转都可以用一个旋转轴和一个旋转角刻画，如何理解这句话呢，我们引入向量的外积的概念，别的你先不用知道，记住这句话：向量的外积的方向垂直于这两个向量。是不是一下子明白了，对于某个坐标系的一次旋转，可以通过移动后与该向量的外积来确定它是怎么旋转的
   * 方向为旋转轴，长度为转过的角度，这就称为轴角或旋转向量 ， w=θn w = θ n
   * 轴角与旋转矩阵的不同：旋转矩阵需要九个量，有正交性约束和行列式约束，轴角：三个量，没有约束
   * 只是表达方式不同，表达内容一样
   * 轴角即为李代数，这里只是简单了解一下，下一篇会介绍它的原理
   * 轴角转旋转矩阵—— R=cosθI+(1−cosθ)nnT+sinθn∧ R = cos ⁡ θ I + ( 1 − cos ⁡ θ ) n n T + sin ⁡ θ n ∧
   * 旋转矩阵转轴角——角度： θ=arccos(tr(R)−12) θ = arccos ⁡ ( t r ( R ) − 1 2 ) ，轴： Rn=n R n = n
2. 欧拉角
   * 将旋转分解为三次不同轴上的转动，以便理解
   * 例如：按照 Z-Y-X 顺序转动
   * 轴可以使定轴或动轴，顺序亦可不同，因此存在许多种定义方式不同的欧拉角
   * 常见的有 yaw-pitch-roll (偏航-俯仰-滚转) 角等等
3. 欧拉角的万向锁问题
   * ZYX顺序中，若 pitch 为正负90度，则第三次旋转和第一次绕同一个轴，使得系统丢失了一个自由度——存在奇异性问题
   * 有点难理解，可以看看这个视频：视频传送门

四、四元数

1. 一种扩展的复数
   * 回忆：复数可以表达二维平面的旋转
   * 怎么理解这句话，首先看一下复数的另一种表达方式——矩阵表达式
   * a+ib↔(ab−ba)=r[cosφsinφ−sinφcosφ]=rexp(φ[01−10]) a + i b ↔ ( a − b b a ) = r [ cos ⁡ φ − sin ⁡ φ sin ⁡ φ cos ⁡ φ ] = r exp ⁡ ( φ [ 0 − 1 1 0 ] )
   * a,b a , b 为实数，此类矩阵的和、积及乘法逆都是此类矩阵， (ab−ba)=a(1001)+b(01−10) ( a − b b a ) = a ( 1 0 0 1 ) + b ( 0 − 1 1 0 ) ,即实数1对应着单位矩阵 (1001) ( 1 0 0 1 ) ，虚数单位 i i 对应着 (01−10) ( 0 − 1 1 0 )
   * 现假设 a a = 1, b b = 1，在复平面上代表点(1,1)，给 1+i 1 + i 乘以 i i ，即原实数单位对应的矩阵成为 (0−1−10) ( 0 − 1 − 1 0 ) ，行列式值为 -1，代表点(-1,1)，相当于逆时针旋转了90度
   * 对于 a+ib a + i b ，如果 a a 本身是 m+jn m + j n ，虚部为 j j ， b b 也是一个复数 x+ky x + k y ，虚部为 k k ，则构成的代数就是四元数
2. 四元数有三个虚部，可以表达三维空间的旋转
   * q=q0+q1i+q2j+q3k q = q 0 + q 1 i + q 2 j + q 3 k
   * q=[s,v]，s=q0∈R，v=[q1,q2,q3]T∈R3 q = [ s , v ] ， s = q 0 ∈ R ， v = [ q 1 , q 2 , q 3 ] T ∈ R 3
   * 虚部之间的关系 ⎧⎩⎨⎪⎪⎪⎪i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j { i 2 = j 2 = k 2 = − 1 i j = k , j i = − k j k = i , k j = − i k i = j , i k = − j
   * 四元数表示空间中的点——若三维空间的一个点的坐标为(x,y,z)，则它用纯四元数(类似于纯虚数，实部为0) xi+yj+zk x i + y j + z k 表示
   * 单位四元数——单位四元数的欧拉公式: cosθ2+(xi+yj+zk)sinθ2 cos ⁡ θ 2 + ( x i + y j + z k ) sin ⁡ θ 2 ，则 q=[cosθ2,nxsinθ2,nysinθ2,nzsinθ2]T q = [ cos ⁡ θ 2 , n x sin ⁡ θ 2 , n y sin ⁡ θ 2 , n z sin ⁡ θ 2 ] T 表示单位四元数，其中 [nx,ny,nz]T [ n x , n y , n z ] T 是一个单位向量
   * 四元数的一些运算和性质
   
   • 加减法： qa±qb=[sa±sb,va±vb] q a ± q b = [ s a ± s b , v a ± v b ]
   • 乘法： qaqb=sasb−xaxb−yayb−zazb+(saxb−xasb−yazb−zayb)i+(sayb−xazb−yasb−zaxb)j+(sazb−xayb−yaxb−zasb)k q a q b = s a s b − x a x b − y a y b − z a z b + ( s a x b − x a s b − y a z b − z a y b ) i + ( s a y b − x a z b − y a s b − z a x b ) j + ( s a z b − x a y b − y a x b − z a s b ) k
   • 乘法(向量表示)： qaqb=[sasb−vTavb,savb+sbva+va×vb] q a q b = [ s a s b − v a T v b , s a v b + s b v a + v a × v b ]
   • 共轭： q∗a=sa−xai−yaj−zak=[sa,−va] q a ∗ = s a − x a i − y a j − z a k = [ s a , − v a ]
   • 模长： ||qa||=s2a+s2a+y2a+z2a−−−−−−−−−−−−−−√ | | q a | | = s a 2 + s a 2 + y a 2 + z a 2
   • 逆： q−1=q∗/||q||2 q − 1 = q ∗ / | | q | | 2
   • 数乘： kq=[ks,kv] k q = [ k s , k v ]
   • 点乘： qa⋅qb=sasb+xaxbi+yaybj+zazbk q a · q b = s a s b + x a x b i + y a y b j + z a z b k
3. 四元数和轴角的关系
   * 来看看旋转向量，某个旋转是绕着单位向量 n=[nx,ny,nz] n = [ n x , n y , n z ] 进行了角度为 θ θ 的旋转，那么其四元数形式为： q=[cosθ2,nxsinθ2,nysinθ2,nzsinθ2]T q = [ cos ⁡ θ 2 , n x sin ⁡ θ 2 , n y sin ⁡ θ 2 , n z sin ⁡ θ 2 ] T ，你可能会产生疑问，为什么是 θ2 θ 2 , 这个问题下面再说
   * 四元数到轴角： {θ=2arccosq0[nx,nynz]T=[q1,q2,q3]T/sinθ2 { θ = 2 arccos ⁡ q 0 [ n x , n y n z ] T = [ q 1 , q 2 , q 3 ] T / sin ⁡ θ 2
   * 类似可知四元数亦可转换为旋转矩阵，欧拉角
4. 如何用单位四元数表示一个三维空间旋转
   
   • 设点 p p 经过一次以 q q 表示的旋转后，得到了 p′ p ′ ，它们的关系如何表示?
   • 将 p p 的坐标用四元数表示(纯四元数)： p=[0,x,y,z]=[0,v] p = [ 0 , x , y , z ] = [ 0 , v ]
   • 旋转之后的关系为： p′=qpq−1 p ′ = q p q − 1
   • 四元数相比于轴角，欧拉角的优势：紧凑，无奇异性
5. 问题
   
   • 为什么旋转了角度 θ θ 要用 θ2 θ 2 ？
   • 为什么用单位四元数表示一个三维空间旋转时，旋转之后的关系为 p′=qpq′ p ′ = q p q ′
6. 解决
   感谢知乎用户 Yang Eninala 的回答
   链接：https://www.zhihu.com/question/23005815/answer/33971127
   
   • 单位四元数有四个变量，完全可以被看作一个四维向量。单位四元数（norm=1）则存在于四维空间的一个球面上。 qaqb q a q b ，四元数 qa q a 乘以四元数 qb q b 其实看作（1）对进行左旋转，或者（2）对进行右旋转。所以从始至终，四元数定义的都是四维旋转，而不是三维旋转！任意的四维旋转都可以唯一的拆分为一个左旋转和一个右旋转，表达出来就是。这里，我们对单位四元数（四维向量）进行了一个左旋转和一个右旋转。结果当然是一个单位四元数
   • 三维旋转就是四维旋转的一个特例，就像二维旋转是三维旋转的一个特例一样。说是特例其实不准确，准确的说是一个子集或者subgroup。为了进行三维旋转运算，汉密尔顿首先在四维空间里划出了一块三维空间。汉密尔顿定义了一种纯四元数（pure quaternion），其表达式为。纯四元数第一项为零，它存在于四维空间的三维超平面上，与三维空间中的三维向量一一对应。然后，就有了我们常见的这种左乘单位四元数，右乘其共轭的表达式。简单的说，当对一个三维向量进行三维旋转后，我们希望得到的是一个三维向量。那么这个左乘单位四元数，右乘其共轭的运算保证了结果是一个在三维超平面上中的纯四元数。
   • 至于为什么是 θ2 θ 2 呢，原因如下：
     
     q q 做的就是一个 θ2 θ 2 的旋转，而 q−1 q − 1 做的也是一个 θ2 θ 2 的旋转，两次旋转的结果是一个旋转角为 θ θ 的旋转