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

一、概述

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

1.歐拉角法：

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

2.方向余弦法：

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

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

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

4.Rodrigues參數(shù)法：

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

5.四元數(shù)法：

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

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

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

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

二、基本概念

地理坐標(biāo)系：

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

機(jī)體坐標(biāo)系：

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

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

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

向量點(diǎn)積：

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

物理意義：

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

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

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

四元數(shù)：

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

2.四元數(shù)

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

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

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

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

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

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

A.矢量式

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

C三角式

D.矩陣式

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

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

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

A.加法與減法

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

B.乘法

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

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

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

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

C.除法(稱為求逆)

逆的定義：

也即有

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

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

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

那么物體從一個(gè)姿態(tài)變化到另一個(gè)姿態(tài)，可等效為物體繞了某一個(gè)軸通過無(wú)中間過程的一次旋轉(zhuǎn)完成，但實(shí)際物體可能經(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)我們研究?jī)蓚€(gè)坐標(biāo)之間的角度變化關(guān)系(地理坐標(biāo)系與物體坐標(biāo)系)時(shí)，取物體坐標(biāo)系與物體固聯(lián)，物體坐標(biāo)系與物體之間角位置始終不變，因此，我們可以以物體(視為剛體)定點(diǎn)轉(zhuǎn)動(dòng)問題來(lái)推導(dǎo)我們想要的變換關(guān)系，下面，我們就來(lái)推導(dǎo)一下我們想要的變化關(guān)系。

由圖可知：

所以有

則有

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

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

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

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

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

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

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

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

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

對(duì)(34)化簡(jiǎn)得

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

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

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

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

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

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

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

1.四元數(shù)微分

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

2.解微分方程

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

設(shè)有一微分方程

六、陀螺儀誤差的消除

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

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

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

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

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

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

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

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

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

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

3.對(duì)加速度值進(jìn)行歸一化

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

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

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

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

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

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