關(guān)于無人機四元數(shù)解算姿態(tài)角解析你知道嗎？

無人機四元數(shù)解算姿態(tài)角解析

一、概述

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

1.歐拉角法：

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

2.方向余弦法：

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

3.三角函數(shù)法：

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

4.Rodrigues參數(shù)法：

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

5.四元數(shù)法：

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

6.等效旋轉(zhuǎn)矢量法：

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

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

二、基本概念

地理坐標(biāo)系：

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

機體坐標(biāo)系：

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

MPU定義坐標(biāo)系：

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

向量點積：

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

物理意義：

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

旋轉(zhuǎn)向量與旋轉(zhuǎn)矩陣的聯(lián)系：

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

四元數(shù)：

1.為什么會用到四元數(shù)

2.四元數(shù)

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

1)四元數(shù)定義：

在數(shù)學(xué)中四元數(shù)定義為

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

2)四元數(shù)的表示形式：

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

A.矢量式

B.復(fù)數(shù)式

C三角式

D.矩陣式

3)四元數(shù)的大小

數(shù)學(xué)中用四元數(shù)的范數(shù)來表示四元數(shù)的大小，具體如下：

4)四元數(shù)的運算法則

A.加法與減法

若有如下兩個四元數(shù)：

B.乘法

乘法又分四元數(shù)與四元數(shù)相乘、四元數(shù)與標(biāo)量相乘，具體如下：

a.與標(biāo)量相乘

b.與四元數(shù)相乘

若有如下兩個四元數(shù)：

C.除法(稱為求逆)

逆的定義：

也即有

三、利用四元數(shù)求解姿態(tài)變換矩陣

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

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

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

變換關(guān)系的推導(dǎo)：

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

由圖可知：

所以有

則有

由于未旋轉(zhuǎn)之前，物體坐標(biāo)系(b系)與地理坐標(biāo)系重合，則有

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

所以記D為物體坐標(biāo)系至地理坐標(biāo)系的坐標(biāo)變換矩陣，記為

我們已經(jīng)知道四元數(shù)的范數(shù)定義為：

所以可以得出描述物體轉(zhuǎn)動的四元數(shù)為規(guī)范化四元數(shù)。

四、歐拉角推導(dǎo)

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

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

1.物體繞z軸旋轉(zhuǎn)

物體繞z軸旋轉(zhuǎn)，可知z軸不變，x、y軸的變換關(guān)系如下圖所示：

將以上三式寫為矩陣形式則得到繞Z軸旋轉(zhuǎn)ɑ角度后的坐標(biāo)關(guān)系，如下所示：

2.物體繞Y軸旋轉(zhuǎn)

物體繞Y軸旋轉(zhuǎn)，可知Y軸不變，x、z軸的變換關(guān)系如下圖所示：

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

3.物體繞x軸旋轉(zhuǎn)

物體繞x軸旋轉(zhuǎn)，可知x軸不變，y、z軸的變換關(guān)系如下圖所示：

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

注：確定了第一個旋轉(zhuǎn)軸后，該旋轉(zhuǎn)軸變?yōu)楦讣?如z軸)、確定了第二個旋轉(zhuǎn)軸后，第二個旋轉(zhuǎn)軸(y軸)變?yōu)榱硪粋€軸(x軸)的父級，所以我們計算姿態(tài)矩陣時，順序應(yīng)為子級(x)(相對于y而言)、子級(y)(相對于z而言)、父級(z)，也就是按上述順序進行矩陣相乘復(fù)合。

對(34)化簡得

以上得到了從地理系變換到物體系的姿態(tài)角變化矩陣，此前我們已經(jīng)得到了利用四元數(shù)表示的從物體系變換到地理系的姿態(tài)變化矩陣，具體矩陣如下：

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

則根據(jù)以上五個式子，可求得姿態(tài)角如下：

這里需要注意：當(dāng)我們求姿態(tài)角矩陣時，旋轉(zhuǎn)順序不一樣，得到的求角公式是不一樣的，求角的時候不能一味的套用公式，使用之前，應(yīng)先了解一下旋轉(zhuǎn)順序。

至此，我們已經(jīng)求出了姿態(tài)變換矩陣以及通過構(gòu)造四元數(shù)，用四元數(shù)的方法來求得對應(yīng)的姿態(tài)角，但我們現(xiàn)在存在一個問題是，我們不知道四元數(shù)具體數(shù)值，所以我們要求出真正的姿態(tài)角，必須找到求出四元數(shù)具體值的辦法。

五、四元數(shù)參數(shù)求解

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

1.四元數(shù)微分

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

2.解微分方程

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

設(shè)有一微分方程

六、陀螺儀誤差的消除

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

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

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

通過四元數(shù)計算的從E系(地理坐標(biāo)系)變換到b系(物體坐標(biāo)系)姿態(tài)矩陣為：

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

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

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

下面我們根據(jù)思維導(dǎo)圖用程序來一步一步實現(xiàn)如何求解歐拉角：

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

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

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

4.提取姿態(tài)矩陣中的重力分量，我們已經(jīng)其數(shù)學(xué)計算公式為

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

6.對誤差積分(定義accex、accey、accez為積分值、ki=0.001為積分系數(shù)、dt=0.005為積分周期時間),其程序?qū)崿F(xiàn)為(目前程序里未使用這一步)：

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

10.計算姿態(tài)角，數(shù)學(xué)公式為

至此，我們就按照思維導(dǎo)圖，一步一步實現(xiàn)了用程序語句求解姿態(tài)角了。