基于機(jī)動加速度補(bǔ)償?shù)母邉討B(tài)無人機(jī)MEMS姿態(tài)實(shí)時解算

郭忠誠，管練武，吉彩妮

中航貴州飛機(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)證所提算法的可行性和有效性。

坐標(biāo)系定義

機(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ī)橫軸與水平面之間的夾角。

坐標(biāo)系轉(zhuǎn)換

由于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慣性測量單元姿態(tài)解算

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慣性測量單元的測量精度。

機(jī)動加速度補(bǔ)償

其中β1和β2分別為無人機(jī)在縱軸和橫軸的機(jī)動加速度補(bǔ)償系數(shù)，該系數(shù)大小由所采用的慣性傳感器精度和無人機(jī)機(jī)動情況共同決定。

自適應(yīng)Mahony互補(bǔ)濾波算法

考慮到低成本姿態(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)角。

無人機(jī)試驗(yàn)驗(yàn)證及分析

將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ī)高速滑行試驗(yàn)驗(yàn)證

在地面共進(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)證

無人機(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)測量。