三维空间刚体运动

三维空间刚体运动

参考书目 《视觉SLAM十四讲》 高翔 张涛 等著

参考链接 https://zhuanlan.zhihu.com/p/32937868

一、 旋转矩阵

1. 点、向量和坐标系

我们一般用三维空间中的点来表示我们的物体，向量来表示点在空间中的移动，简单定义在这边不再赘述。

假定我们现在有向量 a和ba和b 用 < a,b><a,b> 表示向量之间夹角，那么对外积而言我们定义 a与b的外积 = a^b ，外积的结果是一个向量，根据右手定则方向垂直于这两向量形成的平面，大小为 ∣a∣∣b∣sin <a,b>|a||b|sin<a,b> ,我们把 a^ 称为 反对称矩阵 。

2. 坐标系间的欧式变换

对于两个坐标系之间的变化，我们可以用 一个旋转和一个平移 表示，这种运动称为 刚体运动 。

我们的 欧式变换 就是由 旋转和平移 组成。假定我们某点在 [e1,e2,e3]T[e_1,e_2,e_3]^T 坐标系下坐标为 [a1,a2,a3]T[a_1,a_2,a_3]^T , 在 [e1′,e2′,e3′][{e_1}^{'},{e_2}{'},{e_3}^{'}] 下坐标为 [a1′,a2′,a3′]T[{a_1}^{'},{a_2}{'},{a_3}^{'}]T 。

我们对左右两边同乘 [e1T,e2T,e3T]T[{e_1}^{T},{e_2}{T},{e_3}^{T}]T ,左边的系数就变成了单位矩阵。

我们把中间的矩阵定义为 R旋转矩阵(RotationMatrix)R旋转矩阵(Rotation \quad Matrix) , 该矩阵各分量是两个坐标系基的内积，由于基向量模长为1，所以实际上为各基向量夹角的余弦值，所以这个矩阵也称为 方向余弦矩阵(Direction Cosine Matrix) ，或者 旋转矩阵 。

旋转矩阵是行列式为1的正交矩阵，反之，行列式为1的矩阵也是一个旋转矩阵。 我们可以将旋转矩阵的集合定义如下，称为 特殊正交群 。
SO(n)={R∈Rn×n∣RRT=I,det(R)=1} SO(n) = { R \in R^{n \times n} |RR^T=I, det(R)=1 }
通过旋转矩阵，就可以直接谈论两个坐标系之间的旋转变化，而不用再从基谈起。

在欧式变换中当我们把旋转和平移写在一个式子中，如下
a1=R12a2+t12 a_1 = R_{12} a_2 + t_{12}
其中 a1,a2a_1,a_2 分别代表 a 在两个坐标系下的坐标， R12R_{12} 为把坐标系2的向量变换到坐标系1， t12t_{12} 代表平移向量。这样我们就可以实现一个点在两个坐标系下的转换。

3. 变换矩阵和齐次坐标

我们上面引入的变换公式不是线性关系，当进行多次变换的话，算出的结果会很复杂，所以我们引入齐次坐标和变换矩阵。

这样写的好处是可以把旋转和平移写在一个矩阵内，使得整个关系变成线性关系。矩阵 T 称为 变换矩阵（Transform Matrix） 。这个矩阵又被称为 特殊欧式群（Special Euclidean Group） 。

二、 旋转向量和欧拉角

1. 旋转向量

在三维空间中，任意旋转都可以用 一个旋转轴和一个旋转角 来刻画，我们定义一个向量，方向与旋转轴一致，长度等于旋转角，这种向量被称为 旋转向量（轴角/角轴，Axis-Angle） ，只需要一个三维向量即可描述旋转。

假定旋转轴为一个单位长度的向量 n ，角度为 θ\theta ,那么向量 θn\theta n 可表示这个旋转。从旋转向量到旋转矩阵的转换公式由 罗德里格斯公式（Rodrigues’s Formula） 表明。
R=cosθI+(1−cosθ)nnT+sinθn^ R = cos \theta I + (1-cos \theta )nn^T+ sin \theta \hat n
我们也可以反推出 \theta 公式。
θ=arccostr(R)−12 \theta = arccos \frac {tr(R)-1}{2}
关于转轴 n 在旋转之后不发生改变
Rn=n Rn = n

2. 欧拉角

除了旋转向量，我们也可以用 欧拉角 来紧凑地描述旋转。一个旋转可以分解成3次分别绕X，Y，Z 轴的旋转来表示，在航空摄影测量中，一般用 “翻转 - 航偏 - 俯仰”（roll - yaw - pitch) ，也即 XZY 来表示。先绕X 轴旋转roll 角度，再绕Y 轴旋转yaw 角度，最后按照Z 轴旋转pitch 角度。这三个旋转矩阵相乘就得到了总的旋转矩阵。

此时，就可以用 [r,p,y]T[r,p,y]^T 这三个量来描述任意旋转，这种表示方式会比其他方式更直观、更易理解。

但欧拉角会有一个重大缺点，就是著名的 万向锁问题（Gimbal Lock） ，在俯仰角为正负90度时，第一次旋转与第三次旋转将使用同一个轴，使得系统失去一个自由度，这被称为 奇异性 问题。

三、 四元数

既然我们已经有了旋转向量和欧拉角，为什么还有个四元数（Quaternion）？因为欧拉角和旋转向量具有奇异性（万向锁问题）。不存在不带有奇异性的三维向量描述方式 。因此我们需要用到四元数，它既是紧凑的，也没有奇异性 。

定义 ：一个四元数包含一个实部和三个虚部。
q=q0+q1i+q2j+q3k=[s,v] q = q_0 + q_1i + q_2j + q_3k = [s,v]
其中，后面的等式将四元数表达成一个标量和一个向量， i,j,ki,j,k 表示四元数的三个虚部，满足：

若一个四元数虚部全为0，则它是一个实四元数 ；如其实部为零，则称它为虚四元素 。而且，一个虚四元数对应一个空间点 。

我们能用单位四元数 来表示三维空间中的任意一个旋转。我们先考虑下复数。在负数中，乘以i 表示在复平面内旋转90度。但在四元数中，情形却有微妙的不同：乘以i 表示旋转180度，这样才能保证 ij=−kij=-k 的性质。而 i2=−1i^2 = -1 ，说明绕i 轴旋转360度后得到一个相反的东西，而要旋转720度（两周）才能得到它原先的样子。

假设某个旋转的旋转向量为 \theta n , 则
q=[cosθ2,nxsinθ2,nysinθ2,nzsinθ2]T q=[cos \frac {\theta}{2} ,n_x sin \frac {\theta}{2}, n_ysin \frac {\theta}{2},n_zsin\frac {\theta}{2}]^T
反之则有
θ=2arccosq0 \theta = 2 arccosq_0

[nx,ny,nz]T=[q1,q2,q3]T/sinθ2 [n_x,n_y,n_z]^{T=[q_1,q_2,q_3]}T/sin \frac {\theta}{2}

上式给人一种“转了一半”的感觉。将上式中的 θ\theta 加上 2π2\pi 后得到一个相同的旋转，但是对应的四元数却变成了 −q-q . 所以，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示 。

而四元数和旋转矩阵的关系为：

设矩阵 R={m_{ij},i,j \in [1,2,3]} , 则由上式可以推得：

NOTE : 由于 q和−qq和-q 表示同一个旋转，所以一个旋转矩阵对应的四元数表示并不惟一 且存在其他转换公式。在实际中，如果 q0q_0 接近于0，会造成其他3个数的解不稳定 ，应采用其他公式。

总之： 四元数到旋转向量的转换公式为
θ=2arccosq0 \theta = 2arccosq_0

[nx,ny,nz]T=[q1,q2,q3]T/sinθ2 [n_x,n_y,n_z]^T = [q_1,q_2,q_3]^T/sin \frac {\theta}{2}