基于擴展卡爾曼濾波的無人飛行器姿態(tài)解算

趙佳 呂弘 周智愷 冀明

摘要

為解決無人飛行器姿態(tài)傳感器低成本高精度的要求，引入擴展卡爾曼濾波器算法。該算法以低成本姿態(tài)傳感器的角速度和加速度作為輸入，實時解算姿態(tài)數(shù)據(jù)，有效地消除了無人飛行器運動過程對姿態(tài)解算精度的影響，實際飛行驗證了該方法的有效性。

【關鍵詞】擴展卡爾曼濾波 姿態(tài)解算 無人飛行器

1引言

自上世紀六十年代開始，軍事組織、科研院所等學者就開始開展無人飛行器的設計、開發(fā)和研究，并隨著控制技術、攝影技術、導航技術等不斷發(fā)展，功能不斷拓展，智能化程度不斷提升，在戰(zhàn)場監(jiān)控、攝影攝像、地理監(jiān)測、森林防火、農(nóng)業(yè)植保等領域都有著廣泛的應用，己成為目前各方研究的熱點。

無人飛行器由于飛行環(huán)境復雜，自主飛行要求高，需要控制系統(tǒng)實時作出精確的判斷并有效控制輸出。姿態(tài)數(shù)據(jù)作為飛行控制的內(nèi)環(huán)數(shù)據(jù)，對無人飛行器的精確控制起到至關重要的作用，它是保證無人飛行器穩(wěn)定飛行的重要條件之一。因此，對于無人飛行器而言，高精度的姿態(tài)數(shù)據(jù)是必須的。然而，目前大多數(shù)無人飛行器尤其是民用級無人飛行器由于成本等原因，普遍采用低成本微機電慣性系統(tǒng)或其組合導航系統(tǒng)獲取姿態(tài)數(shù)據(jù)。這為無人飛行器的姿態(tài)精確獲取帶來難度，由于姿態(tài)傳感系統(tǒng)本身輸出精度不高，直接輸出無法滿足控制的要求，需要通過算法提高姿態(tài)的測量精度。卡爾曼濾波算法尤其是擴展卡爾曼濾波算法（Extended Kalman Filter，EKF）為此類問題的解決提供了有效途徑。卡爾曼濾波算法采用遞推方式實現(xiàn)實時在線計算，在目標跟蹤、模式識別、導航等領域有著廣泛應用，然而，卡爾曼濾波器由于自身缺陷，無法徹底解決非線性的姿態(tài)解算問題。擴展卡爾曼濾波器則可有效解決此問題，并取得了不錯的成果，但這些成果主要以理論研究和仿真為主，較少考慮實際飛行使用等情況。

據(jù)此，本文提出將擴展卡爾曼濾波算法應用于低成本姿態(tài)傳感器的姿態(tài)解算，并通過某型無人旋翼飛行器的實際飛行試驗驗證本方法的有效性與合理性。

2擴展卡爾曼濾波器姿態(tài)解算

擴展卡爾曼濾波器的解算過程和迭代方式與卡爾曼濾波器相同，其基本解算流程如圖

1所示。

在無人飛行器的姿態(tài)解算中，其系統(tǒng)狀態(tài)函數(shù)可描述為：

xnl=f（xk，Wk） （1）

zk=h（xk，Vk） （2）

其中：x為狀態(tài)量;f為狀態(tài)方程，表征狀態(tài)量從k時刻到k_l時刻的變化;w為系統(tǒng)噪聲;z為觀測量;h為觀測方程，表征x與z之間的關系;v為觀測噪聲。

在無人飛行器的姿態(tài)解算中：

其中，

T為三軸角速度投影;

]T為三軸加速度投影;

為重力向量投影;wbib、rbg分別表示傳感器輸出的原始三軸數(shù)據(jù)。

考慮濾波器兩次濾波時間間隔較小的情況，此時，無人飛行器運動加速度變化相對也比較小，假定兩次濾波時間間隔內(nèi)加速度保持不變，則可得到系統(tǒng)方程：其中，wbkib為wbib構造的反對稱矩陣。利用線性化雅克比矩陣，可得：Xk+l= f（xk，0）

（6）此時，經(jīng)過濾波器可以得到當前狀態(tài)下無人飛行器的最優(yōu)角速度、角加速度和重力向量。

根據(jù)假設，可知無人飛行器在運動情況的加速度和重力加速度之間關系為f=-g。將g單位化，則

由此可得，無人飛行器姿態(tài)角為：

滾轉(zhuǎn)角：Roll=atan（-）

俯仰角：Pitch= -asin（egx）

3飛行驗證

選擇某型旋翼無人飛行器作為驗證對象，該無人飛行器搭載法國SBG的IG-500N微機電組合導航系統(tǒng)，測試飛行器如圖2所示。

在實際飛行情況下，以此傳感器輸出的三軸角速度、加速度作為擴展卡爾曼濾波姿態(tài)算法的輸入進行姿態(tài)解算，獲得姿態(tài)數(shù)據(jù)，同時輸出該傳感器在組合導航下的高精度姿態(tài)數(shù)據(jù)，兩者進行比較，驗證算法的有效性。

圖3.圖5為在實際飛行情況下，俯仰角和滾轉(zhuǎn)角的跟蹤情況，以及兩者的誤差曲線。由圖可知，在整個飛行過程中，擴展卡爾曼濾波算法獲得的姿態(tài)數(shù)據(jù)與傳感器組合導航下獲取的數(shù)據(jù)一致性較好。俯仰角和滾轉(zhuǎn)角的最大誤差分別為0.69°和0.85°，均方根誤差分別0.12°和o.18°考慮到此無人飛行器為油動，本身振動較大，且最大誤差出現(xiàn)時，無人飛行器實際飛行最大俯仰角和滾轉(zhuǎn)角分別為10.9°和-10.3°。因此所獲得的姿態(tài)數(shù)據(jù)完全可以滿足實際飛行需求，算法有效。

4結(jié)論

為解決無人飛行器姿態(tài)傳感器低成本高精度的要求，采用擴展卡爾曼濾波器算法，有效地解決了無人飛行器運動過程對姿態(tài)解算精度的影響，并以旋翼無人飛行器為例，驗證了算法的有效性。下一步將開展固定翼無人飛行器的姿態(tài)解算效果驗證，以確定解算的穩(wěn)定性、一致性和可推廣性。

參考文獻

[1]孫罡，低成本微小型無人機慣性組合導航技術研究[D].南京理工大學博士學位論文，2 014，6：4-6.

[2]申星，面向小型無人飛行器的SINS/GPS紐合導航系統(tǒng)研究[D].浙江大學碩士學位論文，2 012，5：13-2 0.

[3] Kumar N S， Jann T. Estimationof Attitude from a Low-costMiniaturized Inertial PlatformusiingKalman Filter-base SensorFusion Algorithm[J]. Sadhana，2004， 29 （2）： 217-235.

[4]L1 W， Wang J. Effective AdaptiveKalman Filter for MEMS-IMU/Magnetometers Interated Attitude andHeading Reference Sys tem [J]. Journalof Navigation， 2012，1（1）：1-15.

[5]付夢印，鄧志紅，閏莉萍.Kalman濾波理論及其在導航系統(tǒng)中的應用[M].北京：科學出版社，2010.

[6]秦永元，慣性導航（第二版）[M].北京：科學出版社，2014.