0 引言

    四旋翼飛行器因?yàn)槠浣Y(jié)構(gòu)簡單對稱、相對容易控制的特點(diǎn)，在監(jiān)測、檢查、救援、電子情報(bào)等很多重要的領(lǐng)域被廣泛使用^[1-2]，最近幾年在學(xué)術(shù)界和科研領(lǐng)域引起很大興趣。四軸飛行器姿態(tài)角解算的速度和精度直接影響飛行器是否能獲得實(shí)時(shí)、穩(wěn)定的飛行姿態(tài)，所以姿態(tài)解算是四旋翼飛行器研究的核心之一。

    對于四旋翼飛行器的姿態(tài)測量，采用集成了三軸陀螺儀和三軸加速度計(jì)的MPU6050進(jìn)行姿態(tài)數(shù)據(jù)的采集，在較簡單的硬件平臺下，應(yīng)該怎樣濾除外部干擾，得到精確度比較高的數(shù)據(jù)變得十分重要。近幾年姿態(tài)估計(jì)算法隨著電子技術(shù)的發(fā)展相繼出現(xiàn)。文獻(xiàn)[3]應(yīng)用互補(bǔ)濾波^[3]對四旋翼的數(shù)據(jù)進(jìn)行融合，該算法可以有效抑制因電機(jī)轉(zhuǎn)動(dòng)而帶來的周期性干擾和因溫度影響給MPU6050帶來的漂移誤差。但是應(yīng)用互補(bǔ)濾波導(dǎo)致飛控系統(tǒng)有滯后現(xiàn)象，截止頻率附近衰減較慢，處理后的數(shù)據(jù)存在誤差，精確度較低。文獻(xiàn)[4]提出基于歐拉角的迭代擴(kuò)展卡爾曼^[4]（IEKF）姿態(tài)測量方法，該方法使用歐拉角方法描述姿態(tài)角，基于EKF濾波算法，在計(jì)算觀測矩陣H時(shí)進(jìn)行多次迭代，該算法估計(jì)出的姿態(tài)比普通EKF估計(jì)的姿態(tài)精度高。但該算法復(fù)雜、計(jì)算量大、循環(huán)周期長，僅適用于仿真，在實(shí)際工程上應(yīng)用較少。文獻(xiàn)[5-6]提出粒子濾波的算法，粒子濾波算法是在狀態(tài)空間中尋找一組隨機(jī)樣本，近似地表示出概率密度函數(shù)，獲得系統(tǒng)狀態(tài)的最小方差，當(dāng)樣本數(shù)量趨于無窮時(shí)可以逼近任何形式的概率密度函數(shù)，擺脫了解決非線性濾波問題時(shí)隨機(jī)量必須滿足高斯分布的制約，因此粒子濾波能夠比較精準(zhǔn)地表達(dá)基于觀測量和控制量的后驗(yàn)分布。但是粒子濾波需要大量的樣本數(shù)量才能較準(zhǔn)確地近似系統(tǒng)的后驗(yàn)概率密度，使得算法復(fù)雜度較高，而且重采樣階段會造成樣本有效性和多樣性的損失，導(dǎo)致樣本出現(xiàn)貧化現(xiàn)象。

    利用MPU6050采集姿態(tài)信息，為了獲得較精確的姿態(tài)角，對EKF算法進(jìn)行了改進(jìn)，MPU6050在每一個(gè)采樣時(shí)刻可以同時(shí)測得當(dāng)前時(shí)刻飛行器的姿態(tài)角和姿態(tài)角變化率的信息，結(jié)合改進(jìn)EKF算法對數(shù)據(jù)進(jìn)行處理能夠準(zhǔn)確地估算飛行器當(dāng)前的位置和姿態(tài)。飛行器在飛行過程中受非高斯白噪聲的影響，本文在進(jìn)行姿態(tài)解算時(shí)，為有效模擬飛行器測量過程的噪聲，用兩個(gè)加權(quán)高斯概率密度函數(shù)表示非高斯白噪聲函數(shù)，選擇合理高斯函數(shù)的權(quán)重、均值和方差。通過改進(jìn)型EKF有效地濾除該噪聲，減小了飛行器姿態(tài)偏差。

1 系統(tǒng)建模

    為了獲得四旋翼飛行器的姿態(tài)數(shù)據(jù)信息，首先選定兩個(gè)基本坐標(biāo)系：慣性坐標(biāo)系E(XYZ)和機(jī)體坐標(biāo)系B(X′Y′Z′)^[7]，如圖1所示。

式中c代表cos，s代表sin，轉(zhuǎn)換矩陣是非線性矩陣，為避免該矩陣出現(xiàn)奇點(diǎn)問題，飛行器在測試飛行過程中不會做出大幅度機(jī)動(dòng)動(dòng)作，因而可以認(rèn)為系統(tǒng)滿足以下假設(shè)：

    (1)四旋翼飛行器橫滾角與俯仰角滿足以下不等式：

    (2)在實(shí)際飛行中飛行器角速度信號由陀螺儀測量，因此可以假設(shè)飛行器橫滾角速度與偏航角速度為有界信號L_∞，即：w_x∈L_∞，w_z∈L_∞。

    MPU6050可以快速、穩(wěn)定地采集飛行器靜止時(shí)的姿態(tài)信息，但當(dāng)飛行器開始運(yùn)動(dòng)時(shí)，機(jī)體抖動(dòng)及螺旋槳高頻率的運(yùn)動(dòng)使得MPU6050測得的飛行器位姿存在明顯的偏差。利用EKF將MPU6050采集的數(shù)據(jù)進(jìn)行融合。EKF濾波原理是將非線性系統(tǒng)進(jìn)行線性化處理，利用泰勒級數(shù)展開系統(tǒng)函數(shù)，取一次項(xiàng)作為EKF的系數(shù)矩陣，忽略了高階項(xiàng)噪聲。因此采用EKF進(jìn)行姿態(tài)解算存在較大的濾波偏差。針對以上問題，本文提出改進(jìn)型EKF，對四旋翼飛行器的姿態(tài)數(shù)據(jù)進(jìn)行解算。

2 改進(jìn)型擴(kuò)展卡爾曼濾波原理及算法

    為有效濾除噪聲，測得精確的姿態(tài)數(shù)據(jù)，對EKF濾波算法進(jìn)行了改進(jìn)，采用兩個(gè)EKF并行地對四旋翼飛行器的姿態(tài)數(shù)據(jù)進(jìn)行預(yù)測和更新，將它們輸出的狀態(tài)變量加權(quán)整合后輸出飛行器的最優(yōu)狀態(tài)估計(jì)，因此經(jīng)改進(jìn)EKF處理后的飛行器姿態(tài)數(shù)據(jù)達(dá)到最小均方誤差。實(shí)現(xiàn)對飛行器位姿的有效操控，從理論上分析經(jīng)過改進(jìn)EKF濾波后的效果會更加接近真實(shí)值，有效地濾除噪聲，且算法復(fù)雜度低、運(yùn)算量小，不需要時(shí)頻域的頻繁轉(zhuǎn)換，更不會因?yàn)闃颖緮?shù)量的選擇而導(dǎo)致樣本重采樣現(xiàn)象，融合后飛行器姿態(tài)數(shù)據(jù)精度高。具體步驟如下：

    (1)EKF1 與EKF2 對四旋翼飛行器姿態(tài)數(shù)據(jù)并行地進(jìn)行處理，輸出對應(yīng)的狀態(tài)變量X_1，k、X_2，k和協(xié)方差P_1，k、P_2，k；

    (2)計(jì)算EKF1 和EKF2的似然函數(shù)，求出對應(yīng)的權(quán)重a₁、a₂；

    (3)加權(quán)整合后更新四旋翼飛行器狀態(tài)X_k和協(xié)方差P_k，輸出飛行器姿態(tài)的最優(yōu)估計(jì)；

    (4)實(shí)時(shí)調(diào)整飛行器姿態(tài)，待飛行器下一個(gè)狀態(tài)到達(dá)前返回步驟(1)，繼續(xù)下一次循環(huán)。

    具體過程如圖2所示。

    對于一個(gè)隨機(jī)系統(tǒng)，具體的估計(jì)過程如下：

3 仿真與實(shí)驗(yàn)

    在改進(jìn)型EKF理論的基礎(chǔ)上進(jìn)行仿真，在NI my RIO-1900的實(shí)驗(yàn)平臺上對飛行器進(jìn)行實(shí)驗(yàn)。

    為保證飛行器初始時(shí)刻的穩(wěn)定性及減小初始時(shí)刻姿態(tài)誤差，對初始時(shí)刻四旋翼飛行器姿態(tài)數(shù)據(jù)進(jìn)行濾波處理，以橫滾角為例，采集飛行器靜止時(shí)刻姿態(tài)數(shù)據(jù)，改進(jìn)EKF濾波后姿態(tài)與普通EKF濾波后姿態(tài)進(jìn)行對比，如圖3、圖4所示。普通EKF濾波后飛行器姿態(tài)有約1°左右的偏差，改進(jìn)EKF濾波后飛行器姿態(tài)穩(wěn)定性較好，偏差較小，幾乎可以忽略。二者相比較，改進(jìn)型EKF的濾波效果較好，魯棒性強(qiáng)。

    給飛行器橫滾角一個(gè)操控命令，使其傾斜14°，飛行器在執(zhí)行操控命令時(shí)，機(jī)體坐標(biāo)系繞慣性坐標(biāo)系旋轉(zhuǎn)會產(chǎn)生一定旋轉(zhuǎn)角，橫滾角初始狀態(tài)不是絕對的0°，經(jīng)測試飛行器初始時(shí)刻橫滾角為0.41°。如圖5和圖6所示，飛行器接收到操控命令后，普通EKF濾波后的飛行器經(jīng)40 ms達(dá)到期望姿態(tài)，在140 ms 內(nèi)有波動(dòng)，140 ms 之后飛行器波動(dòng)范圍減小，但存在約0.5°左右的偏移。改進(jìn)EKF濾波后的飛行器經(jīng)38 ms后達(dá)到期望姿態(tài)，僅用時(shí)80 ms飛行器就達(dá)到理想狀態(tài)，且偏移較小，可以忽略。從二者的對比圖可得出，改進(jìn)型EKF濾波后飛行器姿態(tài)角較穩(wěn)定，提高了系統(tǒng)的準(zhǔn)確性和可靠性。

    通過MPU6050采集四旋翼飛行器0～100 s內(nèi)的姿態(tài)數(shù)據(jù)，采集的姿態(tài)經(jīng)改進(jìn)EKF濾波后的動(dòng)態(tài)測試如圖7，在4 s～16 s、48 s～54 s、60 s～70 s、72 s～86 s、91 s～96 s時(shí)間段內(nèi)，改進(jìn)EKF估計(jì)值完全跟蹤到理想值，在16 s～18 s、23 s～26 s、56 s～59 s、96 s～99 s時(shí)間段內(nèi)，由于四旋翼飛行器姿態(tài)角度變化幅度較大，改進(jìn)EKF估計(jì)值與理想值之間出現(xiàn)偏差，但在下一刻，即19 s、27 s、60 s、100 s時(shí)，改進(jìn)EKF估計(jì)值快速恢復(fù)實(shí)時(shí)追蹤。

    經(jīng)計(jì)算，改進(jìn)EKF估計(jì)值均方誤差為3.306，普通EKF估計(jì)值均方誤差為5.884 3。改進(jìn)EKF估計(jì)值精度明顯高于普通EKF估計(jì)值精度。精度提高的效率為：

    經(jīng)過多次測試可知：該算法設(shè)計(jì)合理，姿態(tài)精度有較明顯的改善，改進(jìn)的算法已成功應(yīng)用在四旋翼飛行器上，得到了理想的效果。

4 結(jié)論

    本文改進(jìn)的擴(kuò)展卡爾曼了濾波算法，克服了非高斯白噪聲對傳感器姿態(tài)解算的影響，且該算法復(fù)雜度低、精度高、計(jì)算量小。實(shí)驗(yàn)結(jié)果表明，改進(jìn)的算法可以準(zhǔn)確地估計(jì)出姿態(tài)角，與經(jīng)過普通EKF濾波后的姿態(tài)測量值對比，改進(jìn)EKF在飛行器姿態(tài)精度上有較明顯的改善，驗(yàn)證改進(jìn)方法是有效的，為四旋翼無人機(jī)實(shí)時(shí)調(diào)整姿態(tài)提供了一種科學(xué)手段。該算法只對MPU6050采集的數(shù)據(jù)進(jìn)行融合處理，進(jìn)一步可將該算法應(yīng)用至地磁計(jì)模塊，對飛行器數(shù)據(jù)進(jìn)一步進(jìn)行融合處理，以快速實(shí)現(xiàn)飛行器的垂直起飛和懸停的控制操作。

參考文獻(xiàn)

[1] ROSALES C，GANDOLFO D，SCAGLIA G，et al.Trajectory tracking of a mini four-rotor helicopter in dynamic environments-a linear algebra approach[J].Robotica，2015，33(8)：1628-1652.

[2] LUPASHIN S，HEHN M，MUELLER M W，et al.A platform for aerial robotics research and demonstration：the flying machine arena[J].Mechatronics，2014，24(1)：41-54.

[3] LU X，LI J，LIU W.A study of complementary filter algorithm for four-rotor helicopters attitude control system[J].Sensors & Transducers，2014，174(7)：298-306.

[4] 賀海鵬，閻妍，馬良，等.基于IEKF的四旋翼無人機(jī)姿態(tài)測量方法研究[J].計(jì)算機(jī)仿真，2015(4)：56-60.

[5] MERCADO D A，CASTILLO P，LOZANO R.Quadrotors data fusion using a particle filter[C].Unmanned Aircraft Systems(ICUAS)，2014 International Conference on.IEEE，2014.

[6] 曲仕茹，馬志強(qiáng).改進(jìn)的粒子濾波在四旋翼姿態(tài)估計(jì)中的應(yīng)用[J].飛行力學(xué)，2013(5)：458-461，466.

[7] ZHAO S，AN H，LIU J，et al.Identification of aerodynamic parameter for quadrotor model[J].Journal of Electronic Measurement & Instrument，2013，27(8)：744-749.

作者信息：

侯玉涵，王耀力

(太原理工大學(xué) 信息工程學(xué)院，山西 太原030024)