四旋翼姿態解算之理論推導

今天轉載一篇咱們隊長寫的關於四旋翼的博客！

轉載聲明：轉自http://www.cnblogs.com/xuhongbin/p/6538345.html

 

四旋翼姿態解算——基礎理論及推導

對於每一個像我同樣入坑四軸飛行器不久的新手來講，最初接觸也頗爲頭疼的東西之一就是四軸的姿態解算。因爲涉及較多的數學知識，不少人也是以爲十分頭疼。因此，我在這裏分享一些我學習過程當中的筆記和經驗，以便你們學習。

兩個座標系：
首先，在一個姿態航向參考系統（簡稱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 , 每一個四元數都是 一、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、q一、q二、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）能夠獲得：



這四個公式的意義是，給出了四元數與歐拉角之間的關係，咱們能夠很方便地使用這幾個公式將歐拉角與四元數相互轉換。還須要注意一點，由於方向餘弦矩陣的定義不一樣，對應的歐拉角旋轉方式不一樣，公式也會不一樣。

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