最近學(xué)習(xí)了一些關(guān)于三維空間旋轉(zhuǎn)相關(guān)的知識，借此梳理一下備忘。
三維空間的旋轉(zhuǎn)(3D Rotation)是一個很神奇的東東：如果對某個剛體在三維空間進(jìn)行任意次的旋轉(zhuǎn)，只要旋轉(zhuǎn)中心保持不變，無論多少次的旋轉(zhuǎn)都可以用繞三維空間中某一個軸的一次旋轉(zhuǎn)來表示。表示三維空間的旋轉(zhuǎn)有多種互相等價的方式，常見的有旋轉(zhuǎn)矩陣、DCM、旋轉(zhuǎn)向量、四元數(shù)、歐拉角等。本篇文章主要梳理一下這些表示方式及相互轉(zhuǎn)換的方法。
最直觀的表示方式是繞剛體自身的X、Y、Z三個軸分別進(jìn)行旋轉(zhuǎn)某個角度，這就是所謂的歐拉角(Euler Angle)表示方式。
需要注意的是，歐拉角的表示方式里，yaw、pitch、roll的順序?qū)πD(zhuǎn)的結(jié)果是有影響的。給定一組歐拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的順序旋轉(zhuǎn)和按照yaw-roll-pitch的順序旋轉(zhuǎn)，最終剛體的朝向是不同的！換言之，若剛體需要按照兩種不同的旋轉(zhuǎn)順序旋轉(zhuǎn)到相同的朝向，所需要的歐拉角角度值則是不同的！
另外需要注意的是，在歐拉角的表示方式里，三個旋轉(zhuǎn)軸一般是隨著剛體在運(yùn)動，即wikipedia中所謂的intrinsic rotation，見下圖動畫所示(圖來自wikipedia)。相對應(yīng)的另一種表示方式是，三個旋轉(zhuǎn)軸是固定的，不隨剛體旋轉(zhuǎn)而旋轉(zhuǎn)，即extrinsic rotation，這種表示方式在計算機(jī)視覺中不是很常用。
歐拉角的表示方式比較直觀，但是有幾個缺點(diǎn)：
(1) 歐拉角的表示方式不唯一。給定某個起始朝向和目標(biāo)朝向，即使給定yaw、pitch、roll的順序，也可以通過不同的yaw/pitch/roll的角度組合來表示所需的旋轉(zhuǎn)。比如，同樣的yaw-pitch-roll順序，(0,90,0)和(90,90,90)會將剛體轉(zhuǎn)到相同的位置。這其實主要是由于萬向鎖(Gimbal Lock)引起的，關(guān)于萬向鎖的解釋，有條件的同學(xué)看看Youtube的視頻或許會比較直觀。
(2) 歐拉角的插值比較難。
(3) 計算旋轉(zhuǎn)變換時，一般需要轉(zhuǎn)換成旋轉(zhuǎn)矩陣，這時候需要計算很多sin, cos，計算量較大。
在計算坐標(biāo)變換時，旋轉(zhuǎn)更方便的表示形式是旋轉(zhuǎn)矩陣(Rotation Matrix)。三維空間的旋轉(zhuǎn)矩陣可以表示成3x3的矩陣，將歐拉角轉(zhuǎn)換為旋轉(zhuǎn)矩陣的計算方式如下，假設(shè)歐拉角yaw、pitch、roll的角度為alpha, beta, gamma，則旋轉(zhuǎn)矩陣可以計算如下：
其中，
這里也可以看出，如果yaw、pitch、roll的順序有改變，矩陣相乘的順序需要作出相應(yīng)改變，所得的旋轉(zhuǎn)矩陣結(jié)果也會有所改變。
需要注意的是，旋轉(zhuǎn)矩陣的雖然有9個元素，但是只有3個自由度，所以不是任何矩陣都可以作為旋轉(zhuǎn)矩陣，旋轉(zhuǎn)矩陣需要是正交矩陣 (即逆矩陣等于轉(zhuǎn)置矩陣)。
此外，旋轉(zhuǎn)矩陣的另一個名字叫方向余弦矩陣(Direction Cosine Matrix)，簡稱DCM，在陀螺力學(xué)領(lǐng)域較為常用。DCM的名字來歷其實是用歐拉角之外的另一種用3個角度值表示三維旋轉(zhuǎn)的方式，假設(shè)剛體在起始朝向時三個坐標(biāo)軸的向量為I,J,K，而剛體在目標(biāo)朝向時的三個坐標(biāo)軸的向量為i,j,k，則該旋轉(zhuǎn)可以通過三個坐標(biāo)軸分別與原始坐標(biāo)軸的夾角表示，如下圖所示：
DCM可以通過三個夾角的余弦計算如下：
這就是DCM名稱的由來。其實可以驗證，DCM其實就是旋轉(zhuǎn)矩陣，所以，下文不再區(qū)分DCM和旋轉(zhuǎn)矩陣的稱呼。
在Matlab中(R2006a以后的版本中，需安裝Aerospace Toolbox)，可以方便地用angle2dcm和dcm2angle來轉(zhuǎn)換歐拉角和旋轉(zhuǎn)矩陣。下面的Matlab代碼可以驗證，兩個不同的歐拉角方式可以轉(zhuǎn)換到相同的旋轉(zhuǎn)矩陣：
旋轉(zhuǎn)的一個神奇之處就在于，三維空間的任意旋轉(zhuǎn)，都可以用繞三維空間的某個軸旋轉(zhuǎn)過某個角度來表示，即所謂的Axis-Angle表示方法。這種表示方法里，Axis可用一個三維向量(x,y,z)來表示，theta可以用一個角度值來表示，直觀來講，一個四維向量(theta,x,y,z)就可以表示出三維空間任意的旋轉(zhuǎn)。注意，這里的三維向量(x,y,z)只是用來表示axis的方向朝向，因此更緊湊的表示方式是用一個單位向量來表示方向axis，而用該三維向量的長度來表示角度值theta。這樣以來，可以用一個三維向量(theta*x, theta*y, theta*z)就可以表示出三維空間任意的旋轉(zhuǎn)，前提是其中(x,y,z)是單位向量。這就是旋轉(zhuǎn)向量(Rotation Vector)的表示方式，OpenCV里大量使用的就是這種表示方法來表示旋轉(zhuǎn)(見OpenCV相機(jī)標(biāo)定部分的rvec)。
Axis-Angle的表示方法還可以推導(dǎo)出另一種很常用的三維旋轉(zhuǎn)表示方法，叫四元數(shù)(Quaternion)，這里有一篇非常通俗易懂介紹四元數(shù)的文章。同上，假設(shè)(x,y,z)是axis方向的單位向量，theta是繞axis轉(zhuǎn)過的角度，那么四元數(shù)可以表示為[cos(theta/2), x*sin(theta/2), y*sin(theta/2), z*sin(theta/2)]。注意，這里可以推導(dǎo)出，用于表示旋轉(zhuǎn)的四元數(shù)向量也必須是單位向量。四元數(shù)的神奇之處在于，對于三維坐標(biāo)的旋轉(zhuǎn)，可以通過四元數(shù)乘法直接操作，與上述旋轉(zhuǎn)矩陣操作可以等價，但是表示方式更加緊湊，計算量也可以小一些。首先，四元數(shù)的乘法是如下規(guī)定的：
由此定義，四元數(shù)的逆也可以求出。作為旋轉(zhuǎn)四元數(shù)，由于其單位向量的特性，四元數(shù)的逆其實等于四元數(shù)的共軛，也就是如果四元數(shù)q=[a,b,c,d]，由于a^2+b^2+c^2+d^2=1，那么q的逆和共軛都是q'=[a,-b,-c,-d]。需要注意的是，四元數(shù)的乘法是不可交換的。通過四元數(shù)計算旋轉(zhuǎn)的方式為(將三維空間一個點(diǎn)v_I旋轉(zhuǎn)到v_B，四元數(shù)是q)：
在Matlab里，可以用quatmultiply計算四元數(shù)乘法，用quatinv來計算四元數(shù)的逆，用quatconj來計算四元數(shù)的共軛。四元數(shù)的旋轉(zhuǎn)和旋轉(zhuǎn)矩陣的旋轉(zhuǎn)可以由以下matlab代碼驗證：
關(guān)于旋轉(zhuǎn)四元數(shù)的比較好的文檔，這里列幾個參考文獻(xiàn)：
[1] Indirect Kalman Filter for 3D Attitude Estimation (by Nikolas Trawny and Stergios Roumeliotis)
[2] Quaternion Kinematics for Error-State KF (by Joan Sola)
[3] A Mathematical Introduction to Robotic Manipulation (by Richard Murray, Zexiang Li, and S. Sastry)
隨著MEMS陀螺儀的微型化與普及，越來越多的計算機(jī)視覺算法會增加IMU作為輔助信息輸入，增加系統(tǒng)的穩(wěn)定性。關(guān)于陀螺儀的數(shù)據(jù)融合和姿態(tài)角解算，這里列幾篇比較好的參考文獻(xiàn)：
[1]. IMU Data Fusing: Complementary, Kalman, and Mahony Filter
[2]. Crazepony Open Source Project
