如何使用多类车载传感器快速可靠地生成点云地图？

本文介绍了在智行者各产品线广泛使用的点云地图构建方法，其论文发表在32nd IEEE Intelligent Vehicles Symposium, 2021会议上。

论文对应图流

1. 动机

高精地图是L4级别自动驾驶车辆得以自动运行的关键技术。高精地图为自动驾驶车辆提供车道级别的导航信息与拓扑信息，无论是乘用车还是低速车都需要这类信息来进行导航。通常，高精地图是在点云地图基础上生成的。随着自动驾驶车辆的不断普及，如何快速、高效、可靠地为各类场地生成点云地图，成为自动驾驶技术普及的关键。

然而，目前高精地图方案主要针对城市高速道路。在实际运营自动驾驶车辆时，仍然存在诸多问题。比如，图商提供的高精地图方案成本过高，而建图及时性与地图覆盖率明显不足；乘用车方案则对弱GPS场景、非结构化道路、半室内场景支持不够。另一方面，机器人端使用的在线建图与定位方案，虽然能够以很快速度建立地图，但以栅格地图为代表的2D地图无法支持较复杂的交通规则，在室外场景也存在可靠性问题。

本文介绍了一种如何使用多类车载传感器进行融合，快速可靠地生成点云地图的方法。主要贡献如下：

我们使用鲁棒的因子图模型来融合各类传感器数据。我们讨论了需要定义哪些因子，如何平衡各类因子对优化问题的影响。

我们讨论在现实场景中面临的一些建图问题，例如：如何处理RTK异常数据，如何处理激光退化，如何使用回环检测保障地图质量，如何在半室内或全室内场景约束高度和全局姿态，等等。

示例车辆与运行环境的点云地图

一个典型无人车辆点云地图及其局部形状

2.方法

建图流程图如下：

建图算法框架整体由前端加后端组成。前端用IMU和轮速计组成航迹推算模块（Dead Reckoning），然后计算激光里程计。后端由因子图优化以及一些后处理模块组成。在这个框架中，我们面临的问题主要来自后端优化。下面给出一些数据来讨论后端优化面临的问题与处理方式。

2.1 因子图优化建模

后端的一大挑战是在各种传感器输入信息中，找到正确且一致的部分，这在弱GPS、非结构化环境中显得更加重要。为了能够灵活处理传感器的信息，我们使用因子图优化来建模整个问题。因子图优化可以很好地表达多种传感器的自身特性，同时也允许我们灵活地控制优化流程，便于分析哪些因子主导了优化，哪些因子作为异常值被排除。

因子图本身是定义在SE(3)流形上的最小二乘优化问题：

基础的因子包括：RTK因子、DR因子和LiDAR里程计因子。额外可选的因子包括：高度因子、全局旋转因子；不同因子主要用来施加各种场景中的地图形状约束。

2.2RTK因子与RTK信息处理

RTK信号在固定解时标称精度达到cm级别，但其他时刻精度随自身状态变化很大，在单点解时也可能达到10m级别精度。如果受到多径效应影响，RTK接收机算法还可能给出错误的状态值，即使在固定解状态下也可能有很大误差。

由于我们使用了双天线，因此将RTK因子视作6自由度的位姿约束：

下图显示了同一场景下的RTK状态与测量值。

RTK信号不可靠的案例。上：RTK与优化轨迹对比图，蓝色为RTK，红色为优化轨迹，背影颜色为RTK状态；左下：3D视角对比轨迹；右下：点云地图。

由图可见，RTK即使在固定解状态时，也可能有各种跳变、慢变情况。本身精度在高度方面明显劣于水平方面。然而，在建图时，我们一方面要求地图与RTK对齐，一方面要处理RTK异常情况，这就要求算法能够自动化处理RTK异常值。为此，我们设计了一种两轮两阶段优化的处理逻辑。流程如下：

第一轮优化目标是得到基本正确的RTK-DR-Lidar轨迹，并且处理RTK异常值；第二轮目标是通过回环检测处理地图重复区域，减少重影的发生概率。

上图轨迹的误差平方直方图与正常值/异常值分布情况

实际操作当中，RTK异常值阈值可以存在一定的选择范围，只需要合理即可。我们既可以选得严格一些，把一些误差较大的读数剔除，也可以选的宽松一些，保留一些中间的约束。

2.3 回环检测与多机/多轨迹协同

在第二轮优化中，我们利用回环检测机制来检查地图重复区域的约束。

回环检测可以有效抑制地图重影。左：不带回环检测的全局地图和局部地图；右：带上回环检测之后的地图。整体上看，全局点云没有太大的变化，但局部点云确实被回环检测修正了。

同样，如果地图由多台机器采集，或者地图由多段轨迹拼接而成，也可以利用回环检测来修正重复区域。

多段轨迹案例。左上：位姿图；右上：多段轨迹的形状；下方：拼合后地图，地图颜色由轨迹ID着色。

2.4 退化检测与修正

在广场等空旷场景，激光匹配由于缺少特征，其运动估计会由于存在额外自由度而出现漂移、抖动等情况。这种现象通常称为退化。在退化时，我们希望通过其他传感器的信息来补充激光的估计。

退化案例：上图是一个广场案例，广场中央存在一个雕塑。从广场边缘走向中央时，激光出现明显退化现象。中间：退化分值的分布情况；下方：退化修正前/修正后的轨迹。

在前端检测到激光退化时，我们在后端主动缩小激光的信息矩阵，从而使DR起到该段轨迹的主导作用，局部地修正此处的激光结构。

2.5 高度约束与全局姿态约束

对于室内的地图，由于缺少RTK约束，激光里程计在高度估计上可能存在累计误差。在这种场景中，我们引入场地与车辆的假设：场地整体为平地，车辆整体姿态向上。这种约束由高度因子与全局旋转因子实现。

同时，为了允许车辆存在一定程度上的高度变动和姿态抖动，我们给这两种约束增加死区设定。

高度约束施加后的轨迹与点云（侧视图）变化

3.对比实验

我们在采集的数据集上对比了一些经典的开源激光建图算法。实验主要传感器为：RTK服务（千寻FindCM）、6轴IMU（美泰SI3200 MEMS）、轮速计、顶部16线雷达（Velodyne-16）。由于现实世界的轨迹真值难以获取，我们挑选了一部分RTK较好的场景，然后将建图算法选择出的RTK正常值作为真值进行比较。

我们主要对比的模块为：DR、Lego-LOAM、Cartographer、mapping（无RTK）、mapping（有RTK）。其中Lego-LOAM使用了DR作为预测值，而Cartographer则直接使用IMU读取。由于Cartographer不支持轮速计输入，因此没有给Cartographer输入轮速。我们对Cartographer使用了默认参数，没有针对性的调参。另外，由于我们将估计轨迹与RTK对比，因此在各个算法中都不输入RTK数据，以保证公平性。

我们选择了13个场景，按大小分为小型与大型两类，以绝对位置精度（Absolute Translation Error, ATE）作为评价指标。作为对比，也给出了带RTK输入的Mapping位置。

整体而言，我们在大部分场景下比Lego-LOAM和Cartographer取得了更好的精度指标。如果带有RTK信息，那么我们在大部分场景下能取得10cm精度左右的轨迹精度。

一些代表性地图

4.软件的自动化部署与运营

最后，为了保证建图算法的高效运营，我们将每个建图步骤抽象为流水线模型，并在云服务器端部署。流水线模型的每一步可以独立启动或中断，且各个建图任务彼此不发生冲突，保障了多个建图任务可以独立运行，互不干扰。

由于建图是自动触发、运营的，我们将建图结果统计成报告，自动发送给运营人员，实现了整个建图流程的24小时无人化管理。

责任编辑：lq6