三維空間剛體旋轉描述

三維空間中通常可以用旋轉矩陣、旋轉向量、歐拉角和四元數來描述旋轉

---

旋轉矩陣

先回顧下向量的內積和外積

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪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}$$