四旋翼姿態解算——基礎理論及推導
對於每個像我一樣入坑四軸飛行器不久的新手來說，最初接觸也頗為頭疼的東西之一就是四軸的姿態解算。由於涉及較多的數學知識，很多人也是覺得十分頭疼。所以，我在這裡分享一些我學習過程中的筆記和經驗，以便大家學習。
兩個座標系：
首先，在一個姿態航向參考系統（簡稱AHRS）中，我們要定義兩個座標系：導航座標系 n 和載體座標系 b 。導航座標系 n 指的是以地球為參考的座標系，定義為東北天右手直角座標系；載體座標系 b 則是以四軸飛行器自身為參考的座標系， 也定義為右手直角座標系，取飛機向前的方向為 Y 軸正方向，取飛機向右的方向為X軸正方向，取飛機向上的方向為Z軸正方向。
四元數、尤拉角、方向餘弦：
在百度百科中，尤拉角是這樣被描述的：用來確定定點轉動剛體位置的3個一組獨立角參量，由章動角θ、旋進角（即進動角）ψ和自轉角j組成，為尤拉首先提出而得名。簡單點來說，就是：繞Z軸旋轉為偏航角（YAW）ψ，繞Y軸旋轉為橫滾角（ROLL）θ，繞X軸旋轉為俯仰角（PITCH）φ。
繞Z軸旋轉ψ角（YAW）：
定義導航座標系 n 中某一點的座標為（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旋轉的反向旋轉。
這裡已經講得比較清楚了，我們可以把四元數看成一個常數加上一個三維向量，即
四元數的乘法運算：
對於任意一個四元數
滿足乘法關係如下：
舉例：
假設有兩個四元數，
則這兩個四元數相乘結果為：
將上面的運算表示成矩陣形式：
設兩個四元數Q和P的乘積為四元數
則有：
或者
從M(Q)中，第一列為四元數Q本身，第一行為四元數Q的共軛的轉置，不管第一行和第一列，我們可以提取出一個3*3的矩陣VQ，稱其為M(Q)的核。
同理可得，M(P)的核VP：
四元數的相關知識的準備差不多完成了，下面開始推導四元數的公式：
我們定義一個四元數
代入求得：
可以得到旋轉矩陣
到這裡我們就推出了使用四元數表示的旋轉矩陣
前面使用尤拉角也匯出了一個旋轉矩陣
聯立兩者對應項相等，求解方程組即可。解方程的步驟就省略了，直接寫出結果。
令
推出結果：
前面我們用尤拉角推匯出來的旋轉矩陣
這裡我們代入方向餘弦矩陣對應項的值求出尤拉角與四元數的關係，並做一些三角函式的變換整理得到下面的形式：
上式是尤拉角用表示四元數的公式。
還是由方向餘弦矩陣（DCM）可以得到：
這四個公式的意義是，給出了四元數與尤拉角之間的關係，我們可以很方便地使用這幾個公式將尤拉角與四元數相互轉換。還需要注意一點，因為方向餘弦矩陣的定義不同，對應的尤拉角旋轉方式不同，公式也會不同。
到此結束。
這些是我前段時間的學習筆記，最近才開始整理。希望能對更多人的學習提供幫助。歡迎大家互相交流指正。