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

本节的部分内容在机器人学导论课程中都有讲述，相关的知识点在上学期的课后整理过，放在这里，机器人学的数理基础知识,本文将在此基础上做些补充。

一、旋转矩阵

点和向量，坐标系

坐标系间的欧式变换

二、旋转向量和欧拉角

旋转向量

旋转矩阵是一个行列式为1的正交矩阵，反之，行列式为1的正交矩阵也是一个旋转矩阵，所以可以把旋转矩阵的集合定义为如下：
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n) = \{ R \in \mathbb{R}^{n\times n}|RR^T = I,det(R) = 1\} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}

$SO(n)$是特殊正交群(Special Orthogonal Group)的意思。
同时，对于齐次变换矩阵，左上角为旋转矩阵，右侧为平移分量，左下角为0，右下角为1，这种矩阵成为特殊欧式群(Special Euclidean Group)。
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \{T = \begin{bmatrix} R & t\\ 0^T & 1 \end{bmatrix} \in \mathbb{R}^{4\times 4}|R \in SO(3), t \in \mathbb{R}^3 \} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}
三维刚体运动有6个自由度，而SO(3)旋转矩阵有9个量，有些冗余，同时旋转矩阵自身还有约束。
于是有了一种更加紧凑的旋转表示方法，利用一个旋转轴和一个旋转角来表示任意的旋转。构造一种向量，使得其方向与旋转轴方向一致，而长度等于旋转角，这种向量称为旋转向量(轴角，Axis-Angle)。

则绕旋转轴n旋转角度$\theta$对应的旋转向量为$\theta$n,显然这里的n应该为单位向量。

旋转向量到旋转矩阵的变换过程由罗德里格斯公式给出:
$$R=cos\theta \mathbf{I}+(1-cos\theta )\mathbf{n}{\mathbf{n}}^T+sin\theta {\mathbf{n}}^{\wedge }$$

欧拉角

欧拉角用三个分离的转角来描述旋转，最常用的描述方法是“偏航-俯仰-滚转”(yaw-pitch-roll)。但是其存在万向锁的问题，很少在SLAM中直接用来描述姿态。

三、四元数

四元数的定义

四元数是紧凑的，没有奇异性的，可以用来表述三维空间旋转。
一个四元数q拥有一个实部和三个虚部：
$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$$
其中$i,j,k$为四元数的三个虚部，满足
$$\{\begin{array}{r}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$

四元数也可以表示为:
$$\mathbf{q}=[s,\mathbf{v}],s=q_0\in \mathbb{R},\mathbf{v}=[q_1,q_2,q_3{]}^T\in \mathbb{R}$$

四元数的运算

对于两个四元数${\mathbf{q}}_a=[s_a,{\mathbf{v}}_a]=s_a+x_{a}i+y_{a}j+z_{a}k,{\mathbf{q}}_b=[s_b,{\mathbf{v}}_b]=s_b+x_{b}i+y_{b}j+z_{b}k$

• 加法和减法
  $${\mathbf{q}}_a\pm {\mathbf{q}}_b=[s_a\pm s_b,{\mathbf{v}}_a\pm {\mathbf{v}}_b]$$
• 乘法
  $${\mathbf{q}}_a{\mathbf{q}}_b=[s_a{s}_b-{\mathbf{v}}_a^T{\mathbf{v}}_b,s_a{\mathbf{v}}_b+s_b{\mathbf{v}}_a+{\mathbf{v}}_a\times {\mathbf{v}}_b]$$
• 共轭
  $${\mathbf{q}}_a^{\ast }=[s_a,-{\mathbf{v}}_a]$$
• 模长
  $$||{\mathbf{q}}_a||=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$$
• 逆
  $${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/||\mathbf{q}|{|}^2$$
• 数乘与点乘
  $$k\mathbf{q}=[ks,k\mathbf{v}]$$
  $${\mathbf{q}}_a{\mathbf{q}}_b=s_a{s}_b+x_a{x}_b+y_a{y}_b+z_a{z}_b$$

用四元数表示旋转

我们用虚四元数来表示三维空间中的一个点：
$$\mathbf{p}=[0,x,y,z]=[0,\mathbf{v}]$$
而对于一个由轴角$\mathbf{n}$，$\theta$指定的旋转，可以用四元数来表示为:
$$\mathbf{q}=[cos\frac{\theta }{2},\mathbf{n}sin\frac{\theta }{2}]$$
旋转后的点$\mathbf{p}\prime$可以表示为:
$$\mathbf{p}\prime =\mathbf{q}\mathbf{p}{\mathbf{q}}^{-1}$$

四元数到旋转矩阵的转换

同理可以推得四元数$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$对应的旋转矩阵$\mathbf{R}$为：
$$\mathbf{R}=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2-2q_0{q}_3& 2q_1{q}_3+2q_0{q}_2\\ 2q_1{q}_2+2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3-2q_0{q}_1\\ 2q_1{q}_3-2q_0{q}_2& 2q_2{q}_3+2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right]$$

同样的由旋转矩阵 R = { m i j } , i , j ∈ [ 1 , 2 , 3 ] \mathbf{R}=\{m_{ij}\},i,j \in[1, 2,3] R={mij​},i,j∈[1,2,3]也可以得到四元数:
$$q_0=\frac{\sqrt{tr(\mathbf{R})+1}}{2},q_1=\frac{m_{23}-m_{32}}{4q_0},q_2=\frac{m_{31}-m_{13}}{4q_0},q_3=\frac{m_{12}-m_{21}}{4q_0}$$

往期链接
视觉SLAM十四讲 第2讲 初识slam