胡少興, 劉東昌, 張愛武, 朱煜坤
(1.北京航空航天大學(xué) 機械工程及自動化學(xué)院,北京 100191;2.首都師范大學(xué) 三維信息獲取與應(yīng)用教育部重點實驗室,北京 100037)
姿態(tài)信息是自動飛行控制系統(tǒng)最重要最基本的反饋信號。在無人飛艇上, 由于任務(wù)載荷有限, 高精度的大型測量設(shè)備將不能使用。微機電系統(tǒng)(micro-electro-mechanical system,MEMS)慣性傳感器由于其質(zhì)量、體積和功耗等方面的優(yōu)勢得到了廣泛的應(yīng)用。
針對多MEMS傳感器姿態(tài)融合,國內(nèi)外提出了許多先進的方法,如擴展卡爾曼濾波(EKF)方法,是一種遞推濾波方法[1,2],EKF方法廣泛使用證明了它的正確性和有效性,但其是基于對非線性方程的線性化,當(dāng)系統(tǒng)具有強非線性時線性化可能引起大的誤差甚至造成濾波器的不穩(wěn)定,并且EKF設(shè)計困難;比較常用的還有互補濾波法[3,4],結(jié)合傳感器頻域的特點分辨和消除噪聲,不需考慮信號的統(tǒng)計特性,但是文獻中的方法是基于幾何關(guān)系直接計算傾角,沒有優(yōu)化處理,容易造成姿態(tài)誤差[5]。
針對現(xiàn)有算法存在的不足,本文提出了一種高效的姿態(tài)融合算法,充分利用重力場和地磁場特性,每次融合迭代過程只需根據(jù)輔助傳感器計算出梯度,去修正陀螺儀數(shù)據(jù)給出最優(yōu)估計。針對振動干擾問題提出新的思路,從傳感器數(shù)據(jù)預(yù)處理和多傳感器數(shù)據(jù)融合兩方面雙重抑制振動,取得了明顯的效果。該算法采用了四元數(shù)表示方位,避免了歐拉角描述姿態(tài)時出現(xiàn)的奇異性問題。
所描述的無人飛艇航姿系統(tǒng)主要由三軸陀螺儀、三軸加速度計、三軸磁力計以及ARM處理器組成,工作原理如圖1所示,傳感器數(shù)據(jù)經(jīng)過預(yù)處理后經(jīng)過專家系統(tǒng)的判斷進行數(shù)據(jù)融合算法,最終輸出艇體的姿態(tài)角—橫滾角φ、俯仰角θ和航向角ψ。
圖1 系統(tǒng)總體結(jié)構(gòu)圖
機體振動對姿態(tài)的影響主要體現(xiàn)在其對加速度數(shù)據(jù)的影響。由于加速度計對振動極為敏感,當(dāng)發(fā)動機(電機)運轉(zhuǎn)時,盡管測量模塊的周圍會采取減震措施,但加速度的測量值中仍會有高頻噪聲,很不穩(wěn)定,使得姿態(tài)輸出會出現(xiàn)較大的誤差。經(jīng)實驗和分析,振動情況下加速度計數(shù)據(jù)偏差主要是高頻噪聲,為此,設(shè)計了切比雪夫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è)計,并針對加速度計數(shù)據(jù)進行了實驗。
圖2是加速度計Z軸數(shù)據(jù)在一定油門條件下的濾波前后對比效果圖。
圖2 有油門條件下加速度數(shù)據(jù)濾波前后效果圖
由圖2(a),(b)可以看出,經(jīng)過切比雪夫濾波器后,加速度計的數(shù)據(jù)較為平整。由此可見,濾波處理能有效地減少艇體震動對加速度數(shù)據(jù)的影響。
進行姿態(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)
式中g(shù)x,gy,gz為陀螺儀實時的三軸角速率值。設(shè)采樣間隔為Δt,則陀螺儀估計的四元數(shù)姿態(tài)值為
qgyr=qt-1+qtΔt.
(4)
作為輔助傳感器,當(dāng)艇體處于勻速或是靜止時,加速度計和磁力計可以比較精確地估計姿態(tài)分量。根據(jù)重力和地磁場的特性,若規(guī)定重力方向為z軸正方向,則導(dǎo)航坐標(biāo)系下重力加速度矢量,單位化后為G=[0,0,1]T;規(guī)定磁北方向為x軸正方向,單位化后H=[hx,0,hz]為導(dǎo)航坐標(biāo)系下地磁常量,可以看出,這些分量中0和1的存在,在計算時可以極大提高解算效率,減輕計算負荷[7]。
這里定義誤差函數(shù)
(5)
式中Ωqt為導(dǎo)航坐標(biāo)系到艇體坐標(biāo)系的四元數(shù)旋轉(zhuǎn)矩陣
(6)
式中at=(ax,ay,az)為三軸加速度計測量的重力加速度矢量,mt=(mx,my,mz)為三軸磁力計測量的地磁場矢量。‖ΩqtG-at‖為艇體坐標(biāo)系下重力加速度測量值與估計值誤差,‖ΩqtH-mt‖為艇體坐標(biāo)系下磁場測量值與估計值誤差。易知,使得誤差函數(shù)f1(qt)和f2(qt)均取得極小值時的qt=[q0,q1,q2,q3]T,為最優(yōu)姿態(tài)估計,這樣轉(zhuǎn)化為多目標(biāo)最優(yōu)化問題,確定如式(5)所示的優(yōu)化問題來求得姿態(tài)數(shù)據(jù)
minF(qt)=minf1(qt)+minf2(qt).
(7)
因為f1(qt)和f2(qt)均大于等于0,根據(jù)式(7)中的每個目標(biāo)函數(shù)的重要性,采用平方權(quán)函數(shù)法確定如下的目標(biāo)函數(shù)
(8)
(9)
(10)
(11)
式(9)中γt決定了其收斂速率,所以,γt取值接近于艇體實際的姿態(tài)變化速率,取值如式(12)所示,避免步長過大導(dǎo)致姿態(tài)誤差
(12)
在3.1節(jié)中得到了由陀螺儀利用三軸角速率得到的四元數(shù)姿態(tài),3.2節(jié)中利用梯度下降法得到的四元數(shù)姿態(tài),然而實際應(yīng)用中,陀螺儀會產(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)自適應(yīng)調(diào)整融合算法。
1)當(dāng)a≤σ1時,艇體處于靜止或勻速狀態(tài)下,λ值不變;