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

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

(1.北京航空航天大學(xué) 機(jī)械工程及自動(dòng)化學(xué)院，北京 100191；2.首都師范大學(xué) 三維信息獲取與應(yīng)用教育部重點(diǎn)實(shí)驗(yàn)室，北京 100037)

0 引 言

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

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

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

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

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

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

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

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

切比雪夫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工具進(jìn)行了契比雪夫II型濾波器設(shè)計(jì)，并針對(duì)加速度計(jì)數(shù)據(jù)進(jìn)行了實(shí)驗(yàn)。

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

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

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

3 姿態(tài)解算

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

圖3 坐標(biāo)系定義

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

(3)

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

qgyr=qt-1+qtΔt.

(4)

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

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

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

(5)

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

(6)

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

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

(7)

因?yàn)閒1(qt)和f2(qt)均大于等于0，根據(jù)式(7)中的每個(gè)目標(biāo)函數(shù)的重要性，采用平方權(quán)函數(shù)法確定如下的目標(biāo)函數(shù)

(8)

(9)

(10)

(11)

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

(12)

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

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

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

(13)

將式(4)和式(10)代入上式，并忽略極小項(xiàng)，簡(jiǎn)化后得到下式

(14)

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

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

2)當(dāng)σ1

3)當(dāng)a>σ2時(shí)，艇體處于高動(dòng)態(tài)下，λ應(yīng)取值0。

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

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

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

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

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

圖5 靜態(tài)和振動(dòng)條件下實(shí)驗(yàn)結(jié)果

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

圖6 動(dòng)態(tài)下對(duì)比實(shí)驗(yàn)結(jié)果

5 結(jié) 論

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

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

參考文獻(xiàn):

[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),姚敏立,趙 鵬.基于互補(bǔ)濾波的動(dòng)中通天線姿態(tài)估計(jì)[J].中國(guó)慣性技術(shù)學(xué),2011,19(2):194-197.

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

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

[6] 袁 信,鄭 諤.捷聯(lián)式慣性導(dǎo)航原理[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.