無人機四元數解算姿態角解析

一、概述

無人機求解姿態角有多種算法，但由于各種算法的自身限制及計算機計算速度的限制，所以我們需要選擇一個較佳的求解算法，下面我們先來看看幾種求解姿態角的算法：

1.歐拉角法：

歐拉角法（又稱三參數法）是歐拉在1776 年提出來的，其原理是動坐標系相對參考坐標系之間的位置關系可以用一組歐拉角來描述。解算歐拉角微分方程只需要解三個微分方程，與其它方法相比，需要求解的方程個數少一些但在用計算機進行數值積分時，要進行超越函數（三角函數）的運算，從而加大了計算的工作量。用此方法求解得到的姿態矩陣永遠是正交矩陣。在進行加速度信息的坐標變換時變換后的信息中不存在非正交誤差，得到的姿態矩陣不需要進行正交化處理。當載體的縱搖角(俯仰角）為90 °時，將出現奇點，因此該方法不能進行全姿態解算,其使用存在一定的局限。

2.方向余弦法：

方向余弦法（稱九參數法）用矢量的方向余弦來表示姿態矩陣的方法。繞定點轉動的兩個坐標系之間的關系可以用方向余弦矩陣來表示。方向余弦矩陣是隨時間變化的，其變化規律的數學描述就是方向余弦矩陣的微分方程，方向余弦矩陣的即時值就可以通過求解該微分方程而得到。該方法求解姿態矩陣避免了歐拉角法所遇到的奇點問題，可以全姿態工作。但方向余弦矩陣具有九個元素，所以需要解九個微分方程，計算工作量較大，在工程上并不實用。

3.三角函數法：

三角函數法(又稱六參數法)是將繞定點轉動的兩個坐標系之間的關系用三次轉動等效地表示，將三次轉動角度的正、余弦函數來表示姿態函數。該方法雖然避免了歐拉角法的缺點，可以全姿態工作，但需要解六個微分方程，計算量也不小，工程上并不實用。

4.Rodrigues參數法：

Rodrigues 數法是法國數學家Rodrigues 在1840 年提出的，該方法所描述的姿態是唯一的，并且具有簡潔、直觀的優點，其微分方程結構簡單，無多余約束，計算效率優于當前廣泛使用的四元數法。由于Rodrigues 參數法存在旋轉角有奇異值的缺陷 ，因此限制了其在工程上的應用。Schaub 和Junkin 對該方法的缺陷，改進后仍然存在奇異值。但是Rodriguess 參數法仍不失為解算姿態的有效途徑。

5.四元數法：

四元數法 (又稱四參數法) 。英國數學家W.R.Haminlton 在1843 年在數學中引入了四元數。但直到20 世紀60 年代末期這種方法還沒有得到實際應用，隨著空間技術、計算技術SINS 技術的發展，四元數才引起人們的重視。求解四元數微分方程要解四個微分方程。雖然要比解歐拉微分方程多一個方程，但其優越性在于計算量小、精度高、可避免奇異性，該方法是目前研究的重點之一。由于方向余弦法在對載體姿態動力學求解時會產生歪斜、刻度和漂移誤差等，然而，SINS 中在進行姿態求解時估計出這些誤差是很重要的。與方向余弦法相比，四元數法的優點在于不僅歪斜誤差等于零；而且刻度誤差的推導很簡單，能得出便于進一步分析的解析表達式，而方向余弦法只有在特殊的情況下才能分析和檢測到刻度誤差，且不能得出通用的結論。通過從不同角度對歐拉角法、方向余弦法和四元數法進行對比。結果表明四元數法具有最佳的性能。

6.等效旋轉矢量法：

由于歐拉角法、方向余弦法、三角函數法和四元數法對于圓錐運動都有原理誤差，而Rodrigues 參數法又存在奇異值，因此人們又致力于研究能有效抑制圓錐運動所產生的不可交換性誤差的算法。由于剛體的有限轉動不是矢量，其轉動次序不可交換。無限小轉動才是矢量。 1971 年Bortz 提出的轉動向量微分方程為計算SINS 的姿態矩陣建立了全面的理論基礎。用該方法描述繞定點轉動的兩個坐標系之問的關系被稱為等效旋轉矢量法。轉動向量的變化率是慣性測量的角速度向量與計算得到的非互易速率向量二者之和，后者是影響SINS 姿態角精度的一個重要因素，因此，在高角速率動態環境中，為防止姿態誤差積累，必須對非互易速率向量進行補償。對非互易向量進行補償的計算一般稱為圓錐補償算法。要提高算法精度，可以有兩種選擇，一種是對陀螺輸出信號進行高頻率采樣，使用簡單的圓錐算法；另一種是適當降低對陀螺輸出信號進行采樣的頻率，使用復雜但精度高的圓錐算法。

從以上六種算法的介紹中，每個算法求解歐拉角都有其自身的小缺點，但通過綜合對比，在四軸上用來解算歐拉角的更好的算法是四元數法，雖然其也有缺點，但其優越性在于計算量小、精度高、可避免奇異性，因此被大多數人所采用。所以下面我們將接收通過如何通過四元數來計算我們所需要的姿態角。

二、基本概念

地理坐標系：

習慣上，我們以正北方(x軸)、正東方(y軸)、垂直指向天空(z軸)建立地理三維直角坐標系(符合右手定則建立)。圖形示例如下：

機體坐標系：

習慣上以機體正前方(x軸)、機體正右方(y軸)、機體垂直正上方(z軸)建立機體坐標軸。圖形示例如下：

MPU定義坐標系：

令芯片表面朝向自己，將其表面文字轉至正確角度，此時，以芯片內部中心為原點，水平向右的為x軸，豎直向上的為y軸，指向自己的為z軸。見下圖：

向量點積：

向量的叉乘(也叫向量的外積,定義符號“×”為兩向量叉乘符號)：

物理意義：

利用向量的叉積可以創造出一個新的維度，而這個維度是獨立于(垂直于)先前這個空間的，因此，在這個新的維度空間可以當成目標運動的參照系。圖形示例如下：

旋轉向量與旋轉矩陣的聯系：

處理三維旋轉問題時，通常采用旋轉矩陣的方式來描述。一個向量乘以旋轉矩陣等價于向量以某種方式進行旋轉。除了采用旋轉矩陣描述外，還可以用旋轉向量來描述旋轉，旋轉向量的長度（模）表示繞軸逆時針旋轉的角度（弧度）,下文將會推導旋轉矩陣。這里用圖片來表示旋轉向量。

四元數：

1.為什么會用到四元數

2.四元數

通過上述了解，我們知道了四元數可以表示三維空間的旋轉信息，那接下來我們先來了解下四元數的基本概念及運算法則。

1)四元數定義：

在數學中四元數定義為

上述關系可以敘述為：相同單位向量進行四元數乘法呈虛單位特性，相異單位向量進行四元數乘法呈兩向量叉乘特性，故四元數可以看作一個超復數(超復數是復數在抽象代數中的引申，以高維度呈現。),也可看作四維空間中的一個向量。

2)四元數的表示形式：

四元數有多種表現形式，有矢量式、復數式、三角式、矩陣式、指數式，下面我們簡單看一下他們各自的定義式。

A.矢量式

B.復數式

C三角式

D.矩陣式

3)四元數的大小

數學中用四元數的范數來表示四元數的大小，具體如下：

4)四元數的運算法則

A.加法與減法

若有如下兩個四元數：

B.乘法

乘法又分四元數與四元數相乘、四元數與標量相乘，具體如下：

a.與標量相乘

b.與四元數相乘

若有如下兩個四元數：

C.除法(稱為求逆)

逆的定義：

也即有

三、利用四元數求解姿態變換矩陣

前面我們講過，四元數可以描述三維空間的旋轉信息，也即是我們可以通過四元數求出一個物體相對于一個坐標系旋轉之后的坐標信息。

由于人在地面上看運動物體時，是以地理坐標系為參照系的，所以當我們想知道物體發生了怎樣的變化時，就得研究物體相對于參照坐標系到物體坐標系轉化之間的坐標關系。由于地理坐標系與物體坐標系均為直角坐標系，各個軸之間均為直角，當我們只研究兩個坐標系之間的角度變化時，可將物體(等效為剛體)與坐標系固聯，也即是，兩個坐標系之間的角位置關系可用剛體的轉動來表示。

那么物體從一個姿態變化到另一個姿態，可等效為物體繞了某一個軸通過無中間過程的一次旋轉完成，但實際物體可能經過了多次中間過程才變化到了我們想要的姿態，我們不去關心中間過程，我們只需找到一種變化關系，通過這種變換關系，可求出物體從地理坐標系變到物體坐標系坐標或者從物體坐標系變到地理坐標系的坐標。

變換關系的推導：

通過上述內容，我們可以知道，當我們研究兩個坐標之間的角度變化關系(地理坐標系與物體坐標系)時，取物體坐標系與物體固聯，物體坐標系與物體之間角位置始終不變，因此，我們可以以物體(視為剛體)定點轉動問題來推導我們想要的變換關系，下面，我們就來推導一下我們想要的變化關系。

由圖可知：

所以有

則有

由于未旋轉之前，物體坐標系(b系)與地理坐標系重合，則有

對于物體來說，在轉動過程中，位置向量與物體的坐標系之間的相對角位置始終不變，即有

所以記D為物體坐標系至地理坐標系的坐標變換矩陣，記為

我們已經知道四元數的范數定義為：

所以可以得出描述物體轉動的四元數為規范化四元數。

四、歐拉角推導

我們以MPU規定的坐標系作為公式推導的地理坐標系和起始物體坐標系，將芯片水平放置，以芯片內部中心為原點，水平向右的為x軸，豎直向上的為z軸，指向正前方的為z軸。具體如下：

我們求物體的姿態角(在地面看物體運動)時，物體旋轉過后相對于之前的角度變化信息可以等效為物體依次繞三個軸旋轉復合得到，我們規定繞z軸旋轉稱物體的航向角(ψ)、繞y軸旋轉稱物體的俯仰角(γ)、繞x軸旋轉稱物體的翻滾角(θ),下面我們先推導物體分別繞三個軸的變換矩陣，最后根據三個變換矩陣復合得到物體相對于地理坐標系的角度信息。具體推導如下：

1.物體繞z軸旋轉

物體繞z軸旋轉，可知z軸不變，x、y軸的變換關系如下圖所示：

將以上三式寫為矩陣形式則得到繞Z軸旋轉ɑ角度后的坐標關系，如下所示：

2.物體繞Y軸旋轉

物體繞Y軸旋轉，可知Y軸不變，x、z軸的變換關系如下圖所示：

將以上三式寫為矩陣形式則得到物體繞Y軸旋轉β角度后的坐標信息，如下所示：

3.物體繞x軸旋轉

物體繞x軸旋轉，可知x軸不變，y、z軸的變換關系如下圖所示：

將以上三式寫為矩陣形式則得到物體繞x軸旋轉γ角度后的坐標信息，如下所示：

注：確定了第一個旋轉軸后，該旋轉軸變為父級(如z軸)、確定了第二個旋轉軸后，第二個旋轉軸(y軸)變為另一個軸(x軸)的父級，所以我們計算姿態矩陣時，順序應為子級(x)(相對于y而言)、子級(y)(相對于z而言)、父級(z)，也就是按上述順序進行矩陣相乘復合。

對(34)化簡得

以上得到了從地理系變換到物體系的姿態角變化矩陣，此前我們已經得到了利用四元數表示的從物體系變換到地理系的姿態變化矩陣，具體矩陣如下：

由(36)(由剛體定點轉動推導的無中間過程的姿態矩陣)、(37)(由物體分別繞z、y、x軸旋轉復合所得姿態角矩陣)，兩式均為物體坐標系變換到地理坐標系姿態矩陣，所以兩式矩陣相等，也即是有兩矩陣元素一一對應相等，則有

則根據以上五個式子，可求得姿態角如下：

這里需要注意：當我們求姿態角矩陣時，旋轉順序不一樣，得到的求角公式是不一樣的，求角的時候不能一味的套用公式，使用之前，應先了解一下旋轉順序。

至此，我們已經求出了姿態變換矩陣以及通過構造四元數，用四元數的方法來求得對應的姿態角，但我們現在存在一個問題是，我們不知道四元數具體數值，所以我們要求出真正的姿態角，必須找到求出四元數具體值的辦法。

五、四元數參數求解

通過上述推導，若我們知道了q0、q1、q2、q3的具體數值，我們就能求出我們需要的姿態角，我們現在的已知量只有從陀螺儀和加速度計獲得的角速度和加速度，那么我們怎么通過已知信息來求解四元數的具體數值呢，下面，我們引入四元數微分，看從中能不能獲得有用信息。

1.四元數微分

由(44)式可以知道，我們通過陀螺儀獲得角速度可以求出四元數參數，但我們需要求解一個四元數微分方程，通過求解微分方程，就可以得到我們需要的四元數參數，那么怎么求解一個微分方程呢？、

2.解微分方程

在解四元數微分方程之前，我們先以一個數學微分方程表達式來找到求其解的方法。

設有一微分方程

六、陀螺儀誤差的消除

前已提到，由于角度是由角速度積分得到，而積分時，若從陀螺儀獲得的角速度信息存有小的偏差，經過積分之后，就會使誤差加大，從而使獲得的角度存在偏差，但利用加速度計獲得的角度信息不會出現偏差，可是我們也不能直接利用加速度計獲得的角度信息，因為加速度計受噪聲影響較大，在飛行過程中受振動比陀螺儀明顯，短時間內可靠性不高，但積分后的角度信息是可信的，所以我們需要用加速度計獲得的角度信息去矯正陀螺儀獲得的姿態信息，從而使算出來的角度誤差消除。根據我們的思路，利用加速度計獲得的信息去補償陀螺儀的角速度信息，具體有如下步驟實現：

獲取加速度計的值(為物體坐標系下對應的值)，對其歸一化(歸一化的原因是因為姿態變化矩陣中的四元數是規范四元數，利用陀螺儀去更新的四元數也要歸一化，所以加速度計獲得的值也需歸一化才能是兩者對應)。記從加速度計獲得的值為ax、ay、az(分別對應x、y、z軸的值)，其歸一化方法如下：

2.獲取陀螺儀算出的姿態矩陣中的重力分量(因為加速度計是測得的物體坐標系下的值，所以我們也要提取利用角速度算出的姿態矩陣中的物體坐標系下的重力分量)。重力分量記為Vx、Vy、Vz，具體計算方法如下：

通過四元數計算的從E系(地理坐標系)變換到b系(物體坐標系)姿態矩陣為：

4.對誤差進行積分，從而消除誤差，設accex、accey、 accez為x、y、z三軸對應的誤差積分結果(對兩個重力分量叉乘后的誤差進行積分，結果得到角速度值)，ki為積分系數，dt為積分周期時間具體如下(我們程序未用這一步)：

5.互補濾波，將誤差輸入Pid控制器與本次姿態更新中陀螺儀測得的角速度相加，得到一個修正的角速度值，獲得的修正的角速度值去更新四元素，從而獲得準確的姿態角信息。設gx、gy、gz為陀螺儀測得的三個軸的角速度及濾波后的角速度修正值，Kp為互補濾波系數，則修正角速度計算方法如下：

至此，我們將利用四元數求解姿態角的算法講解結束了，但我們講解順序是從后到前一步一步推出我們需要的信息，在實際程序編寫過程中，我們需要從后往前對程序進行編寫，具體步驟我們引用一位網友給出的思維導圖，可以根據該思維導圖對程序進行編寫：

下面我們根據思維導圖用程序來一步一步實現如何求解歐拉角：

1.定義初始四元數的值為q0=1,q1=0,q2=0,q3=0。

2.讀取加速度計值、角速度值，程序定義變量分別為ax、ay、az，gx、gy、gz，將陀螺儀值轉為弧度，轉換如下：

3.對加速度值進行歸一化

4.提取姿態矩陣中的重力分量，我們已經其數學計算公式為

5.求姿態誤差，對兩向量進行叉乘(定義ex、ey、ez為三個軸誤差元素)，數學計算為：

6.對誤差積分(定義accex、accey、accez為積分值、ki=0.001為積分系數、dt=0.005為積分周期時間),其程序實現為(目前程序里未使用這一步)：

7.互補濾波，將誤差輸入PID控制器后與陀螺儀測得的角速度相加，修正角速度值，程序實現如下(Kp為互補濾波系數這里取Kp=0.8，實際值根據需要進行調整)：

10.計算姿態角，數學公式為

至此，我們就按照思維導圖，一步一步實現了用程序語句求解姿態角了。