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

趙佳 呂弘 周智愷 冀明

摘要

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

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

1引言

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

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

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

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

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

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時(shí)刻到k_l時(shí)刻的變化;w為系統(tǒng)噪聲;z為觀測(cè)量;h為觀測(cè)方程，表征x與z之間的關(guān)系;v為觀測(cè)噪聲。

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

其中，

T為三軸角速度投影;

]T為三軸加速度投影;

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

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

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

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

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

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

俯仰角：Pitch= -asin（egx）

3飛行驗(yàn)證

選擇某型旋翼無人飛行器作為驗(yàn)證對(duì)象，該無人飛行器搭載法國(guó)SBG的IG-500N微機(jī)電組合導(dǎo)航系統(tǒng)，測(cè)試飛行器如圖2所示。

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

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

4結(jié)論

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

參考文獻(xiàn)

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

[2]申星，面向小型無人飛行器的SINS/GPS紐合導(dǎo)航系統(tǒng)研究[D].浙江大學(xué)碩士學(xué)位論文，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]付夢(mèng)印，鄧志紅，閏莉萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京：科學(xué)出版社，2010.

[6]秦永元，慣性導(dǎo)航（第二版）[M].北京：科學(xué)出版社，2014.