一种基于ST-EKF的惯性/里程计滑动窗口滤波技术

杜学禹，王茂松，吴文启，张 靖

(1.国防科技大学智能科学学院，长沙 410073；2.北京航天发射技术研究所，北京 100076)

0 引言

近年来，随着智能化水平的不断提高，无人驾驶技术取得了飞速发展，Google、特斯拉、丰田等厂商纷纷将高新的科研成果投入市场，给人们的生活带来了许多便利。但是，现阶段的无人驾驶技术远没有达到十分完善的地步。目前，无人驾驶车辆的导航定位技术广泛使用全球导航卫星系统(Global Navigation Satellite System，GNSS)。在恶劣天气、植被遮挡、缺乏基站覆盖等复杂野外环境下，GNSS难以提供稳定、有效的信号。因此，如何构建在卫星拒止条件下具有更高精度的自主导航定位系统是推动无人驾驶技术进一步发展的关键问题。

与GNSS相比，轮式里程计(Odometer，OD)不需要对外发射接收信号，可以通过测量车轮转过的角度独立提供车辆的行驶速度和路程，对外部因素的抗干扰能力较强，与捷联惯导系统(Strapdown Inertial Navigation System，SINS)结合使用有助于提高复杂环境下车载导航系统的定位精度[1-3]。但是，SINS／OD组合导航系统在缺乏GNSS等外源信息校正时，系统的位置定位误差会随着时间的推移或路程的增长逐渐累积，使最终解算得到的行驶轨迹偏离实际的路线。同时，当导航平台处于复杂外部环境时，传统的基于扩展Kalman滤波(EKF)的捷联惯性组合导航系统容易出现方差估计不一致的问题[4]，这也将影响车辆的导航定位精度。

RTS平滑算法[5]是Rauth等人基于极大似然估计准则推导出的，对一个区间内的状态估计和误差协方差进行最优平滑的固定区间事后处理方法。相比于单向滤波只能基于当前量测信息来估计当前状态，RTS平滑算法则可以利用区间内所有的量测信息对任一时刻状态进行估计，常用于惯性卫星组合导航的高精度数据处理与卫星的事后基准确定[6-7]。文献[8]将RTS平滑器与状态变换Kalman滤波(ST-EKF)[9-11]结合运用在水下自主潜航器(Autonomous Underwater Vehicle，AUV)的水下勘测航迹修正，有效校正了水下航行过程中AUV的位置误差。因此，引入RTS平滑器是一种改善复杂环境下无人驾驶车辆导航定位精度的可行方法。但是，由于传统的RTS平滑器多用于数据的事后处理，缺乏对航迹校正的实时性。同时，传统的RTS平滑器对于平滑区间的选取不够灵活，有时不能发挥出最佳的滤波效果。

本文在基于ST-EKF的SINS／OD组合导航算法[11]基础上，对RTS平滑器的实时性做出改进，提出了一种基于ST-EKF的惯性／里程计滑动窗口滤波技术。先利用系统量测信息进行前向滤波，滤波过程中采用滤波鲁棒性更好的ST-EKF替代传统的EKF，并将解算得到的误差状态向量、误差协方差和状态转移矩阵等数据实时存储在滑动窗口中。当滑动窗口中储存的信息达到设定长度后，系统利用存储数据对区间内的所有状态进行最优平滑。在采用该滤波技术时，可以根据车辆运行轨迹实时改变滑动窗口的长度以取得最优航迹修正效果。

1 基于ST-EKF的惯性/里程计组合导航模型

1.1 ST-EKF与EKF模型对比

常用的线性速度误差微分方程可以定义为[12]

式(2)中，gn为当地重力矢量，εb为陀螺测量零偏，Δb为加速度计测量零偏，wg和wa分别为陀螺和加速度计误差的白噪声向量。

由式(2)可知，其不再含有比力项，而是由重力相关项替代。对于一般的车载导航来说，gn的变化很小。因此，基于ST-EKF的组合导航可以有效避免由于比力量化噪声和姿态的剧烈变化带来的计算不精确的问题，避免了误差的累计，确保了滤波的鲁棒性。

1.2 基于ST-EKF的系统误差状态模型

定义惯性／里程计组合导航系统的21维误差状态向量x

可以构建惯性／里程计组合导航系统的误差状态方程

式(4)中，x为误差状态向量，F为系统矩阵，G为噪声转移矩阵，w为过程噪声矩阵，它们具体定义如下[9-11]

式(5)～式(7)中，τδk为里程计刻度因子误差相关时间，τδαy为俯仰安装误差角相关时间，w中元素分别为陀螺零偏噪声、加速度计零偏噪声、刻度因子误差Markov过程的白噪声、俯仰安装偏角误差Markov过程的白噪声，FSTEKF－INS－NED为ST-EKF滤波框架下的15维状态系统矩阵。FSTEKF－INS－NED的具体定义如下[9-11]

式(9)～式(14)中，vN、vE分别为北向速度、东向速度，RN为子午圈主曲率半径，RE为卯酉圈主曲率半径，L、h分别为纬度、 高度，Ω为地球自转速度。

1.3 基于ST-EKF的系统观测模型

式(15)中，δα为捷联惯导系统与车体之间的安装误差。

由捷联惯导系统解算的速度在车体坐标系(m系)下的投影可以表达为

将捷联惯导系统解算出的速度信息投影到里程计坐标系下与里程计的速度相减作为SINS／OD组合导航系统观测量δzv， 即

由式(17)可以定义基于ST-EKF的SINS／OD组合导航系统的观测方程

式(18)中，v4×1为测量噪声向量，Hv为观测矩阵。Hv满足

式(20)中，vD为里程计测量的前向速度。

2 基于ST-EKF的滑动窗口滤波技术

2.1 技术概论与应用流程

本文提出的基于ST-EKF的惯性／里程计滑动窗口滤波技术是在基于ST-EKF的惯性／里程计组合导航算法的基础上做出了改进，主要由ST-EKF前向滤波器、滑动窗口、RTS平滑器三部分组成[8]。导航平台开始运动后，首先进行前向滤波，记录下系统时间更新与量测更新后的误差状态向量、误差协方差矩阵与状态转移矩阵，并存储到预先设置好容量的滑动窗口中。当滑动窗口中的滤波信息达到设定长度时，提取储存数据对区间内的相关状态进行RTS反向平滑和航迹修正。同时，为了使平滑时刻的滤波结果也接受优化，利用前一时刻的平滑结果对当前时刻进行第二次滤波。

在实际应用过程中，以滑动窗口中存入的第一组滤波信息到储存的滤波信息长度达到滑动窗口的设定长度为一个平滑周期，系统利用周期内存储的所有前向滤波信息进行反向平滑，修正载车航迹，然后初始化滑动窗口空间，继续进行前向滤波和数据存储，直到滑动窗口空间达到存储上限或载车到达行驶终点。

相对于文献[8]提出的ST-EKF平滑算法，本文根据应用平台的改变在SINS／OD组合导航系统上对算法进行了重新定义，并针对该算法只能应用于事后处理的问题做出了改进，通过引入滑动窗口的概念使算法成为一种在线平滑方法。相对于传统的平滑方法，滑动窗口的引进增强了技术的实时性，减小了导航计算机的计算量，滑动窗口中只需要存储当前平滑周期内的xf，k、xf，k／k－1、Pf，k、Pf，k／k－1、Φk，k－1等前向滤波信息， 反向平滑结束后便可以初始化进入下一个周期。该技术的具体应用流程如图1所示。

图1 基于ST-EKF的滑动窗口滤波技术实际应用流程Fig.1 Practicalapplication flow of sliding window filtering technology based on ST-EKF

图1中，fIMU为IMU的输出频率。

2.2 基于ST-EKF的前向滤波与滑动窗口数据存储

基于ST-EKF的前向滤波过程中的误差状态模型与量测模型的建立基本与前文中的基于ST-EKF的SINS／OD组合导航系统相一致，滤波过程可以用以下5个公式表示[5，8]

式(21)～式(25)中，下标f表示前向滤波过程，下标k表示滤波时刻，P为系统的误差协方差矩阵，Q为系统的过程噪声方差矩阵，Γ为离散化后的噪声转移矩阵，R为系统的量测噪声方差矩阵，K为滤波增益矩阵。其中，式(21)、式(22)为系统的时间更新过程，式(23)～式(25)为系统的量测更新过程。

系统前向滤波过程中，每次时间更新与量测更新结束后， 将得到的滤波信息xf，k、xf，k／k－1、Pf，k、Pf，k／k－1、Φk，k－1存入滑动窗口并标注好时刻，滑动窗口的长度L可以根据行车里程、故障点的分布时段等实际情况灵活调整。此外，由于只有受噪声驱动的状态分量才具有可平滑性，不受噪声影响的随机常值状态分量的平滑精度不会优于单向滤波的精度。为了减少不必要的计算，本文仅将姿态失准角、速度误差、位置误差、里程计刻度因子误差作为平滑状态存储到滑动窗口空间中， 即x1－10＝[(φn)T(δvnφ)T(δrn)TδkD]T。

2.3 基于ST-EKF的RTS平滑算法

当滑动窗口空间中实时存储的滤波信息长度l与滑动窗口长度L相等时，停止前向滤波，以当前的误差状态向量xf，k作为初始量xs，k，以当前的误差协方差矩阵Pf，k作为Ps，k， 利用滑动窗口空间中的滤波信息进行反向RTS平滑，具体过程如式(26)～式(28)所示[5，8]

式(26)～式(28)中，下标s表示RTS平滑过程。

由式(26)～式(28)可知，平滑时刻的误差状态向量xs，N和误差协方差矩阵Ps，k没有进行平滑优化而是与前向滤波的相关状态保持一致，不符合无人车导航定位的实时性要求，这也是传统RTS平滑算法的一个缺陷。为了改善算法的实时性，本文采用平滑时刻k前一时刻的平滑后误差状态向量xs，k－1和误差协方差阵Ps，k－1进行再一次的预测与量测更新， 即采用xs，k－1、Ps，k－1替代式(21)、式(22)中的xf，k－1、Pf，k－1进行第二次前向滤波，使得平滑时刻的滤波状态在一个平滑周期结束时也能受到优化改善。

3 仿真实验结果与分析

3.1 实验设计

为了检验本文提出的基于ST-EKF的惯性／里程计滑动窗口滤波技术的实际应用效果，对真实的长行驶里程跑车数据进行事后处理，记录了组合导航系统水平定位精度随滑动窗口长度的变化情况，并与同等条件下的基于EKF的滑动窗口滤波技术处理后的系统水平定位精度作出对比。同时，为了观察将里程计刻度因子误差作为平滑状态后对滤波平滑效果的改善性，对分别采用添加里程计刻度因子误差的十平滑状态和传统的九平滑状态[8]的基于ST-EKF的惯性／里程计滑动窗口滤波技术处理后的系统水平定位精度作出了对比。

本次实验使用的数据来自北京至河北省的一次长行驶里程跑车实验[3]，实时行车时间总计7h，在大约3.5h时到达半程(去程)终点，总行程为489.88km。图2为本次实验在二维地图下的行车轨迹。

图2 二维跑车轨迹图Fig.2 Two dimensionaltrajectory of the land vehicle experiment

实验车辆配备了导航级的高精度激光陀螺IMU装置和比例因子约为53p／m的轮式里程计，IMU的输出频率为200Hz，陀螺零偏稳定性为0.01(°)／h，角度随机游走为0.0003(°)／h1／2，加速度计零偏稳定性为50μg，速度随机游走为5μg／Hz1／2。在跑车实验中，车辆的双位置精对准时间为180s。

3.2 实验结果

图3为不进行平滑处理的EKF和ST-EKF组合导航算法解算得到的水平位置分别相对于基准位置的水平定位误差和精度随时间以及行驶里程的变化而产生的变化趋势，图4为滑动窗口长度L＝6000时采用十平滑状态平滑处理后的EKF和STEKF组合导航算法的水平定位误差和精度变化趋势。基准位置由惯性／卫星组合导航事后平滑算法[11]解算得到。

图3 不加平滑处理的EKF和ST-EKF组合导航算法的水平定位结果Fig.3 Horizontalpositioning results of EKF and ST-EKF integrated navigation algorithms without smoothing

图4 L＝6000时十平滑状态平滑处理后的EKF和ST-EKF组合导航算法的水平定位结果Fig.4 Horizontalpositioning results of EKF and ST-EKF integrated navigation algorithms after ten-smooth state processing when L＝6000

图3、图4对比可以发现，平滑处理后的SINS／OD组合导航算法相对于未处理时，水平定位误差的抑制效果得到了有效提升，误差曲线更加平滑，跳点大幅度减少。同时，图3、图4表明，在非平滑和平滑两种情况下，ST-EKF都比EKF具有更好的滤波效果，水平定位精度优于行程的0.05%DT(Distance Traveled)。

滑动窗口长度L＝6000时，增加里程计刻度因子误差为平滑状态的基于ST-EKF的滑动窗口滤波技术与不增加里程计刻度因子误差为平滑状态时的水平定位误差对比情况如图5所示。

图5 L＝6000时分别采用十平滑状态和九平滑状态处理后的ST-EKF组合导航算法的水平定位结果Fig.5 Horizontalpositioning results of ST-EKF integrated navigation algorithms after ten-smooth state processing and nine-smooth state processing when L＝6000

由图5可知，在同等条件下，增加里程计刻度因子误差为平滑状态的滑动窗口滤波技术具有更好的航迹修正效果。

在实际应用中，滑动窗口的长度可以根据载车行驶里程、故障点分布时段实时进行调整。为了验证滑动窗口长度对航迹修正效果的影响，在采用同一组车载实验数据的前提下，改变L的取值，计算并记录了在不同条件下基于EKF和STEKF的滑动窗口滤波技术在选取十平滑状态时的水平定位结果。同时，考虑到本次车载实验是沿着公路往返行驶，如果只考虑全程的水平定位误差，可能部分误差会相互抵消从而影响实验结果的准确性。因此，为了更客观地评价本文提出的算法的航迹修正效果，表1和表2分别统计了单程(去程)和全程的水平定位结果。

以行驶轨迹中的半程(去程)终点为界，表1统计了五种情况下的半程(去程)水平定位误差的均方根误差(RMSE)以及RMSE相对于这一半行程的百分比，表2则为同等条件下的全程水平定位误差的RMSE以及RMSE相对于全程的百分比。

表1 水平位置的半程(去程)误差统计Table 1 Statistics of half-way(outward voyage)errors in horizontalposition

表2 水平位置全程误差统计表Table 2 Statistics of whole journey error in horizontalposition

由表1、表2可知，基于EKF的惯性／里程计组合导航算法的半程(去程)水平位置的RMSE为57.1512m，全程的RMSE为54.7629m。在滑动窗口长度为6000时，本文提出的采用十平滑状态的基于ST-EKF的惯性／里程计滑动窗口滤波技术的半程(去程)水平位置的RMSE为32.6988m，全程的RMSE为28.6417m，相对于基于EKF的惯性／里程计组合导航算法分别减少了42.79%和47.70%，进一步验证了该技术的有效性与实用性。同时，由表1、表2可知，在样本长度允许的前提下，适当加大滑动窗口的长度可以使平滑取得更好的航迹修正效果，提高了系统的水平定位精度。

4 结论

针对长行驶里程条件下无人驾驶车辆的导航定位精度优化问题，本文提出了一种基于ST-EKF的惯性／里程计滑动窗口滤波技术。在基于ST-EKF的惯性／里程计组合导航方案的基础上引入了滑动窗口和RTS平滑器，利用实时存储的前向滤波数据进行航迹修正，提高了车辆的导航定位精度。实验表明，该技术相比于传统的EKF组合导航方案具有更好的滤波鲁棒性，并且在经过RTS平滑处理后，取得了较好的航迹修正效果，半程(去程)水平位置的RMSE减少了42.79%，全程水平位置的RMSE减少47.70%。因此，在卫星拒止的复杂野外环境下，该技术可以帮助无人车迅速确定自身所处区域，使得视觉等传感器的后续处理能更加精确，具有良好的工程应用价值。