前后向迭代滤波在SINS/OD组合导航中的应用

黄湘远，汤霞清，武 萌，蔺 博



前后向迭代滤波在SINS/OD组合导航中的应用

黄湘远，汤霞清，武 萌，蔺 博

（装甲兵工程学院控制工程系，北京，100072）

为了提高SINS/OD组合导航的精度，推导了载体坐标系下单位时间内位移增量之差的观测模型，利用残差检测对OD信号进行两层故障检测和隔离，使用前后向迭代滤波完成误差参数估计和校正。利用实车实验分别对Kalman滤波、1次迭代滤波、2次迭代滤波进行对比分析，结果表明2次迭代滤波精度最高，但相对于1次迭代滤波精度提高效果不够明显。从滤波精度和计算量上综合考虑，SINS/OD组合导航中1次迭代滤波效果最好。

故障检测；后向平滑；里程计；组合导航

0 引 言

战车导航中，捷联惯导（SINS）和里程计（OD）组合导航是一种完全自主、精度高、安全性好的方案。由于OD不能提供载体绝对位置和速度信息，定位误差随着时间增长而增大，需对模型建立、故障检测、滤波方案等展开更深研究。

SINS/OD组合导航中，需对SINS误差和OD安装误差、系数误差等进行估计和补偿[1]。速度量测中OD输出微分造成噪声放大，影响滤波精度[2,3]；位置量测中可观测度低，计算量大，滤波速度慢[4]；导航系下单位时间内SINS和OD位移增量之差的量测方案[4,5]，能够取得不错的效果。

状态估计根据所用量测信息的情况，可分为预测、滤波和平滑[9~11]。平滑是一种离线处理算法，估计精度优于滤波。曹意[12]设计“前向滤波―后向平滑―前向滤波”的实时滤波框架，获得了精度高于常规滤波的实时滤波算法。该框架可衍生为多阶窗口平滑和多次迭代2种算法，2种方案性能难以直接比较，滤波精度与窗口阶数和迭代次数无明显关系，需进行试凑。

本文推导了载体系下单位时间内位移增量之差的量测方程，在载体系下进行2层故障量测。设计了前后向迭代滤波方案，构成新的SINS/OD组合导航算法，基于Kalman滤波、1次迭代滤波、2次迭代滤波进行了对比实验。

1 SINS/OD组合导航系统模型

1.1 SINS、OD误差模型

记：地心惯性坐标系为i系；地球系为e系；导航系n系为东北天坐标系；载体系为b系；里程计坐标系为m系。b系和m系为右前上坐标系。

（2）

1.2 载体系下位移增量之差

SINS/OD组合导航中，基于滤波周期内的位移增量之差的量测方案可避免速度或位置方案的缺陷[4]。为了便于OD输出的故障定位及隔离，本文利用载体系的位移增量代替导航系的位移增量，此时需推导新的量测方程。

（7）

（9）

（11）

该方案充分利用OD信息，以积分代替微分，有利于提高信噪比；无需进行真正的航位推算，无需考虑OD故障的累积影响。OD故障时，只需抛弃该时刻的量测数据即可，处理方案较为简单。

1.3 组合导航系统设计

2 系统观测的故障检测及隔离

SINS/OD组合导航中，当发生车体侧滑、打滑、颠簸、跳跃等现象时，OD输出不能反映车辆运动情况，系统观测可能出现故障。如果不对故障进行有效隔离，可能导致更差的导航结果。为了实时检测OD信息的有效性，通常采用残差检测法。

2.2 里程计故障分类及隔离

判定观测有效时，Kalman滤波进行量测更新，系统使用SINS/OD组合导航模式；判定观测无效时，Kalman滤波不进行量测更新，抛弃里程计信息，系统使用纯惯性导航模式。该方案较为简单，便于操作，但是没有充分利用系统观测信息。对OD故障信息进一步分类和隔离，引入车辆运动约束、零速修正等方式可提高导航精度[6,8]。

履带式战车运动中，观测异常的主要原因有：OD无输出、滑行、打滑、侧滑、快速转向、原地转向、颠簸、跳跃等。OD无输出意味着车辆行进中OD输出为零；滑行造成OD输出小于车辆行驶距离；打滑造成OD输出大于车辆行驶距离，这3种情况影响前向速度测量，称为第1类故障。侧滑、快速转向、原地转向等导致车辆产生横向运动，称为第2类故障。颠簸、跳跃等导致车辆产生高低向运动，称为第3类故障。根据车辆运动学约束模型，车辆正常行驶条件下，只有前向速度，没有横向和高低向速度。

3 前后向迭代的实时滤波

组合导航系统实时对误差参数进行估计和校正。根据估计时刻所用观测信息的不同，估计可分为预测、滤波和平滑3类。

3.1 R-T-S平滑算法

滤波利用当前及以前的观测信息对当前状态进行估计，平滑在此基础上利用以后的观测信息进行估计，精度优于滤波。平滑可分为R-T-S（Rauch-Tung-Striebel）平滑[9,10]和双滤波器平滑[11]2类。前者采用递推形式逆向求解最优估计，原理简单，易于实现，但数值稳定性稍差。后者基于正向和逆向模型分别进行滤波和数据融合，要求实际系统存在逆向模型，计算量较大。由于数值稳定性可通过平方根滤波、奇异值分解（Singular Value Decomposition，SVD）、UD分解等解决，本文采用R-T-S平滑。

R-T-S平滑可分为前向滤波和后向迭代2个过程。前向滤波采用卡尔曼滤波，按的顺序计算并存储状态估计，，误差协方差阵，，状态转移阵。

R-T-S平滑作为一种离线算法，多用于组合导航数据后处理和精度测评。考虑到计算机处理速度快速发展，R-T-S平滑也可应用到实时滤波中。

3.2 基于R-T-S平滑的实时滤波

将实时滤波算法和事后平滑算法结合起来，基于“前向滤波―后向平滑―前向滤波”的框架，可形成一种实时滤波算法[12]。

图1 滤波算法的基本框架

为保证滤波的实时性（即需在一个滤波周期里面完成所有计算），需合理设置窗口阶数和迭代次数。阶数相同的窗口算法和迭代算法的滤波精度难以直接比较，迭代算法需存储的量较少，本文采用迭代算法。迭代次数的选择没有有效的理论依据，只能采用试凑法，本文分别考虑1次迭代和2次迭代2种方案。

4 实验验证

实验室将某型光纤陀螺SINS安装在某战车上，里程计安装在某侧履带主动轮上。陀螺零偏稳定性小于0.02 (°)/h，加速度计偏值重复性小于0.05 m。行驶过程中使用高精度GPS导航芯片进行实时位置测量。战车在某场地行驶1 h左右，行驶距离约为18 km，行驶路线如图2所示，进行2次独立实验。行驶过程包括加速、匀速、转弯、上下坡、颠簸路面等。

图2 试验跑车路线

实验过程中，以载体系下1 s内位置增量之差作为系统量测，利用残差统计量进行2层故障检测。为了验证前后向迭代滤波的有效性，分别设计Kalman滤波和1次、2次迭代算法的对比实验。

图3 第1次跑车中位置误差

图4 第2次跑车中位置误差

图3、图4给出了2次跑车过程中，纬度误差和经度误差，其中参考位置由高精度GPS给出，高度方向由于没有好的参考值不做比较。=0，1，2表示迭代次数，其中，=0代表Kalman滤波。短时间里，3种算法的估计结果相差不大，随着时间的增长，各算法误差逐步体现出来。Kalman滤波的误差大于1次和2次迭代滤波，表明引入前后向迭代能够有效提高估计精度；1次迭代和2次迭代结果相差不大，表明经过1次迭代后，再次迭代对滤波精度的提高有限。

1次前后向迭代滤波包括平滑和滤波2个过程，而平滑和滤波的计算量相当，因此次迭代滤波的计算量相当于Kalman滤波的（2+1）倍。从滤波精度和计算量上综合考虑，SINS/OD组合导航中，1次前后向迭代滤波效果较好。

5 结束语

为了提高SINS/OD的导航精度，在导航过程中对惯性器件参数进行实时估计和反馈。以载体系下单位时间内的位置增量之差作为系统量测，引入2层故障量测方案，有效对里程计故障进行定位和隔离。重点对比分析了前后向迭代次数对导航精度的影响，结果表明1次迭代修正效果最为明显，2次迭代提高精度有限。

[1] 张小跃, 杨功流, 张春熹. 捷联惯导/里程计组合导航方法算法[J]. 北京航空航天大学学报, 2013, 39(7): 922-926.

[2] Wu Y X, Wu M P, Hu X P, et al. Self-calibration for land navigation using inertial sensors and odometer: observability analysis[C]. Chicago Illinois: AIAA Conference of Guidance, Navigation and Control, 2009: 1-10.

[3] Santana D D S, Furukawa C M, Maruyama N. Sensor fusion with low-grade inertial sensors and odometer to estimate geodetic coordinates in environments without GPS signal[J]. IEEE Latin America Transactions, 2013, 11(4): 1015-1021.

[4] 付强文, 秦永元, 周琪. 改进量测的车载捷联惯导/里程计组合导航算法[J]. 测控技术, 2013, 32(7): 134-137.

[5] 肖煊, 王清哲, 程远, 等. 捷联惯导系统/里程计高精度紧组合导航算法[J]. 兵工学报, 2012, 33(4): 395-400.

[6] 翁浚, 成研, 秦永元, 等. 车辆运动约束在 SINS/ OD系统故障检测中的应用[J]. 中国惯性技术学报, 2013, 21(3): 406-410.

[7] 吴有龙, 王晓鸣, 曹鹏. 抗差估计及Allan方差在车载组合导航系统中的应用研究[J]. 兵工学报, 2013, 34(7): 889-895.

[8] 赵洪松, 缪玲娟, 沈军. 捷联惯导/里程计高精度组合导航算法[J]. 兵工学报, 2014, 35(4): 433-440.

[9] Simo S. Unscented Rauch-Tung-Striebel smoother[J]. IEEE Trans. on Automatic Control, 2008, 53(3): 845-849.

[10] Wang X X, Pan Q, Liang Y, et al. Application of unscented transformation for nonlinear state smoothing[J]. Acta Automatica Sinica, 2012, 38(7): 1107-1112.

[11] 刘帅, 孙付平, 李海峰, 等. 前后向平滑算法在精密单点定位/INS紧组合数据后处理中的应用[J]. 中国惯性技术学报, 2015, 23(1): 85-91.

[12] 曹意, 茅旭初. 基于非线性滤波和平滑相迭代的GPS定位估计算法[J]. 上海交通大学学报, 2009, 43(7): 1108-1112.

Application of Forward-backward Iterative Filter inSINS/OD Integrated Navigation

Huang Xiang-yuan, Tang Xia-qing, Wu Meng, Lin Bo

(Department of Control Engineering, Academy of Armored Force Engineering, Beijing, 100072)

To improve the navigation precision of SINS/OD integrated navigation system, the observation model of the deference between displacement increments per unit time in the body coordinate system is derived. The paper conducts the two-layer fault detection and isolation for the OD’s signal using residualdetection, and estimates and corrects of the error parameters by using forward-backward iterative filter. Vehicle test is conducted to comparatively analyze Kalman filter, one-iteration and two-iteration filters. The results show that the filtering precision of two-iteration filter is the best, which doesn’t have an obvious improvement compared with one-iteration filter. For the comprehensive consideration of filtering precision and computational complexity, the filtering effect of one-iteration filer in SINS/OD integrated navigation system is the best.

Failure detection; Backward smoothing; Odometer; Integrated navigation

1004-7182(2016)03-0063-05

10.7654/j.issn.1004-7182.20160315

U666.1

A

2015-06-24；

2015-10-19

军队计划项目资助课题

黄湘远（1988-），男，博士研究生，主要从事非线性滤波、惯性导航的研究