ICRA | Apollo最新研究成果，城市动态场景中的鲁棒LiDAR定位系统

ICRA（International Conference on Robotics andAutomation）是机器人技术领域最有影响力的国际学术会议之一，每年举办一次。当前，作为机器人技术最受关注的技术应用——自动驾驶，每年都会有相关的突破性技术论文在ICRA中发表。

当下，不断变化的出行环境对自动驾驶定位系统提出了更严峻的挑战，刚刚取得了加州DMV路测MPI第一名好成绩的百度Apollo公布了最新研究成果：鲁棒LiDAR定位系统。该系统由百度自动驾驶技术部—地图定位团队的工程师和研究员们共同打造，使用LiDAR惯性里程计实现的航位推算，在不断变化的城市场景中实现鲁棒的定位。

本次研究提出了基于激光惯性里程计辅助的激光定位系统，解决了因施工、修路等不断变化的城市场景中，定位地图不稳定造成的定位系统失效的问题，同时提出了有效的定位地图更新解决方案。

该系统中，定位框架使用了来自LiDAR全局匹配和LiDAR惯性里程计互补模式的信息，以实现精确和平滑的定位。这项最新的研究成果可以使定位系统更加鲁棒和准确，尤其是在动态变化的城市场景中效果显著。

在过去的一百年里，人类和货物的运输发生了巨大的变化。交通行业的增长潜力引起了研究者的广泛关注，他们试图解决这个复杂的问题。一些颠覆性的技术举措，如共享单车、无人物流、自动驾驶等，也应运而生。

为了实现自动驾驶车辆的自主导航，目前普遍流行的方法是精确定位，但精确定位系统不仅复杂，而且在动态变化的环境下也难以实现。以往的实践表明，使用现有技术可以克服环境中的某些特定变化，例如少量的路面修补、水坑、雪堆等，然而基于LiDAR地图匹配的车辆定位模块的失效问题，仍然是最具挑战的问题之一。

 ▲上图在顶部显示了在线LiDAR数据（棕色）和由LiDAR惯性里程计构建的Submap。中间部分给出了在定位地图中标注的由环境变化检测模块估计的每个单元中存在变化的概率。底部显示了：「A」以及「B」不同时期的现场对比；「C」定位结果的视觉对比：绿色（有LIO）与红色（无LIO）与蓝色（真值）；「D」放大的环境变化检测结果；「E」通过加入新的LiDAR数据更新后的地图。

在这项工作中，Apollo研究人员结合LiDAR惯性里程计（LIO）和基于匹配的全局LiDAR定位，在位姿图优化框架中融合了两个模块的测量数据。考虑到地图匹配和里程计方法的互补特征，该框架构成了一套可以克服环境变化和地图错误的鲁棒定位系统，同时可以提供精确的全局定位结果。

基于激光惯性里程计辅助的激光定位系统，由LiDAR惯性里程计（LIO）、LiDAR全局匹配（LGM）、基于位姿图的融合（PGF）和环境变化检测（ECD）四个模块组成。

LGM是一种全局定位方法，它将在线LiDAR扫描与预先构建的地图相匹配，并进行3自由度（x，y，偏航）估计。其他3自由度（横滚、俯仰、高度）可以通过读取IMU重力测量数据和数字高程模型（DEM）地图来估计，一旦我们成功地进行水平定位。另外两个模块LIO和PGF则根据求解不同的最大后验概率（MAP）估计问题而建立。

在定位系统中，LiDAR惯性里程计在一些具有挑战性的情况下，例如，由于道路建设或恶劣天气导致的地图过期或环境变化，对提高定位性能起着重要的作用。

LiDAR里程计估算帧之间的相对姿态，同时帮助构建一个称之为Submap的局部地图。Submap会根据LiDAR扫描不断更新，始终保持最新状态。该定位系统充分利用了Submap，平滑了预估的轨迹，同时保证了系统在极端情况下的可靠性。

系统中LiDAR惯性里程计的实现遵循了W. Hess[2]等人的工作理论，同时采用了一些重要的扩展来帮助提高其精度。

首先，我们采用三维占位网格代替二维占位网格的方法实现了六自由度的全方位里程测量。这个扩展允许它在三维环境中的应用，如停车场结构或天桥，并简化了下面提到的IMU预积分。

其次，该系统结合了惯性信息以提供运动预测和帧之间的相对约束。更重要的是，惯性信息的加入使我们能够对移动平台引起的LiDAR扫描失真进行运动补偿。为了降低计算时间，我们在实现中采用了预积分方法。

第三，考虑到这些场景中来自车道或路面标记的丰富信息，在占据栅格配准过程中，我们加入LiDAR反射值，作为对每个网格单元占用概率的补充，并提供了有价值的环境纹理信息。

最后，我们在求解非线性优化问题时，采用由粗到细的方法，在我们占据栅格的实现中引入多分辨率。这不仅有助于网格配准的收敛，而且可以有效地控制计算复杂度，实验结果验证了这一点。

我们将LiDAR惯性里程计描述为一个MAP估计问题。给定前一个状态，Submap更新到最近的帧，以及量测，状态的后验概率是：

在零均值类高斯概率的假设下，通过建立目标函数可以表示为：

SSOP(Sumof Squared Occupancy Probability)和SSID（Sumof Squared Intensity Difference) 分别表示占据栅格概率和LiDAR反射值的损失。

给定LiDAR点云中任意一点，分辨率为i的Submap，以及一个位姿状态，可以从Submap中找到命中单元格s。通过连续插入新的LiDAR数据，可以持续更新Submap的占据概率，该更新问题可以使用二值贝叶斯滤波器的反向测量模型和log Odds解决。该MAP估计对应负对数后验的最小值，从而转化成一个非线性最小二乘优化问题，它可以使用迭代算法（例如Levenberg-Marquardt，Gauss-Newton）最小化，在系统中使用Ceres实现。

虽然LiDAR惯性里程计可以在局部坐标系内提供很好的相对约束，但仍然需要全局约束来实现全局定位。LGM模块为该系统提供了全局线索，在本节介绍的基于位姿图的融合框架中，这些互补的局部和全局线索可实现联合优化。我们将此融合问题描述为一个MAP估计，并假设均匀先验分布，将后验概率分解为：

上式的分解可如图进行可视化解读：

在零均值高斯的假设下，量测的似然分布可定义为：

在我们的融合框架中，我们维护一个局部状态和一个全局状态变量，它将转换至全局坐标系。因此定义全局损失残差为，其中

关于全局残差中的协方差写成，其中，旋转和高度协方差和分别是常数对角矩阵，因为LGM模块仅使用2D直方图滤波器估计水平定位不确定性，这里引入的估计不确定性对定位系统的性能至关重要，从而产生自适应融合。

系统利用新到的LiDAR，不断地终止旧的Submap并构造新的Submap。通过局部坐标系与LIO模块输出的Submap相对位姿约束，并保存在滑动窗口的生命周期中。关于Odometry的残差可以表示为，其中：

众所周知，LGM模块依赖于预先构建的本地化地图的新鲜度。我们总是想知道什么时候该更新我们的定位地图。LIO模块生成的Submap不仅可以用来预估帧之间的相对姿态，还可以用来检测环境的变化。这对于定位系统是至关重要的，特别是考虑到它的商业部署。因此，研究人员建立了环境变化检测（ECD）模块，一个基于LIO的子模块。

首先，我们将三维多分辨率submap投影到地平面上，类似于定位地图构建过程；然后得到与系统预先构建的定位地图格式相同的2D Submap。给定系统的定位输出，就可以将Submap覆盖到预先构建的地图上，并通过逐单元比较来确定环境变化的发生。

令和分别Submap和预建地图中对应单元格的反射值平均值、反射值方差和高度平均值，可以得到

其中，和用于评估单元中存在环境变化的可能性。我们将每个单元中发生的变化描述为一个由二值贝叶斯处理的二进制状态估计问题。与占据栅格的更新类似，如果多辆车多次通过同一区域生成更多Submap，我们可以通过使用一个二值贝叶斯滤波器来相似地更新变化发生的概率。如果我们将单元内发生的变化表示为二进制状态变量，则用于更新二进制贝叶斯滤波器的逆测量模型可以定义为：

其中，γ是一个动态权重参数，用于平衡反射值和高度损失的权重。通过将Submap中的和归一化可以进一步定义它们的逆测量模型：

其中分别是和在Submap中的平均值和方差。β和θ是根据经验设置的参数。

目前，该定位系统已经在实际场景中进行了广泛的测试，主要使用了两个数据集：内部数据集和Apollo-SouthBay数据集。

Apollo-SouthBay数据集收集于美国旧金山湾区，覆盖380.5公里的行驶距离。

内部数据集包括了中国北京各种具有挑战性的城市场景，特别是有地图错误或环境变化的场景，如地图过期（渐进环境变化），道路施工、封闭车道、交通密集等。

Apollo车辆平台配备了Velodyne HDL-64E 360度LiDAR和NovAtelPwrPak7D-E1 GNSS RTK接收器，该接收器集成了双天线和Epson EG320N IMU，并安装于林肯MKZ车辆中。

评估中使用的位姿真值是使用离线LiDAR SLAM方法生成的，通常表示为大规模全局最小二乘优化问题。

总体而言，在所有指标下，该定位系统都可与[5]相媲美，并且在Apollo-SouthBay数据集的大多数数据序列中获得了更好的最大水平和偏航均方根误差，特别是在具有挑战性的场景中展示了新设计的好处。

 ▲ 使用内部数据集的定量分析如上表所示

总的来说，通过使用新的LiDAR惯性里程辅助框架，我们的性能比[5]有所提高。为了进一步说明所获得的好处，下图显示了数据集中几个帧的系统输出示例。

在该定位系统中，LIO和PGF模块运行在CPU上（Intel Xeon 3.5ghzcpu和32gbram），而LGM模块运行在一个FPGA卡上。LGM、LIO和PGF模块分别需要44.9ms、79.4ms和2.8ms。需要注意的是，LGM和LIO模块实际上是并行运行的，因此整个系统平均每帧大约需要82ms，从而验证系统的实时性。

在本次ICRA中，Apollo提出了针对自动驾驶应用而设计一个稳健的LiDAR定位框架系统。该系统利用基于位姿图的融合框架，自适应地融合了LiDAR惯性里程结果和LiDAR全局匹配模块的结果，特别是针对不断变化的城市场景中的定位技术挑战。

结果表明，新增加的LiDAR惯性里程计模块可以有效地辅助定位系统，防止由于LiDAR全局匹配失败而引起的定位误差。考虑到道路建设中的不时变化，Apollo研究人员还提出了一种环境变化检测方法，以找出地图应在什么时候和什么部分进行及时更新，从而可靠地支持大型自动驾驶车队，在拥挤城市街道中的日常运营，这些优势使得我们的系统非常适合商业化部署。

在探索自动驾驶领域发展的路上，百度Apollo始终坚持通过独立自主的科研创新，协同百度AI生态体系，通过致力于顶尖的学术研究以及深入可行的落地应用，加速全球自动驾驶产业的智能化进程。

参考文献：

[2]W. Hess, D. Kohler, H. Rapp, and D. Andor, “Real-time loop closure in 2D LiDAR SLAM,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2016, pp. 1271– 1278.

[5] G. Wan, X. Yang, R. Cai, H. Li, Y. Zhou, H. Wang, and S. Song, “Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2018, pp. 4670– 4677. 

  ▼点击左下方阅读原文查看原论文