旋轉矩陣與四元數

點我進入原文

然後還看到一篇講 旋轉矩陣和 四元素之間轉化的 http://blog.csdn.net/shenshikexmu/article/details/53608224?locationNum=8&fps=1

從四元數到左乘旋轉矩陣

四元數Q=cosθ2+uRsinθ2，Q包含了旋轉的全部信息：θ 爲轉過的角度，uR爲先旋轉軸和旋轉方向。 
也看到兩種四元數表達方式，旋轉角度在前，和在後，本文的四元數旋轉角度爲第一項，也就是q0=cosθ2。 
四元數Q=[q0,q1,q2,q3]。 
左乘旋轉矩陣爲： 

Rleft=⎡⎣⎢⎢q20+q21−q22−q232(q1q2−q0q3)2(q0q2+q1q3)2(q1q2+q0q3)q20+q22−q21−q232(q2q3−q0q1)2(q1q3−q0q2)2(q2q3+q0q1)q20+q23−q21−q22⎤⎦⎥⎥

在Gait-Tracking-With-x-IMU 中，四元數轉旋轉矩陣matlab代碼，如下：

function R = quatern2rotMat(q)
    [rows cols] = size(q);
    R = zeros(3,3, rows);
    R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2;
    R(1,2,:) = 2.*(q(:,2).*q(:,3)+q(:,1).*q(:,4));
    R(1,3,:) = 2.*(q(:,2).*q(:,4)-q(:,1).*q(:,3));
    R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4));
    R(2,2,:) = 2.*q(:,1).^2-1+2.*q(:,3).^2;
    R(2,3,:) = 2.*(q(:,3).*q(:,4)+q(:,1).*q(:,2));
    R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3));
    R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2));
    R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2;
end

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14

代碼中的計算和旋轉矩陣一樣，其中q(:,1)代q0，q(:,2)代q1，q(:,3)代q2，q(:,4)代q3，對角元利用了四元數的性質q20+q21+q22+q23=1。

算法2

論文New Method for Extracting the Quaternion from a Rotation Matrix的算法。 
算法先把左乘旋轉矩陣Rleft轉化成K3矩陣，如下： 

K3=13⎡⎣⎢⎢⎢T11−T22−T33T21+T12T31+T13T23−T32T21+T12T22−T11−T33T32+T23T31−T13T31+T13T32+T23T33−T11−T22T12−T21T23−T32T31−T13T12−T21T11+T22+T33⎤⎦⎥⎥⎥

K3矩陣用四元數的方式表示爲： 

13⎡⎣⎢⎢⎢⎢4q21−14q1q24q1q34q1q04q1q24q22−14q2q34q2q04q1q34q2q34q23−14q3q04q1q04q2q04q3q04q20−1⎤⎦⎥⎥⎥⎥

矩陣可轉化爲 

43⎡⎣⎢⎢⎢⎢q1q2q3q0⎤⎦⎥⎥⎥⎥[q1q2q3q0]−13I4∗4

可以看出,K3矩陣只有一個特徵向量，[q1,q2,q3,q0]，其特徵值爲1.

在Gait-Tracking-With-x-IMU 中，旋轉矩陣轉四元數matlab代碼，如下：

function q = rotMat2quatern(R)  
    %wiki URL: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#cite_note-5
    % paper URL: http://arc.aiaa.org/doi/pdf/10.2514/2.4654
    [row col numR] = size(R);
    q = zeros(numR, 4);
    K = zeros(4,4);    
    for i = 1:numR
        K(1,1) = (1/3) * (R(1,1,i) - R(2,2,i) - R(3,3,i));
        K(1,2) = (1/3) * (R(2,1,i) + R(1,2,i));
        K(1,3) = (1/3) * (R(3,1,i) + R(1,3,i));
        K(1,4) = (1/3) * (R(2,3,i) - R(3,2,i));  
        K(2,1) = (1/3) * (R(2,1,i) + R(1,2,i));
        K(2,2) = (1/3) * (R(2,2,i) - R(1,1,i) - R(3,3,i));
        K(2,3) = (1/3) * (R(3,2,i) + R(2,3,i));
        K(2,4) = (1/3) * (R(3,1,i) - R(1,3,i));   
        K(3,1) = (1/3) * (R(3,1,i) + R(1,3,i));
        K(3,2) = (1/3) * (R(3,2,i) + R(2,3,i));
        K(3,3) = (1/3) * (R(3,3,i) - R(1,1,i) - R(2,2,i));
        K(3,4) = (1/3) * (R(1,2,i) - R(2,1,i));    
        K(4,1) = (1/3) * (R(2,3,i) - R(3,2,i));
        K(4,2) = (1/3) * (R(3,1,i) - R(1,3,i));
        K(4,3) = (1/3) * (R(1,2,i) - R(2,1,i));
        K(4,4) = (1/3) * (R(1,1,i) + R(2,2,i) + R(3,3,i)); 
        [V,D] = eig(K);
        %p = find(max(D));
        %q = V(:,p)';    
        q(i,:) = V(:,4)';
        q(i,:) = [q(i,4) q(i,1) q(i,2) q(i,3)];
    end
end

其他的轉化方式可以參考原文： http://blog.csdn.net/shenshikexmu/article/details/53608224?locationNum=8&fps=1

在計算機圖形學的學習中，幾何變換（Transformations）是一塊重要的內容，我們使用齊次座標（Homogeneous coordinates）描述點和向量，使用變換矩陣描述平移、旋轉等變換。

而在平移、旋轉、縮放這幾種變換中，又以旋轉的情況最爲複雜。實際上，計算機圖形學中三維空間的旋轉不僅僅有旋轉矩陣一種表達形式，歐拉角（Euler angles）和四元數（Quaternions）也是常用的方法。

---

旋轉矩陣

三維空間中的一個點 PP ，我們用齊次座標表示：

P=[x,y,z,w]TP=[x,y,z,w]T

我們首先考慮分別繞 X 軸、Y 軸、Z 軸旋轉一定角度的情況。

繞座標軸旋轉

設 PP 繞 X 軸、Y 軸、Z 軸的旋轉角度分別爲 αα、ββ 和 θθ。

我們使用右手座標系，旋轉角度的正向由右手定則確定。

點繞座標軸旋轉可以考慮點在相應座標平面上投影的旋轉。比如繞 Y 軸旋轉，那麼考慮點 PP 在 X-Z 平面上的投影的旋轉，如下圖所示：

繞Y軸旋轉

假設點 PP 在 X-Z 平面上的投影點與座標原點連成的向量長度爲 L ，那麼根據簡單的平面幾何知識，我們可以得到：

[x1z1]=[cosβ−sinβsinβcosβ][xz][x1z1]=[cosβsinβ−sinβcosβ][xz]

用齊次座標表示繞Y軸的旋轉爲：

⎡⎣⎢⎢⎢x1y1z1w1⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢cosβ0−sinβ00100sinβ0cosβ00001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢xyzw⎤⎦⎥⎥⎥[x1y1z1w1]=[cosβ0sinβ00100−sinβ0cosβ00001][xyzw]

同理可分別得到繞X軸與繞Y軸的情況。

繞X軸旋轉：

⎡⎣⎢⎢⎢x2y2z2w2⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢10000cosαsinα00−sinαcosα00001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢xyzw⎤⎦⎥⎥⎥[x2y2z2w2]=[10000cosα−sinα00sinαcosα00001][xyzw]

繞Z軸旋轉：

⎡⎣⎢⎢⎢x3y3z3w3⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢cosθsinθ00−sinθcosθ0000100001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢xyzw⎤⎦⎥⎥⎥[x3y3z3w3]=[cosθ−sinθ00sinθcosθ0000100001][xyzw]

我們可以將繞X、Y和Z座標軸的旋轉矩陣分別記爲 Rx(α),Ry(β),Rz(θ)Rx(α),Ry(β),Rz(θ)，則有：

Rx(α)=⎡⎣⎢⎢⎢10000cosαsinα00−sinαcosα00001⎤⎦⎥⎥⎥Rx(α)=[10000cosα−sinα00sinαcosα00001]

Ry(β)=⎡⎣⎢⎢⎢cosβ0−sinβ00100sinβ0cosβ00001⎤⎦⎥⎥⎥Ry(β)=[cosβ0sinβ00100−sinβ0cosβ00001]

Rz(θ)=⎡⎣⎢⎢⎢cosθsinθ00−sinθcosθ0000100001⎤⎦⎥⎥⎥Rz(θ)=[cosθ−sinθ00sinθcosθ0000100001]

旋轉矩陣可以通過其他旋轉矩陣複合得到（矩陣乘法）。

---

歐拉角

上面討論了繞三條座標軸旋轉的旋轉矩陣，旋轉矩陣的一般形式（這裏沒有用齊次座標）爲：

C=⎡⎣⎢c11c21c31c12c22c32c13c23c33⎤⎦⎥C=[c11c12c13c21c22c23c31c32c33]

物體在三維空間中的旋轉可以從座標系的旋轉來考慮（三維空間中座標軸，即三維線性空間中基的變換）。那麼矩陣 CC 的三個列向量實際對應着原座標系三個座標軸方向的單位向量在旋轉後的新座標系下的座標。

我們知道直角座標系的三個座標軸方向的單位向量實際上是一組標準正交基，於是矩陣 CC 是一個正交矩陣。所以旋轉矩陣表面上看起來依賴於 9 個參數，實際上只有三個是獨立的。

爲了更直接地指出這三個獨立參數，歐拉（Euler）證明了如下事實：任何一個旋轉都可以由連續施行的三次繞軸旋轉來實現，這三次繞軸旋轉的旋轉角就是三個獨立參數，稱爲歐拉角。

根據繞軸旋轉的順序不同，歐拉角的表示也不同。常見的歐拉角表示有 Yaw-Pitch-Roll (Y-X-Z順序)，通過下面的圖片可以形象地進行理解。

Yaw-Pitch-Roll

偏航(Yaw)：

airplane Yaw

仰俯(Pitch)：

airplane Pitch

側偏(Roll)：

airplane Roll

設 Yaw 、Pitch 、Roll 三個角度分別爲 θ,φ,ψθ,φ,ψ，那麼利用歐拉角進行旋轉對應的旋轉變換矩陣爲：

⎡⎣⎢cosψ cosθ−sinψ cosφ sinθcosψ sinθ+sinψ cosφ cosθsinψ sinφ−sinψ cosθ−cosψ cosφ sinθ−sinψ sinθ+cosψ cosφ cosθcosψ sinφsinφ sinθ−sinφ cosθcosφ⎤⎦⎥[cosψ cosθ−sinψ cosφ sinθ−sinψ cosθ−cosψ cosφ sinθsinφ sinθcosψ sinθ+sinψ cosφ cosθ−sinψ sinθ+cosψ cosφ cosθ−sinφ cosθsinψ sinφcosψ sinφcosφ]

實際上 Yaw 、Pitch 、Roll 的旋轉就分別對應着前面我們給出的旋轉矩陣 Rx(θ),Ry(φ),Rz(ψ)Rx(θ),Ry(φ),Rz(ψ)，上面的矩陣就是這三個矩陣的複合。

歐拉角的好處是簡單、容易理解，但使用它作爲旋轉的工具有嚴重的缺陷—萬向節死鎖（Gimbal Lock）。

萬向節死鎖是指物體的兩個旋轉軸指向同一個方向。實際上，當兩個旋轉軸平行時，我們就說萬向節鎖現象發生了，換句話說，繞一個軸旋轉可能會覆蓋住另一個軸的旋轉，從而失去一維自由度。

例如，三維空間中有一個平行於 X 軸的向量，我們將它繞 Y 軸旋轉直到它平行於 Z 軸，這時，我們會發現任何繞 Z 軸的旋轉都改變不了該向量的方向，即出現了萬向節死鎖。

由於萬向節死鎖的存在，使用歐拉角也無法很好地處理旋轉的插值（以實現“平滑”旋轉）。

---

四元數

從前面的討論我們發現三角度系統（three-angle system）無法很好地處理旋轉的插值。下面介紹四元數（Quaternions）以及如何利用四元數描述旋轉。

四元數的定義

四元數是由數學家 William Rowan Hamilton 於1843年所發明的數學概念，是複數的推廣，可以說是“三維的複數”，形式爲 w+x i+y j+z kw+x i+y j+z k ，其中 i,j,ki,j,k 的關係如下：

i2=j2=k2=−1i⋅j=−j⋅i=ki⋅k=−k⋅i=jj⋅k=−k⋅j=ii2=j2=k2=−1i⋅j=−j⋅i=ki⋅k=−k⋅i=jj⋅k=−k⋅j=i

假設有兩個四元數：

q1=w1+x1 i+y1 j+z1 kq2=w2+x2 i+y2 j+z2 kq1=w1+x1 i+y1 j+z1 kq2=w2+x2 i+y2 j+z2 k

四元數的加法定義如下：

q1+q2=(w1+w2)+(x1+x2) i+(y1+y2) j+(z1+z2) kq1+q2=(w1+w2)+(x1+x2) i+(y1+y2) j+(z1+z2) k

四元數的乘法定義，利用簡單的分配律定義如下：

q1⋅q2=(w1⋅w2−x1⋅x2−y1⋅y2−z1⋅z2)+(w1⋅x2+x1⋅w2+y1⋅z2−z1⋅y2) i+(w1⋅y2−x1⋅z2+y1⋅w2+z1⋅x1) j+(w1⋅z2+x1⋅y2−y1⋅x2+z1⋅w2) kq1⋅q2=(w1⋅w2−x1⋅x2−y1⋅y2−z1⋅z2)+(w1⋅x2+x1⋅w2+y1⋅z2−z1⋅y2) i+(w1⋅y2−x1⋅z2+y1⋅w2+z1⋅x1) j+(w1⋅z2+x1⋅y2−y1⋅x2+z1⋅w2) k

爲了方便表示，我們將四元數記爲：

q=(x,y,z,w)=(v⃗ ,w)q=(x,y,z,w)=(v→,w)

注意，這裏四元數的表示形式和“齊次座標”長得一樣，但是它們之間沒什麼關係！

四元數常常用來表示旋轉，很多人將其理解爲“w表示旋轉角度，v表示旋轉軸”，也是錯誤的！

正確的理解應爲：“w與旋轉角度有關，v與旋轉軸有關”。

四元數的模（norm）定義爲

|q|=x2+y2+z2+w2|q|=x2+y2+z2+w2

模爲1的四元數稱爲單位四元數(Unit quaternions)。

四元數的共軛（conjugate）定義爲:

q∗=(−v⃗ ,w)q∗=(−v→,w)

四元數的倒數定義爲：

1/q=q∗/|q|21/q=q∗/|q|2

四元數與旋轉

這裏直接給出結論：如果把單位四元數表示爲：

q=(n⃗ ⋅sinθ2, cosθ2)q=(n→⋅sinθ2, cosθ2)

的形式，那麼該單位四元數可以表示繞軸 n⃗ n→
進行 θθ 角的旋轉。

該單位四元數對應的旋轉矩陣爲

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

這裏的推導用到了軸-角旋轉表示中的Rodrigues’ rotation formula，具體證明這裏不展開了，有興趣的可以查閱相關資料。

我們發現用四元數描述旋轉需要的存儲空間很小，更爲關鍵的是可以使用被稱爲球面線性插值（Slerp Algorithm）的方法對四元數進行插值運算，從而解決了平滑旋轉的插值問題。

在 OpenGL 或者 DirectX 中我們通常使用模型視圖矩陣來進行幾何變換，當我們希望實現光滑旋轉、對旋轉進行插值時，就可以利用四元數這一工具。處理過程爲：

• 模型視圖矩陣 -> 四元數
• 使用四元數進行運算
• 四元數 -> 模型視圖矩陣