基于双因子抗差估计的SINS/BDS有轨电车组合导航算法研究

何浩洋，夏荣斌

(1.南京恩瑞特实业有限公司，江苏 南京 211100; 2. 兰州交通大学 常州研究院，江苏 常州 213000)

随着城市轨道交通的不断发展，作为列车运行控制基础的列车定位技术发挥着日益重要的作用。实时、精确、可靠的列车位置信息是缩短列车追踪间隔，提高列车调度效率，保证列车行车安全，实现列车最佳服务的前提。目前城市轨道交通主要采用速度里程计和多普勒雷达，随着有轨电车、无人驾驶、车车通信和市域城际铁路，以及北斗卫星的不断发展，如何利用北斗卫星的优势，把北斗系统应用于列车定位领域，构建基于北斗卫星的“天空地一体化”列车车载自主定位系统，降低列车定位系统建设和维护成本，提高列车定位精度，确保列车安全行车间隔，提升列车移动闭塞效率，是列控系统向智能化和信息化及四网融合发展的需要。目前，在基于北斗(Beidou Navigation Satellite System, BDS)的定位应用系统中，与其惯性导航系统(Strapdown Inertial Navigation System，SINS) 的组合导航是应用最广的组合模式之一。然而在实际应用系统中，北斗信号容易受到列车复杂运行环境的影响导致观测数据中引入异常噪声，以及列车运行过程中的高频震动会导致SINS解算精度结果下降。并且，组合导航计算机信息融合结果输出时刻一般与实际传感器的输出存在一定的时间滞后，这种滞后性将带来较大的时间不同步误差，降低定位系统的实时性。

目前，针对组合导航系中的噪声抑制，提高滤波估计精度和增强滤波过程的鲁棒性问题，国内外学者做了大量的研究，提出了如基于M估计的抗差滤波方法[1-2]，基于信息χ2检测的粗差探测方法[3-4]以及基于Huber的鲁棒滤波方法[5]等。经典的多传感器信息融合算法多以Kalman滤波理论为核心，后提出的扩展Kalman滤波算法(Extended Kalman Filtering, EKF)[6-7]、无迹Kalman滤波算法(Uncensed Kalman Filtering, UKF)[8]和容积Kalman滤波算法(Cubature Kalman Filtering, CKF)[9-10]解决非线性系统线性化的问题，优化传统Kalman滤波只适用于线性系统的缺陷，而针对系统噪声均方差阵模型与实际噪声特征不匹配则提出了自适应Kalman滤波算法(Adaptive Kalman Filtering, AKF)[11]。随着传感器维数的增加和列车运行环境的复杂多变，传感器有时会产生异常观测，而这些异常观测对融合精度有很大的影响且难以被建模和预测，为了保证系统的容错能力，使系统能实时检测并隔离故障传感器的测量结果，保证整个系统在有故障的情况下依旧能正常工作，需要对传感器融合架构进行优化，改变传统的融合方式。戴海发等[12]等提出一种改进的多传感器数据自适应融合方法, 在贝叶斯理论的框架下，利用传感器测量值之间的差值自适应建立传感器的后验概率分布模型，并结合互信息的理论实时识别和剔除异常观测值，在存在异常观测值的情况下，信息融合的性能明显优于一般的贝叶斯融合方法。严春满等[13]针对滤波算法在模型失配和噪声时变情况下出现精度下降甚至发散的问题，提出一种基于多重渐消因子的自适应IEKF算法，利用一个基于正态分布的限定记忆信息协方差估值器来计算信息协方差估计值，并根据估计均方误差把多重渐消因子分配给各数据通道；再依照χ2检验原理判断系统是否异常，仅在系统异常时才引入渐消因子；最后利用目标与观测站间的径向距离和方位角信息，实现了IEKF迭代次数的自适应控制。在组合导航延时误差修正方面，付廷强等[14]建立了GNSS/SINS系统的延时估计模型，估计时间同步误差，然后构建残差传播方程，利用残差重构的方式进行延时补偿，实现基于软件的时间同步。

针对组合导航中的各导航传感器的信息更新频率异步导致的延时误差和列车复杂环境导致异常噪声导致估计精度下降问题，提出一种基于双因子抗差滤波和时延估计与补偿的SINS/BDS列车组合定位算法，在传统组合估计模型中引入延时估计模型，并利用双因子敏感系统的动力学模型误差和观测模型误差，实现不同导航子系统的时间不同步估计与补偿和降低噪声对导航精度的影响。

1 SINS/BDS松组合误差模型

松组合数据融合常采用Kalman滤波方法，利用惯导的误差方程作为系统方程，根据应用环境及需求来选择系统误差状态量，并构建状态方程。量测方程是利用BDS输出的位置、速度信息与INS更新的位置、速度信息差值作为Kalman滤波器的量测输入，滤波估计的状态量通过反馈校正来实时修正组合系统的导航信息，从而获得组合导航系统的输出值，其系统模型见图1。

图1 SINS/BDS松组合结构图

在位置速度方式下的松组合导航误差模型中，选取惯导系统中的姿态误差、速度误差、位置误差以及陀螺仪和加速度计的常值漂移误差作为组合导航系统的状态变量，采用位置量测值(惯导系统给出的纬度、经度及高度信息和BDS接收机给出的相应位置信息的差值)和2个系统给出的各方向上速度的差值作为量测信息，系统的状态空间模型如下[15]

( 1 )

( 2 )

2 SINS/BDS延时误差模型

在SINS/BDS组合导航系统中，由于BDS和SINS系统更新时间的不同，组合导航计算机对各导航子系统的采样时间也存在差异性，传感器信息之间不同步造成导航计算结果的时间延迟误差，尤其针对160 km/h及以上的市域、城际列车来说，时间滞后将引起较大的速度和位置误差，必须对时间不同步误差进行估计和补偿，BDS与SINS之间的时间不同步模型见图2。

由图2可知，SINS的惯性滞后与BDS的卫星滞后并不相同，两类传感器之间的相对滞后为时间不同步误差，且完整的时间延时补偿应包含SINS与BDS之间导航计算机采集的时间差以及定位更新的耗时时间。

图2 SINS/BDS时间不同步模型

2.1 基于SINS/BDS的列车组合模型

基于SINS/BDS的列车组合模型中，Kalman滤波算法是一个被广泛使用的信息融合算法。然而在实际应用系统中，由于列车运行环境的复杂多变会导致观测数据中存在异常噪声，影响组合导航的定位精度，降低了列车运行的可靠性。因此，提出一种基于双因子抗差估计的SINS/BDS组合算法模型，其原理见图3。由IMU输出的角速度和加速度信息经过惯性导航系统解算出速度和位置信息，然后与BDS输出的速度位置信息进行融合，经过双因子抗差滤波估计，最终输出高精度的组合定位信息，实现列车可靠稳定运行。

图3 基于双因子抗差估计的SINS/BDS的列车组合系统框原理

2.2 系统状态模型

根据式(1)和式(2)，在SINS误差模型的基础上，考虑杆臂误差和时间不同步误差，系统19维状态向量为

( 3 )

2.3 系统量测模型

在惯导误差分析的基础上，考虑SINS与BDS之间的杆臂误差和时间不同步误差，则基于速度和位置观测的SINS/BDS系统量测模型如下

(4)

2.3.1 杆臂误差模型

SINS定位解算基准为IMU的几何中心，BDS以接收机天线的相位中心为参考基准，在列车组合导航系统中，SINS与BDS安装位置之间的偏差通常会引入一定程度的速度和位置误差，杆臂误差示意见图4、图5。

图4 列车安装杆臂误差示意

图5 SINS与BDS之间的杆臂误差

图5中，b系为SINS所在载体坐标系；e系为地理坐标系；n系为东-北-天导航坐标系。假设SINS相对于地心的矢量为r，BDS相对于地心的矢量为R，BDS中心相对于SINS的矢量为δl，则三者之间满足

r=R+δl

( 5 )

对式(5)求导，得相对于地理坐标系e下得关系

( 6 )

式中：ωeb为SINS输出的相对于地理坐标系的角速度，SINS与BDS由杆臂引起的速度误差可表示为

( 7 )

将式(7)投影至导航坐标系，则

( 8 )

相应地，位置误差可表示为

( 9 )

2.3.2 时延误差模型

在基于BDS/SINS的列车车载自主定位系统中，组合导航计算时刻与传感器信息采集之间通常包含一定的时间滞后，见图6。

图6 BDS与SINS时间不同步误差

SINS速度与BDS速度之间的关系可表示为

(10)

式中：an为列车不同步时间的平均线加速度，利用SINS高频率、短时精度高的特点，an可近似为

(11)

相应地，位置不同步误差可表示为

(12)

综合杆臂误差和时延误差模型，式(4)中量测转移矩阵和量测噪声矩阵为

(13)

(14)

式中：Vv为速度噪声；Vp为位置噪声。

3 自适应双因子抗差估计

由于有轨列车跨域长、运行环境复杂多变，BDS受到多径干扰、随机噪声，SINS受到列车机动干扰等多种噪声影响导致系统量测中包含各种异常野值，从而影响组合导航的系统精度。针对传统Kalman系统对噪声不敏感的特点，引入自适应双因子抗差滤波算法，对组合系统的动力学模型和异常野值引起的噪声模型不适应问题，利用动力学模型信息自适应因子和观测模型自适应因子建立系统局部模型认可度，调节模型误差和观测误差对状态估计的影响。

3.1 自适应抗差算法

针对有轨列车非线性特点，对式(1)进行离散化，系统非线性模型为[4]

(15)

式中：Φk/k-1为状态一步转移矩阵；Γk/k-1为系统噪声分配矩阵；Hk为量测矩阵；Wk-1为l维的系统噪声向量；Vk为m维的量测噪声向量，两者都是零均值的高斯白噪声向量序列(服从正态分布)，且它们之间互不相关，即满足

式中：E[]为求矩阵的误差。

根据最小二乘原则构造损失函数Jk

(16)

(17)

(18)

则式(18)可变换为

(19)

综上，根据Kalman估计原则，双因子抗差迭代求解算法为

(20)

3.2 自适应因子求解

(1) ∂k观测自适应因子观测自适应选取

(21)

式中：tr(·)为矩阵的迹；ek为预测残差向量

ek=Zk-Zk,k-1=Zk-HXk,k-1

(22)

相应地

式中：c0和c1为常量，c0取值范围为1.0～1.5，c1取值范围为3.0～4.5。

(2)βk动力学模型信息的自适应因子选取

(23)

式中：dk表示预测状态残差,可表示为

(24)

相应的

4 仿真及实验

式中：c0和c1为常量，c0取值范围为1.0～1.5，c1取值范围为3.0～4.5。

4.1 仿真

为了验证本文所提出的时延计算模型和双因子抗差估计算法的有效性进行相关仿真，传感器具体参数如表1所示。

表1 传感器仿真参数

整个仿真过程持续时间为673 s，初始位置为[32.106°N, 103.726°E, 100 m]，初始姿态为[0°, 0°, 0°]，初始速度为0 m/s，SINS输出频率为200 Hz，BDS输出频率为1 Hz，SINS与BDS之间存在0.1 s的延时，杆臂误差为[1.0，2.0，2.5] m，假设系统噪声为已知的高斯白噪声，仿真理想轨迹和速度见图7、图8。

图7 理想轨迹示意图

图8 理想速度示意图

(1) 延时估计模型仿真比较

为验证组合系统模型的可行性，将传统SINS/BDS松组合模型和延时估计模型进行比较，图9、图10为仿真的速度误差和位置误差。

图9 速度误差比较

图10 位置误差比较

从图9和图10可以看出，当SINS与BDS之间存在杆臂误差和延时误差时，采用延时估计模型能提高更好的精度。图11和图12为系统延时估计结果和杆臂误差估计结果，与系统仿真所设置的0.1 s延时误差和[1.0，2.0，2.5] m的杆臂误差结果相一致。

图11 延时估计结果

图12 杆臂估计结果

(2) 抗差估计仿真验证

为验证本文所提出的双因子抗差滤波算法对定位系统的抗野值效果，在BDS定位系统测量得到的位置和速度信息中增加了一定的噪声，量测噪声根据如下方式产生

(25)

(26)

式中：PBDS为北斗位置的噪声特性；VBDS为北斗速度的噪声特性；～为仿真的北斗速度和位置测量的误差分布；w.p.为概率，即量测信息中有10%的概率出现方差为正常值200倍或以上的量测噪声，以此来模拟BDS量测信息中的异常值。

图13为BDS有无野值的位置仿真结果，图14为BDS有无野值的速度仿真结果，图15和图16为仿真位置误差和速度误差示意。

图13 BDS仿真位置数据

图14 BDS仿真速度数据

图15 位置误差

图16 速度误差

从图15和图16中可以看出，红色曲线所表示的基于线性Kalmam滤波的松组合方法受到BDS系统量测异常值的影响，其速度和位置误差具有较大的波动性，即在量测存在野值的情况下线性卡尔曼滤波方法已不再适用。蓝色曲线代表基于双因子抗差估计的鲁棒滤波松组合方法(抗差Kalman)，在BDS系统量测信息出现野值时，双因子抗差Kalman方法的位置误差仍然会出现一定程度的波动，这是由于抗差估计方法对于量测信息中的较大野值无法完全隔离，量测异常信息仍然对滤波结果造成的一定的影响。然而从蓝色曲线和红色曲线的波动情况可知，本文提出的抗差滤波估计算法具有较高的精度，比传统Kalman算法能更好的提高组合导航系统的定位性能。

为了更加直观的对比量测异常情况下算法的定位精度。表2给出了定位结果的均值、标准差和均方根误差统计结果。

从表2中可以看出，在异常量测噪声的影响下，基于双因子的SINS/BDS抗差估计算法具有更高的精度。

表2 Mean和RMSE统计结果

4.2 车载实验

为了验证基于双因子抗差估计的SINS/BDS有轨列车组合导航算法研究在实际工程中的有效性及可靠性，利用有轨电车车载试验进行验证。图17为测试平台设计原理。

图17 车载定位系统测试平台设计原理

本次实验持续时间为1 500 s，传感器参数与仿真实验相同。由于BDS收到多径干扰、空间环境和各种不确定呢因素的干扰，BDS的位置结果包含不同程度的随机噪声。实验同时比较了传统Kalman算法、基于时延估计的Kalman算法D-Kalman以及双因子抗差估计算法(R-Kalman)对车载自主定位系统的最优估计结果。表3为此次车载实验的速度和位置误差统计。图18、图19和图20给出此次车载实验的轨迹结果、速度误差结果和位置误差结果对比，图21为杆臂估计结果，图22为时延估计结果。

图18 车载实验轨迹结果

图19 速度误差比较

图20 位置误差比较

表3 速度、位置误差统计表

图21 车载实验杆臂估计结果

图22 车载实验延时估计结果

如图19和图20所示，传统Kalman估计对量测数据中的异常值较敏感。通过对表3中的统计结果的分析，可以得出结论，本文所采用的抗差延时估计算法算法具有最佳的定位性能，其x轴速度误差的均方根误差为0.023 1 m/s,比延时Kalman和传统Kalman分别降低了74.53%和78.63%，y轴降低了58.3%和68.38%，x轴位置误差降低了62.51%和71.28%，y轴位置误差降低了67.49%和64.49%。由实验结果分析可知，对SINS与BDS安装位置引起的杆臂误差和数据采集频率差异引起的子系统之间的延时误差进行有效估计和补偿能提高系统的定位精度。并且，针对列车定位复杂环境对量测地影响，本文所采用的双因子抗差估计算法能有效地抑制系统异常噪声，提高定位精度。

5 结论

本文提出了一种基于双因子抗差估计的SINS/BDS有轨列车组合导航算法，对SINS/BDS之间的位置安装引起的杆臂误差和子系统数据采集引起的时间滞后误差进行估计和补偿。同时，为了修正中高速列车高动态复杂环境中异常野值对定位精度的影响，引入双因子抗差估计算法，通过自适应因子降低量测噪声对系统动力学模型和噪声模型的影响。通过仿真和车载实验证明，本文提出的模型和算法能更好适应有轨列车的实际情况，对异常野值具有更好的抑制能力，相对于传统组合导航系统，定位精度更高。