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

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

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

一、旋转矩阵

1. 点、向量、坐标系
   * 点——存在于三维空间之中，点和点组成向量，点本身由原点指向它的向量所描述
   * 向量——带指向性的箭头，可以进行加法减法等运算，定义坐标系后，向量可以由$ R^3 $当中的三个数表示， 如何理解这句话呢。如下图所示：
   
   在代数中，我们用一组基底和向量 (a) 在每个坐标轴上的投影来表示一个向量，对于 (a)，通过某种线性组合，可以表示为(a = a_xe_1+a_ye_2+a_ze_3)
   而上面那句话的意思是在矩阵运算中，(a) 可以表示为 (left[ egin{matrix} a_x \ a_y \ a_z end{matrix} ight])，因为((e_1,e_2,e_3)left[ egin{matrix} a_x \ a_y \ a_z end{matrix} ight] = a_xe_1+a_ye_2+a_ze_3)
   * 坐标系——三个正交的轴，构成线性空间的一组基，分为左手系和右手系
   * 向量的运算可以由坐标运算来表达：加减法，内积，外积

2. 问题的出现——一个最简单的情况，机器人从某一点直线运动到另一点，假设机器人是质点，并且和目标点处于同一平面上，分别以机器人和目标点建立坐标系，在移动过程中机器人的坐标系位置一直在变，要计算与目标点的距离，就需要描述坐标系之间如何变化
   * 进而——如何计算同一个向量在不同坐标系里的坐标
   * 如果刚才的机器人不是直线运动，而会有拐弯，这时坐标系就会旋转，因此描述整个运动过程就是三个轴的旋转和原点间的平移，这就是所谓的欧式变换，保证同一个向量在各个坐标系下的长度和夹角都不会发生变化，通过旋转和平移两部分组成

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

   • 设某坐标系(用三个方向上的单位向量表示) ((e_1,e_2,e_3)) 发生了一次旋转，变成了((e_1^{'},e_2^{'},e_3^{'}))
   • 对于某个固定的向量 (a)(向量不随坐标系旋转)，它的坐标怎么变化，其中 (left[ egin{matrix} a_1 \ a_2 \ a_3 end{matrix} ight]) 是 (a) 在第一个坐标系中的坐标，(left[ egin{matrix} a_1^{'} \ a_2^{'} \ a_3^{'} end{matrix} ight]) 是 (a) 在另一个坐标系中的坐标，如图，P为向量 (a)
     
   • 坐标关系$$ [e_1,e_2,e_3]left[ egin{matrix} a_1 a_2 a_3 end{matrix} ight] = [e_1^{'},e_2{'},e_3^{'}]left[ egin{matrix} a_1^{'} a_2^{'} a_3^{'} end{matrix} ight] $$，乘出来的就是向量本身
   • 左乘(left[ egin{matrix} e_1^{T} \ e_2^{T} \ e_3^{T} end{matrix} ight]),得：(left[ egin{matrix} a_1 \ a_2 \ a_3 end{matrix} ight] = left[ egin{matrix} 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^{'} end{matrix} ight]left[ egin{matrix} a_1^{'} \ a_2^{'} \ a_3^{'} end{matrix} ight] egin{matrix}Delta\=end{matrix}Ra^{'})
   • 中间的矩阵 (R) 称为旋转矩阵
   • 根据定义可以验证
     * (R) 是一个正交矩阵(矩阵的逆是其转置)
     * (R) 的行列式为(+1)
   • 满足这两个性质的矩阵称为旋转矩阵

   *旋转矩阵描述了两个坐标的变换关系，ex：(a_1 = R_{12}a_2)，反之 (a_2 = R_{21}a_1)，于是(R_{21} = R_{12}^{-1} = R_{12}^T)， 进一步，三个坐标系亦有 (a_3 = R_{32}a_2 = R_{32}R_{21}a_1 = R_{31}a_1)
   * 加上平移 (a^{'} = Ra + t)，因此两个坐标系的刚体运动可以由 (R， t) 完全描述

二、变换矩阵

• 变换矩阵的引入
  * 用旋转和平移方式有一点不便之处，比如发生了两次变换 (b = R_1a + t_1，c = R_2b + t_2)
  * 这时 (c = R_2(R_1a + t_1) + t_2)，叠加起来过于复杂
  * 改变形式，写成 $left[egin{matrix} a^{'} 1 end{matrix} ight] = left[ egin{matrix} R & t 0^T & 1 end{matrix} ight]left[ egin{matrix} a 1end{matrix} ight] egin{matrix} Delta = end{matrix} Tleft[ egin{matrix}a 1 end{matrix} ight] $
  * 例如 (b = T_1a，c = T_2b ，)得出 $ c = T_2T_1a$
• 这种用四个数表达三维向量的做法称为齐次坐标，这样旋转和平移可以放入同一个矩阵，称为变换矩阵，即 (left[ egin{matrix} R & t \ 0^T & 1end{matrix} ight]),其反向变换为 (left[ egin{matrix} R^T & -R^Tt \ 0^T & 1 end{matrix} ight])，即矩阵的逆
• 在 SLAM 中，通常定义世界坐标系 (T_W) 与 机器人坐标系 (T_R)，一个点的世界坐标为 (p_W)，机器人坐标系下为 (p_R)，那么满足关系：(p_R = T_{RW}p_W)，反之亦然，实际编程中可以使用 (T_{RW}) 或 (T_{WR}) 来描述机器人位姿

三、旋转向量和欧拉角

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

四、四元数

1. 一种扩展的复数
   * 回忆：复数可以表达二维平面的旋转
   * 怎么理解这句话，首先看一下复数的另一种表达方式——矩阵表达式
   * (a+ib leftrightarrow egin{pmatrix}a & -b \ b & ;; a end{pmatrix} = regin{bmatrix} cosvarphi & -sinvarphi \ sinvarphi & cosvarphi end{bmatrix} = rexpleft(varphiegin{bmatrix} 0 & -1 \ 1 & 0 end{bmatrix} ight))
   * (a,b) 为实数，此类矩阵的和、积及乘法逆都是此类矩阵，(egin{pmatrix}a & -b \ b & ;; a end{pmatrix} = aegin{pmatrix}1 & 0 \ 0 & 1 end{pmatrix} + begin{pmatrix}0 & -1 \ 1 & 0 end{pmatrix}),即实数1对应着单位矩阵(egin{pmatrix}1 & 0 \ 0 & 1 end{pmatrix})，虚数单位 (i) 对应着 (egin{pmatrix}0 & -1 \ 1 & 0 end{pmatrix})
   * 现假设 (a) = 1, (b) = 1，在复平面上代表点(1,1)，给 (1+i) 乘以 (i)，即原实数单位对应的矩阵成为 (egin{pmatrix}0 & -1 \ -1 & 0 end{pmatrix})，行列式值为 -1，代表点(-1,1)，相当于逆时针旋转了90度
   * 对于 (a+ib) ，如果 (a) 本身是 (m+jn) ，虚部为 (j)，(b) 也是一个复数 (x+ky)，虚部为 (k)，则构成的代数就是四元数
2. 四元数有三个虚部，可以表达三维空间的旋转
   * (q = q_0 + q_1i + q_2j + q_3k)
   * (q = [s,v]，s = q_0 ∈ R，v= [q_1,q_2,q_3]^T ∈ R^3)
   * 虚部之间的关系(egin{cases} i^2 = j^2 = k^2 = -1 \ ij = k,ji = -k \ jk = i,kj = -i \ ki = j,ik = -jend{cases})
   * 四元数表示空间中的点——若三维空间的一个点的坐标为(x,y,z)，则它用纯四元数(类似于纯虚数，实部为0) (xi+yj+zk) 表示
   * 单位四元数——单位四元数的欧拉公式: (cosfrac{ heta}{2} + (xi+yj+zk)sinfrac{ heta}{2})，则(q = [cosfrac{ heta}{2},n_xsinfrac{ heta}{2},n_ysinfrac{ heta}{2},n_zsinfrac{ heta}{2}]^T) 表示单位四元数，其中 ([n_x,n_y,n_z]^T) 是一个单位向量
   * 四元数的一些运算和性质

• 加减法：(q_a pm q_b = [s_a pm s_b,v_a pm v_b])
• 乘法：(q_aq_b = s_as_b - x_ax_b -y_ay_b -z_az_b + (s_ax_b - x_as_b -y_az_b -z_ay_b)i + (s_ay_b - x_az_b -y_as_b -z_ax_b)j + (s_az_b - x_ay_b -y_ax_b -z_as_b)k)
• 乘法(向量表示)：(q_aq_b = [s_as_b - v_a^Tv_b , s_av_b+s_bv_a+v_a×v_b])
• 共轭：(q_a^* = s_a - x_ai - y_aj - z_ak = [s_{a,} - v_a])
• 模长：(||q_a|| = sqrt{s_a^2 + s_a^2 + y_a^2 + z_a^2})
• 逆：(q^{-1} = q^*/||q||^2)
• 数乘：(kq = [k_s,k_v])
• 点乘：(q_a · q_b = s_as_b + x_ax_bi + y_ay_bj + z_az_bk)

3. 四元数和轴角的关系
   * 来看看旋转向量，某个旋转是绕着单位向量 (n = [n_x,n_y,n_z]) 进行了角度为 ( heta) 的旋转，那么其四元数形式为： (q = [cosfrac{ heta}{2},n_xsinfrac{ heta}{2},n_ysinfrac{ heta}{2},n_zsinfrac{ heta}{2}]^T)，你可能会产生疑问，为什么是 (frac{ heta}{2}) , 这个问题下面再说
   * 四元数到轴角：(egin{cases} heta = 2arccos q_0 \ [n_x,n_yn_z]^T = [q_1,q_2,q_3]^T/sin frac{ heta}{2}end{cases})
   * 类似可知四元数亦可转换为旋转矩阵，欧拉角
4. 如何用单位四元数表示一个三维空间旋转

• 设点 (p) 经过一次以 (q) 表示的旋转后，得到了 (p^{'})，它们的关系如何表示?
• 将 (p) 的坐标用四元数表示(纯四元数)：(p = [0,x,y,z] = [0,v])
• 旋转之后的关系为：(p^{'} = qpq^{-1})
• 四元数相比于轴角，欧拉角的优势：紧凑，无奇异性

5. 问题

• 为什么旋转了角度( heta) 要用 (frac{ heta}{2}) ？
• 为什么用单位四元数表示一个三维空间旋转时，旋转之后的关系为(p^{'} = qpq^{'})

6. 解决
   感谢知乎用户 Yang Eninala 的回答
   链接：https://www.zhihu.com/question/23005815/answer/33971127

• 单位四元数有四个变量，完全可以被看作一个四维向量。单位四元数（norm=1）则存在于四维空间的一个球面上。(q_aq_b)，四元数(q_a)乘以四元数(q_b)其实看作（1）对进行左旋转，或者（2）对进行右旋转。所以从始至终，四元数定义的都是四维旋转，而不是三维旋转！任意的四维旋转都可以唯一的拆分为一个左旋转和一个右旋转，表达出来就是。这里，我们对单位四元数（四维向量）进行了一个左旋转和一个右旋转。结果当然是一个单位四元数
• 三维旋转就是四维旋转的一个特例，就像二维旋转是三维旋转的一个特例一样。说是特例其实不准确，准确的说是一个子集或者subgroup。为了进行三维旋转运算，汉密尔顿首先在四维空间里划出了一块三维空间。汉密尔顿定义了一种纯四元数（pure quaternion），其表达式为。纯四元数第一项为零，它存在于四维空间的三维超平面上，与三维空间中的三维向量一一对应。然后，就有了我们常见的这种左乘单位四元数，右乘其共轭的表达式。简单的说，当对一个三维向量进行三维旋转后，我们希望得到的是一个三维向量。那么这个左乘单位四元数，右乘其共轭的运算保证了结果是一个在三维超平面上中的纯四元数。
• 至于为什么是 (frac{ heta}{2}) 呢，原因如下：
  
  (q) 做的就是一个 (frac{ heta}{2}) 的旋转，而 (q^{-1}) 做的也是一个 (frac{ heta}{2}) 的旋转，两次旋转的结果是一个旋转角为 ( heta) 的旋转