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

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

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

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

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

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

定位定向；里程計(jì)；安裝誤差；標(biāo)度因數(shù)誤差

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

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

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

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

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

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

里程計(jì)輸出在車體坐標(biāo)系上的投影為：

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

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

1.1 SINS誤差模型

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

式中：

fn： 車輛運(yùn)動(dòng)加速度在導(dǎo)航坐標(biāo)系上的投影；

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

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

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

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

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

1.2 航位推算誤差模型

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

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

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

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

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

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

則里程計(jì)位置增量誤差為：

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

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

式中：

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

式中：

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

式中：

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

式中：

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

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

2.1 狀態(tài)方程

由于研究對(duì)象為陸用車載定位定向系統(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，加速度計(jì)零偏▽x、▽y、▽z航位推算經(jīng)緯度誤差δλm、δLm，姿態(tài)誤差φmx、φmy、φmz；里程計(jì)安裝誤差殘差δαθ、δαφ，標(biāo)度因數(shù)誤差δKD等。

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

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

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

式中：F為系統(tǒng)狀態(tài)矩陣，G為系統(tǒng)噪聲驅(qū)動(dòng)陣，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為加速度計(jì)白噪聲。

2.2 量測(cè)方程

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

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

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

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

則觀測(cè)矩陣為：

3 試驗(yàn)驗(yàn)證

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

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

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

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

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

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

同時(shí)從圖5可以看出，三種組合方法的定向精度在最后時(shí)刻趨于一致，但從變化過程來看，位置增量組合方法航向誤差變化幅度相對(duì)較小，可以在一定時(shí)間內(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/里程計(jì)位置增量組合的定位定向誤差在線估計(jì)方法，并通過實(shí)際跑車試驗(yàn)對(duì)該方法與其他組合方法進(jìn)行了對(duì)比，所得到的結(jié)論如下：①采用位置增量組合方法實(shí)現(xiàn)SINS/里程計(jì)組合導(dǎo)航時(shí)，將里程計(jì)安裝誤差殘差和標(biāo)度因數(shù)誤差擴(kuò)維為狀態(tài)量進(jìn)行估計(jì)的誤差處理方法，相對(duì)于不對(duì)上述誤差進(jìn)行任何處理的組合導(dǎo)航方法，可以有效抑制位置誤差和方位誤差的發(fā)散趨勢(shì)，提高定位定向精度。②位置增量組合方法的定位精度相比較速度組合方法而言可以提高40%，且發(fā)散趨勢(shì)相對(duì)較?。虎畚恢迷隽拷M合方法的定向精度明顯優(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] 羅強(qiáng)力, 韓軍海. 基于遞推最小二乘法的捷聯(lián)慣導(dǎo)與里程計(jì)組合導(dǎo)航系統(tǒng)標(biāo)定[J]. 導(dǎo)彈與航天運(yùn)載技術(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)/里程計(jì)組合導(dǎo)航方法[J]. 北京航空航天大學(xué)學(xué)報(bào), 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)誤差補(bǔ)償研究[J]. 計(jì)算機(jī)測(cè)量與控制, 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ì)的車載高精度定位定向方法研究[J]. 計(jì)算機(jī)測(cè)量與控制, 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