基于图优化的惯性/地磁/激光雷达复合定位技术研究

赵雨楠，季 阳，郭元江，李 昂，赵 豪

(北京自动化控制设备研究所，北京 100074)

0 引言

自动驾驶技术的不断发展对车辆定位导航的精度与可靠性提出了更高的需求，复杂环境下的米级定位已成为车辆安全行驶的前提。

目前，自动驾驶的主流是基于多源信息融合的定位方法。动态实时差分全球定位系统(Real-Time Kinematic difference Global Positioning System，RTK-GPS)技术是目前自动驾驶中重要的定位手段之一，在空旷无遮挡的区域能够实现对车辆的米级甚至更高精度的定位，但其局限性在于城市路况复杂、建筑物或隧道遮挡信号、信号多径效应等因素的影响，会出现精度下降甚至失锁的情况。惯性导航系统(Inertial Navigation System, INS)以其自主性、连续性和全导航参量的优点在各类车辆中应用广泛，但是INS的定位误差随时间累积，必须引入其他辅助手段对其进行校正。地磁匹配方法不依赖高品质的位姿先验信息，即可实现十米级定位，其误差不累积，是一种有效的修正手段。激光雷达点云匹配定位技术的定位精度可达亚米级，是自动驾驶领域的主流定位方法之一。然而，这种匹配对先验位姿信息品质要求较高，一方面点云匹配采用非线性优化方法，若无先验易陷入局部最优；另一方面点云特征数据量大、难以快速遍历，因此，若无先验位姿信息，该方法难以在大范围环境中准确确定位置，且在类几何环境中易失效。

自动驾驶主流的导航方案为卫星/惯性/激光雷达匹配组合方案，在卫星拒止条件下，仅依靠惯性/激光雷达组合，难以适应点云匹配失效的情况，存在点云匹配一旦失效，先验位姿信息会快速发散，导致后面再难匹配上的问题。

因此，为了解决卫星失锁条件下卫星/惯性/激光雷达匹配组合方案在点云匹配失效后难以恢复的问题，本文在该方案的基础上，引入地磁匹配与运动约束信息，提出了一种基于图优化的惯性/地磁/激光雷达复合定位方案。在卫星拒止环境下，在点云正常匹配时，实现米级定位精度；在点云匹配失效时，依靠惯性/运动约束/地磁匹配实现高精度保持。一方面继续为车辆提供高精度、高可靠的位置信息，另一方面为点云再次匹配提供良好的先验。

1 惯性/地磁/激光雷达复合定位方案

在卫星拒止条件下，针对卫星/惯性/激光雷达匹配组合方案的精度、可靠性有限的问题，本文设计了基于图优化的惯性/运动约束/地磁/激光雷达复合定位方案，如图1所示。

图1 基于图优化的惯性/地磁/激光雷达复合定位方案

本文方案主要包括惯性导航、地磁匹配、激光点云匹配、运动约束、信息融合5个模块组成。惯性导航模块完成预积分解算，结合侧向零速约束,抑制误差发散速度;在此基础上，引入地磁匹配模块,完成基于序列趋势的匹配定位，为激光雷达点云匹配提供持续可用的先验信息；激光雷达点云匹配模块完成点云预处理与匹配，可以得到米级定位信息；在信息融合模块完成惯导、运动约束、地磁、激光雷达信息的融合。通过增加运动约束与地磁匹配，提升了点云匹配失效时的精度保持能力，其中地磁匹配虽然匹配率有限，但是一旦完成匹配，可以进行绝对位置修正，有效延长对点云匹配失效的最大容忍距离。

1.1 惯性导航模块

惯性导航模块主要完成预积分解算。惯性测量单元可自主高频率测量运动信息，一般输出频率为数百赫兹，远超过导航系统的输出频率。因此，将惯性测量值在载体系下直接进行预积分，预积分结果用于后续信息融合模块。

(1)

(2)

1.2 地磁匹配模块

地磁匹配模块采用基于序列趋势的匹配定位方案，如图2所示，主要通过一维有限空间的地磁匹配技术实现10m级导航精度。

图2 基于序列趋势的匹配定位方案

一方面，建立的地磁基准库为沿道路轨迹的一维地磁强度参考点，相较于传统的二维地磁地图存储量小、空间分辨率高；另一方面，基于地磁异常特征序列，将实时采集的序列与地磁基准库中的序列进行相似度计算，如式(3)，完成趋势匹配，并采用综合置信度分析方法，提升地磁匹配准确性。

(3)

式中，()为采集的地磁序列；()为地磁基准库中的序列，序列长度为。

1.3 激光雷达高精点云匹配模块

考虑到激光雷达采集点云的数据量大，如果直接采用非线性最小二乘方法，会带来迭代次数多、运算量大等问题。因此，本文设计了一种基于线面特征的两级LM匹配算法，在保证匹配精度的同时，可以大幅提升匹配的实时性。

1)特征提取

为了避免直接对大量的点云数据进行匹配计算，同时减小点云分布对后续匹配定位的影响，对点云信息进行特征提取，并等量筛选不同方向的特征。

首先，根据环境中物体的特点，将特征分为线特征与面特征。根据点云与其临近点(≠)的关系，对的曲率进行提取。

(4)

式中，、分别为激光雷达到与的距离；为的集合，为的同线相邻点云。当大于门限1，则认为其为线特征；当小于门限2，则认为其为面特征。

其次，将360°的视角分割为六部分，在每个方向分别选择等量的最优线特征与面特征，作为后续进行匹配的特征。

2)两级LM匹配

考虑到不同方向的位置、姿态信息的收敛速度不同，根据所提取特征的特点，设计了一种两级LM匹配方案，如图3所示。一方面可以降低迭代次数，提升匹配实时性；另一方面，考虑到激光雷达线数限制，俯仰角、高度精度有限，两级LM匹配方案更便于拓展辅助信息源的水平姿态与高度信息的辅助。

图3 两级LM匹配方案

1.4 信息融合模块

考虑到传感信息的多源性与异步特征，以及兼容低精度惯导的非线性误差特性，传统的基于线性高斯的拓展卡尔曼滤波(Extended State Kalman Filter，ESKF)信息融合方法难以满足需求。因此，搭建更为灵活的基于因子图优化的信息融合架构。信息融合的核心是因子图构建与因子图优化。

1)因子图构建

因子图构建主要是因子图的节点与因子配置。节点是待求解的导航信息，本方案采用时刻的位置与姿态为节点；因子表示节点间的空间约束，本方案的因子包括惯导预积分因子、地磁匹配因子、运动约束因子与激光雷达匹配因子。惯性/地磁/激光雷达复合定位方案的因子图如图4所示。

图4 惯性/地磁/激光雷达复合定位方案的因子图示意

a) 惯导预积分因子

惯导预积分因子与前后2个节点相关，其误差函数可以写作

(5)

b) 运动约束因子

车辆行驶时，侧向速度理应为0，以此运动约束建立相对应的因子，即

(6)

(7)

c) 地磁匹配因子

(8)

d) 激光雷达匹配因子

(9)

2)因子图优化

完成惯性/地磁/激光雷达复合定位方案的节点与因子配置后，需要根据该系统的节点与因子构造优化的目标函数，并推导雅可比矩阵，以最终完成优化。第个节点优化的目标函数()可分为4个部分

(10)

至此，可采用Gauss-Newton法或Levenberg-Marquardt法求解式(10)，重复迭代修正节点，直到目标函数取最小值，此时节点得到优化调整，可以估计出车辆的位置与姿态信息。在构造式(10)时，可根据该节点是否有对应因子灵活增减目标函数，易于融合异步多源的信息。

2 试验结果与分析

2.1 试验条件

为了验证算法的有效性，搭建惯性/地磁/激光雷达试验平台，完成跑车试验(图5)。跑车采用某型号INS，陀螺零偏为0.01(°)/h，加表零偏为30μ；地磁传感器选用PNI公司的TCM-XB三轴磁强计，重复性0.1μT；激光雷达采用速腾聚创RS-LiDAR-16。试验以某高精度惯导/千寻卫星组合导航系统的输出为基准。在北京市丰台区长青路进行验证，全程约7km，耗时10min，平均速度约40km/h，整段路程路况复杂，包括直行路段、十字路口、丁字路口等，跑车轨迹如图6所示。

(a) 原理样机 (b) 跑车试验

图6 定位轨迹曲线

2.2 试验结果

1)精度验证

图7所示为跑车的位置误差曲线，可以看出，采用了基于图优化的惯性/地磁/激光雷达复合定位技术，定位误差均方根为0.48m，最大值为1.24m。其中需要说明的是，由于试验样机输出的信息与基准信息采样周期不一致，所以在车辆高速行驶时两者信息不同步，于是出现误差曲线的毛刺噪声。

图7 定位误差曲线

2)可靠性验证

考虑到高精度基准点云地图的覆盖性，在实际中可能存在部分路段缺乏点云地图的情况(图8)，因此，去掉部分路段的点云地图再次进行试验。

图8 定位误差曲线(部分路段缺乏点云地图)

根据图8可以看出，在行驶97～343s期间，点云地图缺失，点云匹配失效，惯性/运动约束/地磁可以完成精度保持，定位误差均方根为6.48m，最大值为10.29m；当成功加载可用的点云地图时(343s后)，可以顺利完成点云匹配，定位误差均方根为0.48m，最大值为1.19m。这表明，本文提出的方案可以避免卫星拒止环境下，卫星/惯性/激光雷达点云匹配方法存在的点云匹配一旦失效后再难匹配上的问题，具有较好的鲁棒性与可靠性。

3 结论

1)为了解决卫星失锁条件下车辆的高精度定位问题，本文基于惯性导航、运动约束、激光点云匹配方法的互补优势，引入地磁匹配，设计了一种基于图优化的惯性/地磁/激光雷达复合定位方案。

2)试验结果表明，本文提出的复合定位方案具有高精度、高可靠性的优点。一方面，在卫星拒止条件下，本文提出的方案定位精度可达0.48m (RMS)，为车辆自动驾驶提供良好的定位信息；另一方面，在点云匹配失效时，惯性/运动约束/地磁可以完成精度保持，为后续点云匹配提供持续可用的先验信息，直到下次点云匹配成功。

3)后续，还需要对本文所提出的方案进行复杂场景下的试验验证，尤其是高速公路等特征少、几何相似的场景，以验证环境特征对导航精度的影响。