基于运动约束辅助的车载组合导航算法

王立兵，周 俊，刘鹏飞，智奇楠，贾瑞才

(1.中国电子科技集团公司第五十四研究所， 石家庄 050081；2.卫星导航系统与装备技术国家重点实验室， 石家庄 050081；3.中国人民解放军63961部队， 北京 100012；4.陆军航空兵学院 陆军航空兵研究所， 北京 101121)

现代意义的地面战争，要求移动车载系统能够在电磁环境极为复杂、恶劣的地域环境下灵活机动，并为武器发射平台提供精确的地理坐标和姿态基准，要求能够做到边走边打、停车就打。这就对车载导航系统提出了更高的要求[1]。传统意义的车载导航系统通常由惯性导航系统(INS)和卫星定位系统(GNSS)组成。但是在城市高楼区、林荫道、隧道内等复杂环境下GNSS信号会严重丢失甚至中断，惯性导航系统定位精度、车载组合滤波器的估计精度都会严重下降。文献[2-3]给出了车辆运动学约束辅助下的组合导航算法，当车辆停止时滤波器同样会出现发散的现象；文献[4-5]推导了航位推算算法和航位推算误差，在增加了里程计的前提下提高了惯性导航精度；文献[6]针对车辆的动态检测结果提出了混合滤波算法，来降低惯性导航误差，但是文献中的非线性滤波工程实际应用较为困难，计算复杂度较高；文献[7-9]针对典型城区环境，论述了车载运动约束导航算法的具体实现过程，但是没有将零度修正和动态零速修正相互结合起来；

传统的零速修正技术(Zero-velocity Update，ZUPT)要求车辆每隔一定时间停车一次，这样就降低了车辆的灵活机动性能，同时车辆的停车状态不好判断；动态零速修正技术(DZUPT:Dynamic Zero-velocity Update)则要求车辆处于行驶状态下，当车辆停止时DZUPT很有可能会导致组合滤波器发散、估计精度下降。

现有的提高惯性导航精度的方法有惯导/里程计组合的航位推算算法，但是该算法引入了额外的观测信息，里程计的车辆安装和在线校正均不利于机动性较强的载车场景使用；另外就是利用地面车辆现有的侧向和天向约束信息，但是现有文献中均没有将速度约束的零速修正和动态零速修正信息相结合起来，对零速点的检测也没有工程上较为简便可行的方法；

本文采用零速修正与动态零速修正相结合的运动约束辅助方式，在GNSS信号长时间丢失的场景下，可为车辆在行驶、停止两种运行状态下，提供可靠、有效的约束辅助信息，保证组合导航系统的精度。

文章给出了一种简单的适用于工程应用的零速检测方法，在考虑了杆臂效应、安装偏差角的前提下给出了组合滤波器模型。最后的跑车试验验证了方法的有效性。

1 基于运动约束辅助的组合滤波器设计

基于运动约束的组合导航系统框图如图1所示。当GNSS信号的观测质量较好时，组合滤波器可利用GNSS提供的伪距、伪距率信息进行量测更新；当GNSS信号较差甚至无信号时，此时可充分利用惯性导航系统提供的导航信息，首先通过零速检测模块，判断出车辆行驶的状态(车辆行驶中或者处于停止状态)，然后构造相应的运动约束信息，为组合滤波器提供有效的观测。

图1 基于运动约束辅助的组合导航系统原理图

1.1 状态方程的建立

系统状态方程中选取22维的状态量，分别为三维姿态角误差φ、三维速度误差δv、三维位置误差δp、三维陀螺零偏εb、三维加计偏置▽、俯仰安装偏差角αθ、航向安装偏差角αψ、INS中心到GNSS天线中心的三维杆臂误差δl、GNSS接收机钟差δtu以及GNSS接收机钟漂δtru：

X=[φδvδpεb▽αθαψδlδtuδtru]T

(1)

状态方程中的姿态误差方程、速度误差方程以及位置误差方程可由INS误差方程得到，在此不再赘述。

陀螺零偏和加计偏置建模为一阶马尔科夫过程和白噪声的组合。

(2)

使用车辆运动约束辅助信息时，还应考虑到惯导系统在车辆安装时与卫导天线之间的杆臂效应。实际上，在系统运行过程中，杆臂会随着系统的震动或者形变而变化。通常情况下，杆臂误差一般是由于系统机械部件的物理形变引起的，这些物理形变基本上都是低频的，因此实际应用中，将杆臂误差建模为时间常数为无穷大的随机常数过程，满足以下微分方程：

(3)

1.2 量测方程的建立

1) DZUPT条件下量测方程的建立。车体坐标系速度Vm可表示为

(4)

(5)

对式(4)进行微分，可得：

Μ1φ+M2δVn+M3α

(6)

(7)

式中，M1(1,×)表示矩阵M1的第一行，M3(1,3)表示矩阵M3的第1行3列的元素；

针对车辆行进过程中出现的侧滑、跳跃现象对零速修正带来的影响这一问题，本算法的滤波器中有伪距/伪距率提供的GNSS原始测量信息和运动约束构造的辅助信息，当GNSS信号较好时，滤波器中GNSS观测信息会对速度约束信息进行很好的修正作用，侧滑、跳跃现象不会对对载体的精度产生较大影响；

2) ZUPT条件下量测方程的建立

当载车处于停止状态时，由运动约束条件提供的ZUPT观测值即可表示为

(8)

根据观测值Z0，可很容易得到相应的量测矩阵H0：

(9)

1.3 零速检测方法

零速检测方法，主要是对IMU原始测量数据进行检测，同时将惯导系统输出的速度信息作为参考，综合判断车辆的行驶状态，具体步骤如下：

1) 在车辆初始对准阶段，采集1～2 min的IMU原始静态数据，根据下式计算陀螺阈值λg、加计阈值λa，得到的阈值也可根据车辆的实际动态环境做适量的调整。

(10)

式中，|ωi|表示第i(i=1,…,N)次测量得到的陀螺角增量模值，|vi|表示第i(i=1,…,N)次测量得到的加计速度增量模值，g0表示当地重力分量的速度增量。

2) 将采集得到的IMU原始数据与λg、λa进行比较，在此引入速度参考，得到可以进行零速修正的零速判断准则：

(11)

λv为速度参考阈值，考虑车停止状态下的场景，通常选取：λv≤0.5 m/s。

2 动态跑车试验与分析

车载组合导航测试系统由高精度光纤惯组、单位自行研制的多频多模组合导航接收机组成。高精度光纤惯组的陀螺随机漂移为0.02 °/h，加计零偏稳定性为50 μg。

试验地点选择在天津市，试验时间约为5 300 s，以高精度惯组与组合导航接收机的组合结果作为参考基准，通过事后人为增加中断信号以及信号中断时间，测试长时间无卫导信号场景下，速度约束辅助信息对组合导航精度的影响。

针对测试轨迹共设置了八处人为中断，中断时刻以及中断时间间隔如图2所示，信号中断时间间隔范围为300～500 s，中断过程中包括了车辆行进中的所有状态，包括停车等待红灯、直线行驶、转弯行驶以及绕圈行驶等。

图2 信号中断时刻及中断时间间隔

试验轨迹如图3所示，可以看出：在有速度约束辅助信息的情况下，中断时刻的轨迹图与基准轨迹重合度很高，零速检测到的车辆停止时的轨迹也在基准轨迹以内；在无速度约束辅助信息的情况下，中断时刻的轨迹图已严重偏离基准轨迹，特别是中断二时的轨迹图。

图3 车辆行驶轨迹

图4给出了中断信号场景速度约束辅助下的组合导航定位测速误差曲线，可以看到单个方向绝对位置误差不超过10 m；单个方向上的绝对速度误差不超过0.25 m/s；图5给出了中断信号场景在无速度约束辅助下的组合导航定位测速精度，从图中可以看出导航精度要差很多，单向绝对位置误差在200 m以内，单向绝对速度误差在1 m/s以内。

图4 中断信号场景速度约束辅助下的组合导航误差曲线

图5 中断信号场景无速度约束辅助下的组合导航误差曲线

表1给出了中断信号场景不同组合导航类型下的导航精度统计表，中断信号场景下，运动约束辅助下的组合导航水平定位精度可保证在2.7 m以内，垂直定位精度在1.8 m以内，水平测速精度在0.05 m/s以内，垂直测速精度在0.02 m/s以内；在无速度约束辅助信息的情况下，组合导航精度较差，其中水平定位精度要差很多。

表1 中断信号场景不同组合导航类型下的导航精度统计

图6给出了车辆行驶速度与检测到的零速时刻与车辆行驶速度对照图，从图中可以看出，零速检测方法检测到的零速时刻很好地反应了车辆处于停止时的时刻。

图6 零速检测方法检测到零速时刻与车辆行驶速度

3 结论

采用ZUPT与DZUPT相结合的运动约束辅助方式，在考虑惯导系统杆臂误差及安装误差角的前提下，给出了相应的组合滤波模型，并进行了动态跑车试验。试验结果表明，在GNSS信号较差甚至无GNSS信号的路况场景下，零速检测方法均能有效判断出车辆的行驶状态，给出相应的运动约束辅助信息，保证车载组合导航系统的精度。