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

劉濤,孟凡磊，李冀鑫

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

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

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

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

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

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

0 引言

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

1 三維航位推算算法

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

SR=KbpTR，

(1)

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

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

(2)

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

(3)

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

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

2 三維DR系統誤差分析

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

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

式中：

Sd=(SdL+SdR)/2，

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

(4)

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

(5)

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

(6)

因此得到：

(7)

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

(8)

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

航位推算定位誤差方程為

(9)

式中：

由

(10)

代入式(9)得到

(11)

航位推算姿態誤差方程為

(12)

式中：

得到

(13)

考慮：

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

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

因此，姿態誤差方程可簡化為

(14)

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

(15)

3 捷聯慣導系統誤差[11]

慣導系統姿態誤差方程為

(16)

式中：

速度誤差方程為

(17)

式中：

位置誤差方程為

(18)

式中：

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

選取以下狀態參數作為SINS/DR組合導航的系統狀態向量：

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

由慣導系統和航位推算系統的誤差方程得到狀態方程為

(19)

W為系統狀態噪聲。

得到觀測方程為

z=HX+V,

(20)

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

然后采用Kalman濾波器[12]輸出慣導系統校正后的狀態參數估計值：

狀態一步預測

(21)

狀態估計

(22)

濾波增益矩陣

(23)

一步預測誤差方差陣

(24)

估計誤差方差陣

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

(25)

5 仿真實驗

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

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

系統相關設置為：

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

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

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

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

圖3為捷聯慣導系統、三維航位推算系統的定位誤差對比圖，圖4為組合導航方法與航位推算的定位誤差對比圖，表1為車輛運行50 min后組合導航方法與航位推算的定位誤差數據對比。

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

表1 算法定位誤差數據對比
Table 1 Comparison between position errors of algorithms

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

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

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

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

由于系統模型參數與噪聲統計模型不確定性，狀態估計仍呈發散狀態，導致補償后定位誤差的發散，可采取相應的收斂措施，比如自適應濾波或強收斂規則，但這可能會損失系統的穩定性。

6 結束語

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

[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] 王忠，龍宇．航位推算系統非線性過程處理新方法研究[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] 王東升，蔡俊華，林曉彬,等．一種提高車載定位定向系統定位精度的方法[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] 楊宜康，林勇，黃永宣,等．車輛無陀螺航位推算系統的導航原理及其算法[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] 嚴恭敏．車載自主定位定向系統研究[D]．西安：西北工業大學,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]．現代防御技術,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濾波理論及其在導航系統中的應用[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

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

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

通信地址：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