剛體在三維空間的旋轉(關於旋轉矩陣、DCM、旋轉向量、四元數、歐拉角)

最近學習了一些關於三維空間旋轉相關的知識，藉此梳理一下備忘。

三維空間的旋轉(3D Rotation)是一個很神奇的東東：如果對某個剛體在三維空間進行任意次的旋轉，只要旋轉中心保持不變，無論多少次的旋轉都可以用繞三維空間中某一個軸的一次旋轉來表示。表示三維空間的旋轉有多種互相等價的方式，常見的有旋轉矩陣、DCM、旋轉向量、四元數、歐拉角等。本篇文章主要梳理一下這些表示方式及相互轉換的方法。

最直觀的表示方式是繞剛體自身的X、Y、Z三個軸分別進行旋轉某個角度，這就是所謂的歐拉角(Euler Angle)表示方式。

需要注意的是，歐拉角的表示方式里，yaw、pitch、roll的順序對旋轉的結果是有影響的。給定一組歐拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的順序旋轉和按照yaw-roll-pitch的順序旋轉，最終剛體的朝向是不同的！換言之，若剛體需要按照兩種不同的旋轉順序旋轉到相同的朝向，所需要的歐拉角角度值則是不同的！

另外需要注意的是，在歐拉角的表示方式里，三個旋轉軸一般是隨著剛體在運動，即wikipedia中所謂的intrinsic rotation，見下圖動畫所示(圖來自wikipedia)。相對應的另一種表示方式是，三個旋轉軸是固定的，不隨剛體旋轉而旋轉，即extrinsic rotation，這種表示方式在計算機視覺中不是很常用。

歐拉角的表示方式比較直觀，但是有幾個缺點：

(1) 歐拉角的表示方式不唯一。給定某個起始朝向和目標朝向，即使給定yaw、pitch、roll的順序，也可以通過不同的yaw/pitch/roll的角度組合來表示所需的旋轉。比如，同樣的yaw-pitch-roll順序，(0,90,0)和(90,90,90)會將剛體轉到相同的位置。這其實主要是由於萬向鎖(Gimbal Lock)引起的，關於萬向鎖的解釋，有條件的同學看看Youtube的視頻或許會比較直觀。

(2) 歐拉角的插值比較難。

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

在計算坐標變換時，旋轉更方便的表示形式是旋轉矩陣(Rotation Matrix)。三維空間的旋轉矩陣可以表示成3x3的矩陣，將歐拉角轉換為旋轉矩陣的計算方式如下，假設歐拉角yaw、pitch、roll的角度為alpha, beta, gamma，則旋轉矩陣可以計算如下：

其中，

這裡也可以看出，如果yaw、pitch、roll的順序有改變，矩陣相乘的順序需要作出相應改變，所得的旋轉矩陣結果也會有所改變。

需要注意的是，旋轉矩陣的雖然有9個元素，但是只有3個自由度，所以不是任何矩陣都可以作為旋轉矩陣，旋轉矩陣需要是正交矩陣 (即逆矩陣等於轉置矩陣)。

此外，旋轉矩陣的另一個名字叫方向餘弦矩陣(Direction Cosine Matrix)，簡稱DCM，在陀螺力學領域較為常用。DCM的名字來歷其實是用歐拉角之外的另一種用3個角度值表示三維旋轉的方式，假設剛體在起始朝向時三個坐標軸的向量為I,J,K，而剛體在目標朝向時的三個坐標軸的向量為i,j,k，則該旋轉可以通過三個坐標軸分別與原始坐標軸的夾角表示，如下圖所示：

DCM可以通過三個夾角的餘弦計算如下：

這就是DCM名稱的由來。其實可以驗證，DCM其實就是旋轉矩陣，所以，下文不再區分DCM和旋轉矩陣的稱呼。

在Matlab中(R2006a以後的版本中，需安裝Aerospace Toolbox)，可以方便地用angle2dcm和dcm2angle來轉換歐拉角和旋轉矩陣。下面的Matlab代碼可以驗證，兩個不同的歐拉角方式可以轉換到相同的旋轉矩陣：

[plain] view plain copy print?

1. %MatlabcodebyMulinB,AerospaceToolboxisneeded
2. %GimbalLockexperiments
3. yaw1=0;
4. pitch1=90;
5. roll1=0;
6. yaw2=90;
7. pitch2=90;
8. roll2=90;
10. R1=angle2dcm(yaw1/180*pi,pitch1/180*pi,roll1/180*pi);
11. R2=angle2dcm(yaw2/180*pi,pitch2/180*pi,roll2/180*pi);
12. disp(R1);disp(R2);

3. 四元數(Quaternion)、旋轉向量(Rotation Vector)、軸-角表示(Axis-Angle)

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

Axis-Angle的表示方法還可以推導出另一種很常用的三維旋轉表示方法，叫四元數(Quaternion)，這裡有一篇非常通俗易懂介紹四元數的文章。同上，假設(x,y,z)是axis方向的單位向量，theta是繞axis轉過的角度，那麼四元數可以表示為[cos(theta/2), x*sin(theta/2), y*sin(theta/2), z*sin(theta/2)]。注意，這裡可以推導出，用於表示旋轉的四元數向量也必須是單位向量。四元數的神奇之處在於，對於三維坐標的旋轉，可以通過四元數乘法直接操作，與上述旋轉矩陣操作可以等價，但是表示方式更加緊湊，計算量也可以小一些。首先，四元數的乘法是如下規定的：

由此定義，四元數的逆也可以求出。作為旋轉四元數，由於其單位向量的特性，四元數的逆其實等於四元數的共軛，也就是如果四元數q=[a,b,c,d]，由於a^2+b^2+c^2+d^2=1，那麼q的逆和共軛都是q"=[a,-b,-c,-d]。需要注意的是，四元數的乘法是不可交換的。通過四元數計算旋轉的方式為(將三維空間一個點v_I旋轉到v_B，四元數是q)：

在Matlab里，可以用quatmultiply計算四元數乘法，用quatinv來計算四元數的逆，用quatconj來計算四元數的共軛。四元數的旋轉和旋轉矩陣的旋轉可以由以下matlab代碼驗證：

[plain] view plain copy print?

1. %MatlabcodebyMulinB,AerospaceToolboxisneeded
3. pt=[10,20,30];%pointcoordinate
5. yaw=45;
6. pitch=30;
7. roll=60;
9. q=angle2quat(yaw/180*pi,pitch/180*pi,roll/180*pi);
10. R=angle2dcm(yaw/180*pi,pitch/180*pi,roll/180*pi);
12. pt1=R*pt";
13. pt2=quatmultiply(quatconj(q),quatmultiply([0,pt],q));%NOTEtheorder
14. disp(pt1");disp(pt2(2:4));

從上述代碼里也可以看到四元數和歐拉角和dcm的轉換，在matlab里可以很方便的用quat, dcm, angle之間的轉換來任意互轉。另外，從四元數計算axis和angle，可以用以下代碼計算：[plain] view plain copy print?

1. %MatlabcodebyMulinB,Computetheaxisandanglefromaquaternion
2. function[axis,theta]=quat2axisangle(q)
4. theta=acos(q(1))*2;
5. axis=q(2:4)/sin(theta/2);

從OpenCV的rotation vector和quaternion的互轉可以用以下代碼：[plain] view plain copy print?

1. %MatlabcodebyMulinB,Convertaquaterniontoarotationvector
2. functionrvec=quat2rvec(q)
4. theta=acos(q(1))*2;
5. axis=q(2:4)/sin(theta/2);
6. axis=axis/norm(axis);
8. rvec=axis*theta;

[plain] view plain copy print?

1. %MatlabcodebyMulinB,Convertarotationvectortoaquaternion
2. functionq=rvec2quat(rvec)
4. theta=norm(rvec);
5. axis=rvec/theta;
6. sht=sin(theta/2);
7. q=[cos(theta/2),axis*sht];

關於旋轉四元數的比較好的文檔，這裡列幾個參考文獻：

[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陀螺儀的微型化與普及，越來越多的計算機視覺演算法會增加IMU作為輔助信息輸入，增加系統的穩定性。關於陀螺儀的數據融合和姿態角解算，這裡列幾篇比較好的參考文獻：

[1]. IMU Data Fusing:Complementary, Kalman, and Mahony Filter

[2]. Crazepony Open Source Project