基于动态零速修正的SINS/ODO组合导航算法研究*

王 琛 陈安升 陈 帅 韩 林 谭聚豪

1.南京理工大学，南京210094 2.北京自动化控制设备研究所，北京100074

目前，SINS/GNSS组合导航系统应用广泛，但其长时间精度取决于卫星导航。GNSS正常工作时，采用SINS/GNSS组合导航，对SINS误差进行校正，可以实现准确导航，但是卫星易受遮挡和干扰导致卫星导航失效，使得系统工作在纯SINS模式，SINS误差得不到校正，持续累积发散[1]。MEMS惯性器件更是如此。由于其体积小，功耗低等优势被广泛应用到各个领域，但其精度较差也大大制约了导航定位的精度[2]。因此卫星失效时需要额外的导航传感器辅助，来抑制惯性导航误差，提高系统的可靠性。里程计结构简单，成本低，可测量车辆行驶速度，且测量误差不随时间积累，自主性和抗干扰能力强，同SINS具有互补性，两者组合无需停车即可在较长时间内修正SINS系统误差，提高导航系统的精度[3]。

本文提出动态零速修正辅助SINS/ODO进行组合导航的方法，来提高系统的导航定位精度。并将里程计刻度系数误差和IMU安装误差角考虑在内，给出了SINS/ODO松组合的数学模型。跑车试验结果表明在GNSS信号短时拒止时，该算法能够有效抑制SINS的误差发散，导航精度得到明显提升。

1 动态零速修正

定义车体坐标系(v系)如图1所示，坐标系中原点是车体重心位置，Y指向车头方向，X轴指向车体右侧，Z轴指向车顶方向，X轴、Y轴和Z轴构成右手坐标系。

图1 车体坐标系

车辆在道路上正常行驶时，理想情况下不发生侧滑和跳跃，在v系中，车体横向(X轴)和垂向(Z轴)上的速度近似为0[4-5]，即：

(1)

根据实际情况，观测噪声可以考虑为零均值高斯白噪声。动态零速修正构成X轴和Z轴方向上的速度观测量，里程计输出车辆Y轴方向上的速度值，从而构成三维速度量测，建立速度观测方程，进行Kalman滤波，修正MEMS惯导的漂移。

2 里程计

2.1 里程计刻度系数误差

(2)

其中，Tk为采样周期；Pk为采样周期内检测到的脉冲数；wodok为采样噪声；kodo为里程计的刻度系数(即，载体行驶距离和里程计传感器检测到的脉冲数的比值)。

在实际工程环境中，受到车辆行进过程中，路面条件、环境温度、轮胎胎压和轮胎磨损程度等的影响，里程计的刻度系数并不是理想值kodo，总是存在一定的误差Δodo，则刻度系数的真实值为：

(3)

进一步，里程计传感器输出的真实载体前向速度为：

(4)

2.2 里程计故障判断

SINS/ODO组合导航系统中，SINS子系统作为主系统，可靠性高、不易出现故障。而ODO子系统出现故障的主要原因是车辆在行驶过程中出现打滑、侧滑或滑行等不可避免的情况，导致里程计输出的脉冲数和速度与实际不符。其中，打滑会使得脉冲数和速度大于实际值，滑行则小于实际值。

根据SINS子系统短时间内精度较高的特性，在组合滤波进行反馈时速度误差的校正量一般很小，如果ODO子系统出现打滑、滑行等故障，惯性导航解算的速度和里程计输出的速度将存在较大的差值，可以通过阈值判断进行里程计故障检测。

(5)

其中，Cs2为打滑阈值，取值范围为(0,∞)；Cs1为滑动阈值，取值范围为(-VN,0)。

此外，阈值Cs的选取受车辆行驶速度，SINS解算误差，单一SINS工作时长，ODO观测噪声等的综合影响。因此，多次或者连续检测到里程计故障时，阈值需要随故障次数进行调整。

当里程计出现故障时，应停止MINS/ODO组合滤波，改由SINS子系统单独工作，直到里程计输出再次正常时，方可再进行组合滤波。

3 组合导航模型

3.1 SINS安装角误差

(6)

将上式展开，得

(7)

(8)

3.2 状态方程

SINS/GNSS松组合导航系统选取SINS系统误差量、里程计刻度系数、航向安装误差角及俯仰安装误差角作为系统状态向量，

X=[φEφNφUδVEδVNδVUδLδλδhεxεyεz▽x▽y▽zδkαψαθ]T

(9)

其中，φE,φN和φU分别为东北天3个方向的姿态误差角，δVE,δVN和δVU分别为东北天3个方向的速度误差，δL,δλ和δh分别为纬经高3个方向的位置误差，εx,εy和εz分别为载体坐标系下陀螺仪三轴常值漂移，▽x,▽y和▽z分别为载体坐标系下加速度计三轴常值偏置，δk表示里程计的刻度误差系数，αψ,αθ分别表示航向安装误差角和俯仰安装误差角。里程计的δk及安装角误差αψ和αθ均可视作随机常数，其误差方程可表示为：

(10)

SINS系统状态方程表述如下：

(11)

其中，F(t)为18×18的SINS系统状态转移矩阵；G(t)为18×6的SINS系统噪声驱动矩阵；W(t)为6×1的SINS系统噪声矩阵。具体形式如下：

SINS系统状态转移矩阵F(t)：

(12)

其中，FN(t)为SINS系统基本误差矩阵，矩阵中各元素见参考文献[7]；FS(t)和FM(t)为惯性器件的误差矩阵，具体如下：

(13)

(14)

其中，Tgx，Tgy和Tgz为陀螺仪3个轴的相关时间，Tax，Tay和Taz为加速度计的相关时间。

SINS系统噪声驱动矩阵G(t)：

(15)

SINS系统噪声矩阵W(t)：

W(t)=[wgxwgywgzwaxwaywaz]T

(16)

其中，wgx，wgy和wgz分别为陀螺仪3个轴向的量测白噪声，wax，way和waz分别为加速度计三个轴向的量测白噪声。

3.3 量测方程

里程计输出的真实载体前向速度见式(4)，写成三维向量形式，有：

(17)

将ODO子系统输出的载体速度分解到导航坐标系，并与SINS子系统解算的速度进行对比，取两者差值作为观测量[8]，则系统观测方程为：

(18)

其中，HO为观测矩阵，具体见下式;VO为观测噪声。

(19)

本文采用反馈校正来修正SINS的误差。反馈校正是将惯性导航系统导航参数误差的估值反馈到惯性系统中，对误差状态进行校准。反馈校正如图2所示。

图2 反馈校正

由于里程计只输出速度值，故本文采用反馈校正的方法对SINS系统的速度进行校正。

(20)

4 跑车试验与性能分析

为了验证设计的相关算法以及SINS/ODO组合导航系统的性能，进行了跑车试验。跑车试验选择在南京市麒麟科技园空旷路段进行，跑车试验装车效果如图3所示，具体试验路线如图4所示。

图3 跑车试验装车效果图

图4 跑车试验路线图

本文选取MSI3200G作为MEMS惯性器件，其主要性能指标如表1所示。将MSI3200G、里程计与导航板卡组成组合导航系统固定在车体上，MEMS惯性器件的更新率为200Hz。选用耐威公司的POS320作为高精度基准导航系统。

表1 MSI3200G性能指标

本次试验初始纬度32.022071°，初始经度118.884072°，初始高度11.46m；寻北结果-143.65°。

地面跑车试验的具体试验方法如下：

1)跑车试验前，需要在实验室内对试验设备和相关软件进行性能检查，检查无问题后进行试验设备的安装；

2)保证电源设置和系统线路连接正确后，给组合导航系统和高精度基准导航系统(由高精度GNSS接收机与高精度光纤惯导等部分组成)上电；

3)由POS320提供初始航向角并将初始航向角信息通过监控软件上传给导航板卡。待MSI3200G完成自对准后，然后开始SINS/GNSS组合导航，保存组合导航系统和高精度基准导航系统的数据，然后开始跑车；

4)通过拔掉卫星导航的天线馈线获得卫星导航失效状态，之后进行SINS/ODO组合导航；

5)跑车试验的过程中，时刻对组合导航系统监控软件界面显示的导航数据和高精度基准导航系统监控软件界面显示的导航数据进行监测，观察数据是否异常；

6)跑车试验结束后，将组合导航系统输出的数据和高精度基准导航系统的数据进行时标对齐，然后进行误差比对分析，检验系统的可靠性和组合导航算法的性能。

组合导航系统相对高精度基准导航系统的试验结果如图5～6所示。

图5 位置误差

图6 速度误差

从图5和6可以看出，动态零速修正辅助的SINS/ODO组合导航系统的三维位置误差稳定，且误差小；三维速度误差稳定，且误差小。无卫星导航90s时统计值为：纬度误差1.72m，经度误差-1.36m，高度误差-4.38m，东向速度误差0.02m/s，北向速度误差-0.04m/s，天向速度误差-0.23m/s。结果表明，本文设计的基于动态零速修正的SINS/ODO组合导航在实际动态环境下对纯惯性导航误差有较好的约束效果。

5 结论

提出了在卫星信号据止情况下的基于动态零速修正的SINS/ODO组合导航算法，通过跑车试验进行验证。结果表明，在卫星信号拒止时，该算法有较高的导航精度，能够有效抑制MEMS惯导的漂移，可以作为备份导航方案。