何 川 ，李 智 ，，王勇軍

（1.桂林電子科技大學(xué) 電子工程與自動化學(xué)院，廣西 桂林 541004；2.桂林航天工業(yè)學(xué)院無人遙測重點實驗室，廣西 桂林 541004）

0 引言

姿態(tài)最優(yōu)估算是飛行器自主飛行的先決條件，在飛行過程中，實時準(zhǔn)確地獲得飛行器的姿態(tài)信息，可以決定飛行器的控制精度和穩(wěn)定性[1]。隨著微電子技術(shù)的發(fā)展，飛行器姿態(tài)解算系統(tǒng)普遍采用低成本的航姿參考系統(tǒng)（AHRS），其主要包括：基于微機(jī)電系統(tǒng)（MEMS）的三軸陀螺儀、三軸加速度計和三軸磁羅盤。由于系統(tǒng)機(jī)械結(jié)構(gòu)和傳感器自身三軸并非完全正交，引入了不正交誤差，以及數(shù)據(jù)積分產(chǎn)生的發(fā)散誤差和外部干擾產(chǎn)生的隨機(jī)誤差。因此要估計飛行器姿態(tài)的最優(yōu)信息必須對多傳感器進(jìn)行誤差處理，并尋求最佳數(shù)據(jù)融合算法。

目前國內(nèi)對MEMS傳感器姿態(tài)解算系統(tǒng)研究的常用算法有：擴(kuò)展卡爾曼濾波、梯度下降法、互補濾波等。擴(kuò)展卡爾曼濾波應(yīng)用非常廣泛，但為其建立可靠穩(wěn)定的狀態(tài)方程、確定合適的量測噪聲和過程噪聲協(xié)方差矩陣都比較困難。梯度下降法的好處在于姿態(tài)解算過程只有普通的乘法和加法運算，普通的微控制器即能滿足算法要求，但難點在于構(gòu)造合適的目標(biāo)函數(shù)[2]。本系統(tǒng)在Mahony[3]提出的補償濾波器的基礎(chǔ)上進(jìn)行優(yōu)化，采用雙環(huán)補償濾波算法，外環(huán)引用重力場和地磁場進(jìn)行補償，內(nèi)環(huán)引用重力場進(jìn)行補償，并將修正后的陀螺儀和磁羅盤進(jìn)行互補濾波，以期得到最優(yōu)姿態(tài)輸出。

1 飛行器姿態(tài)描述

飛行器常用的姿態(tài)描述方法有歐拉角、方向余弦矩陣和四元數(shù)三種。三維空間中，飛行器有3個轉(zhuǎn)動自由度，分別可以通過3個角度參數(shù)來描述飛行器的姿態(tài)，分別定義為：繞機(jī)體坐標(biāo)系的Z軸轉(zhuǎn)動為偏航角Ψ；繞X軸轉(zhuǎn)動為橫滾角φ；繞Y軸轉(zhuǎn)動為俯仰角θ[4]。

如圖1所示，飛行器在飛行過程中，地理坐標(biāo)系b通過繞不同坐標(biāo)軸三次連續(xù)轉(zhuǎn)動來實現(xiàn)對機(jī)體坐標(biāo)n的轉(zhuǎn)換。第一次轉(zhuǎn)動，OX0Y0Z0坐標(biāo)系即地理坐標(biāo)系ENU繞Z0軸轉(zhuǎn)動Ψ角，得到坐標(biāo)系OX1Y1Z0；第二次轉(zhuǎn)動，OX1Y1Z0坐標(biāo)系繞OX1軸轉(zhuǎn)動φ角，得到坐標(biāo)系OX1Y2Z2；第三次轉(zhuǎn)動，OX1Y2Z2坐標(biāo)系繞OY2軸轉(zhuǎn)動 θ角，得到坐標(biāo)系OXYZ，OY2軸即OY軸[5]。這三次坐標(biāo)軸旋轉(zhuǎn)可以用數(shù)學(xué)方法表述為3個獨立的方向余弦矩陣。兩個坐標(biāo)系的轉(zhuǎn)換可以用這3個獨立變換矩陣的乘積表示：

圖1 飛行器坐標(biāo)系和姿態(tài)角

由于載體的運動，四元數(shù)Q=[q0q1q2q3]T是變量，q0、q1、q2、q3是時間函數(shù)。 剛體繞瞬時轉(zhuǎn)軸 n轉(zhuǎn)過 σ角度來實現(xiàn)坐標(biāo)的轉(zhuǎn)換。為參考坐標(biāo)系下的一個單位矢量，其角速度為：

機(jī)體坐標(biāo)系n和地理坐標(biāo)系b之間的變換也可以用四元數(shù)的三角形式表示[6]：

在忽略地球的自轉(zhuǎn)和公轉(zhuǎn)運動對四旋翼飛行器造成的影響，并假設(shè)飛行器為剛體運動且質(zhì)量分布對稱、均勻，中心為重心位置且不隨著運動而發(fā)生變化[7]，則四元數(shù)矩陣表達(dá)式Q的微分方程解為：

2 飛行器姿態(tài)解算系統(tǒng)構(gòu)建

2.1 系統(tǒng)硬件

本姿態(tài)解算系統(tǒng)硬件主要包括主控制器和航姿參考系統(tǒng)(AHRS)。其中主控器采用ST(意法半導(dǎo)體)公司的Cortex-M4為內(nèi)核的32位ARM微控制器STM32F405RGT6；航姿參考系統(tǒng)采用2款高精度的姿態(tài)傳感器，分別為：InvenSense公司的 MPU-6500和 ST公司的 LSM303D，構(gòu)成了一個九軸的姿態(tài)測量系統(tǒng)。傳感器均選用數(shù)字芯片，內(nèi)部均集成有高精度的ADC。主控制器通過SPI總線，讀取出傳感器采集到的原始數(shù)據(jù)，然后通過四元數(shù)的坐標(biāo)換算、姿態(tài)誤差的雙環(huán)PI控制和互補濾波解算出歐拉角。姿態(tài)解算系統(tǒng)框圖如圖2所示。

圖2 姿態(tài)解算系統(tǒng)框圖

2.2 傳感器數(shù)據(jù)標(biāo)定

在數(shù)據(jù)融合之前需要對傳感器測量得到的原始數(shù)據(jù)進(jìn)行標(biāo)定，從而消除傳感器的零點偏置誤差。陀螺儀消除偏置誤差的方法是保持器件水平位置不變，多次采集后求取輸出平均值；加速度計消除偏置方法為八面校準(zhǔn)法，將飛行器姿態(tài)解算系統(tǒng)保持機(jī)頭方向不變，正面朝上、下、左、右、前、后和機(jī)頭朝上、機(jī)頭朝下。在這八種狀態(tài)下分別測量三軸加速度計 ADC輸出的X、Y、Z三軸的最大最小值，并與±1 g(重力加速度)所對應(yīng)的ADC數(shù)值進(jìn)行校準(zhǔn)；磁羅盤零點數(shù)據(jù)誤差采用橢圓假設(shè)法進(jìn)行標(biāo)定，將姿態(tài)解算系統(tǒng)分別在坐標(biāo)系下XOY和XOZ兩個平面內(nèi)360°旋轉(zhuǎn)，并將采集到的數(shù)據(jù)用MATLAB軟件擬合成橢圓，然后修正橢圓的偏心和半徑。

3 系統(tǒng)數(shù)據(jù)融合與姿態(tài)解算

系統(tǒng)姿態(tài)解算分為兩個部分。第一部分是雙環(huán)PI控制器修正數(shù)據(jù)融合后傳感器偏差。第二部分為修正后的三軸陀螺儀與三軸磁羅盤進(jìn)行互補濾波，消除磁羅盤的高頻干擾，提高磁羅盤的響應(yīng)能力和抗干擾能力。姿態(tài)解算流程圖如圖3所示。

3.1 雙環(huán)PI控制器

陀螺儀可以快速響應(yīng)機(jī)體的旋轉(zhuǎn)，短時間誤差較小、可信度高，但是存在溫漂和零漂，以及積分誤差隨時間累積等因素影響。加速度計在靜止的狀態(tài)下漂移很小，傾角求解過程不存在積分誤差，但是受到飛行過程中電機(jī)和機(jī)架的振動以及轉(zhuǎn)動和運動加速度的干擾。磁羅盤測量得到的地磁向量在一定地理范圍內(nèi)可以認(rèn)為不會發(fā)生改變，但是，磁羅盤容易受到硬磁場和軟磁場的干擾。

圖3 姿態(tài)解算流程圖

因此，系統(tǒng)外環(huán)采用九軸姿態(tài)傳感器（三軸加速度計、三軸磁羅盤和三軸陀螺儀）數(shù)據(jù)融合。由于磁羅盤容易受到干擾，有可能導(dǎo)致外環(huán)九軸數(shù)據(jù)融合后依舊存在較大誤差，所以，內(nèi)環(huán)采用六軸姿態(tài)傳感器（三軸加速度計和三軸陀螺儀）數(shù)據(jù)融合，對數(shù)據(jù)融合后的傳感器姿態(tài)偏差進(jìn)行二次修正。

外環(huán)九軸姿態(tài)傳感器數(shù)據(jù)融合，記在飛行器機(jī)體坐標(biāo)系下 an=[axayaz]T和 mn=[mxmymz]T分別為加速度計和磁羅盤實際測量得到的重力向量和地磁向量。記vn=[vxvyvz]T和wn=[wxwywz]T是將地理坐標(biāo)系下重力向量kb=[0 0 1g]T和地磁向量nb=[nx0nz]T（不考慮地理磁偏角因素，將機(jī)頭固定向北）通過四元數(shù)坐標(biāo)換算成機(jī)體坐標(biāo)系下的重力向量和地磁向量。向量之間的誤差為坐標(biāo)軸的旋轉(zhuǎn)誤差，可以用向量的叉積en=[exeyez]T表示，如式（6）所示。由于陀螺儀是對機(jī)體直接積分，所以，陀螺儀的誤差可以體現(xiàn)為機(jī)體坐標(biāo)的誤差。因此修正坐標(biāo)軸的誤差可以達(dá)到修正陀螺儀誤差的目的，從而將加速度計和磁羅盤進(jìn)行修正陀螺儀，實現(xiàn)了九軸的數(shù)據(jù)融合。即如果陀螺儀按照叉積誤差的軸，轉(zhuǎn)動叉積誤差的角度，就可以消除機(jī)體坐標(biāo)上實際測量的重力向量和地磁向量和坐標(biāo)換算后的重力向量和地磁向量之間的誤差。

PI調(diào)節(jié)器的比例部分用于迅速糾正陀螺儀誤差，積分部分用于消除穩(wěn)態(tài)偏差[7]。通過大量實驗驗證后取PI調(diào)節(jié)器的比例系數(shù)為1.0，積分系數(shù)為0.2。陀螺儀經(jīng)過外環(huán)PI控制器修正姿態(tài)誤差后輸出值為gn=[gxgygz]T：

3.2 互補濾波

本系統(tǒng)姿態(tài)解算的第二部分是磁羅盤的修正，主要采用互補濾波算法?；パa濾波算法簡單可靠且對器件精度要求低。互補濾波的目的在于將陀螺儀和磁羅盤各自在頻域角度上進(jìn)行互補。因為磁羅盤動態(tài)響應(yīng)能力差且易受外界環(huán)境的干擾，而陀螺儀動態(tài)響應(yīng)快，故可在頻域上將二者形成互補，即對磁羅盤低通濾波，對陀螺儀進(jìn)行高通濾波。如圖4所示，將修正后得到的陀螺儀數(shù)據(jù)與磁羅盤數(shù)據(jù)進(jìn)行互補濾波，即可消除磁羅盤的高頻干擾。

圖4 互補濾波

如式(10)所示，r為互補濾波中的權(quán)值，r值越大，證明陀螺儀占的權(quán)重越大。

4 姿態(tài)解算結(jié)果分析

為了驗證姿態(tài)解算系統(tǒng)在多旋翼飛行器實際飛行過程中的性能，搭建了一個半實物仿真平臺。將基于STM32的姿態(tài)解算系統(tǒng)的四旋翼飛行器機(jī)頭朝向地理正北方，機(jī)架底部水平固定在萬向節(jié)一端，萬向節(jié)另外一端固定在水平的桌面上。啟動四旋翼飛行器電機(jī)，將飛行器油門通道值保持在空中懸停狀態(tài)，及四個螺旋槳產(chǎn)生的向上的合力等于飛行器自身重力。然后，通過串口讀取出傳感器采集到的原始數(shù)據(jù)以及姿態(tài)解算后的數(shù)據(jù)，并用MATLAB進(jìn)行數(shù)據(jù)分析。

圖5 俯仰角和橫滾角

圖5中采集的是模擬定點懸停時俯仰角和橫滾角。由于單純地通過陀螺儀采集得到的角速度進(jìn)行積分得到角度會產(chǎn)生積分誤差，并且積分誤差會隨著時間進(jìn)行累加，所以未進(jìn)行姿態(tài)解算所得到的姿態(tài)角零點誤差會不斷變大。圖中的2條線分別代表姿態(tài)解算后的橫滾角和俯仰角。由于采用了雙環(huán)PI修正陀螺儀，所以姿態(tài)角零點誤差與解算前相比不會隨著時間累計不斷變大。但是由于姿態(tài)解算系統(tǒng)處于四旋翼飛行器模擬定點懸停狀態(tài)，所以，電機(jī)引起的機(jī)架振動、載體重力分布不均、機(jī)械連接處阻尼問題等因素，導(dǎo)致零點誤差相對于水平放置在桌面上時的靜態(tài)零點誤差，偏差較大且波動相對厲害。零點誤差基本穩(wěn)定在±2.2°以內(nèi)。隨著時間的推移，零點誤差逐步趨近平穩(wěn)，可以滿足四旋翼飛行器對姿態(tài)數(shù)據(jù)的要求。

圖6中測試結(jié)果分別代表磁羅盤姿態(tài)解算前采集的數(shù)據(jù)和姿態(tài)解算后測量的數(shù)據(jù)。本系統(tǒng)采用的互補濾波算法中權(quán)值r取0.15，偏航角以磁羅盤采集的數(shù)據(jù)為主，數(shù)據(jù)融合后的陀螺儀只進(jìn)行輔助修正?？梢杂^測到細(xì)線代表的是沒有使用濾波算法的磁羅盤測量的偏航角，零點誤差相對較大，達(dá)到了±3°左右，而且上下波動也很大，表示磁羅盤受到外界環(huán)境嚴(yán)重干擾。粗線代表磁羅盤和陀螺儀經(jīng)過互補濾波后采集得到的偏航角，消除了部分干擾信號，上下波動明顯減弱，并且誤差穩(wěn)定保持在±1.7°以內(nèi)。由于磁羅盤自身具有不可忽略的零位、靈敏度及非正交誤差，因此通過軟件算法無法徹底消除零點誤差。

圖6 偏航角

5 結(jié)論

旋翼飛行器飛行過程中獲得姿態(tài)最優(yōu)估計是飛行器系統(tǒng)穩(wěn)定運行的根本保障。本文基于四元數(shù)坐標(biāo)換算，將地理坐標(biāo)系下的重力向量和地磁向量換算到機(jī)體坐標(biāo)系下。然后通過修正坐標(biāo)系旋轉(zhuǎn)誤差，實現(xiàn)了多傳感器的數(shù)據(jù)融合，避免了陀螺儀零點積分誤差的累積，并將修正后的陀螺儀與磁羅盤進(jìn)行互補濾波，消除了磁羅盤的高頻干擾。實驗表明：經(jīng)過雙環(huán)PI控制修正姿態(tài)偏差的陀螺儀，俯仰角和橫滾角零點動態(tài)誤差保持在±2.2°左右；互補濾波后的磁羅盤消除了部分高頻干擾，偏航角誤差保持在±1.7°左右。本文設(shè)計的姿態(tài)解算系統(tǒng)較好地解決了噪聲干擾與姿態(tài)最優(yōu)估計問題，并在半實物仿真平臺上得到了驗證，實現(xiàn)了長時間穩(wěn)定地輸出準(zhǔn)確可靠的姿態(tài)數(shù)據(jù)，滿足了四旋翼飛行器飛行控制對姿態(tài)信息的要求。

[1]張浩，任芊.四旋翼飛行器航姿測量系統(tǒng)的數(shù)據(jù)融合方法[J].兵工自動化，2013，32（1）：28-31.

[2]杜杉杉，吳昊，張繼文，等.一種面向 AHRS的改進(jìn)互補濾波融合算法[J].國外電子測量技術(shù)，2015(3)：13-18.

[3]MAHONY R，HAMEL T.Complementary filters on the special orthogonal group[C].IEEE Transactions on Automatic Control，2008，53（5）：1203-1218.

[4]張?zhí)旃?，王秀萍，王麗?捷聯(lián)慣性導(dǎo)航技術(shù)[M].北京：國防工業(yè)出版社，2007：15-16.

[5]鄧正隆.慣性技術(shù)[M].哈爾濱：哈爾濱工業(yè)大學(xué)出版社，2006：10-11.

[6]李一波，宋述錫.基于模糊自整定PID四旋翼無人機(jī)懸?？刂芠J].控制工程，2013（5）：910-914.

[7]廖懿華，張鐵民，廖貽泳.基于模糊-比例積分偏差修正的多旋翼飛行器姿態(tài)測算系統(tǒng)[J].農(nóng)業(yè)工程學(xué)報，2014(20)：19-27.