郭忠誠,管練武,吉彩妮
中航貴州飛機(jī)有限責(zé)任公司
針對高動態(tài)無人機(jī)機(jī)動加速和發(fā)動機(jī)高頻振動導(dǎo)致MEMS姿態(tài)測量精度降低問題,本文研究MEMS加速度計(jì)和陀螺儀組成的低成本小型姿態(tài)測量系統(tǒng),提出一種自適應(yīng)Mahony互補(bǔ)濾波算法,以補(bǔ)償機(jī)動加速度及動態(tài)整定互補(bǔ)濾波器PI參數(shù),對傳感器數(shù)據(jù)進(jìn)行融合、補(bǔ)償和修正。該方法利用加速度計(jì)三軸輸出模值,以及對水平計(jì)算加速度與當(dāng)?shù)刂亓铀俣鹊谋容^,判斷機(jī)動加速度并進(jìn)行補(bǔ)償,然后通過自適應(yīng)Mahony互補(bǔ)濾波算法完成姿態(tài)實(shí)時解算,最后經(jīng)無人機(jī)試驗(yàn)驗(yàn)證得出,所采用的算法收斂性和平滑性較好,降低了MEMS的誤差影響,可對無人機(jī)姿態(tài)進(jìn)行有效跟蹤和測量,為低成本小型姿態(tài)測量系統(tǒng)的開發(fā)和應(yīng)用提供實(shí)用的方法。
姿態(tài)角是用來描述無人機(jī)空間指向的重要信息,在導(dǎo)航應(yīng)用中對姿態(tài)信息進(jìn)行分析顯得至關(guān)重要。姿態(tài)測量系統(tǒng)(Attitude Measurement System,AMS)利用陀螺儀、加速度計(jì)等慣性傳感器和微處理器等測量飛行器的姿態(tài)角。一般而言,在高動態(tài)及復(fù)雜環(huán)境下,姿態(tài)測量系統(tǒng)采用機(jī)械陀螺儀、光纖陀螺儀或激光陀螺儀獲取姿態(tài)信息,這種姿態(tài)測量系統(tǒng)精度和穩(wěn)定性高,但成本高、體積大、功耗高且維護(hù)復(fù)雜,不能滿足現(xiàn)代無人機(jī)姿態(tài)測量系統(tǒng)在低成本、小型化、低功耗、免維護(hù)等方面的要求。近年來,隨著微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System,MEMS)技術(shù)的發(fā)展,MEMS慣性測量單元(Inertial Measurement Unit,IMU)在控制領(lǐng)域的應(yīng)用日趨廣泛,尤其是對無人機(jī)系統(tǒng)的姿態(tài)測量。因此,近年來基于MEMS慣性測量單元的姿態(tài)測量系統(tǒng)常用于實(shí)時測量和分析高動態(tài)無人機(jī)的姿態(tài)。
MEMS慣性測量單元利用加速度計(jì)和陀螺儀測量數(shù)據(jù),加速度計(jì)主要測量機(jī)體坐標(biāo)系下的線性加速度,陀螺儀主要測量機(jī)體坐標(biāo)系下的角加速度。陀螺儀經(jīng)過積分得到姿態(tài)角,短時間內(nèi)精度高,但陀螺儀自身存在漂移,隨著時間的積累誤差逐漸增加,因此長時間的精度較差。與之相反,機(jī)體高頻振動等因素引發(fā)的噪聲,對加速度計(jì)的影響較大,短期精度較低,但其測量誤差不會隨時間的積累而增加。利用兩者的互補(bǔ)關(guān)系,采用較優(yōu)的算法對姿態(tài)數(shù)據(jù)進(jìn)行融合,能提高姿態(tài)的測量精度和動態(tài)響應(yīng)。姿態(tài)解算的主流數(shù)據(jù)融合算法包括卡爾曼濾波和互補(bǔ)濾波,與卡爾曼濾波相比,互補(bǔ)濾波的計(jì)算量較小、復(fù)雜度較低,多應(yīng)用于低成本、實(shí)時的姿態(tài)測量系統(tǒng)。但是,傳統(tǒng)互補(bǔ)濾波中,PI參數(shù)在滑行、起飛、加速、減速、轉(zhuǎn)彎和降落等各種動作中均為固定值,不能根據(jù)無人機(jī)運(yùn)動幅度和頻率變化調(diào)整PI參數(shù)。因此,在無人機(jī)整個飛行過程中,傳統(tǒng)互補(bǔ)濾波算法的動態(tài)適應(yīng)性和測量精度均較差,更糟糕的是,很難將無人機(jī)速度變化引起的機(jī)動加速度與重力分量分開,尤其是在滑行和拐彎飛行過程中,無人機(jī)大幅度機(jī)動,未補(bǔ)償?shù)臋C(jī)動加速度會引起較大姿態(tài)測量誤差。
針對無人機(jī)大幅度機(jī)動以及發(fā)動機(jī)高頻振動導(dǎo)致無人機(jī)實(shí)時姿態(tài)測量精度降低問題,本文利用MEMS慣性測量單元構(gòu)建無人機(jī)的姿態(tài)測量系統(tǒng),提出一種對機(jī)動加速度進(jìn)行補(bǔ)償?shù)淖赃m應(yīng)Mahony互補(bǔ)濾波算法,以補(bǔ)償機(jī)動加速度以及動態(tài)整定互補(bǔ)濾波器PI參數(shù),對傳感器數(shù)據(jù)進(jìn)行融合、補(bǔ)償和修正,通過開展無人機(jī)地面高速滑行試驗(yàn)和空中飛行試驗(yàn),驗(yàn)證所提算法的可行性和有效性。
機(jī)體坐標(biāo)系(b系)——ObXbYbZb
機(jī)體坐標(biāo)系的三軸分別與無人機(jī)固定連接,其原點(diǎn)O定位在無人機(jī)的重心,ObXb軸沿?zé)o人機(jī)縱軸向前,ObYb軸沿?zé)o人機(jī)橫軸向右,ObZb軸沿?zé)o人機(jī)豎直軸向上。
導(dǎo)航坐標(biāo)系(n系)——OnXnYnZn
導(dǎo)航坐標(biāo)系取當(dāng)?shù)氐乩碜鴺?biāo)系,其原點(diǎn)O定位在無人機(jī)的重心,OnXn軸指向地理東向,OnYn軸指向地理北向,OnZn軸垂直于當(dāng)?shù)匦D(zhuǎn)橢球面指向天向。
b系相對于n系的方位關(guān)系用航向角Ψ、橫滾角γ和俯仰角θ描述。具體而言,航向角是無人機(jī)縱軸在當(dāng)?shù)厮矫嫔系耐队熬€與當(dāng)?shù)氐乩肀毕虻膴A角,俯仰角是無人機(jī)縱軸與水平面之間的夾角,橫滾角是無人機(jī)橫軸與水平面之間的夾角。
由于MEMS慣性測量單元固定在無人機(jī)上,各傳感器基于機(jī)體坐標(biāo)系測量數(shù)據(jù),因此在姿態(tài)解算時,需要將機(jī)體坐標(biāo)系的數(shù)據(jù)轉(zhuǎn)換到導(dǎo)航坐標(biāo)系,在此給出依次繞Z軸、Y軸、X軸旋轉(zhuǎn)所產(chǎn)生的轉(zhuǎn)換矩陣,用Cbn表示如下:
MEMS慣性測量單元的基本原理框圖如圖1所示,MEMS慣性測量單元在初始靜態(tài)條件下由加速度計(jì)感知地球重力分量計(jì)算水平姿態(tài)角,即俯仰角和橫滾角信息;在動態(tài)條件下主要采用陀螺儀進(jìn)行水平姿態(tài)角變化量計(jì)算。此外,無人機(jī)在動態(tài)條件下首先對機(jī)動加速度進(jìn)行補(bǔ)償,然后利用基于重力的自適應(yīng)Mahony互補(bǔ)濾波算法,對陀螺的漂移進(jìn)行實(shí)時跟蹤校正,保障系統(tǒng)動態(tài)測量精度,最終輸出無人機(jī)姿態(tài)角、三軸加速度和三軸角速度等數(shù)字信息。
圖1 MEMS慣性測量單元基本原理框圖。
MEMS慣性測量單元采用的數(shù)據(jù)融合算法是基于重力的自適應(yīng)Mahony互補(bǔ)濾波算法,它能綜合加速度計(jì)和陀螺儀各自的頻率響應(yīng)優(yōu)勢,從頻率角度對兩個傳感器數(shù)據(jù)進(jìn)行融合,以減少測量和估計(jì)誤差。與此同時,為降低無人機(jī)加速起飛、爬升、拐彎、降落等大機(jī)動狀態(tài)對測量誤差的影響,需對無人機(jī)機(jī)動引起的加速度進(jìn)行補(bǔ)償修正,保證MEMS慣性測量單元的測量精度。
其中β1和β2分別為無人機(jī)在縱軸和橫軸的機(jī)動加速度補(bǔ)償系數(shù),該系數(shù)大小由所采用的慣性傳感器精度和無人機(jī)機(jī)動情況共同決定。
考慮到低成本姿態(tài)測量系統(tǒng)的計(jì)算能力和精度要求,自適應(yīng)Mahony互補(bǔ)濾波常作為低成本姿態(tài)測量系統(tǒng)中的姿態(tài)解算算法。本文對MEMS慣性測量單元采集的數(shù)據(jù)進(jìn)行融合,用高通濾波器處理陀螺儀測量信號的同時,采用低通濾波器平滑加速度計(jì)測量信號,并在互補(bǔ)濾波器中加入自適應(yīng)PI調(diào)節(jié),以形成增強(qiáng)型互補(bǔ)濾波器。濾波器設(shè)計(jì)如(3)式所示。
(1)利用經(jīng)過補(bǔ)償后的三軸加速度計(jì)輸出模值|?|的大小,確定調(diào)節(jié)比例參數(shù)kp和ki的值;
(2)將?歸一化;
(3)將導(dǎo)航坐標(biāo)系下的重力向量轉(zhuǎn)換到機(jī)體坐標(biāo)系得到向量d;
(4)計(jì)算誤差向量e;
(5)通過濾波器上一時刻的積分項(xiàng)項(xiàng)In?1和這一時刻的誤差向量e計(jì)算出這一時刻的積分項(xiàng);
(6)計(jì)算信息δ;
(7)得到補(bǔ)償后的陀螺儀值w′;
(8)利用上一時刻的四元數(shù)qn?1和補(bǔ)償后的陀螺儀值w′更新四元數(shù);
(9)將四元數(shù)qn歸一化,并將四元數(shù)轉(zhuǎn)成姿態(tài)角θ和γ。重復(fù)上述步驟,即可實(shí)時解算無人機(jī)姿態(tài)角。
將MEMS慣性測量單元裝載于無人機(jī)航電設(shè)備艙,通過地面高速滑行試驗(yàn)和空中飛行試驗(yàn),驗(yàn)證低成本MEMS慣性測量單元的實(shí)際性能。測試設(shè)備為低成本MEMS慣性測量單元構(gòu)成的姿態(tài)測量系統(tǒng),參考測試設(shè)備為較高精度的捷聯(lián)慣性導(dǎo)航系統(tǒng)(INS),通過對比兩者的實(shí)時姿態(tài)測量,分析MEMS慣性測量單元的姿態(tài)測量精度。
在地面共進(jìn)行3次高速滑行試驗(yàn),驗(yàn)證MEMS慣性測量單元的姿態(tài)測量精度,MEMS慣性測量單元及捷聯(lián)慣性導(dǎo)航系統(tǒng)在無人機(jī)高速滑行時,其姿態(tài)測量值如圖2所示,姿態(tài)測量誤差分析如圖3所示,MEMS慣性測量單元姿態(tài)誤差統(tǒng)計(jì)結(jié)果如表1所示,俯仰角和橫滾角在高速滑行狀態(tài)下誤差均方根值分別為0.181°和0.116°。試驗(yàn)結(jié)果表明,在無人機(jī)高速滑行及發(fā)動機(jī)振動情況下,本文提出的方法可行并有效。
表1 無人機(jī)高速滑行時MEMS慣性測量單元姿態(tài)誤差統(tǒng)計(jì)結(jié)果。
圖2 無人機(jī)發(fā)動機(jī)轉(zhuǎn)速變化時姿態(tài)測量值。
圖3 無人機(jī)滑行速度變化時姿態(tài)測量誤差。
無人機(jī)飛行試驗(yàn)涉及高速滑行、起飛、爬升、平飛、拐彎、下降、著陸等過程,并驗(yàn)證MEMS慣性測量單元的姿態(tài)測量精度。整個飛行過程超過30min,其飛行軌跡如圖4藍(lán)色曲線所示。
圖4 無人機(jī)飛行軌跡。
無人機(jī)在飛行過程中,MEMS慣性測量單元的俯仰角和橫滾角測量值如圖5和圖6上半部分所示,圖5和圖6下半部分分別表示俯仰角和橫滾角測量誤差,MEMS慣性測量單元姿態(tài)誤差統(tǒng)計(jì)結(jié)果如表2所示,其中俯仰角和橫滾角誤差均方根值分別為0.879° 和0.867°,最大誤差主要出現(xiàn)在無人機(jī)拐彎時。試驗(yàn)結(jié)果表明,無人機(jī)飛行狀態(tài)下MEMS慣性測量單元姿態(tài)測量的跟隨性較好,誤差較平滑,所采用的算法具有較好效果。
圖5 無人機(jī)飛行時俯仰角跟隨性測量及誤差。
圖6 無人機(jī)飛行時橫滾角跟隨性測量及誤差。
表2 無人機(jī)飛行時MEMS慣性測量單元姿態(tài)誤差統(tǒng)計(jì)結(jié)果。
利用低成本MEMS慣性測量單元為高動態(tài)無人機(jī)構(gòu)建姿態(tài)測量系統(tǒng),是工程實(shí)踐應(yīng)用中最具挑戰(zhàn)性的研究工作之一。試驗(yàn)結(jié)果表明,本文提出的自適應(yīng)Mahony互補(bǔ)濾波算法,提高了MEMS慣性測量單元姿態(tài)實(shí)時解算的精度,可以應(yīng)用于高動態(tài)無人機(jī)相關(guān)姿態(tài)測量。