基于 UKF 的北斗/SINS 組合導航系統(tǒng)研究

基于UKF的北斗/SINS組合導航系統(tǒng)研究

管軍1,易文俊1,常思江2,袁丹丹1,孫蕾1

(南京理工大學 1.瞬態(tài)物理國家重點實驗室;2.能源與動力工程學院,南京 210094)

摘要:為了進一步提高導航系統(tǒng)的精度、可靠性及安全性,根據(jù)組合導航的基本原理,利用無跡卡爾曼濾波(UKF)算法,對北斗/捷聯(lián)慣導系統(tǒng)(SINS)組合導航系統(tǒng)進行了系統(tǒng)仿真,在一個裝有微型化慣性測量單元(MIMU)和北斗導航芯片的某型精確制導炮彈的自駕儀系統(tǒng)上進行了實際的跑車實驗。實驗結果表明:對中國自主研發(fā)的北斗衛(wèi)星導航系統(tǒng)和SINS的組合,UKF算法能夠有效地估計導航參數(shù)的誤差,進行反饋校正后,可以大幅提高系統(tǒng)的導航精度和數(shù)據(jù)的更新頻率。在所用硬件和算法的前提下,定位精度能夠達到6 m,數(shù)據(jù)更新頻率能夠達到100 Hz,可以滿足實際工程的需要。

關鍵詞:精確制導;無跡卡爾曼濾波;北斗導航系統(tǒng);捷聯(lián)慣導系統(tǒng)

收稿日期:2013-12-15

基金項目:國家自然科學

作者簡介:管軍(1987- ),男,博士研究生,研究方向為彈箭控制技術。E-mail:guanjun8710@163.com。

通訊作者:易文俊(1970- ),男,教授,博士生導師,研究方向為彈箭外彈道學及其控制技術。E-mail:wjy@mail.njust.edu.cn。

中圖分類號:TN967.2文獻標識碼:A

Research on Beidou/SINS Integrated Navigation System Based on UKF

GUAN Jun1,YI Wen-jun1,CHANG Si-jiang2,YUAN Dan-dan1,SUN Lei1

(1.National Key Laboratory of Transient Physics;2.School of Power and Engineering,NUST,Nanjing 210094,China)

Abstract:To further increase the precision,reliability and security of integrated navigation system(INS),the algorithm of unscented Kalman filter(UKF) was used to simulate the Beidou/SINS INS according to the basic principle of integrated navigation,and the real experiments were all proposed in this paper.The experiment on a car was carried out.For the real experiment,an autopilot was employed for guided bomb equipped with the chip of Beidou and a micro inertial measure unit(MIMU).The experiment result shows that the navigation error of Beidou/SINS INS can be effectively estimated by using the algorithm of UKF.Under the process of feedback compensation,it can enormously improve the precision and the data refresh rate of this system.Under the conditions of the hardware and software mentioned,the accuracy of location can reach 6 m,and the data refresh rate can achieve 100 Hz.These indexes can meet the requirement of a precision guided bomb.

Key words:precision guidance;unscented Kalman filter;Beidou navigation system;strapdown inertial navigation system

精確制導炮彈導航系統(tǒng)的最低要求是能夠提供足夠精度的位置、速度和姿態(tài)等信息,并且還要有很好的可靠性。傳統(tǒng)的精確制導炮彈屬于自毀性武器,要求成本越低越好,所以本文采用了價格便宜、體積小、功耗低的微電子機械系統(tǒng)MEMS(micro-electromechanical systems)器件進行系統(tǒng)的設計。

慣性導航系統(tǒng)(INS)主要分為捷聯(lián)式慣性導航系統(tǒng)(SINS)和平臺式慣性導航系統(tǒng)(PINS)。由于平臺式慣性導航系統(tǒng)結構復雜,本文采用結構較簡單、工程上容易實現(xiàn)的捷聯(lián)式慣性導航系統(tǒng)。SINS具有短時精度和穩(wěn)定性高,自主導航能力強,數(shù)據(jù)刷新頻率高等一系列優(yōu)點。但是其導航誤差會隨著時間的增加而發(fā)散,不適合作為長時間的獨立導航系統(tǒng)。GPS導航系統(tǒng)雖然具有長航時、精度高、全天候等優(yōu)點,但畢竟受制于人,在關鍵時刻,若不能正常工作,將產(chǎn)生不可估量的嚴重后果。北斗導航系統(tǒng)已經(jīng)完成區(qū)域衛(wèi)星導航系統(tǒng)的建設,同時開始為服務區(qū)域免費提供開放、穩(wěn)定、可靠的定位定向、實時導航、精密測速、精確授時、位置報告、短信服務等服務,定位精度10 m,測速精度0.2 m/s,授時精度20 ns。將SINS和北斗進行空間和時間上的組合可以有效地利用各個導航系統(tǒng)的優(yōu)點進行系統(tǒng)間的性能互補,這種組合方式可以有效地提高整個系統(tǒng)的定位和測速精度并具有很強的穩(wěn)定性。

1UKF簡介及其實現(xiàn)步驟

(1)

(2)

考慮非線性系統(tǒng):

(3)

式中:Xk為系統(tǒng)的狀態(tài)量,yk為觀測向量,ωk為系統(tǒng)的過程噪聲,vk為量測噪聲。假設過程噪聲和量測噪聲均服從高斯分布,且兩者之間互不相關。在這樣的假設下,得到的UKF濾波算法的計算流程如下所示。

①初始化。

(4)

式中:nw為系統(tǒng)過程噪聲的維數(shù),O為零矩陣。

②計算采樣點。

(5)

③時間更新。

(6)

(7)

(8)

(9)

(10)

④量測更新。

(11)

(12)

(13)

(14)

(15)

從UKF的計算過程可以看出,UKF濾波器至少具有以下優(yōu)點:

①UKF直接利用了非線性模型,不需要進行線性化,避免引入線性化誤差,進而提高了估計精度;

②UKF不需要進行復雜的Jacobin矩陣計算,計算的復雜度有所下降。

2基于UKF的組合導航系統(tǒng)設計

2.1　慣性導航系統(tǒng)的誤差方程

①姿態(tài)誤差方程。

(16)

(17)

(18)

②速度誤差方程。

2ωie(sinL)vU)δL+E

(19)

(20)

(21)

③位置誤差方程。

(22)

式中:φ×表示姿態(tài)誤差;δv×表示速度誤差;δL,δλ,δh表示緯度、經(jīng)度和高度誤差;L,λ,h分別表示緯度、經(jīng)度和高度 ;f×表示比力信息,其它物理量的物理意義可參閱文獻。

2.2　慣性儀表的誤差 [7-10]

①陀螺漂移的誤差模型。

ε=εb+εr+ωg

(23)

式中:εb為隨機常數(shù),εr為一階馬爾科夫噪聲,ωg為高斯白噪聲。設3個軸向的陀螺漂移誤差模型相同,均為

(24)

式中:tr為陀螺儀噪聲相關時間。

②加速度計的誤差模型為一階馬爾科夫噪聲,其數(shù)學模型為

a+ωa

(25)

式中:ta為加速度計噪聲相關時間。

2.3　北斗導航系統(tǒng)的誤差方程

北斗導航系統(tǒng)總的定位和測速誤差可以等效為一階馬爾科夫噪聲:

(26)

式(26)為北斗導航系統(tǒng)位置誤差方程,式中:δLBD,δλBD,δhBD分別為北斗系統(tǒng)的緯度、經(jīng)度和高度誤差;tBD,L,tBD,λ,tBD,h分別為位置一階馬爾科夫過程的相關時間;ωBD,L,ωBD,λ,ωBD,h分別為位置一階馬爾科夫過程的驅動白噪聲。

(27)

式(27)為北斗導航系統(tǒng)速度誤差方程,式中:δvBD,E,δvBD,N,δvBD,U分別為北斗系統(tǒng)的東、北、天3個方向的速度誤差;tBD,E,tBD,N,tBD,U分別為速度一階馬爾科夫過程的相關時間;ωBD,E,ωBD,N,ωBD,U分別為速度一階馬爾科夫過程的驅動白噪聲。

2.4　組合導航系統(tǒng)的狀態(tài)方程

本文以慣性導航系統(tǒng)的誤差方程作為濾波器的狀態(tài)方程,并考慮慣性器件的噪聲,于是取濾波器的總的狀態(tài)量為

X=(φEφNφUδvEδvNδvUδLδλδh

εbxεbyεbzεrxεryεrzxyz)T

(28)

將式(16)～式(27)結合在一起,即可得到組合導航系統(tǒng)的狀態(tài)方程:

(29)

2.5　組合導航系統(tǒng)的量測方程

在位置、速度組合模式中,其量測值有2組:一組為位置量測值,即SINS給出的經(jīng)度、緯度、高度信息和北斗接收機給出的相應的信息的差值作為一組量測值,2個系統(tǒng)給出的速度的差值作為另一組量測值。

①慣導給出的位置信息。

(30)

②北斗給出的位置信息。

(31)

式中:λt,Lt,ht為系統(tǒng)所在的真實位置;NE,NN,NU為北斗接收機3個方向的位置誤差。則位置量測誤差矢量形式為

Hp(t)X(t)+vp(t)

(32)

③慣導給出的速度信息。

(33)

④北斗給出的速度信息。

(34)

式中:vE,vN,vU分別為東、北、天3個方向的真實速度;ME,MN,MU分別為北斗接收機測量的速度誤差。則速度誤差矢量形式為

(35)

將式(32)和式(35)合在一起,即得到了整個系統(tǒng)的量測誤差方程:

(36)

3仿真實驗分析

利用UKF算法對北斗/SINS的位置、速度進行數(shù)據(jù)融合。仿真實驗主要是驗證算法的可行性,為研制實際的系統(tǒng)進行理論上的指導。本文的仿真實驗假設北斗衛(wèi)星的信號都能準確接收,不存在丟星、遮擋等情況,即組合導航系統(tǒng)一直工作在正常模式。仿真參數(shù)如下:SINS輸出周期為0.01 s,北斗輸出周期為1 s,濾波周期為1 s,陀螺儀的仿真參數(shù)與ADXRS610一致,加速度計的仿真參數(shù)與MMA7260一致,北斗接收機的水平位置均方差為10 m,高度均方差為5 m,速度均方差為0.1 m/s。因為角度的誤差對整個慣導系統(tǒng)的影響非常大,所以在仿真實驗部分,本文主要給出了姿態(tài)角誤差的數(shù)據(jù)情況,實驗結果如圖1～圖3所示。根據(jù)實驗數(shù)據(jù),系統(tǒng)運行穩(wěn)定,基于UKF的北斗/SINS算法是可行的,能夠按照預期估計出狀態(tài)量的誤差。

圖1　俯仰角誤差

圖2　滾轉角誤差

圖3　偏航角誤差

4實際跑車實驗分析

在某實驗基地進行實際跑車實驗,該實驗主要在某型制導炮彈的自駕儀平臺上完成,實驗所用的北斗芯片和實驗室自主研發(fā)的慣導模塊如圖4、圖5所示。其中慣導輸出周期為10 ms,UKF濾波周期為1 s,校正方式為反饋校正,校正周期為1 s。

圖4　北斗接收機芯片

圖5　IMU模塊

在該部分的實驗中,分別進行了圓周運動測試和直線運動測試2部分。實驗數(shù)據(jù)如圖6～圖11所示,其中,圖6～圖8為圓周實驗數(shù)據(jù),圖9～圖11為直線運動實驗數(shù)據(jù)。

圖6　圓周運動緯度對比

圖7　圓周運動經(jīng)度對比

圖8　圓周運動組合導航系統(tǒng)運動軌跡

圖9　直線運動緯度對比

圖10　直線運動經(jīng)度對比

圖11　直線運動組合導航系統(tǒng)運動軌跡

通過實驗數(shù)據(jù)分析,在實際實驗中,不管是圓周運動還是直線運動,組合導航系統(tǒng)最終輸出的數(shù)據(jù)都沒有表現(xiàn)出純慣導系統(tǒng)的數(shù)據(jù)發(fā)散現(xiàn)象,數(shù)據(jù)的更新頻率和精度也要比北斗導航系統(tǒng)更優(yōu)秀。經(jīng)過實際實驗數(shù)據(jù)的分析,利用UKF的組合導航系統(tǒng)最終的定位精度可以達到6 m左右。此設計方法可以滿足需要。

5結論

本文利用UKF算法對國產(chǎn)北斗導航系統(tǒng)和基于MEMS器件的捷聯(lián)慣性導航系統(tǒng)進行了系統(tǒng)間的數(shù)據(jù)融合,通過數(shù)值仿真和實際的跑車實驗驗證了該方法的可行性和可靠性,對以后的實際工程具有一定的理論指導意義。

參考文獻

[1]萬德鈞,房建成.慣性導航初始對準.南京:東南大學出版社,1998.

WAN De-jun,FANG Jian-cheng.Initial alignment of inertial navigation.Nanjing:Southeast University Press,1998.(in Chinese)

[2]周翟和,劉建業(yè).一種新的改進高斯粒子濾波算法及其在SINS/GPS深組合導航系統(tǒng)中的應用.控制與決策,2011,26(1):85-88.

ZHOU Zhai-he,LIU Jian-ye.Novel Gaussian particle filter and

it’s application in deeply integrated SINS/GPS navigation system.Control and Decision,2011,26(1):85-88.(in Chinese)

[3]JULIER S.The scaled unscented transformation//Proceedings of the American Control Conference.Anchorage,AK:IEEE Conference Publications,2002:4 555-4 559.

[4]練軍想,吳文啟.UKF濾波技術在AUV組合導航中的應用研究.中國慣性技術學報,2005,13(1):30-34.

LIAN Jun-xiang,WU Wen-qi.Application of unscented Kalman filter in AUV integrated navigation.Journal of Chinese Inertial Technology,2005,13(1):30-34.(in Chinese)

[5]JULIER S J,UHLMANN J K.Unsecented filtering and nonlinear estimation.Proceeding of IEEE,2004,92(3):401-422.

[6]秦永元.慣性導航.北京:科學出版社,2006.

QIN Yong-yuan.Inertial navigation.Beijing:Science Press,2006.(in Chinese)

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

LU Hai-yong.Exploration of strong tracking UKF used in the SINS/GPS integrated navigation.Avionics Technology,2008,39(4):5-10.(in Chinese)

[8]秦永元.卡爾曼濾波與組合導航原理.西安:西北工業(yè)大學出版社,1998.

QIN Yong-yuan.Kalman filter and the theory of integrated navigation.Xi’an:Northwestern Polytechnical University Press,1998.(in Chinese)

[9]周衛(wèi)東,蔡佳楠.GPS/SINS超緊組合導航系統(tǒng)可觀測性分析.北京航空航天大學學報,2013,39(9):1-6.

ZHOU Wei-dong,CAI Jia-nan.Observability analysis of GPS/SINS ultra-tightly coupled system.Journal of Beijing University of Aeronautics and Astronautics,2013,39(9):1-6.(in Chinese)

[10] HAM F M,BROWN R G.Observability,eigenvalues,and Kalman filtering.IEEE Transactions on Aerospace and Electronic Systems,1983,19(2):269-273.