三维空间刚体旋转描述

三维空间中通常可以用旋转矩阵、旋转向量、欧拉角和四元数来描述旋转

---

旋转矩阵

先回顾下向量的内积和外积

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪a⋅b=aTb=∑3i=1aibi=|a||b|cos(a,b)a×b=⎡⎣⎢ia1b1ja2b2ka3b3⎤⎦⎥=⎡⎣⎢a2b3−a3b2a3b1−a1b3a1b2−a2b1⎤⎦⎥=⎡⎣⎢0a3−a2−a30a1a2−a10⎤⎦⎥b=a^b(1) { a ⋅ b = a T b = ∑ i = 1 3 a i b i = | a | | b | c o s ( a , b ) a × b = [ i j k a 1 a 2 a 3 b 1 b 2 b 3 ] = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b = a ^ b ( 1 )

其中 a^=⎡⎣⎢0a3−a2−a30a1a2−a10⎤⎦⎥ a ^ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] 是取向量 a a 的 反对角矩阵

外积只对三维向量存在定义，并且可以表示向量的旋转：
假设两个不平行向量 a a ， b b ，可以用一个与 a,b a , b 所在平面垂直的向量描述 a a 到 b b 的旋转（图1）
　　　　　　　　　　　　　　　　
　　　　　　　　图1. w w 与 a×b a × b 方向一致，相当于旋转轴，模值等于角度

对空间中一个向量 a a ，假设有两组单位正交基 O1=(e1,e2,e3) O 1 = ( e 1 , e 2 , e 3 ) 和 O2=(e′1,e′2,e′3) O 2 = ( e 1 ′ , e 2 ′ , e 3 ′ ) ，向量在两个坐标系下坐标为 [a1,a2,a3]T [ a 1 , a 2 , a 3 ] T 和 [a′1,a′2,a′3]T [ a 1 ′ , a 2 ′ , a 3 ′ ] T ，那么有

[e1,e2,e3][a1,a2,a3]T=[e′1,e′2,e′3][a′1,a′2,a′3]T(1) [ e 1 , e 2 , e 3 ] [ a 1 , a 2 , a 3 ] T = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ , a 2 ′ , a 3 ′ ] T ( 1 )

(1)式两边同左乘 [eT1,eT2,eT3]T [ e 1 T , e 2 T , e 3 T ] T ，有

⎡⎣⎢a1a2a3⎤⎦⎥=⎡⎣⎢⎢eT1e′1eT2e′1eT3e′1eT1e′2eT2e′2eT3e′2eT1e′3eT2e′3eT3e′3⎤⎦⎥⎥⎡⎣⎢a′1a′2a′3⎤⎦⎥=Ra′(2) [ a 1 a 2 a 3 ] = [ 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 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] = R a ′ ( 2 )

(2)式的 R R 称为旋转矩阵，用来描述相机的旋转

旋转矩阵是行列式为1的正交矩阵，反之，行列式为1的正交矩阵也是一个旋转矩阵

SO(n)={R∈Rn×n|RRT=I,det(R)=1} S O ( n ) = { R ∈ R n × n | R R T = I , d e t ( R ) = 1 }

其中 SO(n) S O ( n ) 是特殊正交群，特别的 SO(3) S O ( 3 ) 就是三维空间的旋转

旋转矩阵的劣势

• 旋转矩阵有9个量，但一次旋转只有3个自由度。因此这种表达方式是冗余的
• 旋转矩阵有自身约束：必须是正交矩阵，且行列式为1，优化算法比较难应用

---

旋转向量

旋转向量用一个三维向量来描述旋转：其方向与旋转轴一致，长度等于旋转角

回顾向量外积，图1的 w w 就是 a a 到 b b 的旋转向量

旋转向量与旋转矩阵转换
假设旋转轴为 n n ，角度为 θ θ ，使用罗德里格斯公式可以转换到旋转矩阵

R=cosθI+(1−cosθ)nnT+sinθn^(3) R = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ n ^ ( 3 )

同样可以得到由旋转矩阵到旋转向量的公式

{θ=arccos(tr(R)−12)n=R特征值为1的特征向量(4) { θ = a r c c o s ( t r ( R ) − 1 2 ) n = R 特 征 值 为 1 的 特 征 向 量 ( 4 )

此外，旋转矩阵实际就是 SO(3) S O ( 3 ) 的李群，旋转向量就是对应的李代数 so(3) s o ( 3 ) ，两者通过指数映射相联系（与罗德里格斯等价）

R=exp(ϕ^)(5) R = e x p ( ϕ ^ ) ( 5 )

其中 ϕ^ ϕ ^ 就是对旋转向量 ϕ ϕ 取反对称矩阵

---

欧拉角

欧拉角是一种最直观的旋转描述方式，也是一个3维向量，分别代表绕某个轴的旋转角度

• 相同的角度，旋转次序的不同，旋转结果不一样。一般常见的是rpy角（旋转顺序是ZYX）
• 使用欧拉角一个最大的缺点是万向锁问题：俯仰角为 ±90 ± 90 度时，第一次旋转和第三次旋转将使用同一个轴，使得系统丢失了一个自由度
• 因此欧拉角不适于插值和迭代，往往只用于人机交互中

---

四元数

旋转矩阵用9个量描述3自由度的旋转，具有冗余性
欧拉角和旋转向量用3个量描述3自由度的旋转，是紧凑的，但具有奇异性
四元数用4个量描述3自由度的旋转，紧凑又没有奇异性

一个四元数 q q 拥有1个实部和3个虚部

q=q0+q1i+q2j+q3k(6) q = q 0 + q 1 i + q 2 j + q 3 k ( 6 )

满足

⎧⎩⎨⎪⎪⎪⎪i2+j2+k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j(7) { i 2 + j 2 + k 2 = − 1 i j = k , j i = − k j k = i , k j = − i k i = j , i k = − j ( 7 )

旋转向量与四元数的转换
对于一个旋转向量：绕单位向量 n=[n+x,ny,nz]T n = [ n + x , n y , n z ] T 做了 θ θ 度的旋转，那么其四元数为

q=[cosθ2,nxsinθ2,nysinθ2,nzsinθ2]T(8) q = [ c o s θ 2 , n x s i n θ 2 , n y s i n θ 2 , n z s i n θ 2 ] T ( 8 )

同样，也可以由四元数求得旋转向量

⎧⎩⎨θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]Tsinθ2(9) { θ = 2 a r c c o s q 0 [ n x , n y , n z ] T = [ q 1 , q 2 , q 3 ] T s i n θ 2 ( 9 )

旋转矩阵与四元数的转换
设四元数 q=q0+q1i+q2j+q3k(6) q = q 0 + q 1 i + q 2 j + q 3 k ( 6 ) ，对应的旋转矩阵为

R=⎡⎣⎢⎢1−2q22−2q232q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q21−2q232q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q21−2q22⎤⎦⎥⎥(10) R = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 + 2 q 0 q 3 2 q 1 q 3 − 2 q 0 q 2 2 q 1 q 2 − 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 + 2 q 0 q 1 2 q 1 q 3 + 2 q 0 q 2 2 q 2 q 3 − 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ] ( 10 )

反之也可以由 R R 推得四元数 q q

q0=tr(R)+1−−−−−−−−√2,q1=R23−R324q0,q2=R31−R134q0,q3=R12−R214q0(11) q 0 = t r ( R ) + 1 2 , q 1 = R 23 − R 32 4 q 0 , q 2 = R 31 − R 13 4 q 0 , q 3 = R 12 − R 21 4 q 0 ( 11 )

用四元数表示旋转
对于一个空间三维点 p=[x,y,z] p = [ x , y , z ] ，指定一个绕 n n 做 θ θ 角的旋转，旋转后的点为 p′ p ′
1）首先将三维点用一个虚四元数来描述

p=[0,x,y,z]=[0,v] p = [ 0 , x , y , z ] = [ 0 , v ]

2）用四元数 q q 来表达旋转

q=[cosθ2,nsinθ2] q = [ c o s θ 2 , n s i n θ 2 ]

3）旋转后的点为

p′=qpq−1 p ′ = q p q − 1