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

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

(南京理工大學(xué) 1.瞬態(tài)物理國家重點(diǎn)實(shí)驗(yàn)室;2.能源與動(dòng)力工程學(xué)院,南京 210094)

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

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

收稿日期:2013-12-15

基金項(xiàng)目:國家自然科學(xué)

作者簡介:管軍(1987- ),男,博士研究生,研究方向?yàn)閺椉刂萍夹g(shù)。E-mail:guanjun8710@163.com。

通訊作者:易文俊(1970- ),男,教授,博士生導(dǎo)師,研究方向?yàn)閺椉鈴椀缹W(xué)及其控制技術(shù)。E-mail:wjy@mail.njust.edu.cn。

中圖分類號:TN967.2文獻(xiàn)標(biāo)識(shí)碼: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

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

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

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

(1)

(2)

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

(3)

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

①初始化。

(4)

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

②計(jì)算采樣點(diǎn)。

(5)

③時(shí)間更新。

(6)

(7)

(8)

(9)

(10)

④量測更新。

(11)

(12)

(13)

(14)

(15)

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

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

②UKF不需要進(jìn)行復(fù)雜的Jacobin矩陣計(jì)算,計(jì)算的復(fù)雜度有所下降。

2基于UKF的組合導(dǎo)航系統(tǒng)設(shè)計(jì)

2.1　慣性導(dǎo)航系統(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×表示比力信息,其它物理量的物理意義可參閱文獻(xiàn)。

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

①陀螺漂移的誤差模型。

ε=εb+εr+ωg

(23)

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

(24)

式中:tr為陀螺儀噪聲相關(guān)時(shí)間。

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

a+ωa

(25)

式中:ta為加速度計(jì)噪聲相關(guān)時(shí)間。

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

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

(26)

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

(27)

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

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

本文以慣性導(dǎo)航系統(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)結(jié)合在一起,即可得到組合導(dǎo)航系統(tǒng)的狀態(tài)方程:

(29)

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

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

①慣導(dǎo)給出的位置信息。

(30)

②北斗給出的位置信息。

(31)

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

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

(32)

③慣導(dǎo)給出的速度信息。

(33)

④北斗給出的速度信息。

(34)

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

(35)

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

(36)

3仿真實(shí)驗(yàn)分析

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

圖1　俯仰角誤差

圖2　滾轉(zhuǎn)角誤差

圖3　偏航角誤差

4實(shí)際跑車實(shí)驗(yàn)分析

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

圖4　北斗接收機(jī)芯片

圖5　IMU模塊

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

圖6　圓周運(yùn)動(dòng)緯度對比

圖7　圓周運(yùn)動(dòng)經(jīng)度對比

圖8　圓周運(yùn)動(dòng)組合導(dǎo)航系統(tǒng)運(yùn)動(dòng)軌跡

圖9　直線運(yùn)動(dòng)緯度對比

圖10　直線運(yùn)動(dòng)經(jīng)度對比

圖11　直線運(yùn)動(dòng)組合導(dǎo)航系統(tǒng)運(yùn)動(dòng)軌跡

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

5結(jié)論

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

參考文獻(xiàn)

[1]萬德鈞,房建成.慣性導(dǎo)航初始對準(zhǔn).南京:東南大學(xué)出版社,1998.

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

[2]周翟和,劉建業(yè).一種新的改進(jìn)高斯粒子濾波算法及其在SINS/GPS深組合導(dǎo)航系統(tǒng)中的應(yī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濾波技術(shù)在AUV組合導(dǎo)航中的應(yīng)用研究.中國慣性技術(shù)學(xué)報(bào),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]秦永元.慣性導(dǎo)航.北京:科學(xué)出版社,2006.

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

[7]陸海勇.強(qiáng)跟蹤UKF濾波在SINS/GPS組合導(dǎo)航中的應(yīng)用研究.航空電子技術(shù),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]秦永元.卡爾曼濾波與組合導(dǎo)航原理.西安:西北工業(yè)大學(xué)出版社,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超緊組合導(dǎo)航系統(tǒng)可觀測性分析.北京航空航天大學(xué)學(xué)報(bào),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.