车载定位定向系统误差在线补偿方法

王丽芬，杨功流，单友东，乔立伟，姜 睿

( 1. 北京航空航天大学 仪器科学与光电工程学院，北京 100191；2. 惯性技术国防重点实验室，北京 100191；3. 天津航海仪器研究所，天津 300131 )

车载定位定向系统误差在线补偿方法

王丽芬1,2，杨功流1,2，单友东3，乔立伟3，姜 睿1,2

( 1. 北京航空航天大学 仪器科学与光电工程学院，北京 100191；2. 惯性技术国防重点实验室，北京 100191；3. 天津航海仪器研究所，天津 300131 )

针对里程计输出为位置增量，传统SINS/里程计组合方法会对位置增量进行微分或积分从而会产生一定噪声或误差，且里程计标定残差影响高精度定位定向系统性能的问题，提出了一种基于位置增量组合的定位定向系统误差在线估计和补偿方法。该方法除了将惯性器件误差列入状态量外，还将里程计安装误差残差、标度因数误差等纳入状态量进行实时估计，将惯性导航系统输出的速度增量进行积分获得位置增量，同时与里程计输出位置增量进行比对，构建量测方程。设计跑车试验对该方法进行了验证，结果表明该方法可以有效估计惯性器件误差和里程计参数误差，同时相比速度组合方法而言，定位精度可提高40%。

定位定向；里程计；安装误差；标度因数误差

车载定位定向系统是用于陆基导弹、制导炮弹等武器发射车以及各种侦察车的基准设备，主要为车载设备提供准确的位置和方位信息。车载定位定向系统中的捷联导航系统（SINS）和里程计（OD）这两种关键设备。它们相对于GPS导航[1-2]而言，均具有隐蔽性好，不受外界电磁、气候干扰，不受时间、地点限制等诸多优点；同时，车载定位定向系统中里程计输出的位置增量，不会像惯性导航系统那样出现误差随时间推移而增大的现象。因此利用里程计输出对惯性导航系统的误差进行抑制，可以得到较高的导航精度，其作为军事目的的应用显得尤为重要。

然而里程计与SINS之间存在一定的安装误差，且里程计输出为脉冲当量，需通过刻度因数转换为位置增量或速度信息。虽然应用之前会对安装误差及标度因数进行标定[3]，但由于标定精度的限制，以及后续应用过程中车辆轮胎的变化，残余的安装误差及标度因数误差会对定位定向精度产生一定影响[4]。同时传统的航位推算技术将里程计的位置增量转换为速度信息与惯导系统的速度进行滤波处理，微分计算必然会引入一定的噪声。

本文研究了一种定位定向系统误差在线补偿方法，该方法将里程计残余安装误差、标定因数误差纳入状态方程予以在线估计，并利用位置增量组合方式实现SINS/OD组合导航。通过实际跑车试验，有效验证了该方法的可行性，提高了定位定向系统的精度。

1 SINS/里程计组合导航误差模型

定义导航坐标系(n系)为东北天坐标系，SINS载体坐标系为b系；里程计坐标系与车体坐标系(m系)重合；m系相对于b系存在一定的偏角，即通常所说的里程计的安装误差角α=[αθαγαφ]，其中αθ为俯仰安装误差，αγ为横滚安装误差，αφ为航向安装误差。

假设里程计的安装误差和标度因数都经过了准确标定且应用过程中无任何变化，设里程计在一个采样周期内输出脉冲增量为PD，里程计标度因数为KD，则里程增量为：

里程计输出在车体坐标系上的投影为：

则里程计输出里程增量在导航坐标系上的投影为：

考虑里程计的残余安装误差及标度因数误差时，进一步推导SINS/里程计组合导航误差模型。

1.1 SINS误差模型

由于SINS的误差模型已经相当成熟[5-6]，这里不再推导，引用如下：

式中：

fn： 车辆运动加速度在导航坐标系上的投影；

Vn： 车辆运动速度，其中VE、VN、VU分别为该速度在导航坐标系上的分量；

L、λ、h： 纬度、经度及高度；

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

φins： 惯导系统姿态失准角；

▽b： 加速度计误差在惯导载体坐标系上的投影。

1.2 航位推算误差模型

设里程计残余安装误差分别为δθα、δγα、δφα，标度因数误差为δKD，则里程计输出在SINS坐标系上的投影为：

由式(5)推导得：

由于里程计安装误差均为小角度，因此sinαi≈0，cosαi≈1 (i =α,γ,φ)，则式(6)简化为：

从式(7)可以看出，里程计输出位置增量误差与横滚安装误差角无关。里程计输出位置增量在导航坐标系上的投影为：

式中：mφ为航位推算姿态失准角矢量。

将式(7)代入式(8)推导得：

则里程计位置增量误差为：

与惯导系统位置误差方程相似，航位推算的位置误差方程如下：

式中：T为里程计采样周期。将式(10)代入式(11)得：

式中：

航位推算中姿态更新算法与惯导解算中的一样，只是利用的信息不一样，因此将SINS姿态误差方程中的参数用相应的参数代替，即可得到航位推算姿态误差方程，即：

式中：

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

式中：

将式(15)代入式(13)可得航位推算姿态误差方程：

式中：

2 定位定向系统滤波算法

为了实现对各种误差的在线估计及补偿，需分别建立状态方程和量测方程，利用卡尔曼滤波进行估计。

2.1 状态方程

由于研究对象为陆用车载定位定向系统，因此位置信息和速度信息中的高度通道暂不考虑，即SINS/ OD组合定位定向的状态量（共21维）主要包括：SINS的经纬度误差δλins、δLins，水平速度误差δVE_ins、δVN_ins，姿态误差φx_ins、φy_ins、φz_ins，陀螺漂移εx、εy、εz，加速度计零偏▽x、▽y、▽z航位推算经纬度误差δλm、δLm，姿态误差φmx、φmy、φmz；里程计安装误差残差δαθ、δαφ，标度因数误差δKD等。

假设惯性器件误差为零偏误差与白噪声之和，零偏误差为随机常值；假设里程计安装误差残差及标度因数误差在一次上电后保持不变，即：

SINS/OD 组合定位定向系统的状态量为：

根据式(4)(10)(12)(16)(17)可得到定位定向的状态方程为：

式中：F为系统状态矩阵，G为系统噪声驱动阵，W为系统白噪声。这里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]的误差会进行累积，且位置组合对于各种误差的估计速度较慢。由于里程计位置增量信息精度相对较高，利用该信息与惯导解算的位置增量信息进行对比，构建量测方程，可以在一定程度上提高定位定向精度。

一般情况下，惯导解算参数的更新率是里程更新率的N倍，假设tn-1到tn时间内，里程计输出更新一次，则量测方程如下：

对于离散解算方式而言，式(19)推导为：

假设短时间内惯导系统速度误差几乎不变，则式(20)可进一步化简为：

则观测矩阵为：

3 试验验证

为了验证本文所述方法的有效性，设计跑车试验，分析利用里程计和SINS的速度信息组合和利用位置增量组合两种方法的定位定向精度。

跑车试验中相关参数如下：①惯性器件精度为：陀螺零漂0.02 (°)/h，加速度计零偏100 μg。②跑车起始点为：经度116.3437°，纬度40.0343°；行车里程为34.52 km，行车路线如图1所示；跑车试验持续1小时，其中前10 min车辆静止。③试验中以GPS位置信息作为基准信息，GPS定位精度约为3 m。

将位置增量组合方式下同时考虑里程计相关误差的方法标记为label1，将速度组合方式下同时考虑里程计相关误差的方法标记为label2。试验结果表明，位置增量组合方法和速度组合方法均可以实现对水平陀螺零漂、水平加速度计零偏以及里程计安装误差残差的估计，但跑车时间内里程计标度因数误差收敛不明显，由于真实误差值未知，因此主要通过最后导航结果评价误差估计精度。

利用上述两种处理方法，以及未考虑里程计参数误差的位置增量组合方法（标记为label3）对跑车数据处理得到的结果如图2～5所示。

对比图2和图4可以看出，考虑里程计参数误差时，定位精度会稍有提高。这是因为本试验是在基于标准点（位置精度高于GPS）对里程计参数标定基础上开展的，里程计标定精度相对较高，因此组合导航位置误差区别不明显，但是两图的对比结果还是表明了里程计参数误差对定位精度的影响。同时里程计安装误差残差估计值小于1 mrad，标度因数误差未明显收敛，在一定程度上也与里程计标定精度高相关。

对比图2和图3可以看出，跑车时间段内，在考虑里程计标定残差的情况下，传统速度组合导航方法的经度定位误差最大约为50 m，而位置增量组合方法经度误差曲线的峰峰值约为30 m，两种方法的纬度误差曲线的峰峰值相当，约为25 m。综合分析位置增量组合导航相对于速度组合导航方法而言，定位精度提高了40%，且位置增量组合方法的定位误差能保持在一定范围内，发散趋势不明显。

同时从图5可以看出，三种组合方法的定向精度在最后时刻趋于一致，但从变化过程来看，位置增量组合方法航向误差变化幅度相对较小，可以在一定时间内保证方位精度。

图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 结 论

本文提出了一种基于SINS/里程计位置增量组合的定位定向误差在线估计方法，并通过实际跑车试验对该方法与其他组合方法进行了对比，所得到的结论如下：①采用位置增量组合方法实现SINS/里程计组合导航时，将里程计安装误差残差和标度因数误差扩维为状态量进行估计的误差处理方法，相对于不对上述误差进行任何处理的组合导航方法，可以有效抑制位置误差和方位误差的发散趋势，提高定位定向精度。②位置增量组合方法的定位精度相比较速度组合方法而言可以提高40%，且发散趋势相对较小；③位置增量组合方法的定向精度明显优于速度组合方法。

（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] 罗强力, 韩军海. 基于递推最小二乘法的捷联惯导与里程计组合导航系统标定[J]. 导弹与航天运载技术, 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] 张小跃, 杨功流, 张春熹. 捷联惯导/里程计组合导航方法[J]. 北京航空航天大学学报, 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] 李旦, 秦永元, 张金亮. 车载惯导航位推算组合导航系统误差补偿研究[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] 杨波, 王跃刚, 彭辉煌. 基于捷联惯导/里程计的车载高精度定位定向方法研究[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

国家自然科学基金（61340044）

杨功流（1967—），男，教授，博士生导师，从事惯性技术研究。E-mail：bhu17-yang@139.com