基于改进李群卡尔曼滤波的惯性/里程计组合导航方法

崔加瑞,张礼廉,王茂松,吴文启,杜学禹

(国防科技大学智能科学学院,湖南长沙 410073)

1 引言

陆用车载无人系统作为智能无人系统的重要组成部分,在现代智能物流、无人驾驶以及军事领域得到重要应用[1].目前卫星导航依然是主要的导航手段,但是卫星导航在城市丛林、隧道山洞以及强电磁环境中的可用性会降低甚至不可用.惯导系统具有自主性强的特点,但是其误差积累随时间发散,需要外源传感器进行辅助,里程计具有成本低、自主性强的特点,因此,惯性/里程计组合导航系统是陆用车辆在卫星拒止条件下长航时自主导航定位的主要方法[2].常用的惯性/里程计组合导航算法主要为扩展卡尔曼滤波器,惯导系统的陀螺仪、加速度计零偏以及惯导系统和里程计量测中心之间的俯仰、偏航安装角和杆臂误差以及里程计的标度因数误差是组合导航系统的主要误差源,通常利用最小二乘方法离线标定或作为滤波器的状态进行在线估计和补偿.武元新[3]详细研究了惯性/里程计组合导航系统中误差源的可观测性,付强文等[4]对惯性/里程计组合导航系统建模并实现了主要误差源的在线估计,文献[5]中研究了一种自适应双阶段滤波器,有效地估计了里程计标度因数误差的变化,并对变化的过程噪声具有鲁棒性.此外,也有针对里程计脉冲截断误差所进行的研究工作,例如,欧阳威等[6]考虑了一种多模型自适应方法,周召发等[7]将截断误差建模在状态中并提出一种脉冲观测方法.基于机器学习的方法也在近年来被应用于惯性/里程计自主导航领域[8],但是其主要应用在低精度惯导系统中.对于高精度惯性/里程计组合导航系统来说,目前主流的算法依然是扩展卡尔曼滤波器的变体,以加性的导航状态误差作为卡尔曼滤波器的状态.近年来由于李群理论在组合导航系统模型建模和观测器设计中的表现良好,受到国内外研究者的重视,其本质是利用三维空间中的刚体变换群替代传统扩展卡尔曼滤波器模型中的加法群[9].王茂松等[10]提出的基于状态变换的卡尔曼滤波器(state-transform extended Kalman filter,ST-EKF),将姿态误差的影响更严格地定义在速度误差中[11],得到了良好的效果,进一步研究发现,可以将其视为基于李群非线性状态的卡尔曼滤波器(Lie group extended Kalman filter,LG-EKF)的一种特例.基于李群非线性状态的卡尔曼滤波器在卫星良好的组合导航应用中得到了良好应用和检验[12],但是在惯性/里程计组合导航中尚未得到应用,主要存在两方面问题:一是在文献[12]中,由于非线性速度误差和位置误差的定义是相对惯性系的,其中包含姿态误差与相对于地心的矢径和相对于惯性系速度矢量的耦合,导致在进行一阶近似舍弃高阶项时存在显著的数值计算误差;二是对于车载导航来说,通常情况下在导航系下水平初始误差角相对航向初始误差角更小,水平误差角和航向误差角能够解耦设置,然而使用地心地固坐标系的姿态误差定义的初始角设置使得3个姿态误差角产生了耦合.以上两点原因均增加了滤波器初始设置的难度.针对上述惯性/里程计组合导航中的两个问题,本文提出一种改进的李群卡尔曼滤波器,将世界坐标系同时作为参考系和投影系,简化了初始姿态、速度和位置误差方差的设置,同时降低了非线性误差状态中的计算误差.

本文接下来的安排是: 在第2节中阐述惯性/里程计系统模型,并给出了静态条件下的典型算例,以此说明之前提出的李群卡尔曼滤波器的不足;进而,在第3节中给出了基于世界坐标系改进李群卡尔曼滤波器组合导航方程的推导过程;并在第4节中分别进行了小初始对准误差条件下,4组长航时高精度车载实验的验证和大初始对准误差条件下开环轨迹实验,对比验证了本文提出的方法的有效性和优势;第5节总结了本文工作.

2 惯性/里程计模型与问题阐述

2.1 惯性/里程计模型

通常轮式里程计安装在车辆前轮或者后轮上,与惯导系统的相对空间关系如图1所示.

图1 惯性/里程计空间安装位置示意图Fig.1 The position diagram of SINS and odometer

安装在前轮的里程计会包含车辆转向的速度信息导致不能真实反映车辆前进方向的速度,因此,本文使用的里程计安装位置为后轮.其中:b系为以惯导的等效测量中心为原点构成的右手直角坐标系;m系为以里程计的等效测量中心为原点构成的右手直角坐标系.两者的顺序均为前右下.

由里程计脉冲计算的速度为

其中:vD为由里程计在采样时间间隔T内的脉冲数乘以标度因数K除以采样时间得到的计算速度;为利用非完整约束形成的三维速度矢量.

从空间关系上看,由于惯性系统和里程计的测量中心并不重合,由此导致需要对两个坐标系的空间安装误差角α=[αγ αθ αψ]T进行标定,使用一阶近似即得惯性/里程计组合导航系统的误差源通常考虑俯仰安装角αθ、偏航安装角αψ以及惯导和里程计中心的杆臂误差Lb.考虑到里程计的原理,当胎压由于环境气温或长时间驾驶发生改变时,车轮半径也会发生变化,因此,还需要考虑标度因数误差δkD的影响.本文对里程计的误差源考虑主要包含此3种,脉冲截断误差也具有影响,却不是主要误差源,本文中将其考虑为测量噪声是一种合理的假设,因此,得到含有误差的里程计速度为

2.2 基于相对地心参考系的李群卡尔曼滤波方法

存在的问题

图2 改进的李群卡尔曼滤波参考点示意图Fig.2 The reference point schematic diagram of the improved LG-EKF

世界系下的非线性误差状态数值远小于地球系下的状态数值,因此,在原理上能够减小初始对准误差角带来的数值计算误差的影响,并且在舍弃高阶项时带来的误差更小.需要注意的是,这是在静态条件下的算例,而在载体运动过程中进行数值计算,两者的误差依然存在数量级的差距.因此,经过改进的李群卡尔曼滤波器在原理上能够有效减小数值计算误差,提高滤波器状态估计精度.更加具体的相关符号定义可见组合导航书籍[13]或参考文献[12].

3 基于改进LG-EKF的惯性/里程计组合导航系统建模

改进的李群卡尔曼滤波器系统方程将原本相对于地心的参考原点移动到与地球系固连的起始点处,此坐标系称为世界坐标系,在机器人和视觉导航领域得到广泛使用.改进方法的优势在于减小了速度矢量和位置矢量的数值大小,显著降低了一阶近似过程中的计算误差,且初始姿态误差点处的水平误差角和航向误差角解耦,有利于初始方差的设置,提高状态估计的精度.

3.1 系统状态方程

改进后的导航状态误差定义(6)重写如下:

将姿态误差作一阶近似,关系式(8)成立,即

方向余弦矩阵的微分方程表示如下:

因此,得到姿态误差的微分方程表达式(12):

线性速度和速度误差状态的微分方程为

非线性位置误差状态微分方程推导过程为

由于车辆负载变化以及车胎压力变化通常导致里程计标度因数和俯仰安装误差角时变,因此,将两者建模为一阶马尔科夫过程.杆臂误差以及偏航安装误差角建模为常值.

选取状态变量和系统噪声如下:

结合式(12)(14)(17)-(18),可得LG-EKF的系统方程

其中:

3.2 观测方程

将惯导解算的b系原点速度在m系下表示为

忽略二阶小量得到

建立的观测方程为

其中υ3×1为速度观测噪声.建模为高斯白噪声,观测矢量则是用惯性系统的速度和里程计量测相减得到,即

将式(28)展开,并将式(2)代入得到

相比文献[4]中在扩展卡尔曼滤波器的公式(20),本文中的式(30)中与姿态误差耦合的相关项,在李群卡尔曼滤波器的观测方程中变为了零矩阵,这是应用了李群非线性误差状态的结果.在接下来的实验验证中,将验证本文改进的李群卡尔曼滤波器方法相较于改进前在计算精度上的优势.

3.3 滤波器更新

由于李群卡尔曼滤波器使用了非线性误差状态,导致其更新过程与传统的扩展卡尔曼滤波器的线性更新过程不同,李群卡尔曼滤波器状态更新时需要遵从非线性误差的定义.在组合导航过程中的状态更新为非线性形式,如式(32)所示:

其中约等号后面为将姿态误差的指数函数进行一阶近似得到的结果.

3.4 卡尔曼滤波器离散化

将连续形式的改进李群卡尔曼滤波器进行离散化,其过程与扩展卡尔曼滤波器相同,离散化的改进李群卡尔曼滤波器算法流程如表1所示.

表1 离散化李群卡尔曼滤波器算法流程Table 1 Discrete Lie group Kalman filter algorithm flow

其中Φk/k-1是离散化的系统矩阵

其中ΔT为离散时间.矩阵Q和R是器件随机噪声、马尔科夫随机游走噪声和测量噪声的谱密度

值得注意的是,在卡尔曼滤波器框架中纯惯导解算过程也需要在世界坐标系中进行,其解算过程等效于将当地导航系中的解算结果投影到世界坐标系.在世界坐标系解算的惯导状态进而应用在改进李群卡尔曼滤波器的系统方程和观测方程当中.

4 实验验证

4.1 小初始对准误差角实验验证

本文使用车载实验对所提算法进行对比验证,首先在初始对准良好的条件下,使用惯性/卫星组合进行600 s初始对准后进行惯性/里程计组合导航.初始姿态误差协方差的标准差分别设为0.5°,0.5°,0.5°.车载实验设备包括高精度光纤陀螺捷联惯导系统、轮式里程计,其参数如表2所示.使用惯性/卫星组合前后向平滑结果作为参考值,其精度约为1 m.为了增加实验的可信度和充分性,共进行了4组实验,实验时长均为4 h.实验轨迹如图3所示,其中,横坐标为经度,纵坐标为纬度,其中,轨迹1-3均为长沙市内闭环轨迹,轨迹4为从长沙出发至常德市的开环轨迹.

表2 捷联惯导系统和里程计参数表Table 2 Parameters of SINS and odometer

图3 车载实验轨迹图Fig.3 Diagram of vehicle experiment trajectories

将本文提出的方法命名为LG-EKF-w,与传统扩展卡尔曼滤波器(EKF)[4]、状态变换卡尔曼滤波器STEKF[10]和文献[12]中提出的李群卡尔曼滤波器(命名为LG-EKF-i)进行对比,4组实验的定位误差统计结果如表3所示.从表3中可以看出,在第1,3,4组中,本文提出的改进李群卡尔曼滤波器方法LG-EKF-w能够实现较基于惯性系的李群卡尔曼滤波器方法LG-EKF-i更好的数值计算精度.在第1,3组中,LG-EKF-i方法的精度不如传统EKF的方法精度,误差分别增加了3 m和5 m左右.

表3 车载实验定位误差统计结果(RMSE)Table 3 Position error results of vehicle experiment(RMSE)

更为明显的是,在第4组当中,LG-EKF-i方法出现了较大的数值计算误差导致其位置误差统计结果大幅度超过其他方法,达到了55.06 m,而其他方法保持在21.03～29.27 m的精度范围内.第4组差异较为明显的原因是其轨迹为开环轨迹,误差源被充分激励而显现在位置误差上.第1-3组的闭环轨迹中,确定性误差累积产生的位置误差会随着轨迹的闭环而被部分抵消,导致误差源激励并不明显,使得第1-3组的位置精度明显较第4组高,因而算法差异对比结果不明显.而在第4组完全开环情况下,误差的累积效果能够体现出算法明显的差异,因此,开环轨迹结果在惯性/里程计组合导航算法验证时具有较强的说服力和可信度.另外还能注意到,对惯性系卡尔曼滤波器方法而言,第2组的结果较为正常,并且表现出最高精度的位置误差统计结果.可能的原因是实验2中的初始对准误差角较小,非线性程度很低.并且,由于第2组的闭环较为完整,因此,其计算误差被大部分抵消,使得其精度比其他方法更高,但是与其他算法的差异较小.但是需要注意的是,在实验2中的两者差异较小,仅不足1 m,这和参考基准的精度相当,因此,也不能排除偶然性的因素.总体而言,在统计意义上,其他3组中的LG-EKF-i方法依然表现出数值计算误差较大的问题,并且突出体现在第4组中.

4.2 大初始对准误差角开环轨迹实验验证

由于开环轨迹的算法差异比较具有更高的可信度,所以,此处给出第4组开环估计实验在大初始对准误差角条件下的导航误差结果.首先,使用惯性/卫星组合进行600 s初始对准后,在初始对准结果上加入水平角1°,航向角10°,20°,30°的初始误差角再进行惯性/里程计组合导航.初始姿态误差的协方差的标准差分别设为1°,1°,5°.其中在1°,1°,30°初始对准误差角条件下的结果如图4-6所示.

图4 姿态误差角对比结果Fig.4 Attitude error angle comparison results

图5 水平速度误差对比结果Fig.5 Horizontal velocity error comparison results

图6 第4组车载实验水平定位误差图Fig.6 The horizontal position error of the 4th experiment

可以看出,在大初始对准误差条件下的开环轨迹实验中,基于改进的李群卡尔曼滤波器LG-EKF-w方法得到了更为精确的结果,而基于惯性系的李群卡尔曼滤波器LG-EKF-i的位置误差产生了较大的积累.在不同初始对准误差角条件下的位置误差统计结果如表4所示.

表4 开环长航时车载实验定位误差(RMSE)Table 4 Position error results of long-time openlooped vehicle experiment(RMSE)

这主要得益于两个方面: 一是其借助了李群非线性误差的定义,使得其应用的群结构更能够精确描述刚体运动.关于李群非线性误差的具体描述详见文献[9,12];二是选取了世界坐标系,减小了非线性误差状态中的速度和位置矢量模值,进而减小了卡尔曼滤波器在一阶近似过程中的计算误差.

5 结论

本文对高精度自主定位定向系统在卫星拒止条件下的非线性误差状态估计方法进行了研究,指出了李群卡尔曼滤波器的方法在一阶近似过程中存在的计算误差问题,对惯性/里程计组合导航方法进行了改进,利用与地球系固连的初始点坐标系也称为世界坐标系取代地心惯性坐标系,从而降低了李群卡尔曼滤波器中的非线性误差状态的计算误差,从而得到了更加精确的并适于工程应用的组合导航算法.开环大初始对准误差角条件下的车载实验结果表明,改进的李群卡尔曼滤波方法相对于扩展卡尔曼滤波方法,状态变换卡尔曼滤波方法以及基于惯性系的李群卡尔曼滤波方法精度更高,更加适用于高精度惯性/里程计组合导航的工程应用.