蔣 鈺，諶海云，岑汝平

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

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

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

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

0 引言

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

1 歐拉角算法

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

1.1 坐標系

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

圖1 α、β、γ三個歐拉角

1）地面坐標系（OEXEYEZE）

地理坐標系一般選用地軸系。原點O設在地面上的某一點，OEXE軸指向正北方向，OEYE軸指向正東方向，OEZE軸與XEOEYE平面垂直，方向為垂直向上。

2）機體坐標系（OXYZ）

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

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

圖2 兩個坐標系

1.2 歐拉角微分方程

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

2 四元數(shù)

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

2.1 四元數(shù)表達式

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

1）矢量式：

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

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

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

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

將其寫成矩陣形式：

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

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

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

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

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

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

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

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

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

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

2.4 實驗結(jié)果

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

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

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

圖5 四元數(shù)曲線

圖6 歐拉角曲線

3 結(jié)論

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

[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]程國采.彈道導彈制導方法與最優(yōu)控制[M].長沙:國防科技大學出版社,1987

[3]陳印.GridView中數(shù)據(jù)行批量刪除的實現(xiàn)[J].四川職業(yè)技術(shù)學院學報,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]楊嘉墀,范劍峰,尚秉民,等.航天器軌道動力學與控制(上)[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)性的任務，因此要求飛行器在飛行時具有很好的飛行姿態(tài)，姿態(tài)控制問題就成了當前研究熱點。詳細分析了歐拉角算法和時效性更好的四元數(shù)算法，最后通過實驗驗證了該算法的有效性。

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

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

TP13

A

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

10.3969/j.issn.1009-0134.2015.23.22

2015-07-23

國家級大學生創(chuàng)新創(chuàng)業(yè)訓練計劃項目（201410615002）