三维空间刚体旋转描述

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

---

旋转矩阵

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

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪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)$$\{\begin{array}{l}a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=|a||b|cos(a,b)\\ a\times b=\left[\begin{array}{ccc}i& j& k\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right]=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b=\hat{a}b\end{array}(1)$$

其中a^=⎡⎣⎢0a3−a2−a30a1a2−a10⎤⎦⎥$\hat{a}=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$ 是取向量a$a$ 的反对角矩阵

外积只对三维向量存在定义，并且可以表示向量的旋转：
假设两个不平行向量a$a$ ，b$b$ ，可以用一个与a,b$a,b$ 所在平面垂直的向量描述a$a$ 到b$b$ 的旋转（图1）
　　　　　　　　　　　　　　　　
　　　　　　　　图1. w$w$ 与a×b$a\times 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^{\prime },e_2^{\prime },e_3^{\prime })$ ，向量在两个座标系下座标为[a1,a2,a3]T$[a_1,a_2,a_3{]}^T$ 和[a′1,a′2,a′3]T$[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^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^{\prime },e_2^{\prime },e_3^{\prime }][a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^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)$$\left[\begin{array}{}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{}{e}_1^T{e}_1^{\prime }& e_1^T{e}_2^{\prime }& e_1^T{e}_3^{\prime }\\ e_2^T{e}_1^{\prime }& e_2^T{e}_2^{\prime }& e_2^T{e}_3^{\prime }\\ e_3^T{e}_1^{\prime }& e_3^T{e}_2^{\prime }& e_3^T{e}_3^{\prime }\end{array}\right]\left[\begin{array}{}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]=R{a}^{\prime }(2)$$

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

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

SO(n)={R∈Rn×n|RRT=I,det(R)=1}$$SO(n)=\{R\in R^{n\times n}|R{R}^T=I,det(R)=1\}$$

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

旋转矩阵的劣势

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

---

旋转向量

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

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

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

R=cosθI+(1−cosθ)nnT+sinθn^(3)$$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta \hat{n}(3)$$

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

{θ=arccos(tr(R)−12)n=R特征值为1的特征向量(4)$$\{\begin{array}{l}\theta =arccos(\frac{tr(R)-1}{2})\\ n=R特征值为1的特征向量\end{array}(4)$$

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

R=exp(ϕ^)(5)$$R=exp(\hat{\varphi })(5)$$

其中ϕ^$\hat{\varphi }$ 就是对旋转向量ϕ$\varphi$ 取反对称矩阵

---

欧拉角

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

• 相同的角度，旋转次序的不同，旋转结果不一样。一般常见的是rpy角（旋转顺序是ZYX）
• 使用欧拉角一个最大的缺点是万向锁问题：俯仰角为±90$\pm 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)$$\{\begin{array}{l}{i}^2+j^2+k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}(7)$$

旋转向量与四元数的转换
对于一个旋转向量：绕单位向量n=[n+x,ny,nz]T$n=[n+x,n_y,n_z{]}^T$ 做了θ$\theta$ 度的旋转，那么其四元数为

q=[cosθ2,nxsinθ2,nysinθ2,nzsinθ2]T(8)$$q=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}{]}^T(8)$$

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

⎧⎩⎨θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]Tsinθ2(9)$$\{\begin{array}{l}\theta =2arccos{q}_0\\ [n_x,n_y,n_z{]}^T=\frac{[q_1,q_2,q_3{]}^T}{sin\frac{\theta }{2}}\end{array}(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=\left[\begin{array}{}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](10)$$

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

q0=tr(R)+1−−−−−−−−√2,q1=R23−R324q0,q2=R31−R134q0,q3=R12−R214q0(11)$$q_0=\frac{\sqrt{tr(R)+1}}{2},q_1=\frac{R_{23}-R_{32}}{4q_0},q_2=\frac{R_{31}-R_{13}}{4q_0},q_3=\frac{R_{12}-R_{21}}{4q_0}(11)$$

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

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

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

q=[cosθ2,nsinθ2]$$q=[cos\frac{\theta }{2},nsin\frac{\theta }{2}]$$

3）旋转后的点为

p′=qpq−1$$p^{\prime }=qp{q}^{-1}$$