尤拉角與旋轉矩陣之間的轉化公式及原理
1.背景說明
首先,說明幾點需要注意的問題:
a.左手座標系與右手座標系:既然是在座標系中進行變換,首先就要了解座標系的類別。同樣的資料在左手座標系和右手座標系會有不同的呈現結果。所以,所有涉及到座標系的問題,不說明基於左手座標系還是右手座標系就有可能帶著小白走彎路。下面的文章將分別討論在右手座標系和左手座標系下的兩套轉換。
b.尤拉角順規:尤拉角定義了一組物體的旋轉次序,繞某個軸旋轉多少度。很多人會忽略旋轉順序的問題,很多書上或者部落格叫做順規,可以理解為尤拉角旋轉順序的規定。(α, β, γ)
在不同的旋轉順序下會有不同的結果,先繞X軸旋轉α,還是先繞y軸旋轉β,最後的結果是不一樣的。尤拉角的順規有很多,例如Z-X-Y, X-Y-Z, X-Y-X, Z-X-Y ···
c.旋轉的具體形式:物體旋轉有多種,繞某個定點進行旋轉,繞座標軸進行旋轉,繞任意軸進行旋轉。不同的旋轉需要用不同的數學方式來表達。網上的大部分文章都是預設討論座標軸旋轉的,繞點旋轉和繞任意軸旋轉相對更復雜一點。敲黑板,注意,本文討論的是三維中繞座標軸的旋轉。
2.右手座標系下的轉換
容我自行定義兩個概念,方便之後的敘述。
基礎旋轉矩陣:繞某單一座標軸進行旋轉對應的矩陣,稱之為基礎旋轉矩陣。
組合旋轉矩陣:多次旋轉組合對應的矩陣,稱之為組合旋轉矩陣。
其中,基礎旋轉矩陣的推導過程可以參考文章中三維座標系下的旋轉章節,此處直接使用對應的矩陣。
2.1右手座標系下基礎旋轉矩陣
右手座標系下的基礎旋轉矩陣包括以下三個:
//在右手系中繞X軸旋轉p° 對應的矩陣Rx | 1 0 0 | Rx= | 0 cosp -sinp| | 0 sinp cosp| //在右手系中繞Y軸旋轉h° 對應的矩陣Ry | cosh 0 sinh| Ry= | 0 1 0 | |-sinh 0 cosh| //在右手系中繞Z軸旋轉b° 對應的矩陣Rz |cosb -sinb 0 | Rz= |sinb cosb 0 | | 0 0 1 |
如果物體只是發生了單一的旋轉,例如尤拉角(p,0,0)、(0,h,0)、(0,0,b)這樣的只是單次旋轉,上面對應的矩陣就可以完成尤拉角→旋轉矩陣的轉化
2.2右手座標系下組合旋轉矩陣
根據組合旋轉矩陣的定義也可以瞭解,多次旋轉組合在一起,例如Z-X-Y順規的尤拉角(30°,62°,28°)就是先繞Z軸旋轉28°,然後繞X軸旋轉30°,最後繞Y軸旋轉62°。單一的旋轉矩陣無法滿足這樣的組合旋轉,無法完成對應。此時就需要利用旋轉變換的性質:物體繞軸旋轉對應的矩陣具有可累乘的性質,即多一次旋轉就是多一次矩陣乘法。
因此,組合旋轉矩陣就是對應順序的基礎旋轉矩陣的乘積。例如Z-X-Y順規的尤拉角對應的組合旋轉矩陣就是R = Rz*Rx*Ry
。如果是X-Y-Z順規的尤拉角,對應的組合旋轉矩陣就是R = Rx*Ry*Rz
。依據這個乘法法則即可獲得尤拉角→旋轉矩陣的轉換。
舉一個例子,按照2.1節中的基礎旋轉矩陣,計算Z-X-Y順規下對應的旋轉矩陣,其結果如下:
//尤拉角(p,h,b)在右手系中對應的旋轉矩陣(Z-X-Y順規)
|cosbcosh-sinbsinpsinh -sinbcosp cosbsinh+sinbsinpcosh|
R = Rz*Rx*Ry = |sinbcosh+cosbsinpsinh cosbcosp sinbsinh-cosbsinpcosh|
| -cospsinh sinp cospcosh |
2.3 由旋轉矩陣計算特定順規的尤拉角
旋轉矩陣一般是指3*3
的正交矩陣,如果是4*3的運動矩陣,取前三行也就是旋轉矩陣,給定旋轉矩陣R
| m11 m12 m13 |
M=| m21 m22 m23 |
| m31 m32 m33 |
需要計算哪一種順規的尤拉角,就使得M=R
列方程進行求解。下面以Z-X-Y最常用的順規來舉例說明求解過程中的技巧:
| m11 m12 m13 | |cosbcosh-sinbsinpsinh -sinbcosp cosbsinh+sinbsinpcosh|
M=| m21 m22 m23 | = |sinbcosh+cosbsinpsinh cosbcosp sinbsinh-cosbsinpcosh| = R=Rz*Rx*Ry
| m31 m32 m33 | | -cospsinh sinp cospcosh |
不同順規尤拉角對應的組合旋轉矩陣通常都會有一列和一行資料的形式相對簡單,在上面的例子中是第2列和第3行的資料形式相對簡單。求解三個角度值p h b
從該行該列的資料入手。
通過sinp = m32
求解角度p = asin(m32)
;
通過m31 m33
求解角度h = atan2(-m31/cosp , m33/cosp)
,值得注意的只與兩個引數的比例值有關係,與絕對大小沒有關係,所以進一步化簡為h = atan2(-m31, m33)
;
通過m12, m22
求解角度b = atan2(-m12/cosp, m22/cosp)
,同理化簡為h = atan2(-m12, m22)
。
注:float atan2(float param1_sin , float param2_cos)
是C++的函式,後面的引數分別是正弦值和餘弦值,同時可以同比例縮放,該函式返回值該正弦值和餘弦值對應的角度值。可以在網上查到相關的資料。
講旋轉矩陣轉化為其他順規的尤拉角也是與上面的例子一樣,只是採取的特定的矩陣不一樣。計算的技巧和思路是一樣的。
3.左手座標系的轉換
左手座標系與右手座標系下的轉換的思路是一樣的,不同的地方是基礎旋轉矩陣,基礎旋轉矩陣的不同也就造成了對應的組合旋轉矩陣不同。下面將直接給出相應的資料,思路可以參考2.右手座標系相關的講解。
3.1左手系下的基礎旋轉矩陣
基礎旋轉矩陣的差異體現在sin值的取正負的問題上。仔細分析,這是可以理解的。在右手座標系下繞X軸旋轉p°,相當於右手系中旋轉-p°,而sin(-p) = - sin(p) cos(p) = cos(-p)
,所以有下面的差異。
//在右手系中繞X軸旋轉p° 對應的矩陣Rx
| 1 0 0 |
Rx= | 0 cosp sinp|
| 0 -sinp cosp|
//在右手系中繞Y軸旋轉h° 對應的矩陣Ry
| cosh 0 -sinh|
Ry= | 0 1 0 |
| sinh 0 cosh|
//在右手系中繞Z軸旋轉b° 對應的矩陣Rz
|cosb sinb 0 |
Rz= |-sinb cosb 0 |
| 0 0 1 |
3.2左手系下的組合旋轉矩陣
組合矩陣的計算方式依舊是矩陣連乘,由於基礎旋轉矩陣發生了變化,對應的組合旋轉矩陣也將發生變化。此處以Z-X-Y順規的尤拉角為例,其對應的組合旋轉矩陣為R1,可以看出與右手系下的區別也體現在某些項的正負號,也與上面的sin變號的原理相同。
//尤拉角(p,h,b)在右手系中對應的旋轉矩陣(Z-X-Y順規)
|cosbcosh+sinbsinpsinh sinbcosp -cosbsinh+sinbsinpcosh|
R1 = Rz*Rx*Ry = |-sinbcosh+cosbsinpsinh cosbcosp sinbsinh+cosbsinpcosh|
| cospsinh -sinp cospcosh |
3.3 左手系中矩陣轉化成尤拉角
求解的方式與右手系下的過程一致,只是最後的結果在正負號上有差異。
| m11 m12 m13 | |cosbcosh+sinbsinpsinh sinbcosp cosbsinh+sinbsinpcosh|
M=| m21 m22 m23 | = |-sinbcosh+cosbsinpsinh cosbcosp sinbsinh+cosbsinpcosh| = R=Rz*Rx*Ry
| m31 m32 m33 | | cospsinh -sinp cospcosh |
通過-sinp = m32
求解角度p = asin(-m32)
;
通過m31 m33
求解角度h = atan2(m31/cosp , m33/cosp)
,值得注意的只與兩個引數的比例值有關係,與絕對大小沒有關係,所以進一步化簡為h = atan2(m31, m33)
;
通過m12, m22
求解角度b = atan2(-m12/cosp, m22/cosp)
,同理化簡為h = atan2(-m12, m22)
。
4.萬向鎖問題
在由尤拉角計算對應的旋轉矩陣的時候,套用對應的矩陣公式即可。在由旋轉矩陣方向計算尤拉角的時候需要特別注意萬向鎖的情況。上面所有的計算過程都沒有考慮這個問題,此處單獨列出來說明。
萬向鎖又叫Gimble lock,很多人可能都聽說過,但是不清楚為什麼在由旋轉矩陣計算尤拉角的時候要考慮萬向鎖。在我們上面2.3和3.2節求解中,我們都將cosp
作為分母,預設cosp !=0
。
下面單獨考慮cosp ==0
的情況。當cosp =0
的時候sinp → ±1
,也就是p = ±90°
。百度百科這次非常靠譜的介紹了90°與萬向鎖的關係,引用至此。
p=±90°的時候,如果採用的是靜態尤拉角就會出現萬向鎖。此時的處理方法就是在計算尤拉角的時候首先判斷sinp的取值是不是無限接近於1,如果是採用《3D數學基礎:圖形與遊戲開發》10.6.2章節中的修正內容,即在使用R=M
中將R矩陣中做替換cosp=0; b=0; sinb=0;cosb=1
。修正的方法相對簡單,也不是本文的重點,此處不具體講。