對於每個像我一樣入坑四軸飛行器不久的新手來說，最初接觸也頗為頭疼的東西之一就是四軸的姿態解算。由於涉及較多的數學知識，很多人也是覺得十分頭疼。所以，我在這里分享一些我學習過程中的筆記和經驗，以便大家學習。
兩個坐標系：
首先，在一個姿態航向參考系統（簡稱AHRS）中，我們要定義兩個坐標系：導航坐標系 n 和載體坐標系 b 。導航坐標系 n 指的是以地球為參考的坐標系，定義為東北天右手直角坐標系；載體坐標系 b 則是以四軸飛行器自身為參考的坐標系， 也定義為右手直角坐標系，取飛機向前的方向為 Y 軸正方向，取飛機向右的方向為X軸正方向，取飛機向上的方向為Z軸正方向。
四元數、歐拉角、方向余弦：
在百度百科中，歐拉角是這樣被描述的：用來確定定點轉動剛體位置的3個一組獨立角參量，由章動角θ、旋進角（即進動角）ψ和自轉角j組成，為歐拉首先提出而得名。簡單點來說，就是：繞Z軸旋轉為偏航角（YAW）ψ，繞Y軸旋轉為橫滾角（ROLL）θ，繞X軸旋轉為俯仰角（PITCH）φ。
繞Z軸旋轉ψ角（YAW）：
定義導航坐標系 n 中某一點的坐標為（x，y，z），使用矩陣表示為：
。設該點在載體坐標系中坐標為（x’，y’，z’），使用矩陣表示為：
。對於該任意點，易得到兩個坐標系下坐標之間的關系：
。
表示成矩陣的形式如下：
同理可得：
繞Y軸旋轉θ角（ROLL）：
兩個坐標系下的轉換關系：
繞X軸旋轉φ角（PITCH）：
兩個坐標系下的轉換關系：
由前面的結論可以得到進過三個歐拉角的旋轉，得到導航坐標系下的向量
與旋轉后的載體坐標系下的向量
之間的關系：
給出由
到
的坐標變換矩陣：
。
所以可以得到用歐拉角表示的坐標變換矩陣：
這樣我們就得到了使用歐拉角表示的坐標變換矩陣，這個公式先放在這里，等會再用。
接下來我們來看看四元數：
先看看百度百科中對四元數概念的介紹：（四元數-百度百科 鏈接：http://baike.baidu.com/link?url=oQzRKzHEoKP6SgD9_qhBZKmsTU5NgSLqtxg4pXtw2hN0dXJQ9v9m11aNVW_M64b7vCeQ_9VNsKXQSnl2rR_FK0NVvGKcIF05d-N2_R9vQ0SLtrzKx9WQ19hHUvbYmd1z）
四元數是簡單的超復數。 復數是由實數加上虛數單位 i 組成，其中i^2 = -1。 相似地，四元數都是由實數加上三個虛數單位 i、j、k 組成，而且它們有如下的關系： i^2 = j^2 = k^2 = -1， i^0 = j^0 = k^0 = 1 , 每個四元數都是 1、i、j 和 k 的線性組合，即是四元數一般可表示為a + bk+ cj + di，其中a、b、c 、d是實數。
對於i、j、k本身的幾何意義可以理解為一種旋轉，其中i旋轉代表X軸與Y軸相交平面中X軸正向向Y軸正向的旋轉，j旋轉代表Z軸與X軸相交平面中Z軸正向向X軸正向的旋轉，k旋轉代表Y軸與Z軸相交平面中Y軸正向向Z軸正向的旋轉，-i、-j、-k分別代表i、j、k旋轉的反向旋轉。
這里已經講得比較清楚了，我們可以把四元數看成一個常數加上一個三維矢量，即
四元數的乘法運算：
對於任意一個四元數
來說，q0、q1、q2、q3都是實數，i、j、k為互相正交的單位向量，也是虛單位
。
滿足乘法關系如下：
舉例：
假設有兩個四元數，
和
。
則這兩個四元數相乘結果為：
將上面的運算表示成矩陣形式：
設兩個四元數Q和P的乘積為四元數
。
則有：
或者
從M(Q)中，第一列為四元數Q本身，第一行為四元數Q的共軛的轉置，不管第一行和第一列，我們可以提取出一個3*3的矩陣VQ，稱其為M(Q)的核。
同理可得，M(P)的核VP：
四元數的相關知識的准備差不多完成了，下面開始推導四元數的公式：
我們定義一個四元數
，用來表示從導航坐標系n和載體坐標系b之間的旋轉變換：
代入求得：
可以得到旋轉矩陣
的數學關系：
到這里我們就推出了使用四元數表示的旋轉矩陣
：
前面使用歐拉角也導出了一個旋轉矩陣
：
聯立兩者對應項相等，求解方程組即可。解方程的步驟就省略了，直接寫出結果。
令
推出結果：
前面我們用歐拉角推導出來的旋轉矩陣
也可以叫做方向余弦矩陣（DCM），使用的是Z-Y-X順規，不做贅述，有興趣可以再去查找相關資料。
這里我們代入方向余弦矩陣對應項的值求出歐拉角與四元數的關系，並做一些三角函數的變換整理得到下面的形式：
上式是歐拉角用表示四元數的公式。
還是由方向余弦矩陣（DCM）可以得到：
這四個公式的意義是，給出了四元數與歐拉角之間的關系，我們可以很方便地使用這幾個公式將歐拉角與四元數相互轉換。還需要注意一點，因為方向余弦矩陣的定義不同，對應的歐拉角旋轉方式不同，公式也會不同。
到此結束。
這些是我前段時間的學習筆記，最近才開始整理。希望能對更多人的學習提供幫助。歡迎大家互相交流指正。