基于IMU/ODO预积分的多传感器即插即用因子图融合方法

近年来随着人工智能、机器视觉、传感器等技术的快速发展，无人车得到了广泛关注。在无人车的各类应用中，车载导航系统发挥着重要的感知作用，为自动驾驶控制系统提供准确的速度、位置、姿态等信息。导航系统的精度与可靠性是无人车安全行驶的重要保障。然而，现有的无人车高度依赖全球卫星导航系统（Global Navigation Satellite System,GNSS），当无人车行驶在高密度建筑物、峡谷等复杂环境或是受到电磁压制及干扰时，卫星信号质量较低且容易消失，从而无法对无人车进行定位[1,2]。

当卫星信号不可用时，可以通过集成多种传感器的信息来获得精确可靠的导航解决方案。目前，多传感器融合技术已被深入研究并广泛应用于无人驾驶领域[3,4]。然而，目前的车载多源融合导航系统普遍基于“烟囱式”架构，该种架构是针对特定传感器以及测量源而定制的，每当导航系统需要更改或更新时，都会带来巨大的成本，不具备对异类异步导航信息的“即插即用”能力。为了解决上述问题，许多研究者在滤波器框架的基础上，形成了能够对延迟异步量测信息进行处理的即插即用融合方法[5-6]。但是，基于滤波的方法将状态向量限制为最新状态并边缘化所有过去的信息，从而只能获得次优的结果。与基于过滤的方法不同，因子图融合方法能够保留过去的信息并进行全局优化，因此可以获得比滤波器更高的估计精度[7-8]。文献[9]提出了一种基于因子图的传感器最优选择方法，其利用计算资源、导航精度和可观测性指标来选择传感器的最优子集。为了解决因子图计算量大的问题，文献[10]提出了一种用于无人车的滑动窗口因子图融合方法以保证实时运行。文献[11]评估了基于因子图框架的鲁棒优化技术，结果表明基于因子图的鲁棒优化算法可以在GNSS 受限环境中实现较优的定位性能。但是，上述研究仍使用定制的融合解决方案进行优化，这不足以满足无人车导航系统对于灵活性和可扩展性的需求。

在优化的过程中，需要利用预积分对惯性量测进行处理以避免重复积分[12]。在面向车载导航的因子图融合方法中，由于轮速里程计同样具有递推特性，需要构建轮速里程计的预积分模型。在传统的轮速里程计预积分模型中，通常将地面假设为平面进行模型的推导。然而，当车辆运行在室外环境中时，这种假设会引入较大的误差。文献[13]利用陀螺的数据将轮速里程计的测速进行了分解，克服了平面假设的缺陷。文献[14]则利用惯性测量单元（Inertial Measurement Unit，IMU）构建了轮速里程计的预积分模型，并将轮速里程计的安装误差加入到因子图中进行优化。但是以上方法都没有考虑轮速里程计的标度因数对导航定位的影响。尽管文献[15]在轮速里程计预积分模型中考虑了标度因数的影响，但是没有将IMU 与轮速里程计之间的安装误差加入到因子图中进行优化，因此会导致定位精度的下降。

本文提出了一种基于惯性/轮速里程计预积分的多源信息“即插即用”因子图融合优化方法。考虑到轮速里程计标度因数、惯性传感器与轮速里程计之间安装误差的影响，推导并构建了完整的惯性导航系统和轮速里程计的预积分模型，采用固定频率进行优化解算，并利用预积分信息将无人车的运动状态与优化时刻之间的多源异步量测信息进行关联形成量测模型，加入到因子图的目标函数中，从而实现了一种即插即用式的因子图导航融合与参数优化方法。跑车测试结果表明，该方法可有效兼容多类导航传感器信息。同时，相比基于卡尔曼滤波的多源信息融合方法，定位精度提升60%以上。

1 惯性/轮速里程计预积分模型

本文中，机体系记为b，轮速里程计系记为o。导航坐标系采用东北天坐标系，记为w。惯性/轮速里程计状态传播方程被建模为如下形式：

其中，

式(1)中，和 分别为 tk和tk+1 时刻机体在导航坐标系下的位置向量，和 分别为 tk和tk+1时刻机体在导航坐标系下的速度向量，和 为 tk和tk+1时刻机体系相对于导航系的四元数。Δtk为时间间隔，为t 时刻从机体系到导航系的旋转矩阵，gw为导航系下的重力向量。和分别为t 时刻加速度计与陀螺输出，bat和 bwt为t 时刻加速度计与陀螺的零偏，na 和 nw 分别为加速度计和陀螺的噪声。和 分别为 tk和tk+1 时刻轮速里程计在机体系下的位置向量，为t 时刻轮速里程计系到机体系的旋转矩阵，sot为t 时刻轮速里程计的标度因数，ns为轮速里程计的噪声。

将式(1)中的状态传播模型投影到 tk 时刻的机体系bk下，可得：

其中，

式(3)中，为tk+1时刻的机体相对于 bk 系的预积分位置变化量，为tk+1时刻的机体相对于 bk 系的预积分速度变化量，为tk+1 时刻的机体相对于 bk 系的预积分旋转变化量， 为tk+1时刻的机体相对于bk 系的预积分位置变化量。

式(4)的离散形式可以写为：

连续时刻的误差状态方程为：

其中，Ft 预积分误差的状态转移矩阵，Gt为噪声矩阵，为预积分误差状态向量，nt 为噪声向量。对于预积分量测的雅克比矩阵与协方差矩阵，可以如下方式进行推算：

其中，Q 是噪声的对角协方差矩阵。预积分量测在 tk的雅克比矩阵与协方差矩阵的初值为：

对于预积分量测的修正方程，可以表达为如下形式：

其中，δbak 和δbwk分别代表加速度计和陀螺的零偏误差，δsok和 分别代表轮速里程计标度因数误差以

及安装角度误差。和 分别为预积分位置变化量误差 对应 δb 和δb 的雅克比矩阵，akwk和 b分别为预积分速度变化量误差对应 δbak和的雅克比矩阵，为预积分旋转变化量误差 对应δb 的雅克比矩阵，、和wk分别为预积分位置变化量误差对应、 和 的雅克比矩阵。上述雅克比矩阵均对应tk+1时刻雅克比矩阵的子块矩阵，可以从对应位置中获取。

因此，可以构建惯性/轮速里程计预积分残差为：

2 基于预积分的即插即用因子图融合方法

随着车载传感器种类与数目的增加，当前的定制型多传感器融合方法不具备对异类异步导航信息的即插即用能力。因此，本文提出了一种基于预积分的即插即用因子图融合方法。该方法通过设置固定的频率进行优化解算，并利用预积分信息将无人车的运动状态与优化时刻之间的多源异步量测信息进行关联形成量测模型，加入到因子图的目标函数中，因此能够自适应兼容任何种类的导航信息并避免了基于传感器频率进行优化而导致的计算量增加的问题。

在优化过程中，状态向量包括滑动窗口的 n+1个时刻机体的位置、姿态、速度、加速度计零偏、陀螺零偏、轮速里程计标度因数以及惯性传感器与轮速里程计的安装误差。

基于预积分的即插即用因子图融合方法如图1所示。

图1 基于预积分的即插即用因子图融合方法
Fig.1 Plug-and-play factor graph fusion method based on preintegration

在图1中，xk和xk+1分别为 tk和tk+1时刻无人车的运动状态，tk和tk+1 时刻为优化时刻，每两个优化时刻的时间间隔是相等的。为 t1 ∈ [tk,tk+1]时刻的姿态类量测信息，为 t2 ∈ [tk,tk+1]时刻的位置类量测信息。为 t1时刻机体相对于 bk 系的预积分旋转变化量，为t2时刻机体相对于 bk 系的预积分位置变化量。经过分类的导航量测信息可以通过如下的预积分公式构建量测模型。

因此，导航信息量测模型的残差可以表示为：

将式(13)加入到因子图的目标函数中，可以表示为：

其中，为边缘化的先验信息，为惯性/轮速里程计的残差信息，为利用预积分形成的关联量测残差信息，三项都是基于马氏距离。

3 试验验证

本文开展了实际跑车测试，用于验证所提出的算法。测试系统包括MEMS-IMU、双天线RTK、轮速里程计、激光雷达、摄像头以及百度Apollo 自动驾驶开发套件。IMU 的性能参数如表1所示，跑车测试平台如图2所示。在测试中，利用千寻基站服务提供的厘米级精度RTK 信息作为轨迹基准。跑车测试轨迹如图3所示。

表1 IMU 性能参数
Tab.1 IMU sensors specifications

图2 跑车测试平台
Fig.2 Experimental test platform

图3 跑车测试轨迹
Fig.3 Trajectory of experimental test

为了实现卫星拒止环境下的高精度定位，我们首先利用三维激光雷达对跑车测试的场景进行了构图，用于点云地图匹配以提供无卫星环境下的高精度位置信息。点云地图的俯视图如图4所示。同时，在跑车过程中，我们也利用摄像头对周围的特征点进行提取与匹配，通过视觉里程计实现对车体自身的状态估计。视觉特征点提取与匹配如图5所示。

图4 点云地图俯视图
Fig.4 Top view of point cloud map

图5 视觉特征点提取与匹配
Fig.5 Visual feature point extraction and matching

在整个测试过程中，双天线RTK 的定位结果作为参考值，并没有参与融合定位。利用惯性传感器、轮速里程计、激光雷达点云地图匹配以及视觉里程计信息完成对无人车的导航定位，并在不同的时间段根据导航信息的可用性，多源融合导航系统采用不同的导航传感器配置方案。我们对卡尔曼滤波与本文所提出方法得到的定位结果进行了对比，轨迹对比如图6所示，可以看出利用本文所提出方法得到的融合定位结果优于卡尔曼滤波融合结果。

图6 融合轨迹对比
Fig.6 Comparisons of the fusion trajectory

图7和图8分别为两种方法的水平与高度误差对比。其中IMU 代表惯性信息，ODO 代表轮速里程计信息，VO 代表视觉里程计信息、MAP 代表激光雷达点云地图信息。可以看出，多源融合导航系统能够兼容多种类型的导航信息。当有高精度地图绝对位置量测修正时，卡尔曼滤波与本文所提出的因子图融合方法可以取得等效的定位精度。但是，当缺乏高精度地图修正而依靠惯性/轮速里程计/视觉里程计进行位置推算时，由于本文所提出的因子图融合方法可以对传感器参数进行迭代优化，可以获得比卡尔曼滤波更高的传感器参数估计精度，因此有效抑制递推过程的位置发散。在大约600 s～1200 s 的时间内，卡尔曼滤波融合的水平位置误差最大发散至103 m，高度误差最大发散至27 m。而本文所提出方法的水平位置误差最大发散至34 m，高度误差最大发散至1 m，相比卡尔曼滤波融合，定位精度提升60%以上。

图7 水平位置误差对比
Fig.7 Comparisons of the horizontal position errors

图8 高度误差对比
Fig.8 Comparisons of the vertical position errors

4 结 论

在本文中，我们提出了一种基于惯性/轮速里程计预积分的多源信息“即插即用”因子图融合优化方法。首先推导并构建了惯性导航系统和无人车轮速里程计的预积分模型，利用预积分信息将无人车运动状态与多源异步量测信息进行关联并采用固定频率解算，从而实现了一种即插即用式的因子图导航融合与参数优化方法。该方法避免了优化过程中对惯性以及轮速里程计信息的重复积分以及基于传感器频率进行优化而导致的计算量增加的问题。论文开展了实际跑车测试，结果表明该方法可有效兼容多类导航传感器信息。同时，相比基于卡尔曼滤波的多源信息融合方法，IMU/ODO/VO 定位精度提升60%以上。