三维空间刚体运动

三维空间刚体运动

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

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

一、 旋转矩阵

1. 点、向量和坐标系

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

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

2. 坐标系间的欧式变换

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

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

我们对左右两边同乘 [ e 1 T , e 2 T , e 3 T ] T [{e_1}^{T},{e_2}^{T},{e_3}^{T}]^T [e1​T,e2​T,e3​T]T ,左边的系数就变成了单位矩阵。

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

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

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

3. 变换矩阵和齐次坐标

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

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

二、 旋转向量和欧拉角

1. 旋转向量

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

假定旋转轴为一个单位长度的向量 n ，角度为 $\theta$ ,那么向量 $\theta n$ 可表示这个旋转。从旋转向量到旋转矩阵的转换公式由 罗德里格斯公式（Rodrigues’s Formula） 表明。
R = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ n ^ R = cos \theta I + (1-cos \theta )nn^T+ sin \theta \hat n R=cosθI+(1−cosθ)nnT+sinθn^
我们也可以反推出 $ \theta $ 公式。
θ = a r c c o s t r ( R ) − 1 2 \theta = arccos \frac {tr(R)-1}{2} θ=arccos2tr(R)−1​
关于转轴 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 [r,p,y]T 这三个量来描述任意旋转，这种表示方式会比其他方式更直观、更易理解。

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

三、 四元数

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

定义：一个四元数包含一个实部和三个虚部。
q = q 0 + q 1 i + q 2 j + q 3 k = [ s , v ] q = q_0 + q_1i + q_2j + q_3k = [s,v] q=q0​+q1​i+q2​j+q3​k=[s,v]
其中，后面的等式将四元数表达成一个标量和一个向量， $i,j,k$ 表示四元数的三个虚部，满足：

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

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

假设某个旋转的旋转向量为 $ \theta n$ , 则
q = [ c o s θ 2 , n x s i n θ 2 , n y s i n θ 2 , n z s i n θ 2 ] T q=[cos \frac {\theta}{2} ,n_x sin \frac {\theta}{2}, n_ysin \frac {\theta}{2},n_zsin\frac {\theta}{2}]^T q=[cos2θ​,nx​sin2θ​,ny​sin2θ​,nz​sin2θ​]T
反之则有
θ = 2 a r c c o s q 0 \theta = 2 arccosq_0 θ=2arccosq0​

[ n x , n y , n z ] T = [ q 1 , q 2 , q 3 ] T / s i n θ 2 [n_x,n_y,n_z]^T=[q_1,q_2,q_3]^T/sin \frac {\theta}{2} [nx​,ny​,nz​]T=[q1​,q2​,q3​]T/sin2θ​

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

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

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

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

总之： 四元数到旋转向量的转换公式为
θ = 2 a r c c o s q 0 \theta = 2arccosq_0 θ=2arccosq0​

[ n x , n y , n z ] T = [ q 1 , q 2 , q 3 ] T / s i n θ 2 [n_x,n_y,n_z]^T = [q_1,q_2,q_3]^T/sin \frac {\theta}{2} [nx​,ny​,nz​]T=[q1​,q2​,q3​]T/sin2θ​

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！