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

劉濤,孟凡磊，李冀鑫

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

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

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

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

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

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

0 引言

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

1 三維航位推算算法

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

SR=KbpTR，

(1)

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

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

(2)

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

(3)

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

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

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

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

載車坐標系下差速里程計的增量輸出為

式中：

Sd=(SdL+SdR)/2，

則導航坐標系下里程計增量輸出的表達式為[10]

(4)

里程儀測量誤差的主要來源是等效標度因素誤差δKd，因此得到測量實際里程值表達式為

(5)

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

(6)

因此得到：

(7)

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

(8)

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

航位推算定位誤差方程為

(9)

式中：

由

(10)

代入式(9)得到

(11)

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

(12)

式中：

得到

(13)

考慮：

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

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

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

(14)

而同樣忽略Md2δpd位置誤差方程可簡化為

(15)

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

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

(16)

式中：

速度誤差方程為

(17)

式中：

位置誤差方程為

(18)

式中：

4 三維SINS/DR組合導航算法

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

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

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

(19)

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

得到觀測方程為

z=HX+V,

(20)

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

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

狀態(tài)一步預測

(21)

狀態(tài)估計

(22)

濾波增益矩陣

(23)

一步預測誤差方差陣

(24)

估計誤差方差陣

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

(25)

5 仿真實驗

設定載車的運動軌跡為：載車航向角為45°，俯仰角為5°，從靜止狀態(tài)以1 m/s2的加速度開始做直線加速運動，速度達到10 m/s后做勻速運動，行進1 h。載車真實軌跡如圖2。運動軌跡包括每秒的速度、加速度、里程、航向角、位置信息等。

圖2 載車真實運動軌跡Fig.2 Real trajectory of the vehicle

系統(tǒng)相關設置為：

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

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

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

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

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

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

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

項目航位推算算法組合導航算法東向位置誤差/m29．117．9北向位置誤差/m48．224．2高度誤差/m14．59．4

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

圖4 航位推算與組合導航算法的定位誤差對比圖Fig.4 Comparison between dead reckoning and combined navigation algorithms

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

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

6 結束語

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

[1] 吳富梅，秦顯平，唐穎哲．低成本車載GPS/INS組合導航速度更新算法[J]．測繪科學技術學報，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]．電子科技大學學報,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] 賈繼超，吳訓忠，夏家和,等．一種車輛航位推算改進方法[J]．中國慣性技術學部．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]．中國慣性技術學報,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)的導航原理及其算法[J]．西安交通大學學報,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] 賈繼超，李歲勞，趙成志,等．高精度車載航位推算誤差補償技術研究[J]．儀器儀表學報，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] 嚴恭敏．車載自主定位定向系統(tǒng)研究[D]．西安：西北工業(yè)大學,2006. YAN Gong-min．Research on the Autonomous Positioning and Heading System for Land Vehicle[D]．Xi’an：Northwest industrial University，2006.

[10] 賈繼超，吳訓忠，夏家和．一種車輛航位推算改進方法[J]．中國慣性技術學報，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] 張坤，彭雪明．車載定位定向技術研究[J]．現(xiàn)代防御技術,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] 付夢印，鄧志紅，張繼偉．Kalman濾波理論及其在導航系統(tǒng)中的應用[M]．北京：科學技術出版社,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

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

劉濤(1982-)，男，江蘇灌云人。講師，博士，研究方向為兵器發(fā)射技術、導航技術。

通信地址：264001 山東省煙臺市二馬路188號海軍航空工程學院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