方向余弦陣，歐拉角，四元數

1，方向余弦陣

坐標變換矩陣的每一個元素都是i系某個坐標系與b系坐標系的夾角的余弦。這個轉換方法也同樣適用於二維坐標轉換。只不過二維坐標轉換不僅需要考慮旋轉，同時也需要考慮坐標的平移變換。

如上圖所示，表示只進行旋轉的時候二維坐標轉換公式。此時旋轉矩陣表示為P=

 

 

2，歐拉角

 經典的歐拉角有多種定義，下面只介紹內旋方式的定義。

 

所以，歐拉角只是繞着兩個本機坐標軸在旋轉，第三個坐標軸就沒有用到。

之后提出 Tait-Bryan 角

經常說的yaw，pitch，roll就是用Tait-Bryan角實現的。並且yaw指的是繞z軸旋轉的角度值，pitch指的是繞y軸旋轉的角度值，roll指的是繞x軸旋轉的角度值。

通常yaw角也用ψ表示，pitch角也用θ表示，roll角也用φ表示。並且yaw和roll的范圍是[-∏,∏],pitch的范圍是[-Π/2,Π/2].

不同的旋轉組合雖然可以得到相同的坐標轉換結果，但是得到的roll，pitch，yaw是不同的。

按照x，y，z順序旋轉得到由roll，pitch，yaw表示的方向余弦如下

由方向余弦得到roll pitch yaw的表達式如下

roll(φ)=atan2(R(3,2),R(3,3));

pitch(θ)=-asin(R(3,1));

yaw(ψ)=atan2(R(2,1),R(1,1));

3,四元數

四元數的理解可以參考http://www.qiujiawei.com/understanding-quaternions/（理解四元數，比較通俗的解釋了四元數）

推導請查看秦永元版慣性導航中相關章節。

四元數的幾何意義：實部表示旋轉角度的1/2的余弦值。虛部表示旋轉軸與旋轉角度的1/2的正弦值的乘積。

四元數得到方向余弦矩陣的表達式

上式和下式是相同的，因為四元數的平方和等於1。

由方向余弦陣到四元數的轉換可以參考以下matlab程序

function q=dcm2q(R)

T = 1 + R(1,1) + R(2,2) + R(3,3);

if T > 10^-8
    
    S = 0.5 / sqrt(T);
    qw = 0.25 / S;
    qx = ( R(3,2) - R(2,3) ) * S;
    qy = ( R(1,3) - R(3,1) ) * S;
    qz = ( R(2,1) - R(1,2) ) * S;

else
    
    if (R(1,1) > R(2,2)) && (R(1,1) > R(3,3))
        
        S = sqrt( 1 + R(1,1) - R(2,2) - R(3,3)) * 2; % S=4*qx
        qw = (R(3,2) - R(2,3)) / S;
        qx = 0.25 * S;
        qy = (R(1,2) + R(2,1)) / S;
        qz = (R(1,3) + R(3,1)) / S;
        
    elseif (R(2,2) > R(3,3))
        
        S = sqrt( 1 + R(2,2) - R(1,1) - R(3,3) ) * 2; %S=4*qy
        qw = (R(1,3) - R(3,1)) / S;
        qx = (R(1,2) + R(2,1)) / S;
        qy = 0.25 * S;
        qz = (R(2,3) + R(3,2)) / S;

    else
        
        S = sqrt( 1 + R(3,3) - R(1,1) - R(2,2) ) * 2; % S=4*qz
        qw = (R(2,1) - R(1,2)) / S;
        qx = (R(1,3) + R(3,1)) / S;
        qy = (R(2,3) + R(3,2)) / S;
        qz = 0.25 * S;

    end

end
q = [qx qy qz qw]';
end

4，方向余弦陣的微分

5，四元數微分

詳細的推導參看秦永元版慣性導航