基于四元數(shù)的四旋翼飛行器姿態(tài)解算算法

蔣 鈺，諶海云，岑汝平

（1.西南石油大學(xué) 電氣信息學(xué)院，成都 610500；2.西南石油大學(xué) 電氣信息學(xué)院，成都 610500；3.西南石油大學(xué) 電氣信息學(xué)院，成都 610500）

基于四元數(shù)的四旋翼飛行器姿態(tài)解算算法

蔣 鈺1，諶海云2，岑汝平3

（1.西南石油大學(xué) 電氣信息學(xué)院，成都 610500；2.西南石油大學(xué) 電氣信息學(xué)院，成都 610500；3.西南石油大學(xué) 電氣信息學(xué)院，成都 610500）

0 引言

四旋翼飛行器是一種能垂直升降的微型飛行器，它能夠完成近地偵察、監(jiān)視等任務(wù)，具有廣泛的軍事、民事等應(yīng)用前景，其所擁有的巨大吸引力鼓舞著科研人員不懈地進(jìn)行各種研究和開發(fā)。但是四旋翼飛行器的控制難度較大，難點(diǎn)主要是因?yàn)轱w行器具有欠驅(qū)動(dòng)、多變量這些比較復(fù)雜的特性[1]。因此四旋翼飛行器的姿態(tài)控制也成為了控制領(lǐng)域的熱點(diǎn)和難點(diǎn)。四旋翼飛行器的姿態(tài)控制是飛行器總體設(shè)計(jì)的重要組成部分，我們常說的飛行器飛行的“好壞”指的就是飛行器飛行時(shí)的姿態(tài)效果。實(shí)現(xiàn)飛行器飛行姿態(tài)的控制有多種方法，如歐拉角、方向余弦、四元數(shù)法等。

1 歐拉角算法

飛行器在飛行過程中相對于水平面發(fā)生了一定角度的傾斜，此時(shí)我們不能直觀的說飛行器傾斜了多少角度，因?yàn)槲矬w的運(yùn)動(dòng)是合運(yùn)動(dòng)，我們可以把它的運(yùn)動(dòng)矢量正交分解為幾個(gè)運(yùn)動(dòng)的合成。同理，我們把飛行器的旋轉(zhuǎn)分解為三個(gè)軸上的旋轉(zhuǎn)，這個(gè)旋轉(zhuǎn)的角度就是歐拉角[2]，如圖1所示，圖中α、β、γ就是歐拉角。

1.1 坐標(biāo)系

飛行器飛行的參數(shù)必須在一個(gè)確定的坐標(biāo)系下才能進(jìn)行描述。對于飛行器來說常用的坐標(biāo)系有地理坐標(biāo)系和機(jī)體坐標(biāo)系。

圖1 α、β、γ三個(gè)歐拉角

1）地面坐標(biāo)系（OEXEYEZE）

地理坐標(biāo)系一般選用地軸系。原點(diǎn)O設(shè)在地面上的某一點(diǎn)，OEXE軸指向正北方向，OEYE軸指向正東方向，OEZE軸與XEOEYE平面垂直，方向?yàn)榇怪毕蛏稀?/p>

2）機(jī)體坐標(biāo)系（OXYZ）

機(jī)體坐標(biāo)系是固定在飛行器身上并與之一起移動(dòng)的動(dòng)態(tài)坐標(biāo)系。原點(diǎn)O為飛行器重心，OX軸與機(jī)身軸線平行并規(guī)定指向機(jī)頭方向?yàn)檎较?，相對OX軸順時(shí)針旋轉(zhuǎn)90°為OY軸，OZ軸垂直XOY平面向上。

當(dāng)我們的飛機(jī)頭朝北水平放置時(shí)載體坐標(biāo)系和參考坐標(biāo)系是重合的，那么接下來我們繞飛機(jī)的Z軸旋轉(zhuǎn)30°，這個(gè)旋轉(zhuǎn)的歐拉角就是我們所說的Yaw，同樣，繞飛機(jī)的Y軸旋轉(zhuǎn)30°，我們得到Pitch，繞飛機(jī)X軸旋轉(zhuǎn)得到Roll。

圖2 兩個(gè)坐標(biāo)系

1.2 歐拉角微分方程

式中xω、yω、zω三個(gè)角速度分量可由直接安裝在飛行器上的三個(gè)角速度陀螺儀測量出來，可以認(rèn)為是已知量。因此，求解這個(gè)微分方程式就可以直接得到飛行器的三個(gè)歐拉角。

2 四元數(shù)

由于方向余弦方程的計(jì)算量較大，工作效率很低，因此我們采用四元數(shù)法實(shí)時(shí)解算姿態(tài)角[3]。四元數(shù)（Quaternions）是威廉·盧云·哈密爾頓（William Rowan Hamilton,1805-1865）1843年在愛爾蘭發(fā)現(xiàn)的數(shù)學(xué)概念。

2.1 四元數(shù)表達(dá)式

四元數(shù)由一個(gè)實(shí)數(shù)和三個(gè)虛數(shù)構(gòu)成，所以是一個(gè)四維空間的向量，但是它的三個(gè)虛數(shù)又有三維空間的性質(zhì)[4]。因此，三維空間中的一個(gè)矢量，可以看作一個(gè)實(shí)部為0的四元數(shù)，這個(gè)四元數(shù)是這個(gè)三維空間的一個(gè)矢量在四維空間里的“映像”，或者叫做這個(gè)矢量的“四元數(shù)映像”。這樣，我們就把三維空間和一個(gè)四維空間聯(lián)系起來，用四維空間中四元數(shù)的性質(zhì)和運(yùn)算規(guī)律來研究三維空間中剛體定點(diǎn)轉(zhuǎn)動(dòng)的問題。四元數(shù)有如下四種表示方式：

1）矢量式：

q0為四元數(shù)Q的標(biāo)量部分，q為Q的矢量部分，q是三維空間的一個(gè)向量。

2）復(fù)數(shù)式：Q=q0+q1i+q2j+q3k，它的共軛復(fù)數(shù)：Q*=q0?q1i?q2j?q3k。

2.2 四元數(shù)的矢量變換

四元數(shù)算法的基本原理是：一個(gè)坐標(biāo)系變換到另一個(gè)坐標(biāo)系，可以通過繞一個(gè)定義在參考坐標(biāo)系中的矢量u的單次轉(zhuǎn)動(dòng)來實(shí)現(xiàn)[5]。在載體系定義一個(gè)矢量rb=xi+yi+zk，設(shè)其在參考系中表示為rn。則有rn=qrbq*，其中q=a+bi+cj+dk，q*為共軛復(fù)數(shù)。

將其寫成矩陣形式：

這個(gè)矩陣就是四元數(shù)旋轉(zhuǎn)矩陣。

用歐拉角直接表示四元數(shù)：

2.3 四元數(shù)姿態(tài)解算

我們開始可以隨便假設(shè)一個(gè)四元數(shù)，且認(rèn)為它表征了兩個(gè)坐標(biāo)系之間的旋轉(zhuǎn)關(guān)系了，只不過這個(gè)旋轉(zhuǎn)矩陣跟真實(shí)的矩陣相比誤差很大，我們假設(shè)這個(gè)矩陣是正確的，然后得到兩個(gè)坐標(biāo)下同一個(gè)向量的差別，用這個(gè)差別來糾正這個(gè)矩陣。也就是說我們只知道重力在地理坐標(biāo)系下向量是（0,0,g），如果我們的矩陣是準(zhǔn)確的，那么用這個(gè)矩陣將（0,0,g）轉(zhuǎn)換到機(jī)體坐標(biāo)，那我們應(yīng)該得到在機(jī)體坐標(biāo)系下的重力的向量，如果矩陣很準(zhǔn)確，那我們從加速度計(jì)讀取的數(shù)值應(yīng)該也是這個(gè)向量，所以我們讀取加速度計(jì)得到重力向量在機(jī)體坐標(biāo)系下的向量，這是測量值，這個(gè)值和理論值有差別，用這個(gè)差別來糾正旋轉(zhuǎn)矩陣[6]。

1）將機(jī)體坐標(biāo)系的電子羅盤測到的矢量轉(zhuǎn)成地理坐標(biāo)系下的磁場矢量hxyz（測量值）然后令其在Y方向?yàn)?、X方向大小等于磁場矢量在hxy平面上的投影。得到標(biāo)定后的矢量bxyz。

2）將地理坐標(biāo)系的標(biāo)準(zhǔn)重力向量（0,0,g）及標(biāo)定以后的磁場矢量分別左乘上四元素旋轉(zhuǎn)矩陣，轉(zhuǎn)到機(jī)體坐標(biāo)系。

3）將加速度計(jì)測出來的重力向量和參考矢量做叉積，磁場的測量矢量和參考矢量也做叉積，都用來修正陀螺儀。

4）由于叉積向量大小與陀螺積分誤差成正比，因此在誤差上乘上一個(gè)系數(shù)去修正陀螺儀數(shù)據(jù)。

5）計(jì)算四元數(shù)的微分方程、并進(jìn)行歸一化處理：四元數(shù)微分方程本來只是基于角速度的，也就是說，已知上個(gè)周期的姿態(tài)，和本次測量得到的角速度，得到本周期的姿態(tài)，在角速度里加入誤差反饋，來調(diào)節(jié)姿態(tài)，起到減小誤差的作用。

6）最后將四元數(shù)轉(zhuǎn)化成歐拉角進(jìn)行表示。

2.4 實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)采集到650組數(shù)據(jù)，并利用MATLAB對數(shù)據(jù)進(jìn)行了處理，結(jié)果如圖3和圖4所示。

圖3 分別為加速度X(g)、Y(g)、Z(g)曲線

圖4 陀螺儀X、Y、Z軸(°/s)曲線

圖5 四元數(shù)曲線

圖6 歐拉角曲線

3 結(jié)論

本文在基于歐拉角、四元數(shù)微分方程的基礎(chǔ)上，用四元數(shù)把三軸陀螺儀以及加速度計(jì)的數(shù)據(jù)進(jìn)行融合，并且經(jīng)過實(shí)驗(yàn)證明了四元數(shù)算法的有效性，從而說明四元數(shù)算法在飛行器姿態(tài)控制方面有著很好的應(yīng)用前景。

[1]White K C, Tunnell P J. A Guidance Scheme for Lunar Desent Based on Linear Perturbation Theory[R].NASA TN D-3147.1965,1-20.

[2]程國采.彈道導(dǎo)彈制導(dǎo)方法與最優(yōu)控制[M].長沙:國防科技大學(xué)出版社,1987

[3]陳印.GridView中數(shù)據(jù)行批量刪除的實(shí)現(xiàn)[J].四川職業(yè)技術(shù)學(xué)院學(xué)報(bào),2014,24(1).

[4]Be lbruno E A. Luna rCapture Orb its, A M e thod o fC onstructing Ea rth M oon Tra jectories and the Lunar Gas M ission[C].A IAA-87-1054. In:19th A IAA /DGLR /JSASS Interna tiona l E lec tric Propulsion Confe rence. Co lo rado Sp ring s, Co lo rado, M ay,1987.

[5]Serban R,KoonW S, Lo M W etc.,H alo O rbitM ission Correction M aneuvers Using Op tim a l Contro l[J].Automa tica,2002,38:571-583.

[6]楊嘉墀,范劍峰,尚秉民,等.航天器軌道動(dòng)力學(xué)與控制(上)[M].北京:宇航出版社,1995,547-549.

Attitude solution algorithm for four rotor aircraft based on four element number

JIANG Yu1, CHEN Hai-yun2, CEN Ru-ping3

由于四旋翼飛行器參與了許多極具挑戰(zhàn)性的任務(wù)，因此要求飛行器在飛行時(shí)具有很好的飛行姿態(tài)，姿態(tài)控制問題就成了當(dāng)前研究熱點(diǎn)。詳細(xì)分析了歐拉角算法和時(shí)效性更好的四元數(shù)算法，最后通過實(shí)驗(yàn)驗(yàn)證了該算法的有效性。

四旋翼飛行器；歐拉角；四元數(shù)；姿態(tài)解算

蔣鈺（1990 -），女，廣西人，碩士研究生，主要從事自動(dòng)化方面的研究工作。

TP13

A

1009-0134(2015)12(上)-0077-04

10.3969/j.issn.1009-0134.2015.23.22

2015-07-23

國家級大學(xué)生創(chuàng)新創(chuàng)業(yè)訓(xùn)練計(jì)劃項(xiàng)目（201410615002）