卡爾曼濾波在四軸飛行器導(dǎo)航中的應(yīng)用

徐華中，余 飛，何家俊

(武漢理工大學(xué)自動(dòng)化學(xué)院，湖北武漢430070)

四軸飛行器是一種四旋翼的小型飛行器，相比固定翼飛行器，其機(jī)械結(jié)構(gòu)較簡(jiǎn)單，成本較低，同時(shí)擁有較好的飛行穩(wěn)定性。四軸飛行器作為一個(gè)欠驅(qū)動(dòng)的系統(tǒng)，只能通過(guò)控制4個(gè)旋翼的升力來(lái)改變飛行姿態(tài)，使飛行姿態(tài)控制和導(dǎo)航控制相對(duì)復(fù)雜。目前對(duì)于四軸飛行器的姿態(tài)控制已經(jīng)比較成熟，但是對(duì)于導(dǎo)航控制的精度問(wèn)題仍處在研究階段。

應(yīng)用于四軸飛行器上的微加速度計(jì)及微機(jī)械陀螺是基于MEMS(微機(jī)電系統(tǒng))技術(shù)的一種新型慣性傳感器，目前其精度相對(duì)常規(guī)慣性器件要低。用于導(dǎo)航控制時(shí)，由于MIMU系統(tǒng)誤差隨時(shí)間快速累積，不能滿足長(zhǎng)時(shí)間導(dǎo)航的需求［1－2］。筆者將基于卡爾曼濾波的MIMU/GPS組合導(dǎo)航用于四軸飛行器的導(dǎo)航控制，結(jié)合MIMU和GPS導(dǎo)航的優(yōu)點(diǎn)，提高導(dǎo)航的精度［3－4］。

首先介紹MIMU與GPS系統(tǒng)組合方式，然后建立系統(tǒng)動(dòng)態(tài)模型，并以此來(lái)建立離散化的卡爾曼濾波模型，最后進(jìn)行仿真驗(yàn)證并得出結(jié)論［5］。

1 系統(tǒng)組合框圖

MIMU系統(tǒng)和GPS接收機(jī)在定位領(lǐng)域內(nèi)進(jìn)行松性組合。兩個(gè)子系統(tǒng)分別進(jìn)行定位運(yùn)算，然后將兩者的定位結(jié)果整合到一起。這種組合方式不需要對(duì)MIMU子系統(tǒng)或GPS接收機(jī)內(nèi)部做任何改動(dòng)，同時(shí)也能獲得較好的融合效果。MIMU子系統(tǒng)對(duì)傳感器值做積分定位后輸出一個(gè)平滑的以低頻噪聲為主的定位結(jié)果，而GPS定位結(jié)果中的噪聲呈高頻特性，通過(guò)一個(gè)前饋式互補(bǔ)型卡爾曼濾波器可以較好地對(duì)定位結(jié)果進(jìn)行組合［6］。系統(tǒng)的組合框圖如圖1所示。

圖1 系統(tǒng)組合框圖

在該組合系統(tǒng)中，MIMU子系統(tǒng)可以運(yùn)行在一個(gè)較高的定位頻率上，而卡爾曼濾波器運(yùn)行在一個(gè)較低的GPS定位頻率上。當(dāng)GPS接收機(jī)缺少可見(jiàn)衛(wèi)星而不能完成定位時(shí)，組合系統(tǒng)可以直接輸出MIMU子系統(tǒng)定位的結(jié)果。

2 系統(tǒng)動(dòng)態(tài)模型的建立

2.1 狀態(tài)方程的建立

對(duì)于低動(dòng)態(tài)性能的四軸飛行器，它的運(yùn)行狀況可以用6個(gè)狀態(tài)變量來(lái)描述，即3個(gè)位置分量和3個(gè)速度分量，其狀態(tài)向量為:

式中:lx，ly，lz分別為飛行器相對(duì)于起飛點(diǎn)在直角坐標(biāo)系中的位置;vx，vy，vz分別為飛行器在3個(gè)方向上的速度分量。

系統(tǒng)的狀態(tài)方程可表示為:

式中:F為系統(tǒng)矩陣;G為控制矩陣;ax，ay，az分別為3個(gè)坐標(biāo)軸上加速度的均值;ωax，ωay，ωaz分別為3個(gè)方向上的加速度白噪聲［7－8］。

2.2 觀測(cè)方程的建立

由于系統(tǒng)采用松組合方式，其測(cè)量值為位置量觀測(cè)差。位置量觀測(cè)值是由MIMU給出的位置信息與GPS接收機(jī)計(jì)算出的響應(yīng)位置信息求差。取GPS接收機(jī)的定位結(jié)果 Lx，Ly，Lz為觀測(cè)變量［9－10］，則測(cè)量方程為:

式中:Hk為輸出矩陣;μk為噪聲矢量;ωlx，ωly，ωlz分別為各個(gè)坐標(biāo)軸上的觀測(cè)白噪聲。

3 卡爾曼濾波模型的建立

系統(tǒng)的狀態(tài)方程為時(shí)間連續(xù)，將狀態(tài)方程離散化，并將其轉(zhuǎn)變?yōu)榭柭鼮V波模型狀態(tài)方程的標(biāo)準(zhǔn)形式:

式中:Ak－1為k－1時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣;Ts為狀態(tài)方程的差分步長(zhǎng)，也是測(cè)量過(guò)程的采樣周期;wk－1為噪聲向量［11］。

設(shè)離散形式的狀態(tài)噪聲的均值和方差為:

而測(cè)量噪聲μi的均值和方差為:

采用上述的狀態(tài)方程和觀測(cè)方程，可建立標(biāo)準(zhǔn)的卡爾曼濾波方程為:

在卡爾曼濾波校正過(guò)程中，式(9)包含矩陣的求逆運(yùn)算計(jì)算量較大?？紤]到各個(gè)方向上的噪聲互不相關(guān)，可將狀態(tài)向量分解為xx=(lxvx)T，xy=(lyvy)T，xz=(lzvz)T，分別進(jìn)行濾波，這樣，可以減少計(jì)算量。

4 仿真與分析

對(duì)系統(tǒng)進(jìn)行動(dòng)態(tài)仿真，根據(jù)四軸飛行器傳感器誤差建立系統(tǒng)的誤差模型，得到MIMU的誤差仿真數(shù)據(jù)，建立MIMU/GPS組合導(dǎo)航的卡爾曼濾波器，進(jìn)行數(shù)據(jù)融合。使用Matlab進(jìn)行仿真，仿真初始條件如下:建立以飛行器起飛地點(diǎn)為坐標(biāo)原點(diǎn)的空間直角坐標(biāo)系，四軸飛行器當(dāng)前在空間某位置以某一速度飛行。具體參數(shù)為lx(0)=50 m，ly(0)=20 m，lz(0)=10 m，vx(0)=2 m/s，vy(0)=3 m/s，vz(0)=0 m/s。系統(tǒng)初始誤差方陣 P0為對(duì)角陣，P(1，1)=P(2，2)=P(3，3)=12，P(4，4)=P(5，5)=P(6，6)=12。GPS 觀測(cè)噪聲方差σ1=σ2=σ3=52。采樣周期為200 ms。仿真結(jié)果如圖2和圖3所示。

圖2 x方向上GPS觀測(cè)值、融合后的估計(jì)值和真實(shí)值

仿真結(jié)果表明，采用卡爾曼濾波進(jìn)行GPS與MIMU的數(shù)據(jù)融合，可以較好地濾除GPS觀測(cè)值的隨機(jī)誤差，獲得較為精確的位置信息，同時(shí)對(duì)于MIMU系統(tǒng)積累誤差的消除也有較好的效果。

5 結(jié)論

圖3 x方向上GPS觀測(cè)值誤差和融合之后的誤差

筆者對(duì)四軸飛行器MIMU/GPS組合導(dǎo)航系統(tǒng)進(jìn)行研究，利用卡爾曼濾波對(duì)MIMU與GPS數(shù)據(jù)進(jìn)行融合，結(jié)合兩個(gè)系統(tǒng)各自的優(yōu)點(diǎn)，提高了導(dǎo)航定位的精度。將上述方法用于四軸飛行器的實(shí)際導(dǎo)航控制，與單純使用GPS導(dǎo)航相比，精度得到了提高，可以較好地實(shí)現(xiàn)尋路、返回等功能。

［1］劉輝，席德科，晏登洋.基于MIMU/GPS組合導(dǎo)航定位系統(tǒng)的一種新型卡爾曼濾波算法的研究［J］.彈箭與制導(dǎo)學(xué)報(bào)，2007，27(4):36 －39.

［2］謝鋼.GPS原理與接收機(jī)設(shè)計(jì)［M］.北京:電子工業(yè)出版社，2009:201－203.

［3］蔡艷輝，程鵬飛，李夕銀.用卡爾曼濾波進(jìn)行GPS動(dòng)態(tài)定位［J］.測(cè)繪通報(bào)，2006(7):6 －8.

［4］鐘珞，劉劍.基于混合高斯和均值濾波法的運(yùn)動(dòng)檢測(cè)方法［J］.武漢理工大學(xué)學(xué)報(bào):信息與管理工程版，2010，32(5):691 －693.

［5］伍小潔，房建成.一種帶速度觀測(cè)量的GPS動(dòng)態(tài)定位自適應(yīng)卡爾曼濾波算法［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2009(9):27－29.

［6］張銳，李文秀.多傳感器跟蹤型數(shù)據(jù)濾波融合算法［J］.哈爾濱工程大學(xué)學(xué)報(bào)，2002(8):106 －109.

［7］陳禮斌，劉釗.一種新的自適應(yīng)擴(kuò)展卡爾曼濾波算法［J］.激光與紅外，2006(3):210－213.

［8］阮建國(guó)，陳炯.一種適于快速實(shí)現(xiàn)的卡爾曼濾波算法的改進(jìn)［J］.電測(cè)與儀表，2009(2):4－7.

［9］張謙，景占榮.一種過(guò)程噪聲自適應(yīng)調(diào)節(jié)的卡爾曼濾波算法［J］.電子測(cè)量技術(shù)，2007(5):18－20.

［10］卞鴻巍，金志華，王俊璞，等.組合導(dǎo)航系統(tǒng)新息自適應(yīng)卡爾曼濾波算法［J］.上海交通大學(xué)學(xué)報(bào)，2006(6):1000－1004.

［11］BIERMAN G.Factorization methods for discrete sequential estimation［M］.［S.l.］:Dover Publications Inc，2006:120 －125.