王麗芬，楊功流，單友東，喬立偉，姜 睿

( 1. 北京航空航天大學(xué) 儀器科學(xué)與光電工程學(xué)院，北京 100191；2. 慣性技術(shù)國防重點實驗室，北京 100191；3. 天津航海儀器研究所，天津 300131 )

車載定位定向系統(tǒng)誤差在線補償方法

王麗芬1,2，楊功流1,2，單友東3，喬立偉3，姜 睿1,2

( 1. 北京航空航天大學(xué) 儀器科學(xué)與光電工程學(xué)院，北京 100191；2. 慣性技術(shù)國防重點實驗室，北京 100191；3. 天津航海儀器研究所，天津 300131 )

針對里程計輸出為位置增量，傳統(tǒng)SINS/里程計組合方法會對位置增量進行微分或積分從而會產(chǎn)生一定噪聲或誤差，且里程計標定殘差影響高精度定位定向系統(tǒng)性能的問題，提出了一種基于位置增量組合的定位定向系統(tǒng)誤差在線估計和補償方法。該方法除了將慣性器件誤差列入狀態(tài)量外，還將里程計安裝誤差殘差、標度因數(shù)誤差等納入狀態(tài)量進行實時估計，將慣性導(dǎo)航系統(tǒng)輸出的速度增量進行積分獲得位置增量，同時與里程計輸出位置增量進行比對，構(gòu)建量測方程。設(shè)計跑車試驗對該方法進行了驗證，結(jié)果表明該方法可以有效估計慣性器件誤差和里程計參數(shù)誤差，同時相比速度組合方法而言，定位精度可提高40%。

定位定向；里程計；安裝誤差；標度因數(shù)誤差

車載定位定向系統(tǒng)是用于陸基導(dǎo)彈、制導(dǎo)炮彈等武器發(fā)射車以及各種偵察車的基準設(shè)備，主要為車載設(shè)備提供準確的位置和方位信息。車載定位定向系統(tǒng)中的捷聯(lián)導(dǎo)航系統(tǒng)（SINS）和里程計（OD）這兩種關(guān)鍵設(shè)備。它們相對于GPS導(dǎo)航[1-2]而言，均具有隱蔽性好，不受外界電磁、氣候干擾，不受時間、地點限制等諸多優(yōu)點；同時，車載定位定向系統(tǒng)中里程計輸出的位置增量，不會像慣性導(dǎo)航系統(tǒng)那樣出現(xiàn)誤差隨時間推移而增大的現(xiàn)象。因此利用里程計輸出對慣性導(dǎo)航系統(tǒng)的誤差進行抑制，可以得到較高的導(dǎo)航精度，其作為軍事目的的應(yīng)用顯得尤為重要。

然而里程計與SINS之間存在一定的安裝誤差，且里程計輸出為脈沖當(dāng)量，需通過刻度因數(shù)轉(zhuǎn)換為位置增量或速度信息。雖然應(yīng)用之前會對安裝誤差及標度因數(shù)進行標定[3]，但由于標定精度的限制，以及后續(xù)應(yīng)用過程中車輛輪胎的變化，殘余的安裝誤差及標度因數(shù)誤差會對定位定向精度產(chǎn)生一定影響[4]。同時傳統(tǒng)的航位推算技術(shù)將里程計的位置增量轉(zhuǎn)換為速度信息與慣導(dǎo)系統(tǒng)的速度進行濾波處理，微分計算必然會引入一定的噪聲。

本文研究了一種定位定向系統(tǒng)誤差在線補償方法，該方法將里程計殘余安裝誤差、標定因數(shù)誤差納入狀態(tài)方程予以在線估計，并利用位置增量組合方式實現(xiàn)SINS/OD組合導(dǎo)航。通過實際跑車試驗，有效驗證了該方法的可行性，提高了定位定向系統(tǒng)的精度。

1 SINS/里程計組合導(dǎo)航誤差模型

定義導(dǎo)航坐標系(n系)為東北天坐標系，SINS載體坐標系為b系；里程計坐標系與車體坐標系(m系)重合；m系相對于b系存在一定的偏角，即通常所說的里程計的安裝誤差角α=[αθαγαφ]，其中αθ為俯仰安裝誤差，αγ為橫滾安裝誤差，αφ為航向安裝誤差。

假設(shè)里程計的安裝誤差和標度因數(shù)都經(jīng)過了準確標定且應(yīng)用過程中無任何變化，設(shè)里程計在一個采樣周期內(nèi)輸出脈沖增量為PD，里程計標度因數(shù)為KD，則里程增量為：

里程計輸出在車體坐標系上的投影為：

則里程計輸出里程增量在導(dǎo)航坐標系上的投影為：

考慮里程計的殘余安裝誤差及標度因數(shù)誤差時，進一步推導(dǎo)SINS/里程計組合導(dǎo)航誤差模型。

1.1 SINS誤差模型

由于SINS的誤差模型已經(jīng)相當(dāng)成熟[5-6]，這里不再推導(dǎo)，引用如下：

式中：

fn： 車輛運動加速度在導(dǎo)航坐標系上的投影；

Vn： 車輛運動速度，其中VE、VN、VU分別為該速度在導(dǎo)航坐標系上的分量；

L、λ、h： 緯度、經(jīng)度及高度；

RM、RN： 子午圈曲率半徑及卯酉圈曲率半徑；

φins： 慣導(dǎo)系統(tǒng)姿態(tài)失準角；

▽b： 加速度計誤差在慣導(dǎo)載體坐標系上的投影。

1.2 航位推算誤差模型

設(shè)里程計殘余安裝誤差分別為δθα、δγα、δφα，標度因數(shù)誤差為δKD，則里程計輸出在SINS坐標系上的投影為：

由式(5)推導(dǎo)得：

由于里程計安裝誤差均為小角度，因此sinαi≈0，cosαi≈1 (i =α,γ,φ)，則式(6)簡化為：

從式(7)可以看出，里程計輸出位置增量誤差與橫滾安裝誤差角無關(guān)。里程計輸出位置增量在導(dǎo)航坐標系上的投影為：

式中：mφ為航位推算姿態(tài)失準角矢量。

將式(7)代入式(8)推導(dǎo)得：

則里程計位置增量誤差為：

與慣導(dǎo)系統(tǒng)位置誤差方程相似，航位推算的位置誤差方程如下：

式中：T為里程計采樣周期。將式(10)代入式(11)得：

式中：

航位推算中姿態(tài)更新算法與慣導(dǎo)解算中的一樣，只是利用的信息不一樣，因此將SINS姿態(tài)誤差方程中的參數(shù)用相應(yīng)的參數(shù)代替，即可得到航位推算姿態(tài)誤差方程，即：

式中：

將式(10)代入式(14)得：

式中：

將式(15)代入式(13)可得航位推算姿態(tài)誤差方程：

式中：

2 定位定向系統(tǒng)濾波算法

為了實現(xiàn)對各種誤差的在線估計及補償，需分別建立狀態(tài)方程和量測方程，利用卡爾曼濾波進行估計。

2.1 狀態(tài)方程

由于研究對象為陸用車載定位定向系統(tǒng)，因此位置信息和速度信息中的高度通道暫不考慮，即SINS/ OD組合定位定向的狀態(tài)量（共21維）主要包括：SINS的經(jīng)緯度誤差δλins、δLins，水平速度誤差δVE_ins、δVN_ins，姿態(tài)誤差φx_ins、φy_ins、φz_ins，陀螺漂移εx、εy、εz，加速度計零偏▽x、▽y、▽z航位推算經(jīng)緯度誤差δλm、δLm，姿態(tài)誤差φmx、φmy、φmz；里程計安裝誤差殘差δαθ、δαφ，標度因數(shù)誤差δKD等。

假設(shè)慣性器件誤差為零偏誤差與白噪聲之和，零偏誤差為隨機常值；假設(shè)里程計安裝誤差殘差及標度因數(shù)誤差在一次上電后保持不變，即：

SINS/OD 組合定位定向系統(tǒng)的狀態(tài)量為：

根據(jù)式(4)(10)(12)(16)(17)可得到定位定向的狀態(tài)方程為：

式中：F為系統(tǒng)狀態(tài)矩陣，G為系統(tǒng)噪聲驅(qū)動陣，W為系統(tǒng)白噪聲。這里W=[Wεx,Wεy,Wεz,W▽x,W▽y,W▽z]，其中Wεx、Wεy、Wεz為陀螺白噪聲，W▽x、W▽y、W▽z為加速度計白噪聲。

2.2 量測方程

里程計輸出信息為位置增量，如果將其微分，所得速度信息的噪聲必然增大，如果將其積分，得到位置信息[7-8]的誤差會進行累積，且位置組合對于各種誤差的估計速度較慢。由于里程計位置增量信息精度相對較高，利用該信息與慣導(dǎo)解算的位置增量信息進行對比，構(gòu)建量測方程，可以在一定程度上提高定位定向精度。

一般情況下，慣導(dǎo)解算參數(shù)的更新率是里程更新率的N倍，假設(shè)tn-1到tn時間內(nèi)，里程計輸出更新一次，則量測方程如下：

對于離散解算方式而言，式(19)推導(dǎo)為：

假設(shè)短時間內(nèi)慣導(dǎo)系統(tǒng)速度誤差幾乎不變，則式(20)可進一步化簡為：

則觀測矩陣為：

3 試驗驗證

為了驗證本文所述方法的有效性，設(shè)計跑車試驗，分析利用里程計和SINS的速度信息組合和利用位置增量組合兩種方法的定位定向精度。

跑車試驗中相關(guān)參數(shù)如下：①慣性器件精度為：陀螺零漂0.02 (°)/h，加速度計零偏100 μg。②跑車起始點為：經(jīng)度116.3437°，緯度40.0343°；行車里程為34.52 km，行車路線如圖1所示；跑車試驗持續(xù)1小時，其中前10 min車輛靜止。③試驗中以GPS位置信息作為基準信息，GPS定位精度約為3 m。

將位置增量組合方式下同時考慮里程計相關(guān)誤差的方法標記為label1，將速度組合方式下同時考慮里程計相關(guān)誤差的方法標記為label2。試驗結(jié)果表明，位置增量組合方法和速度組合方法均可以實現(xiàn)對水平陀螺零漂、水平加速度計零偏以及里程計安裝誤差殘差的估計，但跑車時間內(nèi)里程計標度因數(shù)誤差收斂不明顯，由于真實誤差值未知，因此主要通過最后導(dǎo)航結(jié)果評價誤差估計精度。

利用上述兩種處理方法，以及未考慮里程計參數(shù)誤差的位置增量組合方法（標記為label3）對跑車數(shù)據(jù)處理得到的結(jié)果如圖2～5所示。

對比圖2和圖4可以看出，考慮里程計參數(shù)誤差時，定位精度會稍有提高。這是因為本試驗是在基于標準點（位置精度高于GPS）對里程計參數(shù)標定基礎(chǔ)上開展的，里程計標定精度相對較高，因此組合導(dǎo)航位置誤差區(qū)別不明顯，但是兩圖的對比結(jié)果還是表明了里程計參數(shù)誤差對定位精度的影響。同時里程計安裝誤差殘差估計值小于1 mrad，標度因數(shù)誤差未明顯收斂，在一定程度上也與里程計標定精度高相關(guān)。

對比圖2和圖3可以看出，跑車時間段內(nèi)，在考慮里程計標定殘差的情況下，傳統(tǒng)速度組合導(dǎo)航方法的經(jīng)度定位誤差最大約為50 m，而位置增量組合方法經(jīng)度誤差曲線的峰峰值約為30 m，兩種方法的緯度誤差曲線的峰峰值相當(dāng)，約為25 m。綜合分析位置增量組合導(dǎo)航相對于速度組合導(dǎo)航方法而言，定位精度提高了40%，且位置增量組合方法的定位誤差能保持在一定范圍內(nèi)，發(fā)散趨勢不明顯。

同時從圖5可以看出，三種組合方法的定向精度在最后時刻趨于一致，但從變化過程來看，位置增量組合方法航向誤差變化幅度相對較小，可以在一定時間內(nèi)保證方位精度。

圖1 車輛行駛軌跡Fig.1 Trajectory of the vehicle

圖2 label1位置誤差曲線Fig.2 Position error curves of label1

圖3 label2位置誤差曲線Fig.3 Position error curves of label2

圖4 label3位置誤差曲線Fig.4 Position error curves of label3

圖5 方位角誤差曲線局部放大圖Fig.5 Partially enlarged view of the azimuth errors

4 結(jié) 論

本文提出了一種基于SINS/里程計位置增量組合的定位定向誤差在線估計方法，并通過實際跑車試驗對該方法與其他組合方法進行了對比，所得到的結(jié)論如下：①采用位置增量組合方法實現(xiàn)SINS/里程計組合導(dǎo)航時，將里程計安裝誤差殘差和標度因數(shù)誤差擴維為狀態(tài)量進行估計的誤差處理方法，相對于不對上述誤差進行任何處理的組合導(dǎo)航方法，可以有效抑制位置誤差和方位誤差的發(fā)散趨勢，提高定位定向精度。②位置增量組合方法的定位精度相比較速度組合方法而言可以提高40%，且發(fā)散趨勢相對較??；③位置增量組合方法的定向精度明顯優(yōu)于速度組合方法。

（References）：

[1] Cui Ping-yuan, Xu Tian-lai . Data fusion algorithm for INS/GPS/Odometer integrated navigation system[C]// 2007 Second IEEE Conference on Industrial Electronics and Applications. 2007: 1893-1897.

[2] Trung D T, Huang Yun-wen, Chiang Kai-wei. Improving the accuracy of MEMS IMU/GPS POS systems for landbased mobile mapping system by using tightly coupled integration and auxiliary odometer[C]//31st Asian Conference on Remote Sensing. 2010: 515-520.

[3] 羅強力, 韓軍海. 基于遞推最小二乘法的捷聯(lián)慣導(dǎo)與里程計組合導(dǎo)航系統(tǒng)標定[J]. 導(dǎo)彈與航天運載技術(shù), 2014, 330: 29-33. Luo Qiang-li, Han Jun-hai. Calibration of strapdown inertial navigation system/odometer integrated navigation system based on recursive least square method[J]. Missilesand Space Vehicles, 2014, 330: 29-33.

[4] 張小躍, 楊功流, 張春熹. 捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航方法[J]. 北京航空航天大學(xué)學(xué)報, 2013, 39(7): 922-926. Zhang Xiao-yue Yang Gong-liu Zhang Chun-xi. Integrated navigation method for SINS and odometer[J]. Journal of Beijing University of Aeronautics and Astronautics, 2013, 39(7): 922-926.

[5] Rogers R M. Applied mathematics in integrated navigation systems[M]. 2nd ed. Reston, VA: American Institute of Aeronautics and Astronautics, 2003: 77-84.

[6] Rogers R M. Velocity error representations in inertial navigation system error models[R]. AIAA-95-3193-CP, 1995.

[7] 李旦, 秦永元, 張金亮. 車載慣導(dǎo)航位推算組合導(dǎo)航系統(tǒng)誤差補償研究[J]. 計算機測量與控制, 2011, 19(2): 389-391. Li Dan, Qin Yong-yuan, Zhang Jin-liang. Research on error compensation of integrated vehicular navigation system[J]. Computer Measurement & Control, 2011, 19(2): 389-391.

[8] 楊波, 王躍剛, 彭輝煌. 基于捷聯(lián)慣導(dǎo)/里程計的車載高精度定位定向方法研究[J]. 計算機測量與控制, 2011, 19(10): 2501-2505. Yang Bo, Wang Yue-gang, Peng Hui-huang. Research on precise position and orientation determination for vehicle based on SINS/Odometer[J]. Computer Measurement & Control, 2011, 19(10): 2501-2505.

On-line error-compensation of vehicular position and orientation determination system

WANG Li-fen1,2, YANG Gong-liu1,2, SHAN You-dong3, QIAO Li-wei3, JIANG Rui1,2
( 1. School of Instrumentation Science and Opt-electronics Engineering, Beijing University of Aeronautics and Astronautics, Beijing 100191, China; 2. Science and technology on inertial laboratory, Beijing 100191, China; 3. Tianjin Navigation Instrument Research Institute, Tianjin 300131, China )

In traditional SINS/odometer system, the odometer’s output is position increment and need to be differenced or integrated. This would bring about some noises or errors, and the odometer calibration residual will affect the performance of the vehicular position and orientation determination system. To solve this problem, an online error-estimation and -compensation method of the positioning and orientation systems is proposed based on position increment. The errors of the gyros and the accelerometers, the install residuals, and the error of the odometer scale factor are included in the state variables to be estimated. Two kinds of position increments which come from the inertial navigation systems and the odometer respectively are used to construct the measurement equation. A car test is accomplished to verify the feasibility of this method, and the results show that the method can effectively estimate the errors of the gyros, accelerometers and odometer parameters, and the positioning accuracy can be increased by 40% comparing with the velocity combination method.

position and orientation systems; odometer; installation error; scale factor error.

V249.32

A

1005-6734(2015)02-0145-05

10.13695/j.cnki.12-1222/o3.2015.02.002

2014-11-24；

2015-03-23

國家自然科學(xué)基金（61340044）

楊功流（1967—），男，教授，博士生導(dǎo)師，從事慣性技術(shù)研究。E-mail：bhu17-yang@139.com