四元數與歐拉角（RPY角）的相互轉換

 

• RPY角與Z-Y-X歐拉角

　　描述座標系{B}相對於參考座標系{A}的姿態有兩種方式。第一種是繞固定（參考）座標軸旋轉：假設開始兩個座標系重合，先將{B}繞{A}的X軸旋轉γγ，而後繞{A}的Y軸旋轉ββ，最後繞{A}的Z軸旋轉αα，就能旋轉到當前姿態。能夠稱其爲X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。

　　Roll:橫滾

　　Pitch: 俯仰

Yaw: 偏航（航向）

　　因爲是繞固定座標系旋轉，則旋轉矩陣爲（cαcα is shorthand for cosαcos⁡α, sαsα is shorthand for sinαsin⁡α,and so on.）

 

RXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)=⎡⎣⎢cαcβsαcβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ⎤⎦⎥RXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)=[cαcβcαsβsγ−sαcγcαsβcγ+sαsγsαcβsαsβsγ+cαcγsαsβcγ−cαsγ−sβcβsγcβcγ]

 

　　另外一種姿態描述方式是繞自身座標軸旋轉：假設開始兩個座標系重合，先將{B}繞自身的Z軸旋轉αα，而後繞Y軸旋轉ββ，最後繞X軸旋轉γγ，就能旋轉到當前姿態。稱其爲Z-Y-X歐拉角，因爲是繞自身座標軸進行旋轉，則旋轉矩陣爲：

 

RZ′Y′X′(α,β,γ)=RZ(α)RY(β)RX(γ)=⎡⎣⎢cαcβsαcβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ⎤⎦⎥RZ′Y′X′(α,β,γ)=RZ(α)RY(β)RX(γ)=[cαcβcαsβsγ−sαcγcαsβcγ+sαsγsαcβsαsβsγ+cαcγsαsβcγ−cαsγ−sβcβsγcβcγ]

 

　　能夠發現這兩種描述方式獲得的旋轉矩陣是同樣的，即繞固定座標軸X-Y-Z旋轉(γ,β,α)(γ,β,α)和繞自身座標軸Z-Y-X旋轉(α,β,γ)(α,β,γ)的最終結果同樣，只是描述的方法有差異而已。In gerenal: three rotations taken about fixed axes yield the same final orientation as the same three rotations taken in opposite order about the axes of the moving frame.

• Axis-Angle與四元數

　　繞座標軸的屢次旋轉能夠等效爲繞某一轉軸旋轉必定的角度。假設等效旋轉軸方向向量爲K⃗=[kx,ky,kz]TK→=[kx,ky,kz]T，等效旋轉角爲θθ，則四元數q=(x,y,z,w)q=(x,y,z,w)，其中：

 

xyzw=kx⋅sinθ2=ky⋅sinθ2=kz⋅sinθ2=cosθ2x=kx⋅sinθ2y=ky⋅sinθ2z=kz⋅sinθ2w=cosθ2

 

　　且有x2+y2+z2+w2=1x2+y2+z2+w2=1

　　即四元數存儲了旋轉軸和旋轉角的信息，它能方便的描述剛體繞任意軸的旋轉。

　　四元數轉換爲旋轉矩陣：

 

R=⎡⎣⎢1−2y2−2z22(xy+zw)2(xz−yw)2(xy−zw)1−2x2−2z22(yz+xw)2(xz+yw)2(yz−xw)1−2x2−2y2⎤⎦⎥R=[1−2y2−2z22(xy−zw)2(xz+yw)2(xy+zw)1−2x2−2z22(yz−xw)2(xz−yw)2(yz+xw)1−2x2−2y2]

 

 　　已知旋轉矩陣爲：

　　則對應的四元數爲：

 

---

• 四元數與歐拉角的相互轉換

　　定義兩個四元數：

　　

　　

　　其中  表示矢量 

 

 ；而 

 

 表示矢量 

 

四元數加法：

　　跟複數、向量和矩陣同樣，兩個四元數之和須要將不一樣的元素加起來。

　　加法遵循實數和複數的全部交換律和結合律。

四元數乘法：

　　四元數的乘法的意義相似於矩陣的乘法，能夠表示旋轉的合成。當有屢次旋轉操做時，使用四元數能夠得到更高的計算效率。

　　因爲四元數乘法的非可換性，pq並不等於qp，qp乘積的向量部分是：

　　

　　Mathematica中有四元數相關的程序包 Quaternions Package，須要先導入才能使用。下面計算了三個四元數的乘積：

 

　　計算結果爲：Quaternion[-12, 4, 14, 2]

 

　　那麼將Z-Y-X歐拉角（或RPY角：繞固定座標系的X-Y-Z依次旋轉 αα,ββ,γγ角）轉換爲四元數：

 

q=⎡⎣⎢⎢⎢⎢⎢cosγ200sinγ2⎤⎦⎥⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢⎢cosβ20sinβ20⎤⎦⎥⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢cosα2sinα200⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢⎢cosα2cosβ2cosγ2+sinα2sinβ2sinγ2sinα2cosβ2cosγ2−cosα2sinβ2sinγ2cosα2sinβ2cosγ2+sinα2cosβ2sinγ2cosα2cosβ2sinγ2−sinα2sinβ2cosγ2⎤⎦⎥⎥⎥⎥⎥⎥⎥q=[cos⁡γ200sin⁡γ2][cos⁡β20sin⁡β20][cos⁡α2sin⁡α200]=[cos⁡α2cos⁡β2cos⁡γ2+sin⁡α2sin⁡β2sin⁡γ2sin⁡α2cos⁡β2cos⁡γ2−cos⁡α2sin⁡β2sin⁡γ2cos⁡α2sin⁡β2cos⁡γ2+sin⁡α2cos⁡β2sin⁡γ2cos⁡α2cos⁡β2sin⁡γ2−sin⁡α2sin⁡β2cos⁡γ2]

 

 　　根據上面的公式能夠求出逆解，即由四元數 q=(q0,q1,q2,q3)q=(q0,q1,q2,q3)或q=(w,x,y,z)q=(w,x,y,z)到歐拉角的轉換爲：

 

⎡⎣⎢αβγ⎤⎦⎥=⎡⎣⎢⎢⎢⎢arctan2(q0q1+q2q3)1−2(q21+q22)arcsin(2(q0q2−q1q3))arctan2(q0q3+q1q2)1−2(q22+q23)⎤⎦⎥⎥⎥⎥[αβγ]=[arctan⁡2(q0q1+q2q3)1−2(q12+q22)arcsin⁡(2(q0q2−q1q3))arctan⁡2(q0q3+q1q2)1−2(q22+q32)]

 

　　因爲arctan和arcsin的取值範圍在 −π2−π2和π2π2之間，只有180°，而繞某個軸旋轉時範圍是360°，所以要使用atan2函數代替arctan函數：

 

⎡⎣⎢αβγ⎤⎦⎥=⎡⎣⎢atan2(2(q0q1+q2q3),1−2(q21+q22))arcsin(2(q0q2−q1q3))atan2(2(q0q3+q1q2),1−2(q22+q23))⎤⎦⎥[αβγ]=[atan2(2(q0q1+q2q3),1−2(q12+q22))arcsin⁡(2(q0q2−q1q3))atan2(2(q0q3+q1q2),1−2(q22+q32))]

 

對於tan( θ) = y / x ：

　　θ = ATan(y / x)求出的θ取值範圍是[-PI/2, PI/2]；

　　θ = ATan2(y, x)求出的θ取值範圍是[-PI,   PI]。

• 當 (x, y) 在第一象限, 0 < θ < PI/2

• 當 (x, y) 在第二象限 PI/2 < θ≤PI

• 當 (x, y) 在第三象限, -PI < θ < -PI/2

• 當 (x, y) 在第四象限, -PI/2 < θ < 0

 　　將 四元數轉換爲歐拉角能夠參考下面的代碼。須要注意 歐拉角有12種旋轉次序，而上面推導的公式是按照Z-Y-X順序進行的，因此有時會在網上看到不一樣的轉換公式（由於對應着不一樣的旋轉次序），在使用時必定要注意旋轉次序是什麼。好比ADAMS軟件裏就默認Body 3-1-3次序，即Z-X-Z歐拉角，而VREP中則按照X-Y-Z歐拉角旋轉。

 

　　 上面的代碼存在一個問題，即奇異性沒有考慮。下面看一種特殊的狀況（參考Maths - Conversion Quaternion to Euler）：假設一架飛機繞Y軸旋轉了90°（俯仰角pitch=90），機頭垂直向上，此時如何計算航向角和橫滾角？

　　這時會發生自由度丟失的狀況，即Yaw和Roll會變爲一個自由度。此時再使用上面的公式根據四元數計算歐拉角會出現問題：

　　arcsin(2(q0q2−q1q3))arcsin⁡(2(q0q2−q1q3))的定義域爲[−1,1][−1,1]，所以(q0q2−q1q3)∈[−0.5,0.5](q0q2−q1q3)∈[−0.5,0.5]，當q0q2−q1q3=0.5q0q2−q1q3=0.5時（在程序中浮點數不能直接進行等於判斷，要使用合理的閾值），俯仰角ββ爲90°，將其帶入正向公式計算出四元數(q0,q1,q2,q3)(q0,q1,q2,q3)，而後能夠發現逆向公式中atan2函數中的參數所有爲0，即出現了0000的狀況！沒法計算。

　　β=π/2β=π/2時，sinβ2=cosβ2=0.707sin⁡β2=cos⁡β2=0.707，將其帶入公式中有

 

q=⎡⎣⎢⎢⎢wxyz⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢⎢⎢⎢0.707(cosα2cosγ2+sinα2sinγ2)0.707(sinα2cosγ2−cosα2sinγ2)0.707(cosα2cosγ2+sinα2sinγ2)0.707(cosα2sinγ2−sinα2cosγ2)⎤⎦⎥⎥⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢0.707cosα−γ20.707sinα−γ20.707cosα−γ20.707sinα−γ2⎤⎦⎥⎥⎥⎥⎥⎥q=[wxyz][0.707(cos⁡α2cos⁡γ2+sin⁡α2sin⁡γ2)0.707(sin⁡α2cos⁡γ2−cos⁡α2sin⁡γ2)0.707(cos⁡α2cos⁡γ2+sin⁡α2sin⁡γ2)0.707(cos⁡α2sin⁡γ2−sin⁡α2cos⁡γ2)]=[0.707cos⁡α−γ20.707sin⁡α−γ20.707cos⁡α−γ20.707sin⁡α−γ2]

 

　　則xw=zy=tanα−γ2xw=zy=tan⁡α−γ2，因而有

 

α−γ=2⋅atan2(x,w)α−γ=2⋅atan2(x,w)

 

 　　一般令α=0α=0，這時γ=−2⋅atan2(x,w)γ=−2⋅atan2(x,w)。能夠進行驗證：當四元數爲(w,x,y,z)=(0.653,-0.271,0.653,0.271)時，根據這些規則計算出來的ZYX歐拉角爲α=0°，β=90°，γ=45°

　　當俯仰角爲-90°，即機頭豎直向下時的狀況也與之相似，能夠推導出奇異姿態時的計算公式。比較完整的四元數轉歐拉角（Z-Y-X order）的代碼以下：

 

---

　　在DirectXMath Library中有許多與剛體姿態變換相關的函數能夠直接調用：

• 四元數乘法：XMQuaternionMultiply method --Computes the product of two quaternions.
• 旋轉矩陣轉四元數：XMQuaternionRotationMatrix method --Computes a rotation quaternion from a rotation matrix.
• 四元數轉旋轉矩陣：XMMatrixRotationQuaternion method -- Builds a rotation matrix from a quaternion.
• 歐拉角轉四元數：XMQuaternionRotationRollPitchYaw method --Computes a rotation quaternion based on the pitch, yaw, and roll (Euler angles).
• 四元數轉Axis-Angle：XMQuaternionToAxisAngle method --Computes an axis and angle of rotation about that axis for a given quaternion.
• 歐拉角轉旋轉矩陣：XMMatrixRotationRollPitchYaw method --Builds a rotation matrix based on a given pitch, yaw, and roll (Euler angles).
• Axis-Angle轉旋轉矩陣：XMMatrixRotationAxis method --Builds a matrix that rotates around an arbitrary axis.
• 構造繞X/Y/Z軸的旋轉矩陣：XMMatrixRotationX method --Builds a matrix that rotates around the x-axis.(Angles are measured clockwise when looking along the rotation axis toward the origin)

 　　下面的代碼中座標系繞X軸旋轉90°（注意這裏不是按照右手定則的方向，而是沿着座標軸向原點看過去以順時針方式旋轉，所以與傳統的右手定則恰好方向相反），來進行變換：

　　結果以下圖所示:

 

參考：

quaternions.online

DirectXMath Library Quaternion Functions

Convert quaternion to euler rotations

Conversion between quaternions and Euler angles

Maths - Conversion Quaternion to Euler

Coordinate Transformations in Robotics—MATLAB

Introduction to Robotics - Mechanics and Control. Chapter 2 Spatial descriptions and transformations