一種基于MEMS傳感器的無人飛艇航姿測量系統(tǒng)*

胡少興， 劉東昌， 張愛武， 朱煜坤

(1.北京航空航天大學 機械工程及自動化學院，北京 100191；2.首都師范大學 三維信息獲取與應用教育部重點實驗室，北京 100037)

0 引 言

姿態(tài)信息是自動飛行控制系統(tǒng)最重要最基本的反饋信號。在無人飛艇上, 由于任務載荷有限, 高精度的大型測量設備將不能使用。微機電系統(tǒng)(micro-electro-mechanical system,MEMS)慣性傳感器由于其質(zhì)量、體積和功耗等方面的優(yōu)勢得到了廣泛的應用。

針對多MEMS傳感器姿態(tài)融合，國內(nèi)外提出了許多先進的方法，如擴展卡爾曼濾波(EKF)方法，是一種遞推濾波方法[1,2]，EKF方法廣泛使用證明了它的正確性和有效性，但其是基于對非線性方程的線性化，當系統(tǒng)具有強非線性時線性化可能引起大的誤差甚至造成濾波器的不穩(wěn)定，并且EKF設計困難；比較常用的還有互補濾波法[3,4]，結(jié)合傳感器頻域的特點分辨和消除噪聲，不需考慮信號的統(tǒng)計特性，但是文獻中的方法是基于幾何關(guān)系直接計算傾角，沒有優(yōu)化處理，容易造成姿態(tài)誤差[5]。

針對現(xiàn)有算法存在的不足，本文提出了一種高效的姿態(tài)融合算法，充分利用重力場和地磁場特性，每次融合迭代過程只需根據(jù)輔助傳感器計算出梯度，去修正陀螺儀數(shù)據(jù)給出最優(yōu)估計。針對振動干擾問題提出新的思路，從傳感器數(shù)據(jù)預處理和多傳感器數(shù)據(jù)融合兩方面雙重抑制振動，取得了明顯的效果。該算法采用了四元數(shù)表示方位，避免了歐拉角描述姿態(tài)時出現(xiàn)的奇異性問題。

1 無人飛艇航姿測量系統(tǒng)結(jié)構(gòu)與工作原理

所描述的無人飛艇航姿系統(tǒng)主要由三軸陀螺儀、三軸加速度計、三軸磁力計以及ARM處理器組成，工作原理如圖1所示，傳感器數(shù)據(jù)經(jīng)過預處理后經(jīng)過專家系統(tǒng)的判斷進行數(shù)據(jù)融合算法，最終輸出艇體的姿態(tài)角—橫滾角φ、俯仰角θ和航向角ψ。

圖1 系統(tǒng)總體結(jié)構(gòu)圖

2 傳感器的數(shù)據(jù)預處理

機體振動對姿態(tài)的影響主要體現(xiàn)在其對加速度數(shù)據(jù)的影響。由于加速度計對振動極為敏感，當發(fā)動機(電機)運轉(zhuǎn)時，盡管測量模塊的周圍會采取減震措施，但加速度的測量值中仍會有高頻噪聲，很不穩(wěn)定，使得姿態(tài)輸出會出現(xiàn)較大的誤差。經(jīng)實驗和分析，振動情況下加速度計數(shù)據(jù)偏差主要是高頻噪聲，為此,設計了切比雪夫II型數(shù)字低通濾波器，對加速度的測量值進行低通濾波處理。

切比雪夫II型濾波器的特性是在通帶內(nèi)是單調(diào)的(在Ω=0附近最平坦)，在阻帶內(nèi)是等波紋的。濾波器的系統(tǒng)函數(shù)為

(1)

根據(jù)濾波要求的Ωc(截止頻率)、N(迭代次數(shù))和ε(波紋參數(shù))，便可得到式(1)中的a和b值,繼而得到差分方程

y(n)=b0x(n)+b1x(n-1)+…bN-1x(n-N+1)-

a1y(n-1)-a2y(n-2)-…aNy(n-N).

(2)

本節(jié)以采樣頻率為100 Hz，Ωc=12.5，ε=1，α=60為例，結(jié)合Matlab中的cheby2工具進行了契比雪夫II型濾波器設計，并針對加速度計數(shù)據(jù)進行了實驗。

圖2是加速度計Z軸數(shù)據(jù)在一定油門條件下的濾波前后對比效果圖。

圖2 有油門條件下加速度數(shù)據(jù)濾波前后效果圖

由圖2(a),(b)可以看出，經(jīng)過切比雪夫濾波器后，加速度計的數(shù)據(jù)較為平整。由此可見，濾波處理能有效地減少艇體震動對加速度數(shù)據(jù)的影響。

3 姿態(tài)解算

進行姿態(tài)解算首先得定義坐標系，此處定義的導航坐標系，坐標軸Xe，Ye，Ze分別指向北、東和地；艇體坐標系中，X軸與艇身軸線平行，指向艇頭；Z軸垂直于X軸指向艇身右側(cè)；Y軸根據(jù)右手定則確定,如圖3所示。

圖3 坐標系定義

3.1 陀螺儀的姿態(tài)角初始估計

(3)

式中g(shù)x，gy，gz為陀螺儀實時的三軸角速率值。設采樣間隔為Δt，則陀螺儀估計的四元數(shù)姿態(tài)值為

qgyr=qt-1+qtΔt.

(4)

3.2 基于最速下降法的姿態(tài)估計

作為輔助傳感器，當艇體處于勻速或是靜止時，加速度計和磁力計可以比較精確地估計姿態(tài)分量。根據(jù)重力和地磁場的特性，若規(guī)定重力方向為z軸正方向，則導航坐標系下重力加速度矢量，單位化后為G=[0，0，1]T；規(guī)定磁北方向為x軸正方向，單位化后H=[hx，0，hz]為導航坐標系下地磁常量，可以看出，這些分量中0和1的存在，在計算時可以極大提高解算效率，減輕計算負荷[7]。

這里定義誤差函數(shù)

(5)

式中Ωqt為導航坐標系到艇體坐標系的四元數(shù)旋轉(zhuǎn)矩陣

(6)

式中at=(ax,ay,az)為三軸加速度計測量的重力加速度矢量，mt=(mx,my,mz)為三軸磁力計測量的地磁場矢量。‖ΩqtG-at‖為艇體坐標系下重力加速度測量值與估計值誤差，‖ΩqtH-mt‖為艇體坐標系下磁場測量值與估計值誤差。易知，使得誤差函數(shù)f1(qt)和f2(qt)均取得極小值時的qt=[q0，q1，q2，q3]T，為最優(yōu)姿態(tài)估計，這樣轉(zhuǎn)化為多目標最優(yōu)化問題，確定如式(5)所示的優(yōu)化問題來求得姿態(tài)數(shù)據(jù)

minF(qt)=minf1(qt)+minf2(qt).

(7)

因為f1(qt)和f2(qt)均大于等于0，根據(jù)式(7)中的每個目標函數(shù)的重要性，采用平方權(quán)函數(shù)法確定如下的目標函數(shù)

(8)

(9)

(10)

(11)

式(9)中γt決定了其收斂速率，所以，γt取值接近于艇體實際的姿態(tài)變化速率，取值如式(12)所示，避免步長過大導致姿態(tài)誤差

(12)

3.3 姿態(tài)融合與專家系統(tǒng)

在3.1節(jié)中得到了由陀螺儀利用三軸角速率得到的四元數(shù)姿態(tài)，3.2節(jié)中利用梯度下降法得到的四元數(shù)姿態(tài)，然而實際應用中，陀螺儀會產(chǎn)生漂移，非靜止或勻速狀態(tài)下，加速度數(shù)據(jù)不能用于計算姿態(tài)角，再加上測量噪聲的影響，因此，只依靠以上2種方法之一不能較好地估計姿態(tài)數(shù)據(jù)，需要將2種數(shù)據(jù)進行融合。本文采用常用的互補濾波器對2種數(shù)據(jù)進行實時融合，其常用的時域算法如下

qt=(1-αt)qgyr+αtqgra.

(13)

將式(4)和式(10)代入上式，并忽略極小項，簡化后得到下式

(14)

考慮到艇體震動等有害加速度的影響，特提出專家系統(tǒng)規(guī)則，用于不同情況下根據(jù)運動狀態(tài)自適應調(diào)整融合算法。

1)當a≤σ1時，艇體處于靜止或勻速狀態(tài)下，λ值不變；

2)當σ1

3)當a>σ2時，艇體處于高動態(tài)下，λ應取值0。

步驟(1)中融合算法提供的姿態(tài)是由加速度傳感器和三軸磁強計對重力向量和磁向量的觀測去補償陀螺漂移和姿態(tài)角誤差；步驟(2)，(3)中融合算法主要依賴于陀螺儀來估計姿態(tài)。σ1可根據(jù)加速度計的零漂值進行確定，而σ2應結(jié)合具體的應用條件確定，如σ2=1。

圖4給出了整個系統(tǒng)的算法流程。可以看出，融合算法經(jīng)推倒簡化，最終只借助輔助傳感器計算相應梯度值，每次迭代無需借助輔助傳感器得到完整姿態(tài)，算法得到了優(yōu)化。

4 實驗結(jié)果

為驗證所提出的無人飛艇航姿測量系統(tǒng)，將航姿系統(tǒng)安裝在飛艇吊艙內(nèi)，進行了靜態(tài)和振動條件下的對比試驗。為了進一步驗證算法的有效性，結(jié)合飛艇運動的特點，進行了動態(tài)條件下本系統(tǒng)和高精度GPS慣性組合導航系統(tǒng)SPAN-CPT(姿態(tài)精度為±0.05°，方位角精度為±0.01°)的對比試驗。

圖5是靜態(tài)條件下和振動條件下航姿系統(tǒng)采集到橫滾角和俯仰角數(shù)據(jù)的對比。可以看出，振動條件下，航姿系統(tǒng)的姿態(tài)數(shù)據(jù)沒有劇烈的跳動，航姿系統(tǒng)可以抑制一定艇體振動的影響。

圖5 靜態(tài)和振動條件下實驗結(jié)果

圖6是動態(tài)條件下本系統(tǒng)和SPAN-CPT組合導航定位系統(tǒng)同時采集的姿態(tài)數(shù)據(jù)對比。將SPAN-CPT系統(tǒng)采集到的數(shù)據(jù)作為參考值，本系統(tǒng)的采集的數(shù)據(jù)作為測量值，分別繪制了姿態(tài)變化曲線和誤差曲線。從圖6(a)，(c)中可以看出，二者輸出的姿態(tài)數(shù)據(jù)趨勢相同，基本吻合，只是在高動態(tài)下姿態(tài)誤差較大，但基本在±2°以內(nèi)，滿足機動性較低的應用需求；對比曲線的水平段為靜態(tài)數(shù)據(jù)，可以誤差圖中估計出系統(tǒng)的靜態(tài)誤差在±0.8°以內(nèi)。

圖6 動態(tài)下對比實驗結(jié)果

5 結(jié) 論

本文研究了一種基于MEMS傳感器的無人飛艇航姿測量方法。提出了基于梯度下降法的姿態(tài)融合方法，將加速度計和磁力計的數(shù)據(jù)用于校正陀螺儀估計的姿態(tài)，計算負荷小，速度快；針對振動干擾姿態(tài)問題，提出使用切比雪夫II型數(shù)字濾波器對加速度數(shù)據(jù)進行處理，從傳感器數(shù)據(jù)處理和姿態(tài)解算過程雙重抑制干擾。

通過多組實驗，驗證了文中的方法對于艇體震動和有害加速度具有明顯的抑制作用，低動態(tài)下能有效地估計姿態(tài)，精度能滿足無人飛艇飛行導航的應用要求。

參考文獻:

[1] Markley F L.Attitude representation for Kalmanfiltering[C]∥Proceedings of the 2001 AAS/AIAA Astro Dynamics Specialists Conference,Guebec City,Guebec,Canada:AAS/AIAA,2001.

[2] Marins J L,Yun X,Bachmann E R,et al.An extended Kalman filter for quaternion-based orientation estimation using margsensors[C]∥Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems,2001.

[3] 沈曉衛(wèi),姚敏立,趙 鵬.基于互補濾波的動中通天線姿態(tài)估計[J].中國慣性技術(shù)學,2011,19(2):194-197.

[4] 郭曉鴻,楊 忠,陳 喆,等.EKF和互補濾波器在飛行姿態(tài)確定中的應用[J].傳感器與微系統(tǒng)，2011,30(11):149-152.

[5] 傅建國,王孝通,金良安,等.一種基于地球重力場和磁場的姿態(tài)估計新算法[J].電子學報,2005,33(3):567-570.

[6] 袁 信,鄭 諤.捷聯(lián)式慣性導航原理[M].北京:航空專業(yè)教材編審組,1985.

[7] Madgwick Sebastian O H,Harrison Andrew J L,Vaidyanathan Ravi.Estimation of IMU and MARG orientation using a gradient descent algorithm[C]∥Proceedings of IEEE International Conference on Rehabilitation Robotics Rehab Week Zurich,ETH Zurich Science City,Switzerland,2011.