0 引言

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

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

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

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

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

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

    在忽略地球的自轉(zhuǎn)和公轉(zhuǎn)運動對四旋翼飛行器造成的影響，并假設(shè)飛行器為剛體運動且質(zhì)量分布對稱,、均勻,，中心為重心位置且不隨著運動而發(fā)生變化^[7]，則四元數(shù)矩陣表達式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ù)的坐標換算,、姿態(tài)誤差的雙環(huán)PI控制和互補濾波解算出歐拉角,。姿態(tài)解算系統(tǒng)框圖如圖2所示。

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

    在數(shù)據(jù)融合之前需要對傳感器測量得到的原始數(shù)據(jù)進行標定,，從而消除傳感器的零點偏置誤差,。陀螺儀消除偏置誤差的方法是保持器件水平位置不變,，多次采集后求取輸出平均值；加速度計消除偏置方法為八面校準法,，將飛行器姿態(tài)解算系統(tǒng)保持機頭方向不變，正面朝上,、下,、左、右,、前,、后和機頭朝上、機頭朝下,。在這八種狀態(tài)下分別測量三軸加速度計ADC輸出的X,、Y、Z三軸的最大最小值,，并與±1 g(重力加速度)所對應(yīng)的ADC數(shù)值進行校準,；磁羅盤零點數(shù)據(jù)誤差采用橢圓假設(shè)法進行標定，將姿態(tài)解算系統(tǒng)分別在坐標系下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ù)融合后傳感器偏差,。第二部分為修正后的三軸陀螺儀與三軸磁羅盤進行互補濾波，消除磁羅盤的高頻干擾,，提高磁羅盤的響應(yīng)能力和抗干擾能力,。姿態(tài)解算流程圖如圖3所示。

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

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

    因此，系統(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)偏差進行二次修正。

    外環(huán)九軸姿態(tài)傳感器數(shù)據(jù)融合,，記在飛行器機體坐標系下aⁿ=[ax  ay  az]^T和mⁿ=[mx  my  mz]^T分別為加速度計和磁羅盤實際測量得到的重力向量和地磁向量,。記vⁿ=[vx  vy  vz]^T和wⁿ=[wx  wy  wz]^T是將地理坐標系下重力向量k^b=[0  0  1g]^T和地磁向量n^b=[nx  0  nz]^T（不考慮地理磁偏角因素，將機頭固定向北）通過四元數(shù)坐標換算成機體坐標系下的重力向量和地磁向量,。向量之間的誤差為坐標軸的旋轉(zhuǎn)誤差,，可以用向量的叉積eⁿ=[ex  ey  ez]^T表示，如式（6）所示,。由于陀螺儀是對機體直接積分,，所以，陀螺儀的誤差可以體現(xiàn)為機體坐標的誤差,。因此修正坐標軸的誤差可以達到修正陀螺儀誤差的目的,，從而將加速度計和磁羅盤進行修正陀螺儀，實現(xiàn)了九軸的數(shù)據(jù)融合,。即如果陀螺儀按照叉積誤差的軸,，轉(zhuǎn)動叉積誤差的角度，就可以消除機體坐標上實際測量的重力向量和地磁向量和坐標換算后的重力向量和地磁向量之間的誤差,。

    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)誤差后輸出值為gⁿ=[gx  gy  gz]^T：

3.2 互補濾波

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

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

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

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

    圖5中采集的是模擬定點懸停時俯仰角和橫滾角,。由于單純地通過陀螺儀采集得到的角速度進行積分得到角度會產(chǎn)生積分誤差，并且積分誤差會隨著時間進行累加,，所以未進行姿態(tài)解算所得到的姿態(tài)角零點誤差會不斷變大,。圖中的2條線分別代表姿態(tài)解算后的橫滾角和俯仰角。由于采用了雙環(huán)PI修正陀螺儀,，所以姿態(tài)角零點誤差與解算前相比不會隨著時間累計不斷變大,。但是由于姿態(tài)解算系統(tǒng)處于四旋翼飛行器模擬定點懸停狀態(tài)，所以,，電機引起的機架振動,、載體重力分布不均、機械連接處阻尼問題等因素,，導(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ù)融合后的陀螺儀只進行輔助修正,。可以觀測到細線代表的是沒有使用濾波算法的磁羅盤測量的偏航角,，零點誤差相對較大,，達到了±3°左右,，而且上下波動也很大，表示磁羅盤受到外界環(huán)境嚴重干擾,。粗線代表磁羅盤和陀螺儀經(jīng)過互補濾波后采集得到的偏航角,，消除了部分干擾信號，上下波動明顯減弱,，并且誤差穩(wěn)定保持在±1.7°以內(nèi),。由于磁羅盤自身具有不可忽略的零位、靈敏度及非正交誤差,，因此通過軟件算法無法徹底消除零點誤差,。

5 結(jié)論

    旋翼飛行器飛行過程中獲得姿態(tài)最優(yōu)估計是飛行器系統(tǒng)穩(wěn)定運行的根本保障。本文基于四元數(shù)坐標換算,，將地理坐標系下的重力向量和地磁向量換算到機體坐標系下,。然后通過修正坐標系旋轉(zhuǎn)誤差，實現(xiàn)了多傳感器的數(shù)據(jù)融合,，避免了陀螺儀零點積分誤差的累積,，并將修正后的陀螺儀與磁羅盤進行互補濾波，消除了磁羅盤的高頻干擾,。實驗表明：經(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)定地輸出準確可靠的姿態(tài)數(shù)據(jù),，滿足了四旋翼飛行器飛行控制對姿態(tài)信息的要求。

參考文獻

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

[2] 杜杉杉,，吳昊,，張繼文，等.一種面向AHRS的改進互補濾波融合算法[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].控制工程,，2013（5）：910-914.

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