视觉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′=Ra+t$a^{{}^{\prime }}=Ra+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]$\left[\begin{array}{c}{a}^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]\begin{array}{c}\mathrm{\Delta }\\ =\end{array}T\left[\begin{array}{c}a\\ 1\end{array}\right]$
  * 例如 b=T1a，c=T2b，$b=T_{1}a，c=T_{2}b，$得出 c=T2T1a$c=T_2{T}_{1}a$
• 这种用四个数表达三维向量的做法称为齐次坐标，这样旋转和平移可以放入同一个矩阵，称为变换矩阵，即 [R0Tt1]$\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$,其反向变换为 [RT0T−RTt1]$\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$，即矩阵的逆
• 在 SLAM 中，通常定义世界坐标系 TW$T_W$ 与 机器人坐标系 TR$T_R$，一个点的世界坐标为 pW$p_W$，机器人坐标系下为 pR$p_R$，那么满足关系：pR=TRWpW$p_R=T_{RW}{p}_W$，反之亦然，实际编程中可以使用 TRW$T_{RW}$ 或 TWR$T_{WR}$ 来描述机器人位姿

三、旋转向量和欧拉角

1. 旋转向量
   * 旋转矩阵和变换矩阵固然可以表示旋转，但是要求太多：每次旋转只需要三个自由度，也就是x，y，z，但是旋转矩阵用9个量表达了3个自由度，变换矩阵用16个量表达了6个自由度，这种表达方式是冗余的，而且旋转矩阵和变换矩阵都必须是正交矩阵，自身约束会在实际求解中增加困难
   * 任意旋转都可以用一个旋转轴和一个旋转角刻画，如何理解这句话呢，我们引入向量的外积的概念，别的你先不用知道，记住这句话：向量的外积的方向垂直于这两个向量。是不是一下子明白了，对于某个坐标系的一次旋转，可以通过移动后与该向量的外积来确定它是怎么旋转的
   * 方向为旋转轴，长度为转过的角度，这就称为轴角或旋转向量 ，w=θn$w=\theta n$
   * 轴角与旋转矩阵的不同：旋转矩阵需要九个量，有正交性约束和行列式约束，轴角：三个量，没有约束
   * 只是表达方式不同，表达内容一样
   * 轴角即为李代数，这里只是简单了解一下，下一篇会介绍它的原理
   * 轴角转旋转矩阵——R=cosθI+(1−cosθ)nnT+sinθn∧$R=\cos \theta I+(1-\cos \theta )n{n}^T+\sin \theta n^{\wedge }$
   * 旋转矩阵转轴角——角度： θ=arccos(tr(R)−12)$\theta =\arccos (\frac{tr(R)-1}{2})$，轴： Rn=n$Rn=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+ib\leftrightarrow \left(\begin{array}{cc}a& -b\\ b& a\end{array}\right)=r\left[\begin{array}{cc}\cos \phi & -\sin \phi \\ \sin \phi & \cos \phi \end{array}\right]=r\exp \left(\phi \left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]\right)$
   * a,b$a,b$ 为实数，此类矩阵的和、积及乘法逆都是此类矩阵，(ab−ba)=a(1001)+b(01−10)$\left(\begin{array}{cc}a& -b\\ b& a\end{array}\right)=a\left(\begin{array}{cc}1& 0\\ 0& 1\end{array}\right)+b\left(\begin{array}{cc}0& -1\\ 1& 0\end{array}\right)$,即实数1对应着单位矩阵(1001)$\left(\begin{array}{cc}1& 0\\ 0& 1\end{array}\right)$，虚数单位 i$i$ 对应着 (01−10)$\left(\begin{array}{cc}0& -1\\ 1& 0\end{array}\right)$
   * 现假设 a$a$ = 1, b$b$ = 1，在复平面上代表点(1,1)，给 1+i$1+i$ 乘以 i$i$，即原实数单位对应的矩阵成为 (0−1−10)$\left(\begin{array}{cc}0& -1\\ -1& 0\end{array}\right)$，行列式值为 -1，代表点(-1,1)，相当于逆时针旋转了90度
   * 对于 a+ib$a+ib$ ，如果 a$a$ 本身是 m+jn$m+jn$ ，虚部为 j$j$，b$b$ 也是一个复数 x+ky$x+ky$，虚部为 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\in R，v=[q_1,q_2,q_3{]}^T\in R^3$
   * 虚部之间的关系⎧⎩⎨⎪⎪⎪⎪i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j$\{\begin{array}{l}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$
   * 四元数表示空间中的点——若三维空间的一个点的坐标为(x,y,z)，则它用纯四元数(类似于纯虚数，实部为0) xi+yj+zk$xi+yj+zk$ 表示
   * 单位四元数——单位四元数的欧拉公式: cosθ2+(xi+yj+zk)sinθ2$\cos \frac{\theta }{2}+(xi+yj+zk)\sin \frac{\theta }{2}$，则q=[cosθ2,nxsinθ2,nysinθ2,nzsinθ2]T$q=[\cos \frac{\theta }{2},n_x\sin \frac{\theta }{2},n_y\sin \frac{\theta }{2},n_z\sin \frac{\theta }{2}{]}^T$ 表示单位四元数，其中 [nx,ny,nz]T$[n_x,n_y,n_z{]}^T$ 是一个单位向量
   * 四元数的一些运算和性质
   
   • 加减法：qa±qb=[sa±sb,va±vb]$q_a\pm q_b=[s_a\pm s_b,v_a\pm 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\times v_b]$
   • 共轭：q∗a=sa−xai−yaj−zak=[sa,−va]$q_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k=[s_{a,}-v_a]$
   • 模长：||qa||=s2a+s2a+y2a+z2a−−−−−−−−−−−−−−√$||q_a||=\sqrt{s_a^2+s_a^2+y_a^2+z_a^2}$
   • 逆：q−1=q∗/||q||2$q^{-1}=q^{\ast }/||q|{|}^2$
   • 数乘：kq=[ks,kv]$kq=[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]$ 进行了角度为 θ$\theta$ 的旋转，那么其四元数形式为： q=[cosθ2,nxsinθ2,nysinθ2,nzsinθ2]T$q=[\cos \frac{\theta }{2},n_x\sin \frac{\theta }{2},n_y\sin \frac{\theta }{2},n_z\sin \frac{\theta }{2}{]}^T$，你可能会产生疑问，为什么是 θ2$\frac{\theta }{2}$ , 这个问题下面再说
   * 四元数到轴角：{θ=2arccosq0[nx,nynz]T=[q1,q2,q3]T/sinθ2$\{\begin{array}{l}\theta =2\arccos q_0\\ [n_x,n_y{n}_z{]}^T=[q_1,q_2,q_3{]}^T/\sin \frac{\theta }{2}\end{array}$
   * 类似可知四元数亦可转换为旋转矩阵，欧拉角
4. 如何用单位四元数表示一个三维空间旋转
   
   • 设点 p$p$ 经过一次以 q$q$ 表示的旋转后，得到了 p′$p^{{}^{\prime }}$，它们的关系如何表示?
   • 将 p$p$ 的坐标用四元数表示(纯四元数)：p=[0,x,y,z]=[0,v]$p=[0,x,y,z]=[0,v]$
   • 旋转之后的关系为：p′=qpq−1$p^{{}^{\prime }}=qp{q}^{-1}$
   • 四元数相比于轴角，欧拉角的优势：紧凑，无奇异性
5. 问题
   
   • 为什么旋转了角度θ$\theta$ 要用 θ2$\frac{\theta }{2}$ ？
   • 为什么用单位四元数表示一个三维空间旋转时，旋转之后的关系为p′=qpq′$p^{{}^{\prime }}=qp{q}^{{}^{\prime }}$
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$\frac{\theta }{2}$ 呢，原因如下：
     
     q$q$ 做的就是一个 θ2$\frac{\theta }{2}$ 的旋转，而 q−1$q^{-1}$ 做的也是一个 θ2$\frac{\theta }{2}$ 的旋转，两次旋转的结果是一个旋转角为 θ$\theta$ 的旋转