張文，孫瑞勝

(南京理工大學(xué) a.泰州科技學(xué)院，江蘇 泰州 225300； b.動(dòng)力工程學(xué)院，江蘇 南京 210094)

低成本SINS/GPS組合導(dǎo)航系統(tǒng)的研究*

張文a，孫瑞勝b

(南京理工大學(xué) a.泰州科技學(xué)院，江蘇 泰州 225300； b.動(dòng)力工程學(xué)院，江蘇 南京 210094)

以簡(jiǎn)易制導(dǎo)航彈為對(duì)象，進(jìn)行了低成本組合導(dǎo)航系統(tǒng)的研究。慣性導(dǎo)航系統(tǒng)具有體積小，功耗低等特點(diǎn)，但定位誤差隨時(shí)間積累，所以長期穩(wěn)定性比較差。目前應(yīng)用最多的衛(wèi)星導(dǎo)航是美國的GPS，GPS具有良好的長期穩(wěn)定性,但使用受制于美國政府，而且它的動(dòng)態(tài)定位性能比較差以及更新率比較低。將兩者結(jié)合起來，建立了適合工程應(yīng)用的低成本SINS/GPS組合導(dǎo)航系統(tǒng)，應(yīng)用自適應(yīng)卡爾曼濾波技術(shù)實(shí)現(xiàn)數(shù)據(jù)的融合，并對(duì)系統(tǒng)進(jìn)行了動(dòng)態(tài)跑車試驗(yàn)，試驗(yàn)結(jié)果表明，該組合導(dǎo)航系統(tǒng)提高了導(dǎo)航精度。此方案具有較好的工程應(yīng)用價(jià)值。

捷聯(lián)慣性導(dǎo)航系統(tǒng)；全球定位系統(tǒng)；組合導(dǎo)航系統(tǒng)；卡爾曼濾波

0 引言

為了使得飛行器按照指定的軌道運(yùn)行，現(xiàn)在有多種導(dǎo)航設(shè)備可供使用，如慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)，全球定位系統(tǒng)(global positioning system,GPS)，北斗導(dǎo)航系統(tǒng)等。這些導(dǎo)航設(shè)備都具有各自的特點(diǎn)。慣性導(dǎo)航系統(tǒng)依靠自身的傳感器完成導(dǎo)航任務(wù)，具有價(jià)格低，功耗低，環(huán)境適應(yīng)能力強(qiáng)等優(yōu)點(diǎn)，但是它的誤差隨時(shí)間積累，以致導(dǎo)航精度隨時(shí)間發(fā)散，在需要長時(shí)間定位的時(shí)候，不能夠滿足導(dǎo)航的要求。以GPS為代表的衛(wèi)星定位系統(tǒng)則沒有這個(gè)問題，具有很好的定位精度。但是，其抗干擾能力較差。如果將慣性導(dǎo)航系統(tǒng)與衛(wèi)星定位系統(tǒng)相結(jié)合，兩者取長補(bǔ)短，可以提高導(dǎo)航系統(tǒng)的長期精度，降低成本。目前，SINS(strapdown INS)/GPS組合導(dǎo)航是導(dǎo)航領(lǐng)域研究的熱點(diǎn)，本文設(shè)計(jì)了一種適合于工程應(yīng)用的低成本SINS/GPS組合導(dǎo)航系統(tǒng)。并對(duì)該系統(tǒng)的導(dǎo)航定位性能進(jìn)行了實(shí)驗(yàn)測(cè)試。

1 組合導(dǎo)航原理

1.1 SINS/GPS組合導(dǎo)航原理

捷聯(lián)慣導(dǎo)是將敏感器件安裝在載體上，利用陀螺儀測(cè)量載體的角速率，計(jì)算出姿態(tài)矩陣，確定載體的姿態(tài)和航向信息。特點(diǎn)是成本低，自主性和實(shí)時(shí)性好，但導(dǎo)航定位的誤差會(huì)隨時(shí)間推移而增長，難以長期獨(dú)立工作。另一方面, 對(duì)一般慣性導(dǎo)航系統(tǒng)而言, 加溫和初始對(duì)準(zhǔn)也需要一定的時(shí)間。在這種情況下, 遠(yuǎn)距離、高精度的導(dǎo)航和其他特定條件下的快速反應(yīng)等性能要求, 就成了慣導(dǎo)系統(tǒng)比較難解決的問題。因而純慣導(dǎo)系統(tǒng)對(duì)陀螺儀有很高的要求，一般情況下，陀螺具有0.002～0.01(°)/h的精度才能夠完成導(dǎo)航任務(wù)，所以純慣導(dǎo)系統(tǒng)的成本很高[1]。GPS衛(wèi)星提供的導(dǎo)航電文可以實(shí)時(shí)提供載體的位置、運(yùn)動(dòng)速度等導(dǎo)航信息，具有精度高，成本低的優(yōu)點(diǎn)，但其抗干擾能力差以及更新率比較低，利用GPS測(cè)量出來的位置、速度信息與慣導(dǎo)系統(tǒng)測(cè)量的載體的位置、速度信息的差值作為量測(cè)值，并以此對(duì)慣導(dǎo)系統(tǒng)的誤差進(jìn)行修正，將兩者進(jìn)行結(jié)合，相互取長補(bǔ)短，可以使綜合后的系統(tǒng)導(dǎo)航精度高于2個(gè)系統(tǒng)單獨(dú)工作的精度[2]。國內(nèi)一些研究單位都對(duì)組合導(dǎo)航系統(tǒng)展開了大量有針對(duì)性的研究工作，并取得一定成果，如：北京航天時(shí)代光電科技有限公司針對(duì)某型無人機(jī)的應(yīng)用特點(diǎn)，利用低成本硅微MEMS陀螺儀與加速度計(jì)以及GPS信號(hào)實(shí)現(xiàn)了導(dǎo)航定位功能，組合定位誤差小于15 m，速度誤差小于0.6 m/s[2]。文獻(xiàn)[3]中介紹的組合導(dǎo)航系統(tǒng)在靜態(tài)條件下, 位置精度可達(dá)到5 m( 標(biāo)準(zhǔn)差) , 速度精度優(yōu)于0.1 m/s。

導(dǎo)航原理如圖1所示。

1.2 組合導(dǎo)航系統(tǒng)模型

本文結(jié)合彈載導(dǎo)航系統(tǒng)的整體精度要求，在滿足導(dǎo)航精度的前提下，提出了一個(gè)適合于工程應(yīng)用的簡(jiǎn)化模型[4]。選取了簡(jiǎn)單的位置、速度組合模式，組合系統(tǒng)的數(shù)學(xué)模型分為2部分：一是系統(tǒng)狀態(tài)方程，其狀態(tài)量為慣導(dǎo)系統(tǒng)的誤差，包括導(dǎo)航信息誤差和慣性器件誤差，共18個(gè)狀態(tài)量；二是系統(tǒng)的量測(cè)方程，利用GPS和SINS給出的位置，速度差作為量測(cè)值[5-6]。組合導(dǎo)航系統(tǒng)的狀態(tài)方程：

(1)

圖1 卡爾曼組合導(dǎo)航原理圖Fig.1 Principle of the Kalman integrated navigation

XI=(φe,φn,φu,δve,δvn,δvu,δφ,δλ,δh,εbx,εby，εbz，εrx，εry，εrz，▽ax，▽ay，▽az)，

式中：φe，φn，φu為慣導(dǎo)系統(tǒng)的3個(gè)失準(zhǔn)角；δve，δvn，δvu為東向、北向、天向的速度誤差；δφ，δλ，δh為緯度、經(jīng)度、高度的誤差；εbx,εby,εbz為陀螺儀三維方向的常值漂移;εrx,εry,εrz為一階馬爾可夫過程漂移；▽ax,▽av,▽az為加速度計(jì)在三維方向的一階馬爾可夫過程漂移。

采用位置、速度組合時(shí)，有2組觀測(cè)值，一組是位置觀測(cè)值，即慣導(dǎo)系統(tǒng)給出的經(jīng)緯度和高度信息與GPS給出的相應(yīng)信息的差值，而慣導(dǎo)系統(tǒng)和GPS給出的位置差值作為另一組觀測(cè)值[7]。慣導(dǎo)和GPS的位置信息可分別表示為

(2)

式中：RM是地球參考橢球子午圈上各點(diǎn)的曲率半徑；RN是地球參考橢球卯酉圈上各點(diǎn)的曲率半徑；λt,φt,ht分別為經(jīng)度，緯度和高度值；NE,NN,NU為GPS接收機(jī)沿東、北、天方向的位置誤差，作白噪聲處理。

速度信息可分別表示為

(3)

式中：ME，MN，MU分別對(duì)應(yīng)GPS接收機(jī)沿東、北、天方向的測(cè)速誤差，同樣作白噪聲處理。則慣導(dǎo)系統(tǒng)的位置及速度信息與GPS位置及速度信息的差值為

定義系統(tǒng)的量測(cè)方程為

H(t)X(t)+N(t)，

(4)

式中：

N(t)=(NN，NE，NU，ME，MN，MU)T.

2 Kalman濾波方程[8-11]

將系統(tǒng)的狀態(tài)方程和量測(cè)方程進(jìn)行離散化，得下列公式：

(5)

Kalman濾波方程為

(6)

(7)

(8)

(9)

(10)

3 擴(kuò)展卡爾曼濾波

卡爾曼濾波針對(duì)的是線性系統(tǒng)，可以對(duì)目標(biāo)作出最優(yōu)的估計(jì)，而實(shí)際生活應(yīng)用中，大部分的對(duì)象都是非線性的，針對(duì)非線性系統(tǒng)對(duì)卡爾曼濾波進(jìn)行了改進(jìn)即擴(kuò)展卡爾曼濾波(extended Kalman filter,EKF)，將非線性函數(shù)的局部進(jìn)行線性化，然后再進(jìn)行卡爾曼濾波，完成濾波跟蹤[12-13]。假設(shè)系統(tǒng)的狀態(tài)空間方程如下：

X(k)=f(X(k))+W(k-1),

(11)

Y(k)=h(X(k))+V(k).

(12)

將式(11)，(12)作一階Taylor級(jí)數(shù)展開并略去二次以上項(xiàng)，得到線性化的方程，

(13)

(14)

(15)

狀態(tài)誤差協(xié)方差矩陣為

(16)

EKF的濾波方程為

(17)

(18)

(19)

(20)

設(shè)定狀態(tài)向量和估計(jì)誤差矩陣的初始值X(0)和P(0)，就可以利用EKF的濾波方程進(jìn)行狀態(tài)估計(jì)。本系統(tǒng)將Kalman濾波和EKF這2種算法都進(jìn)行了測(cè)試并進(jìn)行了比較。

4 試驗(yàn)結(jié)果及分析

設(shè)置系統(tǒng)的參數(shù)及初始條件，陀螺白噪聲漂移為50 (°)/h，陀螺一階馬爾可夫漂移為10 (°)/h,加速度計(jì)一階馬爾可夫漂移為5 mg，相關(guān)時(shí)間3 600 s。水平位置誤差為10 m，高度誤差為20 m， 水平速度誤差為0.2 m/s，方位速度誤差為0.3 m/s，經(jīng)緯度誤差為0.001°，高度誤差為10 m，速度誤差為0.5 m/s。

軟件編制和系統(tǒng)調(diào)試之后，選取了下面一段路線，對(duì)該組合導(dǎo)航系統(tǒng)進(jìn)行跑車驗(yàn)證，為比較2種算法， 取了4個(gè)點(diǎn)進(jìn)行了比較， 得到的曲線分別如圖2，3所示。

圖2 組合導(dǎo)航系統(tǒng)航跡圖及測(cè)試點(diǎn)(KF方法)Fig.2 Track of integrated navigation system and test points (method of KF)

圖3 組合導(dǎo)航系統(tǒng)航跡圖及測(cè)試點(diǎn)(EKF方法)Fig.3 Track of integrated navigation system and test points (method of EKF)

從繪制曲線上可以看出，采用KF濾波精度要低于采用EKF。同時(shí)，KF在點(diǎn)4上還出現(xiàn)了發(fā)散的情況，這種發(fā)散點(diǎn)的產(chǎn)生是隨機(jī)的，不能用于導(dǎo)航。在進(jìn)行數(shù)據(jù)分析時(shí)，不應(yīng)該包含這些發(fā)散的點(diǎn)。將這些不合理的數(shù)據(jù)剔除后對(duì)測(cè)量數(shù)據(jù)進(jìn)行了誤差分析計(jì)算，表1，2分別是采用2種濾波方法在緯度和精度方向上所產(chǎn)生的平均誤差，可以看出采用EKF濾波的時(shí)候精度要明顯高于KF。

表1 緯度誤差Table 1 Longitude error

表2 經(jīng)度誤差Table 2 Latitude error

5 結(jié)束語

本文針對(duì)工程應(yīng)用中的SINS/GPS組合導(dǎo)航系統(tǒng)進(jìn)行了研究，建立了一種適合于工程應(yīng)用的導(dǎo)航計(jì)算模型。同時(shí)，采用自適應(yīng)卡爾曼濾波器的改進(jìn)算法擴(kuò)展卡爾曼濾波對(duì)誤差進(jìn)行了估計(jì)，有效克服了純慣導(dǎo)系統(tǒng)誤差發(fā)散的現(xiàn)象，提高了系統(tǒng)的導(dǎo)航精度。通過實(shí)驗(yàn)結(jié)果表明, 此組合導(dǎo)航系統(tǒng)性能穩(wěn)定，具有良好的重復(fù)性和穩(wěn)定性。所建立的模型能夠滿足工程實(shí)際的使用要求。

[1] 蔣慶仙.關(guān)于MEMS慣性傳感器的發(fā)展及在組合導(dǎo)航中的應(yīng)用前景[J]. 測(cè)繪通報(bào),2006(9)：5-8. JIANG Qing-xian. The Development of the MEMS Inertial Sensor and Its Application Perspective in Integrated Navigation [J]. Bulletin of Surveying and Mapping, 2006(9)：5-8.

[2] 蔡春龍,劉翼,劉一薇.MEMS儀表慣性組合導(dǎo)航系統(tǒng)發(fā)展現(xiàn)狀與趨勢(shì)[J]. 中國慣性技術(shù)學(xué)報(bào),2009，17(10)：562-567. CAI Chun-long, LIU Yi, LIU Yi-wei. Status Quo and Trend of Inertial Integrated Navigation System Based on MEMS [J]. Journal of Chinese Inertial Technology, 2009，17(10)：562-567.

[3] 唐康華，吳美平，胡小平.基于自適應(yīng)濾波器的MIMU/ GPS組合系統(tǒng)的研究[J].壓電與聲光，2007，29(4):398-401. TANG Kang-hua, WU Mei-ping, HU Xiao-ping. Study on MIMU/GPS Integrated Navigation System Based on Adaptive Kalman Filter[J]. Piezoelectrics and Acoustooptics,2007,29(4):398-401.

[4] 于潔，王新龍.SINS/GPS緊密組合導(dǎo)航系統(tǒng)仿真研究[J].航空兵器,2008(6)：8-13. YU Jie, WANG Xin-long. Simulation of SINS/GPS Tightly Coupled Integrated Navigation System[J].Aero Weaponry,2008(6):8-13.

[5] QI Hong-hui,MOORE J B. Direct Kalman Filtering Approach for GPS/INS Integration Aerospace and Electronic Systems[J]. IEEE Transactions on Aerospace and Electronic Systems,2002,38(2):687-693.

[6] 曾洪貴，羅大成，王明福.一種SINS/CNS緊耦合組合導(dǎo)航信息融合方法[J].現(xiàn)代防御技術(shù)，2009，37(4):35-37. ZENG Hong-gui,LUO Da-cheng,WANG Ming-fu .Performance Analysis of a New Tightly Coupled SINS/CNS Integration. [J].Modern Defence Technology ,2009，37(4):35-37.

[7] 吳平，雷虎民，劉代軍，等. 基于UKF算法的SINS/GPS全組合導(dǎo)航系統(tǒng)研究 [J].現(xiàn)代防御技術(shù)，2010，38(4)：47-52. WU Ping, LEI Hu-ming, LIU Dai-jun，et al. Fully Integrated SINS/GPS Navigation System Based on Unscented Kalman Filter Algorithm[J].Modern Defence Technology ,2010，38(4)：47-52.

[8] 秦永元，張洪鋮，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M]．西安：西北工業(yè)大學(xué)出版社，1998. QIN Yong-yuan,ZHANG Hong-cheng,WANG Shu-hua.Kalman Filtering and Integrated Navigation Principle[M].Xi’an:Northwestern Polytechnical University Press,1998.

[9] 馬云峰.MSINS/GPS組合導(dǎo)航系統(tǒng)及其數(shù)據(jù)融合技術(shù)研究[D].南京：東南大學(xué),2006. MA Yun-feng.Research on MSINS/GPS Integrated Navigation System and Its Fusion Technology[D].Nanjing:Southeast University，2006.

[10] 高社生，周濤，趙忠.卡爾曼濾波直接法在INS/GPS組合導(dǎo)航系統(tǒng)中的應(yīng)用[J].彈箭與制導(dǎo)學(xué)報(bào).2002,22(1):28-30. GAO She-sheng，ZHOU Tao， ZHAO Zhong.The Application of Kalman Direct Filtering in INS/GPS Integrated Navigation System[J].Journal of Projectiles Rockets Missiles an Guidance，2002，22(1):28-30.

[11] 黃麗斌，周百令.低成本MINS/GPS組合導(dǎo)航中Kalman濾波算法的綜合應(yīng)用研究[J].中國慣性技術(shù)學(xué)報(bào)，2005,13(5)：16-19. HUANG Li-bin,ZHOU Bai-ling. Synthetical Application on Kalman Filtering Algorithms of Low Cost MINS/GPS Navigation System[J]. Journal of Chinese Inertial Technology,2005,13(5)：16-19.

[12] 范煒，李勇. 近似二階擴(kuò)展卡爾曼濾波方法研究[J].空間控制技術(shù)與應(yīng)用，2009,35(1)：30-35. FAN Wei, LI Yong. A Method of Approximate Second-Order Extended Kalman Filter[J]. Aerospace Control and Application , 2009,35(1)：30-35.

[13] 鄒衛(wèi)軍，薄煜明. 差分線性化EKF 濾波方法研究[J].計(jì)算機(jī)工程與應(yīng)用，2009,45(9)：64-66. ZOU Wei-jun，BO Yu-ming. A Research of EKF Based on Differential Linearization [J]. Computer Engineering and Applications , 2009,45(9)：64-66.

Low-Cost SINS/GPS Integrated Navigation System

ZHANG Wena，SUN Rui-shengb

(Nanjing University of Science and Technology, a.Taizhou Institude,Jiangsu Taizhou 225300,China; b.School of Power Engineering,Jiangsu Nanjing 210094, China)

Taking the navigation system of the low cost guided missile project as the research object, the low cost integrated navigation system is investigated. The inertial navigation system features small size, low power and so on. But its long-term stability is poor because of position error accumulating with the growing time. Now the most widely used satellite navigation is the global navigation satellite system. Long-term accuracy of GPS (global positioning system) is good but is subject to the American government. Its dynamic positioning performance is poor and the rate of updating is low. SINS (strapdown inertial navigation system)/GPS integrated navigation system is made by using the advantages of two systems, which is suitable for engineering application. The integrated navigation is implemented by adopting Kalman filtering algorithm. In the end, the result of the vehicle experiment indicates that the scheme can improve the precision of the navigation system and has high engineering practice.

strapdown inertial navigation system(SINS)； global positioning system(GPS)；integrated navigation system；Kalman filter

2013-11-30；

2014-07-05

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

張文(1982-)，女, 江蘇泰州人。講師,碩士,主要從事組合導(dǎo)航,嵌入式系統(tǒng)等方面的研究。

通信地址：225300 江蘇省泰州市梅蘭東路8#南京理工大學(xué)泰州科技學(xué)院 E-mail：18814219@qq.com

10.3969/j.issn.1009-086x.2015.03.011

V249.32+8;P228.4

A

1009-086X(2015)-03-0060-05