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