擴展卡爾曼濾波在組合導航中的應用

沈 凱， 管雪元， 李文勝

(南京理工大學 瞬態(tài)物理國家重點實驗室，江蘇 南京 210094)

?

擴展卡爾曼濾波在組合導航中的應用

沈 凱， 管雪元， 李文勝

(南京理工大學 瞬態(tài)物理國家重點實驗室，江蘇 南京 210094)

松組合導航中，采用組合導航系統(tǒng)(SINS)誤差作為狀態(tài)量，用SINS解算的位置和速度與全球定位系統(tǒng)(GPS)測量的位置速度之差作為量測信息。為了提高組合導航的精度在松組合導航中應用擴展卡爾曼(EKF)濾波算法，通過仿真對載體軌跡的速度、姿態(tài)、位置進行跟蹤。仿真結果表明：在仿真進入8 min之后系統(tǒng)進入穩(wěn)態(tài)，能準確跟蹤載體。因此,采用基于EKF的非線性濾波能有效跟蹤載體的位置、速度和姿態(tài)。

松組合； 擴展卡爾曼濾波； 組合導航

0 引 言

全球定位系統(tǒng)(global positioning system,GPS)憑借其全天候、實時性等優(yōu)點，投入運行以來，在各個領域得到越來越多的應用。而GPS的信號可能被干擾，或者在隧道，森林等地區(qū)無法接收[1]。因此，在導航系統(tǒng)中常用慣導和GPS進行組合。組合導航系統(tǒng)(INS)穩(wěn)定性好，不受外界因素影響，更新速率高等優(yōu)點正好彌補了GPS的不足[2]。

本文采用微系統(tǒng)(MEMS)慣性傳感器，具有體積小、重量輕、成本低等諸多優(yōu)點[3，4]。GPS和慣導的組合導航常用卡爾曼濾波進行數(shù)據(jù)融合，當慣性器件精度較差或者載體做高速運動時，系統(tǒng)中的非線性因素影響不可忽略，工程上常用非線性濾波算法進行處理[5]。本文采用擴展卡爾曼濾波(extended Kalman filtering,EKF),通過仿真計算分析其性能。

1 組合導航原理

圖1為松組合系統(tǒng)的結構框圖。松組合中采用的量測信息為位置與速度，利用GPS子系統(tǒng)解算得到的位置、速度與慣導解算出的位置、速度之差作為卡爾曼濾波器的輸入，得到慣導系統(tǒng)狀態(tài)誤差估計值[6,7]。將狀態(tài)誤差估計值的陀螺儀和加速度計漂移反饋給慣導系統(tǒng)以對其進行校正，狀態(tài)誤差估計值中的位置和速度誤差對慣導解算后的位置和速度信息進行校準，輸出為組合導航的最終結果。其中慣導數(shù)據(jù)由利用軌跡發(fā)生器產(chǎn)生，根據(jù)載體對應的比例和角速率解算出SINS系統(tǒng)的導航輸出信息，即位置和速度。

2 組合導航算法

2.1 組合導航數(shù)學模型

2.1.1 松組合系統(tǒng)狀態(tài)方程

(1)

圖1 松組合系統(tǒng)結構

狀態(tài)方程為

F(t)為系統(tǒng)狀態(tài)轉移矩陣

(2)

式中FINS由慣導基本誤差方程決定

(3)

式中G(t)為系統(tǒng)噪聲傳播矩陣

(4)

2.1.2 松組合系統(tǒng)量測方程

設載體的真實緯度為L、經(jīng)度λ、高度h；慣導輸出緯度LI、經(jīng)度λI、高度hI；GPS輸出緯度LG、經(jīng)度λG、高度hG；NN,NE和NU為GPS接收機導航下的位置量測誤差，則有

(5)

取慣導和GPS的輸出的位置差值作為系統(tǒng)量測值，則位置量測方程為

(6)

設載體真實東向速度為VE、北向速度VN和天向速度VU，慣導輸出速度分別為VIE,VIN和VIU,GPS輸出速度分別為VGE，VGN和VGU,NVE，NVN，NVU為GPS速度量測誤差，則

(7)

取慣導和GPS輸出的速度差值作為系統(tǒng)量測值，則速度量測方程為

(8)

將式(6)和式(8)組合，得到量測方程

(9)

2.2 擴展卡爾曼濾波算法

擴展卡爾曼濾波(extendedKalmanfiltering,EKF)建立在線性卡爾曼濾波的基礎之上，其核心思想是對非線性系統(tǒng)用泰勒級數(shù)展開，忽略其高階項，得到近線性化模型，然后進行濾波[8～10]。設非線性模型為

(10)

EKF方程組如下

(11)

(12)

(13)

3 實驗結果

如圖2、圖3所示為載體的運動軌跡，載體以北緯44.974°，西經(jīng)93.147°，高321.00 m，為起始坐標點(0,0)，環(huán)繞運動。

圖2 載體運動軌跡

圖3 載體高度軌跡

圖4 加速度計和陀螺誤差估計

圖5、圖6分別為姿態(tài)誤差角(φE,φN,φU)、速度誤差(δVE,δVN,δVU)的狀態(tài)曲線。

圖5 姿態(tài)誤差

圖6 速度誤差

由圖6可以看出，系統(tǒng)仿真在8 min之后誤差穩(wěn)定，速度誤差在5 m以內。能有效跟蹤載體的姿態(tài)和速度。

4 結束語

主要研究了EKF在松組合中的應用，建立了組合導航系統(tǒng)數(shù)學模型，對EKF算法在組合導航系統(tǒng)中的應用進行分析。仿真結果表明，使用EKF算法的組合導航系統(tǒng)能有效跟蹤系統(tǒng)，穩(wěn)定性良好。

[1] Wang X, Li Y.An innovative scheme for SINS/GPS ultra-tight integeration system with low-grade IMU[J].Aerospace Science and Techology,2012,23(1):452-460.

[2] 葛 俊，王新龍，車 歡.一種SINS輔助GPS跟蹤環(huán)路的新型松組合導航系統(tǒng)方案設計[J].航空兵器,2015(1):10-15.

[3] 趙 巖，程烘炳，吳訓忠，等.基于MEMS的低成本SINS_GPS組合導航系統(tǒng)研究[J].傳感器與微系統(tǒng),2011,30(8):18-21.

[4] 劉 澤，黎福海，肖祥慧，等.MEMS技術在車載導航中的應用[J].傳感器與微系統(tǒng),2010,29(12):131-134.

[5] 陸海勇，趙 偉，熊 劍，等.強跟蹤UKF濾波在SINS-GPS組合導航中的應用研究[J].航空電子技術,2008,39(4):5-10.

[6] 吳春光，李雙明，潘玉純.EKF和UKF在非線性組合導航系統(tǒng)中的對比研究[J].長春理工大學學報:自然科學版,2016,39(2):82-85.

[7] 王君帥，王新龍.SINS緊組合和松組合導航系統(tǒng)性能仿真分析[J].航空兵器,2013(2):14-19.

[8] 于 潔，王新龍.SINS/GPS緊密組合導航系統(tǒng)仿真研究[J].航空兵器,2009(6):8-13.

[9] 郭慶峰，閆連山，肖辰彬，等.帶傾角補償?shù)牡统杀綠PS/BD-DR組合導航系統(tǒng)設計[J].傳感器與微系統(tǒng),2013,32(10):68-73.

[10] 包 超，郭美鳳，周 斌，等.MIMU-GPS組合導航系統(tǒng)小型化設計[J].傳感器與微系統(tǒng),2014,33(1):116-119.

Application of EKF in integrated navigation system

SHEN Kai, GUAN Xue-yuan, LI Wen-sheng

(National Key Laboratory of Transient Physics,Nanjing Universty of Science and Technology,Nanjing 210094，China )

In loose combined navigation system, using the SINS error as state quantity ,the difference between the position and velocity of the SINS solution and the difference of position and velocity measured by GPS as measurement information.In order to improve the precision of integrated navigation, using the eatended Kalman filtering(EKF) algorithm in loose combination of navigation, through simulation,trajectory speed, attitude, position tracking of the carrier are carried out.The simulation results show that the system enters the steady state after 8 minutes, and it can accurately track the carrier.Therefore, the nonlinear filtering based on EKF can effectively track the position, velocity and attitude of the carrier.

loose combination; extended Kalman filtering(EKF); integrated navigation

10.13873/J.1000—9787(2017)08—0158—03

2016—09—12

TN 966

A

1000—9787(2017)08—0158—03

沈 凱(1991- )，男，碩士研究生，研究方向為組合導航,E—mail:214598145@qq.com。