轉載請注明出處：http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/
文章鏈接：http://blog.csdn.net/hongbin_xu/article/details/55667899或http://hongbin96.com/116

對于每個像我一樣入坑四軸飛行器不久的新手來說，最初接觸也頗為頭疼的東西之一就是四軸的姿態解算。由于涉及較多的數學知識，很多人也是覺得十分頭疼。所以，我在這里分享一些我學習過程中的筆記和經驗，以便大家學習。

兩個坐標系：
首先，在一個姿態航向參考系統（簡稱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）可以得到：



這四個公式的意義是，給出了四元數與歐拉角之間的關系，我們可以很方便地使用這幾個公式將歐拉角與四元數相互轉換。還需要注意一點，因為方向余弦矩陣的定義不同，對應的歐拉角旋轉方式不同，公式也會不同。

到此結束。
這些是我前段時間的學習筆記，最近才開始整理。希望能對更多人的學習提供幫助。歡迎大家互相交流指正。

總結

以上是生活随笔為你收集整理的四旋翼姿态解算——基础理论及推导的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。