旋轉矩陣、歐拉角、四元數理論及其轉換關係

旋轉矩陣、歐拉角、四元數理論及其轉換關係

---

[email protected]_ql（lql0716）
http://blog.csdn.net/lql0716

---

1、概述

旋轉矩陣、歐拉角、四元數主要用於表示座標系中的旋轉關係，它們之間的轉換關係可以減小一些算法的複雜度。

本文主要介紹了旋轉矩陣、歐拉角、四元數的基本理論及其之間的轉換關係。

2、原理

2.1 旋轉矩陣

• 對於兩個三維點 p1(x1,y1,z1) $p_1(x_1,y_1,z_1)$， p2(x2,y2,z2) $p_2(x_2,y_2,z_2)$，由點 p1 $p_1$ 經過旋轉矩陣 R $R$ 旋轉到 p2 $p_2$，則有：

R=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥ $$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

⎡⎣⎢x2y2z2⎤⎦⎥=R⎡⎣⎢x1y1z1⎤⎦⎥ $$\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]=R\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$$

注：旋轉矩陣爲正交矩陣 RRT=E $R{R}^T=E$

• 繞x軸旋轉：

Rx(θ)=⎡⎣⎢1000cosθsinθ0−sinθcosθ⎤⎦⎥ $$R_x(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\theta & -sin\theta \\ 0& sin\theta & cos\theta \end{array}\right]$$

• 繞y軸旋轉：

Ry(θ)=⎡⎣⎢cosθ0−sinθ010sinθ0cosθ⎤⎦⎥ $$R_y(\theta )=\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]$$

• 繞z軸旋轉：

Rz(θ)=⎡⎣⎢cosθsinθ0−sinθcosθ0001⎤⎦⎥ $$R_z(\theta )=\left[\begin{array}{ccc}cos\theta & -sin\theta & 0\\ sin\theta & cos\theta & 0\\ 0& 0& 1\end{array}\right]$$

• 任意旋轉矩陣：
  任何一個旋轉可以表示爲依次繞着三個旋轉軸旋三個角度的組合。這三個角度稱爲歐拉角。
  三個軸可以指固定的世界座標系軸，也可以指被旋轉的物體座標系的軸。三個旋轉軸次序不同，會導致結果不同。

Rx(−θ1)Ry(−θ2)Rz(θ)Ry(θ2)Rx(θ1) $$R_x(-{\theta }_1)R_y(-{\theta }_2)R_z(\theta )R_y({\theta }_2)R_x({\theta }_1)$$

2.2 歐拉角

• 歐拉角有兩種：
  靜態：即繞世界座標系三個軸的旋轉，由於物體旋轉過程中座標軸保持靜止，所以稱爲靜態。
  動態：即繞物體座標系三個軸的旋轉，由於物體旋轉過程中座標軸隨着物體做相同的轉動，所以稱爲動態。
  使用動態歐拉角會出現萬向鎖現象；靜態歐拉角不存在萬向鎖的問題。

• 對於在三維空間裏的一個參考系，任何座標系的取向，都可以用三個歐拉角來表現。參考系又稱爲實驗室參考系，是靜止不動的。而座標系則固定於剛體，隨着剛體的旋轉而旋轉。

• 如圖1，設定xyz-軸爲參考系的參考軸。稱xy-平面與XY-平面的相交爲交點線，用英文字母（N）代表。zxz順規的歐拉角可以靜態地這樣定義：

  α $\alpha$ 是x-軸與交點線的夾角，
  β ${\displaystyle \beta }$ 是z-軸與Z-軸的夾角，
  γ ${\displaystyle \gamma }$ 是交點線與X-軸的夾角。

• 圖1：
  
  圖中三個歐拉角分別爲： (α,β,γ) $(\alpha ,\beta ,\gamma )$；
  藍色的軸爲： xyz軸 $xyz軸$
  紅色的軸爲： XYZ $XYZ$軸
  綠色的線爲交線： N $N$
  α∈[0,2π],β∈[0,π],γ∈[0,2π] $\alpha \in [0,2\pi ],\beta \in [0,\pi ],\gamma \in [0,2\pi ]$

• 很可惜地，對於夾角的順序和標記，夾角的兩個軸的指定，並沒有任何常規。科學家對此從未達成共識。每當用到歐拉角時，我們必須明確的表示出夾角的順序，指定其參考軸。

• 實際上，有許多方法可以設定兩個座標系的相對取向。歐拉角方法只是其中的一種。此外，不同的作者會用不同組合的歐拉角來描述，或用不同的名字表示同樣的歐拉角。因此，使用歐拉角前，必須先做好明確的定義。

• 歐拉角的基本思想是將角位移分解爲繞三個互相垂直軸的三個旋轉組成的序列。所以，歐拉旋轉的三個角，可以對應於三個旋轉矩陣。
  Yaw（偏航）：歐拉角向量的y軸
  Pitch（俯仰）：歐拉角向量的x軸
  Roll（翻滾）： 歐拉角向量的z軸

• Unity3D中，歐拉旋轉是按照ZYX的順序旋轉的。（不同的旋轉順序最終得到的結果是不一樣的，要引起重視。給定一組歐拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的順序旋轉和按照yaw-roll-pitch的順序旋轉，最終剛體的朝向是不同的！換言之，若剛體需要按照兩種不同的旋轉順序旋轉到相同的朝向，所需要的歐拉角角度值則是不同的！）

• 歐拉角的缺點：

  1、 歐拉角的表示方式不唯一。給定某個起始朝向和目標朝向，即使給定yaw、pitch、roll的順序，也可以通過不同的yaw/pitch/roll的角度組合來表示所需的旋轉。這其實主要是由於萬向鎖(Gimbal Lock)引起的；

  2、歐拉角的插值比較難；

  3、計算旋轉變換時，一般需要轉換成旋轉矩陣，這時候需要計算很多sin, cos，計算量較大；

2.2.1 由歐拉角求旋轉矩陣

設三個軸x，y，z的歐拉角分別爲 θx,θy,θz ${\theta }_x,{\theta }_y,{\theta }_z$，正弦值、餘弦值分別爲 sx,cx,sy,cy,sz,cz $s_x,c_x,s_y,c_y,s_z,c_z$那麼旋轉矩陣爲：

R(θx,θy,θz)=Rz(θz)Ry(θy)Rx(θx)=⎡⎣⎢cyczcysz−syczsxsy−cxszcxcz+sxsyszcysxsxsz+cxczsycxsysz−czszcxcy⎤⎦⎥ $$R({\theta }_x,{\theta }_y,{\theta }_z)=R_z({\theta }_z)R_y({\theta }_y)R_x({\theta }_x)=\left[\begin{array}{ccc}{c}_y{c}_z& c_z{s}_x{s}_y-c_x{s}_z& s_x{s}_z+c_x{c}_z{s}_y\\ c_y{s}_z& c_x{c}_z+s_x{s}_y{s}_z& c_x{s}_y{s}_z-c_z{s}_z\\ -s_y& c_y{s}_x& c_x{c}_y\end{array}\right]$$

2.2.2 由旋轉矩陣求歐拉角

R=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥=⎡⎣⎢cyczcysz−syczsxsy−cxszcxcz+sxsyszcysxsxsz+cxczsycxsysz−czszcxcy⎤⎦⎥ $$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]=\left[\begin{array}{ccc}{c}_y{c}_z& c_z{s}_x{s}_y-c_x{s}_z& s_x{s}_z+c_x{c}_z{s}_y\\ c_y{s}_z& c_x{c}_z+s_x{s}_y{s}_z& c_x{s}_y{s}_z-c_z{s}_z\\ -s_y& c_y{s}_x& c_x{c}_y\end{array}\right]$$

解方程可得：

θx=atan2(r32,r33) $${\theta }_x=atan2(r_{32},r_{33})$$

θy=atan2(−r31,r232+r233−−−−−−−√) $${\theta }_y=atan2(-r_{31},\sqrt{r_{32}^2+r_{33}^2})$$

θz=atan2(r21,r11) $${\theta }_z=atan2(r_{21},r_{11})$$

注：atan2()爲C++中的函數，atan2(y，x) 求的是y/x的反正切，其返回值爲[-pi,+pi]之間的一個數。

2.3 四元數

四元數（Quaternions）是由愛爾蘭數學家哈密頓（William Rowan Hamilton）在1843年提出。

三維空間的任意旋轉，都可以用繞三維空間的某個軸旋轉過某個角度來表示，即所謂的Axis-Angle表示方法。這種表示方法裏，Axis可用一個三維向量 (x,y,z) $(x,y,z)$來表示， θ $\theta$可以用一個角度值來表示，直觀來講，一個四維向量 (θ,x,y,z) $(\theta ,x,y,z)$就可以表示出三維空間任意的旋轉。注意，這裏的三維向量 (x,y,z) $(x,y,z)$只是用來表示axis的方向朝向，因此更緊湊的表示方式是用一個單位向量來表示方向axis，而用該三維向量的長度來表示角度值 θ $\theta$。這樣以來，可以用一個三維向量 (θ∗x,θ∗y,θ∗z) $(\theta \ast x,\theta \ast y,\theta \ast z)$就可以表示出三維空間任意的旋轉，前提是其中 (x,y,z) $(x,y,z)$是單位向量。這就是旋轉向量(Rotation Vector)的表示方式，OpenCV裏大量使用的就是這種表示方法來表示旋轉(見OpenCV相機標定部分的rvec)。

• 單位向量 (x,y,z) $(x,y,z)$旋轉 θ $\theta$角度後的四元數：

  (cosθ2,x∗sinθ2,y∗sinθ2,z∗sinθ2) $$(cos\frac{\theta }{2},x\ast sin\frac{\theta }{2},y\ast sin\frac{\theta }{2},z\ast sin\frac{\theta }{2})$$

• 對於三維座標的旋轉，可以通過四元數乘法直接操作，與旋轉矩陣操作可以等價，但是表示方式更加緊湊，計算量也可以小一些。

2.3.1 四元數基本概念

• 四元數的複數定義：

  q=q0+q1i+q2j+q3k=[s,v] $$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k=[s,\mathbf{v}]$$

  • 其中 q0,q1,q2,q3 $q_0,q_1,q_2,q_3$均爲實數, s=q0,v=[q1,q2,q3] $s=q_0,\mathbf{v}=[q_1,q_2,q_3]$， i2=j2=k2=−1 $i^2=j^2=k^2=-1$
  • 對於 i,j,k $i,j,k$ 本身的幾何意義可以理解爲一種旋轉，其中 i $i$ 代表 x $x$ 軸與 y $y$ 軸相交平面中 x $x$ 軸正向向 y $y$ 軸正向的旋轉， j $j$ 旋轉代表 z $z$ 軸與 x $x$ 軸相交平面中 z $z$ 軸正向向 x $x$ 軸正向的旋轉， k $k$ 旋轉代表 y $y$ 軸與 z $z$ 軸相交平面中 y $y$ 軸正向向 z $z$ 軸正向的旋轉， −i,−j,−k $-i,-j,-k$ 分別代表 i,j,k $i,j,k$ 的反向旋轉。

• 四元數的模

  |q|=q20+q21+q22+q23−−−−−−−−−−−−−−√ $$|q|=\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}$$

• 四元數的優點：

  • 非奇異表達（和歐拉角之類的表示相比）
  • 比矩陣更緊湊（更快速）
  • 單位四元數的對可以表示四維空間中的一個旋轉

• 四元數與羣

  所有單位四元數的集合組成一個三維球S3和在乘法下的一個羣（李羣）。S3是行列式爲1的實正交3x3正交矩陣的羣SO(3,R)的雙面覆蓋，因爲每兩個單位四元數通過上述關係對應於一個轉動。羣S3和SU(2)同構，SU(2)是行列式爲1的復酉2x2矩陣的羣。

  設 A={q=q0+q1i+q2j+q3k|q0,q1,q2,q3均爲實數} $A=\{\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k|q_0,q_1,q_2,q_3均爲實數\}$，則 A $A$是一個環，並且是一個格， A $A$中存在24個四元數，而它們是施萊夫利符號爲 {3,4,3} $\{3,4,3\}$的正二十四胞體的頂點。

• 四元數的運算：

  p1=[s1,v1]=(a1b1c1d1)T $${\mathbf{p}}_1=[s_1,{\mathbf{v}}_1]={\left(\begin{array}{cccc}{a}_1& b_1& c_1& d_1\end{array}\right)}^T$$

  p2=[s2,v2]=(a2b2c2d2)T $${\mathbf{p}}_2=[s_2,{\mathbf{v}}_2]={\left(\begin{array}{cccc}{a}_2& b_2& c_2& d_2\end{array}\right)}^T$$

  • 四元數的加法

  p1+p2=s1+s2+v1+v2=(a1+a2)+(b1+b2)i+(c1+c2)j+(d1+d2)k $$\begin{gathered} {\mathbf{p}}_1+{\mathbf{p}}_2=s_1+s_2+{\mathbf{v}}_{\mathbf{1}}+{\mathbf{v}}_{\mathbf{2}} \\ =(a_1+a_2)+(b_1+b_2)i+(c_1+c_2)j+(d_1+d_2)k \end{gathered}$$

  • 四元數的乘法

  p1p2=s1s2−v1v2+s1v2+s2v1+v1×v2 $${\mathbf{p}}_1{\mathbf{p}}_2=s_1{s}_2-{\mathbf{v}}_{\mathbf{1}}{\mathbf{v}}_{\mathbf{2}}+s_1{\mathbf{v}}_{\mathbf{2}}+s_2{\mathbf{v}}_{\mathbf{1}}+{\mathbf{v}}_{\mathbf{1}}\times {\mathbf{v}}_{\mathbf{2}}$$

  
  

  p1p2=⎛⎝⎜⎜⎜a1a2−b1b2−c1c2−d1d2a1b2+b1a2+c1d2−d1c2a1c2−b1d2+c1a2+d1b2a1d2+b1c2−c1b2+d1a2⎞⎠⎟⎟⎟ $${\mathbf{p}}_1{\mathbf{p}}_2=\left(\begin{array}{c}{a}_1{a}_2-b_1{b}_2-c_1{c}_2-d_1{d}_2\\ a_1{b}_2+b_1{a}_2+c_1{d}_2-d_1{c}_2\\ a_1{c}_2-b_1{d}_2+c_1{a}_2+d_1{b}_2\\ a_1{d}_2+b_1{c}_2-c_1{b}_2+d_1{a}_2\end{array}\right)$$

  注：四元數不滿足交換律，故 p1p2≠p2p1 ${\mathbf{p}}_1{\mathbf{p}}_2\ne {\mathbf{p}}_2{\mathbf{p}}_1$，四元數的乘法不等同於外積。

  • 四元數的點積（內積）
    點積也叫歐幾里得內積，四元數的點積等同於一個四維向量的點積。點積的值是 p1,p2 ${\mathbf{p}}_{\mathbf{1}},{\mathbf{p}}_{\mathbf{2}}$中相應元素的數值的乘積的和。

  p1⋅p2=s1s2+v1⋅v2=a1a2+b1b2+c1c2+d1d2 $${\mathbf{p}}_{\mathbf{1}}\cdot {\mathbf{p}}_{\mathbf{2}}=s_1{s}_2+{\mathbf{v}}_{\mathbf{1}}\cdot {\mathbf{v}}_{\mathbf{2}}=a_1{a}_2+b_1{b}_2+c_1{c}_2+d_1{d}_2$$

  • 四元數的外積： Outer(p1,p2) $Outer({\mathbf{p}}_{\mathbf{1}}\mathbf{,}{\mathbf{p}}_{\mathbf{2}})$

  • 四元數的偶積： Even(p1,p2) $Even({\mathbf{p}}_{\mathbf{1}}\mathbf{,}{\mathbf{p}}_{\mathbf{2}})$

  • 四元數的叉積： p1×p2 ${\mathbf{p}}_{\mathbf{1}}\times {\mathbf{p}}_{\mathbf{2}}$

  p1×p2=p1p2−p2p22 $${\mathbf{p}}_{\mathbf{1}}\times {\mathbf{p}}_{\mathbf{2}}=\frac{{\mathbf{p}}_{\mathbf{1}}{\mathbf{p}}_{\mathbf{2}}-{\mathbf{p}}_{\mathbf{2}}{\mathbf{p}}_{\mathbf{2}}}{2}$$

  p1×p2=v1×v2 $${\mathbf{p}}_{\mathbf{1}}\times {\mathbf{p}}_{\mathbf{2}}={\mathbf{v}}_{\mathbf{1}}\times {\mathbf{v}}_{\mathbf{2}}$$

  p1×p2=(c1d2−c2d1)i+(b1d2−b2d1)j+(b1c2−b2c1)k $${\mathbf{p}}_{\mathbf{1}}\times {\mathbf{p}}_{\mathbf{2}}=(c_1{d}_2-c_2{d}_1)i+(b_1{d}_2-b_2{d}_1)j+(b_1{c}_2-b_2{c}_1)k$$

  • 四元數的共軛 q∗ ${\mathbf{q}}^{\ast }$

  q∗=s−v=[s,−v] $${\mathbf{q}}^{\ast }=s-\mathbf{v}=[s,-\mathbf{v}]$$

  qq∗=q20+q21+q22+q23 $$\mathbf{q}{\mathbf{q}}^{\ast }=q_0^2+q_1^2+q_2^2+q_3^2$$

  • 四元數的逆 q−1 ${\mathbf{q}}^{-1}$

  qq−1=q−1q=1 $$\mathbf{q}{\mathbf{q}}^{-1}={\mathbf{q}}^{-1}\mathbf{q}=1$$

  q−1=q∗||q||2 $${\mathbf{q}}^{-1}=\frac{{\mathbf{q}}^{\ast }}{||\mathbf{q}|{|}^2}$$

  • 單位四元數
    單位四元數的共軛和逆相等 q−1=q∗ ${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }$

• 四元數乘法的性質：

  1、滿足結合律
  2、不滿足交換律
  3、乘積的模等於模的乘積
  4、乘積的逆等於各個四元數的逆以相反的順序相乘

2.3.2 四元數求旋轉矩陣

• 方法1：

  • 已知四元數
    

    q=q0+q1i+q2j+q3k=[s,v] $$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k=[s,\mathbf{v}]$$

  • 利用Rodrigues公式可以由四元數求得旋轉矩陣 R $R$:
    

    R=⎡⎣⎢⎢1−2q22−2q232q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q21−2q232q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q21−2q22⎤⎦⎥⎥ $$R=\left[\begin{array}{ccc}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]$$

• 方法2：

• 亦可以將四元數轉化爲軸角 θ $\theta$ 與向量 (x,y,z) $(x,y,z)$，然後再計算旋轉矩陣（這樣需要多計算一個arccos，不划算）
  

  q=(θxyz)T $$\mathbf{q}={\left(\begin{array}{cccc}\theta & x& y& z\end{array}\right)}^T$$

• 利用Rodrigues公式可以由單位向量 ω=(x,y,z) $\omega =(x,y,z)$旋轉 θ $\theta$角度後的四元數求得旋轉矩陣 R $R$:

R=eωθ=I+ω×sinθ+ω2×(1−cosθ)=I+⎡⎣⎢0z−y−z0xy−x0⎤⎦⎥sinθ+⎡⎣⎢−y2−z2xyxzxy−x2−z2yzxzyz−x2−y2⎤⎦⎥(1−cosθ)=⎡⎣⎢1zsinθ−ysinθ−zsinθ1xsinθysinθ−xsinθ1⎤⎦⎥+⎡⎣⎢−y2−z2xyxzxy−x2−z2yzxzyz−x2−y2⎤⎦⎥(1−cosθ)=⎡⎣⎢1−(y2+z2)(1−cosθ)zsinθ+xy(1−cosθ)−ysinθ+xz(1−cosθ)−zsinθ+xy(1−cosθ)1−(x2+z2)(1−cosθ)xsinθ+yz(1−cosθ)ysinθ+xz(1−cosθ)−xsinθ+yz(1−cosθ)1−(x2+y2)(1−cosθ)⎤⎦⎥=⎡⎣⎢cosθ+x2(1−cosθ)zsinθ+xy(1−cosθ)−ysinθ+xz(1−cosθ)−zsinθ+xy(1−cosθ)cosθ+y2(1−cosθ−zsinθ+xy(1−cosθ)cosθ+y2(1−cosθ)xsinθ+yz(1−cosθ)ysinθ+xz(1−cosθ)−cosθ)ysinθ+xz(1−cosθ)−xsinθ+yz(1−cosθ)cosθ+z2(1−cosθ)⎤⎦⎥z2(1−