基于滤波和优化的惯性/里程计组合导航精度对比分析

王茂松,崔加瑞,刘若辰

（国防科技大学智能科学学院,长沙 410073）

0 引言

目前,基于因子图优化的方法在多传感器融合领域吸引了许多学者[1],其利用概率图模型对状态估计方程进行建模,利用非线性估计技术对状态进行迭代估计,在理论上具有对历史状态的修正和平滑作用。但是,目前因子图优化的研究多针对于惯性/卫星[2]或惯性/视觉组合[3],属于多约束系统,而对惯性/里程计组合导航缺乏对比研究。因此,本文将对比因子图优化技术和Kalman滤波器技术在惯性/里程计组合导航中的应用,并分析两者的适用场景和优劣,以期给出有益的参考。

惯性/里程计组合导航可以不借助外界信息,如无线电信息,可以作为陆地车辆及机器人的自主导航实现方法,基于Kalman 滤波技术的组合导航方法已经有较多研究并具有较为成熟的工程应用: 严恭敏[4]对高精度惯性/里程计组合导航以及航位推算模型中的安装角及杆臂误差进行了精确建模和补偿;Wu[5]针对惯性/里程计应用提出了一种自标定和初始对准技术;Ouyang 等[6]进一步对精确建模的惯性/里程计模型应用了多模型自适应估计算法提高定位精度。对比而言,因子图将组合导航系统的联合概率分布用概率图模型表示,利用系统的稀疏性,具有即插即用的能力。Kaess等[7]在因子图上应用了一种高效的增量推理算法,即ISAM2（Incremental Smoothing and Mapping）算法,使其性能接近于批量优化,进一步基于IMU 预积分的技术大幅缩减了需要优化的状态量,使得因子图优化的效率进一步提升。

本文首先介绍了基于因子图的惯性/里程计组合导航建模方法;然后,对比了基于Kalman 滤波和因子图优化的状态估计算法,说明其在状态估计原理上的差异;最后,基于陆用无人平台进行了惯性/里程计组合导航试验,对比了惯性/里程计组合导航在不同方法下的精度差异,分析了原因以及基于优化的惯性/里程计组合导航方法还需要解决的问题。

1 惯性/里程计因子图建模方法

因子图建模技术是一种利用概率图模型进行建模的技术,因子图F=（U,V,E） 是一种二分图,包含两种节点,即因子节点ϕi∈U、变量节点xj∈V以及连接两者的边eij∈E。连接变量节点ϕi的集合写作N（ϕi）,Xi是其上的一个子集,Xi表示任意节点xj的集合,则因子函数集可以写作ϕ（X）,因子图的全局函数可以写作

接下来给出惯性/里程计组合导航的因子图模型,如图1所示。

图1 基于因子图的惯性/里程计组合导航模型Fig.1 Diagram of factor-graph-based SINS/ODO integrated navigation

图1中,圆形框为系统历史的n个状态变量,即因子图中的变量节点xi（i=1,2,…,n）;黑色方框为因子节点ϕ（∗）,包括量测信息节点ϕODO、ϕIMU和先验节点x0。

1.1 预积分因子模型

预积分因子模型将两次观测间隔内的加速度计和陀螺仪的数据进行融合,使其能够在整个状态进行重新线性化的过程中保持不变,因此避免了重复计算,减低了计算量。预积分因子节点建模为如下残差函数

式（2）中,xk-1为模型在时刻tk-1的状态,预测函数hIMU（·）利用时刻tk-1到时刻tk的加速度计和陀螺仪增量数据递推出下一观测时刻的载体位姿,表示协方差为Σi的变量间的马氏距离。Σi实质上是加速度计和陀螺仪噪声强度的函数,协方差值应谨慎设置,因为它影响到最大后验（Maximum A Posteriori,MAP）估计的结果。

1.2 里程计速度约束因子模型

为了和Kalman 滤波器中速度观测的形式相一致,本文将里程计的脉冲进行差分得到里程计的速度,将当前时刻里程计速度作为速度约束因子节点,第m个里程计因子节点的残差函数如下

式（3）中,hODO（·）为将导航系下的速度状态转换为里程计坐标系下的速度状态,Rv为里程计的速度噪声。

2 基于因子图优化和Kalman 滤波的惯性/里程计组合导航算法对比

2.1 基于因子图优化的算法

为了得到最优状态估计,基于因子图的优化算法可以使用合适的优化算法来求得式（4）的最优估计结果,例如使用梯度下降法、Gauss-Newton 法和LM（Levenberg-Marquarelt）算法[8]等

为了评估因子图优化算法和Kalman 滤波器算法,本文采用GTSAM 开源因子图库[9]进行算法设计,利用ISAM2 算法进行增量递推,优化算法使用LM 算法,稀疏矩阵分解方法使用具有良好数值稳定性的QR 分解算法。以下为LM 算法的迭代计算公式

LM 的思想为使用Jacobi 矩阵的平方近似Hessian 矩阵并且添加阻尼项使其可逆。式（5）中,hlm为LM 算法的变量迭代更新方向,α为更新步长,J为Jacobi 矩阵,f为因子图模型构造的非线性最小二乘函数。通过调节LM 算法的μ可以使其转化为Gauss-Newton 法与梯度下降法。

ISAM2 算法解决了增量优化的问题,其思路是将因子图转化为Bayes 树,当系统添加一个新的因子时,仅影响包含相关变量的团和其父节点。受影响的团可以被重新组合为新的团结构,并形成新的子树,而不必更新整个Bayes 树,以此实现了增量优化过程。

2.2 基于Kalman 滤波器的算法

本文中,Kalman 滤波器使用状态变换Kalman滤波器（State-transform Extended Kalman Filter,STEKF）,采用考虑里程计的安装角和标度因数误差的模型,其相关算法见文献[10]。此处给出主要STEKF 的算法公式:

系统变量如下

式（6） 中,ϕn、δvnϕ、δrn、εb、Δb、δkODO、δαy-ODO、δαz-ODO分别为当地导航系下的姿态失准角、状态变换速度误差、位置误差、陀螺仪和加速度计零偏以及里程计标度因数误差和安装角误差。在里程计坐标系m系下的观测方程如下

式（7）中,×表示将三维矢量转换为对应的反对称矩阵,其他的相关参数可进一步参考文献[11]。

利用Kalman 滤波预测过程和更新过程的5 个公式即可完成对于当前状态的最优估计。

时间更新公式为

量测更新公式为

2.3 算法流程对比

基于Kalman 滤波算法和因子图优化算法的流程图如图2所示。

图2 Kalman 滤波器和因子图优化算法流程图Fig.2 Flowchart of Kalman filter and factor graph optimization algorithms

通过上述的算法流程对比可以看出: 基于Kalman 滤波器的状态估计是一种递推算法,其利用历史信息得到当前状态的最优估计;而因子图优化算法是一种批量估计算法,其能够利用历史信息得到所有状态的最优估计。在线性Gauss 的假设下,两者在最大后验估计的意义下都能够实现对当前状态的最优估计;但是在非线性非Gauss 系统中,因子图优化的主要优势在于对历史状态的重新估计,在重新估计的过程中,相当于对原系统的线性化点进行重新线性化,因此最小化系统整体的非线性误差,这也是因子图优化算法鲁棒性和相对较高后处理精度的来源。

3 试验与分析

3.1 地面无人平台配置及试验轨迹

为了对基于Kalman 滤波和基于因子图优化的惯性/里程计组合导航算法进行对比,本文基于如图3所示的陆用无人平台进行数据采集和算法验证,采用的平台为松灵机器人SCOUT2.0 移动机器人底盘,平台上主要配置的设备包括光纤陀螺惯性导航系统、STIM300 惯性导航系统、轮式里程计以及卫星接收机。

图3 陆用惯性/里程计组合导航无人系统Fig.3 Diagram of land SINS/ODO integrated navigation unmanned system

本文中的试验采用光纤陀螺捷联惯性导航系统和卫星速度位置进行松组合后再进行RTS（Rauch Tung Striebel）平滑的结果作为参考基准,本试验中的陆用无人平台传感器配置参数如表1～表3所示。

表1 光纤陀螺捷联惯导系统参数Table 1 Parameters of FOG-SINS

表2 STIM300 参数Table 2 Parameters of STIM300

表3 轮式里程计参数Table 3 Parameters of wheeled odometer

试验地点位于湖南省长沙市内,试验整体持续时长为1520s,起点和终点相同,试验轨迹如图4所示。

图4 地面无人平台试验轨迹卫星图Fig.4 Satellite trajectory of ground unmanned platform test

3.2 试验结果及分析

为保证相对的公平性,因子图模型和Kalman滤波器在状态估计过程中将安装角、标度因数误差提前进行了补偿,并使用相同的惯性器件和里程计速度噪声参数。

对含有稀疏位置观测的惯性/里程计组合导航算法和仅有速度约束的惯性/里程计组合导航算法进行验证,得到的试验结果如图5、图6和表4所示。

图5 不同位置观测间隔的惯性/里程计组合导航优化算法结果Fig.5 Optimization algorithm results of SINS/ODO integrated navigation with different position measurement intervals

图6 惯性/里程计组合导航优化算法（100s 稀疏位置约束）与Kalman 滤波算法（无位置约束）对比结果Fig.6 Comparison results of SINS/ODO integrated navigation optimization algorithm （100s sparse position measurement） and Kalman filtering algorithm （without position measurement）

表4 基于Kalman 滤波器（无位置观测）和因子图优化（含稀疏位置约束）的惯性/里程计组合导航结果Table 4 Results of SINS/ODO integrated navigation based on Kalman filter （without position measurement）and factor graph optimization （with position measurement of different frequencies）

图5中的水平误差为东向和北向合成误差,其中的100s～500s 为位置量测出现的时间间隔,数值的变化表征了观测信息密度程度不同,时间间隔越长表示观测信息密度逐渐稀疏。此试验结果表明,基于Kalman 滤波器的惯性/里程计能够在无位置观测的条件下达到较高的定位精度,其整体误差小于3m,水平定位误差的均方差小于2m;而在没有位置信息的条件下,仅依靠惯性/里程计因子图优化方法不能达到理想的效果,而随着位置观测稀疏程度的下降,惯性/里程计组合导航精度又能够得到显著提升。限制因子图优化定位精度的挑战有如下几个: 1）采用因子图优化技术的概率图模型建模理论较为复杂,相比而言,基于递推方法的Kalman 滤波器理论简单,故因子图模型的建模难度较Kalman 滤波器大;2）优化算法容易陷入局部最优解,当缺乏约束信息时,其批量优化结果反而会将未约束的变量“带偏”,由此导致的整体解算结果精度会下降,这点显著体现在增加位置约束后位置精度提升上,此处的位置约束可以理解为常规Kalman 滤波器当中的位置观测。

另一方面,在状态估计的平滑性以及鲁棒性上,当含有稀疏位置观测时,因子图优化算法表现得比Kalman 滤波算法更好。在图6中能够看出,基于Kalman 滤波器的算法其导航结果含有水平位置误差尖峰,分别出现在200s、600s～700s、900s～1000s、1100s 等处;相对而言,因子图优化算法出现的尖峰较小,轨迹比较平滑。

4 讨论和结论

本文对基于Kalman 滤波器和因子图的惯性/里程计组合导航算法进行了算法的验证和结果对比分析,得到的结论为: 在约束不充分的情况下,例如仅含有速度约束或位置观测十分稀疏的条件下,基于Kalman 滤波器的可靠性和精度能够优于基于因子图优化的算法;随着约束条件增多,如当位置约束（位置观测）增多、卫星信号或地面路标信号较为丰富准确时,因子图优化算法有比基于滤波算法更高的状态估计精度表现。此外,如果因子图优化算法能够建立更为准确的模型,如在因子图模型中考虑对里程计标度因数误差、安装角误差的实时估计等,其性能将会有进一步提升。