基于自动驾驶车辆新型的视觉语义定位算法

摘要

高度精确且鲁棒的定位能力对于城市场景中自动驾驶车辆（AVs）至关重要。传统的基于视觉的方法由于光照、天气、视角和外观变化而导致定位丢失。在本文中，我们提出一种新型的视觉语义定位算法，它基于高精度地图和具有紧凑表达的语义特征。语义特征广泛出现在城市道路，并且对于光照、天气、视角和外观变化具有鲁棒性。重复的结构、漏检和误检使得数据关联（DA）具有高度奇异性。为此，本文提出了一种考虑局部结构一致性、全局模式一致性和时序一致性的鲁棒DA方法。此外，本文引入一种滑动窗口因子图优化框架，以融合关联结果和里程计测量信息，而不需要地图特征的高精度绝对高度信息。

我们在仿真和真实城市道路上评估所提出的定位框架。实验结果表明，所提出方法能够实现高度精确定位，其平均纵向误差为0.43m，平均横向误差为0.12m，平均偏航角误差为0.11°。

介绍

近年来，AVs已经受到工业界和学术界的广泛关注。高精度定位对于AVs而言是一项关键威廉希尔官方网站 ，因为决策、规划和控制等各种模块都严重依赖于定位结果。为了实现精确定位，AVs装载了各种传感器，例如GNSS、相机、激光雷达、IMU、车轮编码器等。由于激光雷达的价格昂贵，因此低成本相机和IMU更适用于商用级AVs定位。

城市场景中具有各种复杂的道路条件，例如城市峡谷、隧道、高架等，这使得AVs定位更具有挑战性。为了在该场景中实现鲁棒定位，涌现了各种方法，例如基于GNSS的方法、基于视觉的方法、基于视觉-惯性的方法、基于激光雷达的方法。基于GNSS的方法能够在开阔场景中实现厘米级精度，但是它在遮挡和多路径条件下不够可靠。所以，提出了融合GNSS和IMU或者里程计的方法，以解决GNSS的问题，但是由于里程计漂移，它们在长期缺少全局定位信息的场景中仍然会失效。为了解决漂移的问题，广泛应用基于先验地图的方法。最常用的地图为点云地图，它能够通过ICP或者NDT方法实现厘米级定位，但是点云地图的存储对于商用级AVs而言是一个巨大的挑战。传统的视觉特征地图也已经被尝试用于定位，但是由于光照、天气、视角和外观变化，它们受到跟踪丢失的影响。

为了解决这个问题，我们提取视觉语义特征，并且基于高精度地图进行定位。与传统视觉特征相比，语义特征广泛出现在城市道路上，并且对于天气、光照、视角和外观变化是长期稳定且鲁棒的。由于语义特征的奇异性、误检和漏检，DA是最大的挑战之一。因此，我们提出一种具有一致DA的精确且鲁棒的视觉语义定位系统。本文的主要贡献如下：

1）一种基于视觉语义特征和轻量级高精度地图的精确且鲁棒的定位算法，无需地图特征的高精度绝对高度信息；

2）一种基于局部结构一致性、全局模式一致性和时序一致性的鲁棒DA方法来解决DA的奇异性；

3）一种紧耦合视觉语义测量和里程计测量的因子图优化框架用于鲁棒定位；

4）在仿真和真实城市道路上进行大量实验来验证DA的有效性和定位的精度。

相关工作

A.基于传统视觉特征的方法

基于传统视觉特征的方法提取几何特征，例如点、线和平面，并且通过描述子执行特征匹配。Mul-Artal和Sons分别使用ORB和BIRD描述子来构建特征地图，接着通过特征匹配来获取位姿。ETH ASL LAB在这一领域已经完成大量的工作，包括多地图汇总、基于外观的在线路标选择。然而，这些方法仍然无法摆脱天气、光照、视角和外观变化的影响。

B.基于道路上语义特征的方法

基于道路上语义特征的方法被广泛应用于AVs。语义特征由道路标记、交通信号灯、交通标志、电线杆等组成。Schreiber和Poggenhans检测道路标记和路缘，并且通过将特征与地图匹配来定位AVs。Lu应用chamfer匹配来构建道路标记的约束，并且求解一个非线性优化问题来估计6自由度位姿。此外，Jeong对道路标记分类来避免奇异性，并且通过子地图匹配、回环和位姿图优化来实现精确定位。Wilbers和Spangenberg通过具有深度的电线杆来实现位姿估计。同时，Sefati融合来自相机和激光雷达的道路标记和交通标志以通过PF进行定位。此外，Wu通过从相机图像中提取的车道线和从占据栅格中提取的blob特征来执行定位。Ma通过概率直方图滤波来融合INS、GPS、车道线和交通标志的定位结果。除了道路标志和电线杆之外，Kummerle还通过激光提取建筑物垂直表面的几何信息来实现精确定位。 在本文中，我们仅使用一个单目相机进行定位。我们无需绝对高度，而是需要相对于当前位置的路面高度。这简化了地图构建的难度，并且大大降低了成本。

C.语义数据关联

由于语义特征的奇异性、误检和漏检，实现正确且鲁棒的数据关联是极其具有挑战性的。Spangenberg通过欧式距离和电线杆宽度将感知的电线杆与地图关联。同时，Hu和Xiao应用RANSAC来消除误匹配。进一步，Kummerle和Wilbers通过随着时间累积检测结果来构建子图，从而解决DA的奇异性。在目标跟踪的领域内，运行Hungarian算法和多假设跟踪。为了解决DA的奇异性，Bowman将概率DA引入到语义SLAM系统中。与这些方法不同，本文提出一种基于局部结构一致性、全局模式一致性和时序一致性的鲁棒DA方法来消除由奇异性造成的误匹配。

系统概述

全局定位问题可以被定义为：给定一系列传感器测量和高精度地图，估计位姿序列，其表示状态轨迹。位姿和特征位置被定义为和。定位问题被表示为如下最大后验（MAP）推断问题： MAP问题可以基于DA被划分为两步，包括DA过程和位姿估计过程。公式（1）表示DA过程，有必要在执行位姿估计之前基于先验位姿来建立测量和地图之间的数据关联。因此，MAP推断问题能够被重新定义为： 因此，定位框架被分为四个部分，即传感器和地图、检测器、关联以及优化，如图2所示。

传感器由一个单目相机、一个IMU、两个车轮编码器和一个GNSS接收器组成。相机用于检测语义特征。IMU和车轮编码器构成里程计来提供局部相对运动估计。GNSS接收器可以提供当前位姿的粗略估计，其用于系统初始化。检测器层从图像检测道路标记、电线杆、交通信号灯和交通标志。关联层关联从图像中提取的语义特征和高精度地图中的特征。关联过程被分为五个步骤。第一步，在先验位姿附近生成候选位姿，并且基于每个采样的位姿将地图特征投影到图像上。第二步，实现基于局部结构一致性的粗略关联以寻找近似最优的采样位姿。第三步，执行一种考虑匹配数量、匹配相似度和局部结构相似度的最优关联方法，以实现最优的全局一致匹配。第四步，执行连续帧之间的特征跟踪。第五步，执行时序平滑以获取时序一致的DA。在最后的优化层中，引入位姿图优化以估计基于DA的位姿和里程计测量。

方法

A.语义特征和检测

语义特征的选择对于定位性能而言是至关重要的。在本文中，我们基于工作Accurate and efficient self-localization on roads using basic geometric primitives中提出的准则来选择特征，因此选择道路标记、电线杆、交通信号灯和交通标志来定位。它们易于检测、在场景中频繁出现、存储高效、表示紧凑，对天气、光照、视角和外观具有不变性。 我们采用一种主流的卷积神经网络（CNN）方法YOLOV3来检测特征。一个检测到的标志包含一个检测到的类别，一个表示检测结果置信度的分数和一个边界框。标志的四个轮廓点存储在高精度地图中，并且每个点的高度为相对于当前位置所在路面的高度。一个检测到的电线杆由一个检测到的类别、一个表示检测结果置信度的分数和表示两个顶点的组成。电线杆通过两个顶点存储在高精度地图中。道路标记在图像平面和高精度地图中被表示为采样点。

B.语义数据与高精度地图关联

由于语义特征的奇异性、误检和漏检，DA成为语义定位系统中最具挑战性的问题之一。在本文中，我们提出一种基于局部结构、全局模式和时序一致性的鲁棒DA方法以解决DA的奇异性问题，并且确保空间和时序一致性。为了说明提出的DA方法，在Algorithm 1中提供了伪代码。

DA过程的细节信息如下： 步骤1：在由里程计获取的先验位姿附近通过采样生成候选位姿。对于每个采样位姿，将地图特征投影到图像平面上： 其中，为第个地图特征的位置。和为相机的内参和外参。为相机坐标系中第个地图特征的z轴位置。 步骤2：执行基于局部结构一致性的粗略关联来寻找近似最优的采样位姿来消除由巨大先验位姿误差造成的误匹配。局部结构一致性使感知特征和对应重投影特征的横向位置分布保持一致。首先，根据横向位置对感知特征和重投影特征进行升序排序。其次，我们计算每个感知特征和每个重投影特征之间的相似度： 其中，和可以通过感知结果的离线学习来获取。似然由位置和大小相似度组成： 其中，为一个学习的超参数，用于权衡位置相似度和大小相似度。、、和分别表示地图特征和感知特征的位置和大小。和可以从感知结果中离线学习。电线杆的似然由位置、姿态和重叠相似度组成。 如果感知特征的最大相似度分数大于一个阈值并且保留了局部结构，则将它们作为匹配对。对于每个采样的位姿，基于匹配数量和匹配误差计算代价以近似评估它： 其中，为超参数。定义为特征和之间的横向距离，如图3所示。

具有最大值的候选位姿被认为是近似最优的匹配采样位姿，并且将在步骤3中使用。 步骤3：基于近似最优的匹配采样位姿，执行一种考虑匹配数量、匹配相似度和局部结构相似度的最优关联方法，以实现最优的全局一致匹配。通过求解以下优化问题，将其表述为多次图匹配问题： 其中，和位置特征和重投影特征的数量，为两个特征之间边的数量。、和为超参数。表示感知特征是否与重投影特征匹配。表示感知特征和重投影特征之间的相似度，它由公式（4）计算获得。表示边和之间的相似度： 其中，和表示特征和、以及特征和之间的横向距离，如图3所示。可以离线学习。优化问题将通过通用的random re-weighted walk framework求解。 步骤4：特征跟踪：该过程建立连续帧中特征之间的关联。由于感知的特征是静态且保持局部结构的，我们将该过程表述为一个多次图匹配问题，类似于公式（7）。 步骤5：时序平滑：该过程构建连续帧中感知到的特征与地图特征之间的最优一致性匹配。通过滑动窗口中先前匹配结果可以验证当前帧的匹配正确性。进一步，如果当前帧中出现误匹配，可以基于先前匹配和跟踪找到并且校正。时序平滑通过在滑动窗口中对每一帧上的匹配和匹配置信度进行加权以获取地图特征对应的感知特征： 其中，表示是否地图特征与感知特征相匹配。通过评估特征和局部结构相似度来给出匹配置信度： 如果最佳感知特征的累积置信度比次佳感知特征的累积置信度高很多，则最佳感知特征将被认为是地图特征的匹配对。否则，地图特征被认为具有不确定的匹配，并且可以给出每个感知特征的匹配概率。该过程区分确定和不确定的匹配，这可以解决由奇异性造成的误匹配问题。

C.位姿图优化

公式（2）的位姿估计过程可以被定义为先验概率和似然的乘积： 根据高斯分布假设，通过里程计的相对运动估计来获取先验分布。我们基于里程计测量和特征的匹配对表述一个滑动窗口非线性最小二乘估计器来估计最新的个位姿。与常用的滤波方法相比，优化方法可以处理异步且延时的测量数据，并且以相同的计算资源实现更高的精度。优化目标表示为： 其中，每个误差项与对应的信息矩阵一起被认为是一个因子，每个状态变量可以认为是一个节点，因此定位问题可以通过因子图表示，如图4所示。

误差项由里程计误差、语义测量误差和地图误差组成。里程计误差定义为： 语义测量误差因子表示为： 其中，表示向量的首个元素。测量误差仅采用横向误差来消除高度误差的影响和地图特征对精确绝对高度的要求，如图5所示。

地图误差因子表示为： 其中，为第个地图特征的位置。在本文中，我们采用参考文献Localization with sliding window factor graphs on third-party maps for automated driving中提出的地图特征方差构造方法。在地图因子的各向同性假设下，根据假设的地图质量，地图因子的方差可以定义为： 其中，为逆卡方累积分布函数，表示置信度，表示半径。 非线性优化问题可以通过一个迭代算法直接求解。采用滑动窗口而不是批量方法来提高计算效率，同时确保定位精度。旧的状态被截断并且直接忽略。边缘化方法也可以处理老的状态，但是它累积了线性误差，使得系统矩阵变得稠密，并且导致死锁。边缘化方法基于过去的数据约束位姿，但是使用地图特征作为先验足以约束车辆位姿。

实验评估

A.典型场景中仿真

我们首先评估感知和地图误差如何影响所提出算法的性能。选择一个典型的交叉路口场景，其中道路两侧有四根电线杆，车辆前方有两个交通信号灯和两个交通标志。在不同感知和地图误差下的位置和偏航角误差如图6所示。

所有结果均为1000次实验的平均值，以消除随机性。结果表明，所提出方法获得高度精确的定位结果，其在0.2m地图误差和5个像素感知误差下，位置误差为0.23m，偏航角误差为0.064°。 所提出算法不需要地图特征的高度精确绝对高度。我们将本文方法与6自由度位姿估计方法进行比较，以展示对地图特征不同高度误差的影响。我们还在典型的交叉路口进行实验。图7说明了结果，它表明本文方法在位置和偏航角方面均获得明显更小的误差。因此，我们的横向误差测量模型对所提出系统的性能有益。

不同的特征分布也会影响定位性能。为了探索所提出算法的性能限制，我们在具有不同特征分布的各种场景中对其进行量化。我们将它们分为两种情况，包括车辆和特征之间不同的纵向距离，以及车辆坐标系下特征之间不同的横向距离。本文选择一个典型场景，其中车辆两侧的两个电线杆具有不同的距离。地图和感知特征假设分别具有0.05m和2个像素的标准差。实验结果如图8所示，它表明纵向距离越长，纵向定位越差，但是横向和偏航定位越好。然而，当横向距离增加时，纵向精度变得越好，但是横向和偏航定位变得越差。

B.真实城市道路场景

我们在一条长30km且具有各种场景（例如城市峡谷、隧道和高架）的城市道路上使用我们的车辆来评估所提出算法的性能。为了评估定位精度，融合激光雷达、RTK和里程计的定位结果作为真值。所提出系统的定位性能与基于最近邻（NN）的6自由度位姿估计方法在各种场景中的比较如表格I所示，它描述了所提出系统在各种场景中实现了更高的精度，其平均纵向误差为0.43m，平均横向误差为0.12m，平均偏航角误差为0.11°。

图9给出具有定位误差的轨迹，并且在高架道路上使用GNSS比较了定位精度。结果表明，本文方法具有明显更低的误差，这证明了本文方法在复杂道路场景中的有效性，在该场景中GNSS会失效。

图10给出横向、纵向和偏航误差分布。

所提出DA方法的正确性也通过在各种具有误检和漏检场景中的实验进行评估。图11给出两个场景的DA结果。左图展示本文方法在一般场景中的正确匹配结果。中间和右边图像说明关联方法有效地处理由误检造成的奇异性，这主要获益于局部结构和时序一致性约束。

我们还比较高精度地图与特征地图和点云地图的规模。高精度地图仅需要大约每公里10KB的存储空间，而特征地图和压缩的点云地图分别需要每公里53MB和每公里3MB的存储空间。

总结

在本文中，我们提出一种新型的语义定位算法，它利用图像中提取的语义特征和高精度地图中的特征。为了处理由重复结构、漏检和误检造成的DA奇异性，使用一种考虑局部结构、全局模式和时序一致性的鲁棒DA方法。一种紧耦合数据关联和里程计测量数据的滑动窗口因子图优化框架被设计用于精确且鲁棒的定位。对所提出系统进行验证，并且结果表明，所提出方法实现了亚米级的定位精度。 未来，我们希望使用更全面的理论来扩展我们的框架，以实时地评估定位性能。我们还计划检测更多的语义特征来提高所提出系统的鲁棒性。