自动驾驶定位|结构化场景中基于高精度地图自动驾驶的由粗到精语义定位——纽劢科技

• ​论文链接：​​https://arxiv.org/pdf/2107.02557.pdf​​
  

摘要

鲁棒且精确的定位是机器人导航和自动驾驶的重要组成部分。使用相机在高精度地图中定位提供了一套价格合理的定位传感器组件。由于容易出错的数据关联或者具有精确初始位姿要求的初始化，现有方法受到位姿估计失败的影响。在本文中，我们提出一种使用高精度地图的高性价比自动驾驶车辆定位系统，其使用相机作为主要传感器。为此，我们将基于视觉的定位表述为一个数据关联问题，将视觉语义映射到高精度地图中的路标。具体而言，通过结合粗略全球定位系统（GPS）测量和精细位姿搜索，以由粗到精的方式完成系统初始化。在跟踪阶段，通过使用光度一致性对齐图像和高精度地图中路标之间的语义结果，从而优化车辆位姿。最后，以滑动窗口方式通过位姿图优化来计算车辆位姿。我们在两个数据集上评估本文方法，并且证明所提出方法在不同驾驶场景中获得不错的结果。此外，本文方法对单目相机和多相机均适用，这为定位系统提供了灵活性并且提高了鲁棒性。

介绍

车辆定位（即世界坐标系中位置和姿态估计）是自动驾驶系统的一个重要组成部分。例如，精确且鲁棒的定位可以为自动驾驶的决策和感知模块提供有用的信息。尽管基于IMU和RTK-GNSS的惯性导航系统可以获得足够精度，但是它在GNSS信号较差的场景（例如隧道和摩天大楼）下会失效。此外，对于大规模量产而言，它也不具有成本效益。

近年来，很多工作采用一种离线构建的先验地图来解决车辆定位问题。先验地图通常作为自动驾驶场景的语义或者几何表示。激光雷达点和视觉特征点是基于点云的先验地图的两个主要类别。尽管点云地图可以提供足够的定位精度，但是它对环境敏感，且难以在长期运行后提供鲁棒的定位。此外，由于点云地图内存要求，其无法缩放。与点云地图相比，矢量形式的高精度地图包含重要且丰富的语义几何信息。高精度地图是高度结构化的，通过具有几何和属性的实体元素组织。若干工作探索基于高精度地图的定位方法。然而，数据关联容易出错，或者定位系统不是完整的。

为了解决上述问题，本文方法的目标是要提供一个基于视觉的鲁棒且精确的定位系统。我们通过结合矢量形式的高精度地图和图像语义信息来引入一种由粗到精的视觉定位。在系统初始化步骤中，通过车载GPS提供粗略的初始化，然后通过穷举位姿搜索进行优化。在跟踪阶段，通过对齐图像语义感知结果与高精度地图中相同语义的路标来估计位姿。具体而言，给定一张图像或者多张图像，首先通过深度学习方法获取高精度地图中元素的语义分割结果。基于分割结果，通过利用类似距离变换的函数构建代价地图。最小化代价可以被定义为将路标的光度误差投影到代价地图上。使用额外的车轮里程计信息，通过滑动窗口策略中位姿图优化来计算最终的车辆位姿。最后，当跟踪阶段发生失败时，定位丢失恢复模块负责系统重新初始化。所提出系统的运行图像如图1所示。

总之，我们的主要贡献为：

1）通过利用语义分割和高精度地图，我们提出一种完整的视觉定位系统，其包含初始化、跟踪和定位丢失恢复模块；

2）本文解决方案对于处理单目相机和多相机系统均是灵活的；

3）在两个数据集上评估本文方法，并且证明本文方法在不同驾驶场景中产生不错的定位结果。

相关工作

最近，有很多使用先验环境地图进行视觉定位的研究工作。

点云地图vs矢量格式地图：定位中的先验地图可以被分类为基于点云的地图和矢量格式的地图。点云地图可以通过激光雷达或者相机作为传感器来构建。与点云地图相比，紧凑的矢量格式高精度地图是轻量级的，易于部署和更新。在工作Vision global localization with semantic segmentation and interest feature points中，为了扩大定位范围，SuperPoint特征点地图和语义分割特征点地图均用于车辆定位。在工作Monocular camera localization in prior lidar maps with 2d-3d line correspondences中，通过使用激光雷达点云地图，并且寻找图像中2d线与激光雷达地图中3d线之间的对应关系，从而计算单目定位系统中的相机位姿。分层定位方法通过将图像检索引入到定位流程中，以使用不同的范例来完成视觉定位任务。例如，在工作From coarse to fine: Robust hierarchical localization at large scale和Back to the feature: Learning robust camera localization from pixels to pose中，通过使用全局图像特征将查询图像与数据库图像相匹配，以获取图像特征点和先验环境特征点地图之间的对应关系，从而估计查询图像的位姿。然而，这些方法仅适用于室内或者小型非动态场景。很多工作利用语义定位中的稀疏语义高精度地图，如工作Monocular localization in hd maps by combining semantic segmentation and distance transform和Exploiting sparse semantic hd maps for self-driving vehicle localization。在工作In-lane localization and ego-lane identification method based on highway lane endpoints中，定位任务被分为两部分：自车道识别和车道内定位。地图和图像中虚线车道的端点用于将车辆定位到车道的正确位置。然而，仅使用虚线车道将限制定位应用场景。

基于特征方法：利用环境中低级别几何特征或者高级别语义特征来建立图像和地图之间的关联。几何特征包括点、线和面。根据提取的图像特征和地图之间的匹配可以估计相机位姿。几何特征的示例包含ORB、SIFT等。这些几何特征为匹配任务提供可区分的描述。然而，它们对于环境变化不鲁棒，例如从白天到夜晚或者从冬天到夏天的场景变化。因此，它们对于长期定位表现不佳。与几何特征相比，语义表示是另一种方法，其广泛用于自动驾驶中的定位。这类表示包括车道标记、路缘、电线杆等。

直接方法：不需要明确的关键点检测器或者特征描述子。它可以自然地从所有图像区域中采样具有强度梯度的像素。例如，基于图像梯度点的图像对齐来估计帧间位姿。最近，边缘特征被进一步用于产生距离变换图像以进行位姿优化。

本文方法与使用高精度地图进行单目定位密切相关。在工作Monocular
localization in hd maps by combining semantic segmentation and distance transform中，高精度地图中元素的图像特征根据语义分割提取。距离变换操作被应用在高精度地图中每个元素的二进制分割结果上以生成位姿优化的代价图像。最后，根据初始位姿，将地图元素的代价重投影到代价图像上，以逐步地优化相机位姿。然而，该方法不是一个完整的定位系统，它仅包含视觉跟踪模块。初始化步骤和定位丢失恢复模块对于定位系统而言是重要的组成部分。当图像信息不足以约束车辆位姿时，其6自由度优化策略可能产生估计误差。所提出系统支持多相机传感器配置，即使当一些相机被遮挡时也能进行连续定位。此外，使用语义特征和鲁棒策略使系统能够在具有挑战性条件的已建图环境中运行。

方法

A.高精度地图

B.语义分割和后处理

为了寻找高精度地图元素和图像之间的对应关系，应用语义分割来提取图像的语义特征。我们提出一种轻量级的深度学习网络，它可以提供高效的分割结果。通常，主干网络为Resnet-18，并且在Cityscape数据集上预训练。该网络是一个multi-head结构，每个head是高精度地图中一个元素（LA、PO或者SB）的二进制分割，其用于定位。

通过使用语义分割地图进行非线性优化来实现车辆位姿估计。我们使用不同的后处理方法对高精度地图中不同元素进行语义分割。给定车道和电线杆的分割结果，使用腐蚀和膨胀操作生成梯度图像。

C.初始化

搜索和优化代价由所有语义路标的光度残差之和来定义，这可以被写为：

D.跟踪

如果驾驶场景满足纵向约束设置，则执行从全局地图中剪裁局部地图的步骤。否则，首先应用纵向位置校正过程。

从全局地图中剪裁局部地图：使用当前粗略的车辆位姿在一个预先定义的短距离内从全局地图查询地图元素（LA、PO和SB）。然后，被查询的局部地图应用于无漂移的视觉定位。地图元素E被投影回到图像点P。为了获取精确的位姿优化，P中的点在图像空间中均匀采样。

最后，为了给规划模块提供更平滑的位姿，并且提高定位系统的鲁棒性，通过滑动窗口应用位姿图。在滑动窗口中包含良好跟踪的帧。如果窗口尺寸超过一个阈值，则根据车辆状态将历史帧从滑动窗口中移除。例如，如果车辆的里程计测量接近于零，则选择次新的帧，否则使用最旧的帧。

在位姿优化过程中，因子图有两个组成部分。第一部分为每帧的先验位姿因子，其约束视觉对齐的先验分布。另一部分为车轮里程计因子，其建立相邻帧之间的连接来确保平滑的位姿输出。位姿图优化的总残差如公式3所示。将G20框架用于优化过程。

E.优化

关于损失函数的梯度细节信息在如下公式中获取。误差相对于优化状态的雅可比通常用于加速非线性优化方法（例如，Gauss-Newton或者LM）的过程：

F.定位丢失恢复

然而，系统在下面三种情况中可能丢失：（1）车辆超出高精度地图的操作域；（2）位姿优化失败的总次数超过一个阈值；（3）具有严重遮挡的连续帧数量超过一个阈值（例如，这发生在交通拥堵的情况下，语义地图元素都不可见）。跟踪置信度计算模块将基于上述统计指标来确定系统状态。当定位系统处于丢失状态时，激活丢失恢复模块。丢失帧的位姿通过由车轮里程计推算的备用位姿（即优化前的位姿）替换。给定下一帧，为了激活跟踪阶段，系统再次切换到初始化状态。

实验

所提出算法在两个数据集上进行评估。第一个数据集包含上海大约30公里的高架结构化场景，由第三方地图供应商提供。地图元素包含车道标记、标志牌和电线杆。由于矢量格式的紧凑环境表示，地图的存储空间为KB级别。定位系统在很多场景中进行评估，包括各种天气条件、光照强度和不同路线。第二个数据集为公开的Kaist数据集。由于Kaist数据集没有提供语义地图，而语义地图对所提出算法是非常重要的，因此使用双目相机数据以及来自激光雷达和INS的高精度定位位姿来构建一个语义路标地图。使用定性和定量的实验结果来评估本文方法的精度和鲁棒性。

A.定性结果

精确的定位确保地图元素在图像上的投影与语义感知结果是完全一致的（例如，图5（a））。图5列出高精度地图与图像语义分割结果之间的对齐示例。图5（b）-（g）的结果为（a）中使用的车辆位姿在小扰动下的对齐效果。角度扰动量为2°，平移扰动量为1m。

我们可以看出，在俯仰、偏航、y和z方向的扰动下，高精度路标的投影变化很大。相比之下，投影结果受到横滚角和车辆前向扰动的影响较小。此外，由于图像尺度与车辆高度密切相关，因此使用错误的车辆高度将使车道标记投影扩展到图像边界或者收缩到图像中心。所以，如果没有标志牌或者电线杆存在，则横滚角和车辆纵向位置不包含在优化阶段中。

Shanghai数据集和Kaist数据集的投影结果如图6和图7所示。例如，图6包含不同场景的结果：（1-2）弯路；（3）在晴天的长直路；（4）雨天；（5）挡风玻璃雨刮遮挡部分图像；（6）分叉匝道；（7）低光照；（8-9）交通拥堵。本文视觉定位系统在这些场景中取得鲁棒的结果。

B.定量评估

由于加密问题，第三方地图无法精确匹配GNSS惯性导航系统Novatel的高精度轨迹。相对位姿误差（RPE）被用作定位精度的评估度量。因此，横向和纵向定位精度比其它度量更为关键。这两个误差在实验中给出。我们使用EVO作为精度评估工具。帧间隔设置为5，大约12米，图像频率为10Hz，车速大约为80km/h。

若干数据序列的定位精度评估在表格I和图8中给出。

在本实验中，序列3使用广角相机（FOV为120°），其它序列使用FOV为42.5°的相机。平均旋转误差小于1°。横向误差和纵向误差大约为20cm。如果通过从序列中任意一帧初始化成功定位，则该帧被标记为初始化成功帧。通过手动标记和观测，可以计算初始化成功率。本文提出的定位初始化策略获得大约90%的成功率。初始化失败主要由于信号遮挡而造成GPS信号较差所导致的。此外，由于通过广角相机在横向采集到更多的语义元素，因此序列3的定位精度被提高。

C.运行耗时分析

表格II显示了不同模块（没有分割）在跟踪阶段的运行耗时。KD树被用于从城市地图中查询路标以提高搜索效率。最耗时的步骤为图像后处理，即梯度场构建过程。在i7-8700k CPU和GTX 1050 T1 GPU机器配置下，定位运行频率接近100Hz。

D.场景变化

高精度地图通常无法及时更新，但是本文提出的算法对于小尺度城市环境变化是鲁棒的。我们的方法还能够确定地图的变化区域，这对于定位和建图应用是非常重要的。图9显示在驾驶场景中标志牌布局发生了变化。本文方法不仅能够实现鲁棒定位，还能根据相机和地图之间的缺省对齐给出地图中的更新区域。

E.多相机支持

具有42.5°视场角的前视相机和具有195°视场角的后视鱼眼相机被用作本文多相机定位实验的传感器装置。为了简化计算，原始的鱼眼图像被转换为针孔图像。图10通过使用两个相机和仅使用后视相机（模拟前视相机不可用的情况）给出定位结果。结果表明，即使前视相机不起作用，仍然可以获得成功的定位结果。因此，多相机装置提高了定位系统的鲁棒性和精度。

总结

在本文中，我们提出一种基于视觉的定位系统，它使用车辆车轮里程计、普通的车载消费级GPS、高精度地图和相机。本文系统能够处理单目相机和多相机传感器装置。我们还证明，本文系统对于不同环境条件和驾驶场景变化是非常鲁棒的，并且获得精确的定位结果。在未来工作中，我们将引入IMU到定位系统中以构建一个视觉惯性里程计和GNSS-IMU惯性导航系统。VIO系统和INS系统的位姿输出结果将有效地与本文算法的定位结果融合，以形成一个实用的低成本量产定位系统。

文章转载自公众号：自动驾驶专栏