劉濤,孟凡磊，李冀鑫

(1. 海軍航空工程學(xué)院，山東 煙臺(tái) 264001； 2. 海軍航空兵學(xué)院，遼寧 葫蘆島 125001)

一種新的車載SINS/DR組合定位定向算法*

劉濤1,孟凡磊1，李冀鑫2

(1. 海軍航空工程學(xué)院，山東 煙臺(tái) 264001； 2. 海軍航空兵學(xué)院，遼寧 葫蘆島 125001)

針對(duì)軍用車輛衛(wèi)星定位信號(hào)缺失的問題，推導(dǎo)了三維條件下的無陀螺航位推算算法，在分別對(duì)航位推算和捷聯(lián)慣導(dǎo)算法系統(tǒng)誤差分析的基礎(chǔ)上，應(yīng)用kalman濾波器設(shè)計(jì)了一種新的SINS/DR組合算法，仿真結(jié)果顯示，采用組合算法，系統(tǒng)三維定位精度分別提高37.8%、49.7%和35.1%。

組合定位；航位推算；三維條件；卡爾曼濾波

0 引言

復(fù)雜地形條件下，軍用車輛的衛(wèi)星定位系統(tǒng)信號(hào)易受遮擋和干擾，需要自主性高的定位系統(tǒng)，其中慣導(dǎo)系統(tǒng)技術(shù)成熟、應(yīng)用較廣，但存在導(dǎo)航誤差隨時(shí)間增長(zhǎng)不斷積累發(fā)散的缺點(diǎn)[1-2]。航位推算算法是一種成本低、可靠性高的自主式定位方式，是車載定位導(dǎo)航系統(tǒng)的重要組成部分，但現(xiàn)有的研究基本上都將車輛運(yùn)行軌跡限定在二維平面內(nèi)[3-7]，或是借助平臺(tái)慣導(dǎo)的姿態(tài)信息[8-9]。本文在提出基于差速里程儀的獨(dú)立三維航位推算算法的基礎(chǔ)上，提出一種新的SINS/DR組合定位定向算法，克服了共用姿態(tài)信息帶來的誤差二次累積放大的缺點(diǎn)，提高了系統(tǒng)導(dǎo)航精度。

1 三維航位推算算法

設(shè)車輛在三維地形條件下行駛，采樣周期T內(nèi)，高度計(jì)測(cè)量載車質(zhì)心處的高度變化量Δh，水平儀測(cè)量載車的滾轉(zhuǎn)角γ，差速里程儀測(cè)量載車的單位時(shí)間左右車輪的行駛距離SR和SL：

SR=KbpTR，

(1)

式中：Kb為標(biāo)定系數(shù)；pTR為右輪里程儀的輸出計(jì)數(shù)脈沖；SL同理可得。

車輛在非水平面內(nèi)作轉(zhuǎn)動(dòng)(如圖1)，偏航角增量設(shè)為Δψ：

(2)

式中：K為左右車輪輪距；K′為其在水平面內(nèi)的投影；θ為車輛俯仰角。

(3)

得到了每時(shí)刻的姿態(tài)角和車輛運(yùn)行距離，就可以根據(jù)導(dǎo)航原理推算出車輛位置。

圖1 三維航位推算算法示意圖Fig.1 Sketch map of 3D dead reckoning (DR) algorithm

2 三維DR系統(tǒng)誤差分析

選取地理坐標(biāo)系(東北天ENU)為導(dǎo)航坐標(biāo)系，記為n系，載車坐標(biāo)系取(車右一車前一車上)，記為b系。

載車坐標(biāo)系下差速里程計(jì)的增量輸出為

式中：

Sd=(SdL+SdR)/2，

則導(dǎo)航坐標(biāo)系下里程計(jì)增量輸出的表達(dá)式為[10]

(4)

里程儀測(cè)量誤差的主要來源是等效標(biāo)度因素誤差δKd，因此得到測(cè)量實(shí)際里程值表達(dá)式為

(5)

設(shè)載車獲取的姿態(tài)誤差角為Δθ，Δψ，Δγ，都為小角度，則里程計(jì)的實(shí)際增量輸出在導(dǎo)航坐標(biāo)系下的表達(dá)式為

(6)

因此得到：

(7)

將式(4),(6)代入展開并忽略其中的高階小量，得

(8)

由式(8)可以得到里程計(jì)水平方向的位置誤差主要由載車的姿態(tài)角誤差Δθ，Δψ和里程計(jì)標(biāo)度因數(shù)誤差δKd影響。

航位推算定位誤差方程為

(9)

式中：

由

(10)

代入式(9)得到

(11)

航位推算姿態(tài)誤差方程為

(12)

式中：

得到

(13)

考慮：

(1) 位置誤差一般在幾十米之內(nèi)，因此姿態(tài)誤差方程中的Md1δpd項(xiàng)可忽略；

(3) 里程儀刻度系數(shù)誤差δKd一般小于1%，姿態(tài)誤差方程中可忽略。

因此，姿態(tài)誤差方程可簡(jiǎn)化為

(14)

而同樣忽略Md2δpd位置誤差方程可簡(jiǎn)化為

(15)

3 捷聯(lián)慣導(dǎo)系統(tǒng)誤差[11]

慣導(dǎo)系統(tǒng)姿態(tài)誤差方程為

(16)

式中：

速度誤差方程為

(17)

式中：

位置誤差方程為

(18)

式中：

4 三維SINS/DR組合導(dǎo)航算法

選取以下狀態(tài)參數(shù)作為SINS/DR組合導(dǎo)航的系統(tǒng)狀態(tài)向量：

X=(φ，δv，δp，ε，▽，δpd，δKd)T.

由慣導(dǎo)系統(tǒng)和航位推算系統(tǒng)的誤差方程得到狀態(tài)方程為

(19)

W為系統(tǒng)狀態(tài)噪聲。

得到觀測(cè)方程為

z=HX+V,

(20)

式中：H=(03×6，I3×3，03×6，-I3×3，03×1)。

然后采用Kalman濾波器[12]輸出慣導(dǎo)系統(tǒng)校正后的狀態(tài)參數(shù)估計(jì)值：

狀態(tài)一步預(yù)測(cè)

(21)

狀態(tài)估計(jì)

(22)

濾波增益矩陣

(23)

一步預(yù)測(cè)誤差方差陣

(24)

估計(jì)誤差方差陣

Pk=(I-KkHk)Pk|k-1.

(25)

5 仿真實(shí)驗(yàn)

設(shè)定載車的運(yùn)動(dòng)軌跡為：載車航向角為45°，俯仰角為5°，從靜止?fàn)顟B(tài)以1 m/s2的加速度開始做直線加速運(yùn)動(dòng)，速度達(dá)到10 m/s后做勻速運(yùn)動(dòng)，行進(jìn)1 h。載車真實(shí)軌跡如圖2。運(yùn)動(dòng)軌跡包括每秒的速度、加速度、里程、航向角、位置信息等。

圖2 載車真實(shí)運(yùn)動(dòng)軌跡Fig.2 Real trajectory of the vehicle

系統(tǒng)相關(guān)設(shè)置為：

(1) 起始點(diǎn)緯度L=30°，經(jīng)度λ=117°；

(2) 里程儀的標(biāo)度因數(shù)誤差為δKd=0.2%，高度計(jì)誤差為0.3%；

(3) 初始對(duì)準(zhǔn)后，慣導(dǎo)系統(tǒng)初始姿態(tài)誤差為：偏航誤差角 δψ=2′，俯仰誤差角δθ=1′，滾轉(zhuǎn)誤差角δγ=1′；

(4) 陀螺儀常值漂移為εx=εy=εz=0.01(°)/h，加速度計(jì)常值零偏為5×10-5g。

圖3為捷聯(lián)慣導(dǎo)系統(tǒng)、三維航位推算系統(tǒng)的定位誤差對(duì)比圖，圖4為組合導(dǎo)航方法與航位推算的定位誤差對(duì)比圖，表1為車輛運(yùn)行50 min后組合導(dǎo)航方法與航位推算的定位誤差數(shù)據(jù)對(duì)比。

圖3 慣導(dǎo)系統(tǒng)位置誤差與航位推算位置誤差對(duì)比Fig.3 Comparison between position error of inertial navigation system and position error of dead reckoning

表1 算法定位誤差數(shù)據(jù)對(duì)比
Table 1 Comparison between position errors of algorithms

項(xiàng)目航位推算算法組合導(dǎo)航算法東向位置誤差/m29．117．9北向位置誤差/m48．224．2高度誤差/m14．59．4

由仿真結(jié)果圖2可以看出，慣導(dǎo)系統(tǒng)前100 s定位誤差較小，后來迅速變大、發(fā)散，而航位推算系統(tǒng)定位誤差隨時(shí)間緩慢增大，這主要是由于航位推算的定位誤差主要來自傳感器的測(cè)量誤差，誤差累計(jì)較小。

圖4 航位推算與組合導(dǎo)航算法的定位誤差對(duì)比圖Fig.4 Comparison between dead reckoning and combined navigation algorithms

由仿真結(jié)果圖4和表1可以看出，相比較三維航位推算算法，采用組合導(dǎo)航算法能夠有效減小系統(tǒng)定位誤差，經(jīng)過50 min后，東向定位精度提高了37.8%，北向定位精度提高了49.7%，高度定位精度提高了35.1%。

由于系統(tǒng)模型參數(shù)與噪聲統(tǒng)計(jì)模型不確定性，狀態(tài)估計(jì)仍呈發(fā)散狀態(tài)，導(dǎo)致補(bǔ)償后定位誤差的發(fā)散，可采取相應(yīng)的收斂措施，比如自適應(yīng)濾波或強(qiáng)收斂規(guī)則，但這可能會(huì)損失系統(tǒng)的穩(wěn)定性。

6 結(jié)束語

車載慣導(dǎo)定位誤差隨時(shí)間迅速發(fā)散，航位推算算法定位誤差隨時(shí)間增加緩慢，兩者具有互補(bǔ)性。本文在設(shè)計(jì)三維獨(dú)立航位推算算法的基礎(chǔ)上，借助kalman濾波技術(shù)提出了一種新的SINS/DR組合定位算法，有效降低了系統(tǒng)定位誤差，提高了系統(tǒng)精度，具有一定的工程指導(dǎo)意義。

[1] 吳富梅，秦顯平，唐穎哲．低成本車載GPS/INS組合導(dǎo)航速度更新算法[J]．測(cè)繪科學(xué)技術(shù)學(xué)報(bào)，2010, 27(4)：243-246. WU Fu-mei，QIN Xian-ping，TANG Ying-zhe．Velocity Update Algorithms of Low-Cost GPS/INS Integrated Navigation in Land Vehicle System [J]．Journal of Geomatics Science and Technology，2010, 27(4)：243-246.

[2] SHIN E. Accuracy Improvement of Low Cost INS/GPS for Land Application [D]. Calgary, Canada: University of Calgary, 2001.

[3] 王忠，龍宇．航位推算系統(tǒng)非線性過程處理新方法研究[J]．電子科技大學(xué)學(xué)報(bào),2010, 39(3)：351-354. WANG Zhong, LONG Yu．Study of Nonlinear Process Processing Method for DR Navigation System [J]．Journal of University of Electronic Science and Technology of China,2010, 39(3)：351-354.

[4] Seong Yun Cho, Wan Sik Choi．Robust Positioning Technique in Low-Cost DR/GPS for Land Navigation[J]．IEEE Transactions on Instrumentation and Measurement，2006，55(4)：1132-1142.

[5] 賈繼超，吳訓(xùn)忠，夏家和,等．一種車輛航位推算改進(jìn)方法[J]．中國(guó)慣性技術(shù)學(xué)部．2009，17(4)：453-455. JIA Ji-chao, WU Xun-zhong, XIA Jia-he．Improved Method for Vehicular Dead Reckoning[J]．Journal of Chinese Inertial Technology．2009，17(4)：453-455.

[6] 王東升，蔡俊華，林曉彬,等．一種提高車載定位定向系統(tǒng)定位精度的方法[J]．中國(guó)慣性技術(shù)學(xué)報(bào),2012，20(2)：187-191. WANG Dong-sheng,CAI Jun-hua,LIN Xiao-bin．Method of Improving the Positioning Precision About Inertial Positioning and Heading System for Land Vehicle [J]．Journal of Chinese Inertial Technology,2012，20(2)：187-191.

[7] 楊宜康，林勇，黃永宣,等．車輛無陀螺航位推算系統(tǒng)的導(dǎo)航原理及其算法[J]．西安交通大學(xué)學(xué)報(bào),2001，35(2)：1284-1287. YANG Yi-kang, LIN Yong,HUANG Yong-xuan．Navigation Principle and Algorithm for Vehicular Dead Reckoning System without Gyroscope [J]．Journal of Xi’an Jiaotong University,2001，35(2)：1284-1287.

[8] 賈繼超，李歲勞，趙成志,等．高精度車載航位推算誤差補(bǔ)償技術(shù)研究[J]．儀器儀表學(xué)報(bào)，2010，31(10) ：2198-2202. JIA Ji-chao，LI Sui-lao，ZHAO Cheng-zhi，et al．Research on Temperature Compensation on Vehicular High Precision Dead Reckoning System[J]． Chinese Journal of Scientific Instrument，2010，31(10) ：2198-2202.

[9] 嚴(yán)恭敏．車載自主定位定向系統(tǒng)研究[D]．西安：西北工業(yè)大學(xué),2006. YAN Gong-min．Research on the Autonomous Positioning and Heading System for Land Vehicle[D]．Xi’an：Northwest industrial University，2006.

[10] 賈繼超，吳訓(xùn)忠，夏家和．一種車輛航位推算改進(jìn)方法[J]．中國(guó)慣性技術(shù)學(xué)報(bào)，2009，17(4) ：331-333. JIA Ji-chao，WU Xun-zhong，XIA Jia-he． An Improved Method for Vehicular Dead Reckoning[J]．Journal of Chinese Inertial Technology， 2009，17(4) ：331-333．

[11] 張坤，彭雪明．車載定位定向技術(shù)研究[J]．現(xiàn)代防御技術(shù),2006，34(4) ：61-65. ZHANG Kun，PENG Xue-min． Study on Vehicular Orienting and Locating Technology[J]．Modern Defense Technology，2006，34(4) ：61-65.

[12] 付夢(mèng)印，鄧志紅，張繼偉．Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M]．北京：科學(xué)技術(shù)出版社,2003：93-101. FU Meng-yin，DENG Zhi-hong，ZHANG Ji-wei．Theory of Kalman Filter and Implication in Navigation System[M]．Beijing：Publishing House of Science Technique,2003：93-101.

New Combined Location Algorithm of SINS/DR for Land Vehicle

LIU Tao, MENG Fan-lei1，LI Ji-xin2

(1. Naval Aeronautical and Astronautical University, Shandong Yantai 264001，China;2. Naval Flying Academy,Liaoning Huludao 125001,China)

To solve the vehicle positioning problem caused by signal missing of satellite system, three-dimensional dead reckoning (DR) algorithm is figured out based on gyroscope-free idea. Based on the analysis of system error of dead reckoning and strap-down inertial navigation system (SINS), a new combined positioning algorithm of SINS and DR is given by using Kalman Filter. The simulation result shows that the new combined algorithm can improve the location precision by about 37.8%, 49.7% and 35.1%.

combined algorithm of location; dead reckoning ; three-dimensional; Kalman filter

2014-08-15；

2014-09-20

教育部新世紀(jì)優(yōu)秀人才支持計(jì)劃(NCET-04-1001)；學(xué)院青年科研基金資助項(xiàng)目(HYQN2012-05)

劉濤(1982-)，男，江蘇灌云人。講師，博士，研究方向?yàn)楸靼l(fā)射技術(shù)、導(dǎo)航技術(shù)。

通信地址：264001 山東省煙臺(tái)市二馬路188號(hào)海軍航空工程學(xué)院104教研室 E-mail：haitianlt@163.com

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

U666.11;TP301.6

A

1009-086X(2015)-04-0080-06