基于擴展卡爾曼濾波的無人飛行器姿態解算

趙佳 呂弘 周智愷 冀明

摘要

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

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

1引言

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

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

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

2擴展卡爾曼濾波器姿態解算

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

1所示。

在無人飛行器的姿態解算中，其系統狀態函數可描述為：

xnl=f（xk，Wk） （1）

zk=h（xk，Vk） （2）

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

在無人飛行器的姿態解算中：

其中，

T為三軸角速度投影;

]T為三軸加速度投影;

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

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

（6）此時，經過濾波器可以得到當前狀態下無人飛行器的最優角速度、角加速度和重力向量。

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

由此可得，無人飛行器姿態角為：

滾轉角：Roll=atan（-）

俯仰角：Pitch= -asin（egx）

3飛行驗證

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

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

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

4結論

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

參考文獻

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

[2]申星，面向小型無人飛行器的SINS/GPS紐合導航系統研究[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濾波理論及其在導航系統中的應用[M].北京：科學出版社，2010.

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