一种改进的SINS_ODO_ZUPT组合导航算法*

2021-04-27陈安升姚晓涵

航天控制 2021年1期

张敏陈安升陈帅王琛姚晓涵

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

0 引言

目前车载组合定位导航常用SINS/GNSS 组合导航来实现车辆定位导航。然而，GNSS由于受到多径效应的影响，在城市峡谷地段、桥梁下、隧道内等复杂环境下，其信号会严重丢失甚至中断[1]。由于惯性导航系统自主定位精度会随时间而降低，基于GNSS_SINS的车载定位导航的估计精度必然会严重下降[2]。SINS/ODO组合和零速修正是解决上述问题的2种廉价且有效的导航方法。里程计[3-4]成本低，自主性强，利用里程计提供的速度，推导航位推算算法和航位推算误差，可以提高惯性导航精度。零速修正技术[5-6]是提高车载惯导系统的一种简单廉价的方法。在卫星信号拒止情况下，该方法利用车辆停车时惯导系统导航系的速度输出作为系统速度误差的观测量，进而对位置等其它误差量进行修正，从而有效抑制和补偿车载惯性导航误差。文献[7]将动态零速修正和SINS/ODO组合方案相结合，利用零速修正辅助SINS/ODO进行组合导航，提高了一定的车载导航精度。但是该方法存在高度发散的问题。

本文提出了一种零速修正辅助SINS_ODO的组合导航算法。将里程计刻度系数误差和IMU安装误差角考虑在内，建立了组合导航误差状态模型、量测模型以及估计误差修正方法。针对传统的动态零速修正辅助的SINS_ODO的算法存在的高度发散问题，提出了一种新的量测模型，即对天向速度以及天向高度进行约束的量测模型。最后采用仿真计算验证了本文的组合导航方法，并与传统的动态零速修正辅助的SINS_ODO方法进行了比较。该算法能够在卫星拒止时有效抑制SINS的误差发散，实现高精度自主定位。

1 里程计导航定位原理与误差分析

定义以下坐标系：惯导坐标系b系为惯导的右前上坐标系，导航坐标系n系为东北天坐标系，载体坐标系m系为车体的右前上坐标系，m系中原点是车体重心，Y轴指向车头方向，X轴指向车体右侧，Z轴指向车顶方向，X轴、Y轴和Z轴构成右手坐标系。

1.1 里程计定位原理

(1)

1.2 里程计误差模型

里程计的误差主要包括刻度系数误差和安装角误差。里程计的刻度系数误差是指在车辆行进过程中，由于环境温度，轮胎状况等的影响下，产生的误差。若安装误差角为0，则受里程计刻度系数误差影响，此时b系的速度为

(2)

(3)

式中，αφ为航向安装误差角，αθ为俯仰安装误差角。

1.3 里程计故障检测

当车辆在行驶过程中出现打滑、滑行等情况时，里程计输出的速度会与实际速度产生偏差。因此，当里程计出现故障时，需对里程计进行故障隔离。可利用残差χ2检验对系统的故障进行检测和隔离[9]。残差χ2检验原理如下：

(4)

则第k时刻系统量测的预测值为

(5)

2)计算第k时刻的Kalman滤波器的残差及方差

其中，k时刻的残差为：

(6)

k时刻的方差为：

(7)

3)计算rk的统计特性，统计量构造如下：

(8)

4)判断系统是否发生故障

当系统正常工作时，λk服从自由度为x的χ2分布，x为Zk的维数，当系统出现故障时，λk不再服从自由度为x的χ2分布。

因此故障判定准则为

(9)

其中，T是预先设定的门限值。当检测到里程计故障时，应停止SINS/ODO组合导航，改由 SINS 子系统单独工作，直到里程计输出再次正常时，方可再进行组合。

2 零速修正

2.1 状态方程

车载惯导导航系统选取SINS系统误差量、里程计刻度系数、航向安装误差角及俯仰安装误差角作为系统状态向量，

(10)

(11)

则SINS系统误差方程可以表示如下：

2.2 量测方程

(12)

式中：V为观测噪声，H为观测矩阵，如下所示

H=[03×3I3×303×12]

(13)

3 组合导航模型

3.1 状态方程

系统状态方程同式(10)系统状态方程一致，此处不再赘述。

3.2 量测方程

1) 传统SINS_ODO_ZUPT量测方程

利用SINS系统与ODO东北天坐标系下的速度误差作为观测量，系统观测方程表示如下：

(14)

式中，H0为观测矩阵，V0为观测噪声。

(15)

2) 本文量测方程

(16)

(17)

Hp=[01×8101×9]

(18)

3.3 估计误差修正

采用组合导航过程中估计所得的误差量对SINS状态进行反馈校正。导航参数误差估计量的修正如式(19)所示

(19)

4 跑车试验与仿真分析

为了验证本算法对高度约束效果的有效性，利用试验车进行了跑车试验。跑车试验选择在南京市麒麟科技园某空旷路段进行，行驶时间大约为6min。跑车实验现场图和行车路线图如图1所示。

图1 跑车实验现场图

车载试验系统主要由MEMS捷联惯导系统、里程计和高精度组合导航系统组成。MEMS捷联惯导系统由MSI3200G构成，数据输出频率为200Hz，MEMS陀螺仪的量程为±300(°)/s，零偏稳定性≤5(°)/h；MEMS加速度计的量程为±50g，零偏稳定性≤0.5mg。高精度组合导航系统由耐威公司的POS320组成，数据输出频率为20Hz，此导航系统为高精度基准系统。通过测量POS320组合导航系统可以获得初始对准点位置。

同时记录惯性测量单元(IMU)数据、里程计数据以及高精度基准数据，离线进行导航算法验证。由高精度基准数据可以得到本次试验初始纬度 32.0161°，初始经度118. 8892°。跑车实验具体步骤如下：

1)跑车实验前，先检查软硬件设备是否有异常，检查无误后，将所有硬件装车并安装好；

2)连接好系统线路和电源，给惯导系统、里程计以及高精度基准导航系统上电；

3)系统上电完成后，同时采集惯导系统、里程计以及高精度基准导航系统数据并准备开始跑车；

4)跑车实验过程中，观察惯导系统、里程计以及高精度基准导航系统数据是否异常并记录；

5)跑车结束后，将采集到的数据进行仿真分析，验证算法的有效性。

检查数据无误，完成仿真运算后，列出相关计算结果。图2为纬度、经度轨迹对比。