文章：Coarse-to-fine Semantic Localization with HD Map for Autonomous Driving in Structural Scenes
作者：Chengcheng Guo, Minjie Lin, Heyang Guo, Pengpeng Liang and Erkang Cheng
编译：点云PCL
来源：arxiv 2021 (纽劢科技)
本文来自点云PCL博主的分享，未经作者允许请勿转载，欢迎各位同学积极分享和交流。
摘要
鲁棒准确的定位是机器人导航和自动驾驶车辆的重要组成部分，而利用相机在高精地图（HD map）种进行定位则提供了一种低成本的定位传感器，现有的方法大多由于容易出错的数据关联性或初始姿势要求准确性而导致位姿估计失败。在本文中，我们提出了一种经济高效的车辆定位系统，该系统使用相机作为主要传感器，在具有高精地图环境中用于自动驾驶，为此，我们将基于视觉的定位描述为一个数据关联问题，将视觉语义信息映射到高精地图中的路标。具体地说，系统初始化是通过结粗略的GPS（全球定位系统）测量值和精细位姿搜索以从粗到精的方式完成的，在跟踪阶段，通过将高精地图中的图像和路标之间的语义分割结果与光度一致性隐式对齐来精细化求解车辆位姿，最后，以滑动窗口方式通过位姿图优化计算车辆位姿，我们在两个数据集上评估了该方法，并证明了所提出的方法在不同的驾驶场景中产生了积极的定位结果，此外，我们的方法适用于单目摄像机和多摄像机，为定位系统提供了灵活性并提高了鲁棒性。
主要贡献
近年来，基于视觉的先验环境地图定位方法得到了广泛的研究，主要有以下三种方式：
- 点云地图与矢量格式地图：定位中的先验地图可分为基于点云的地图和矢量格式地图，点云地图可以由激光雷达或相机作为传感器构建，与点云地图相比，紧凑的矢量格式高精地图轻量化，易于部署和更新。
- 基于特征的方法：利用环境中的低级几何特征或高级语义特征建立图像与地图之间的关联性。
- 直接法：不需要显式的关键点检测器或特征描述子，它可以自然地从所有具有强度梯度的图像区域中采样像素，例如，基于图像梯度点的图像对齐来估计帧间姿势，利用边缘特征进一步用于生成用于姿势优化的深度图像等。
该文章的目标是提供一个鲁棒和准确的基于视觉的定位系统，结合矢量形式的高精地图和图像语义信息，提出了一种由粗到精的视觉定位方法，在系统初始化步骤中，由车载GPS提供粗略初始化，然后通过穷举姿势搜索进行细化，在跟踪阶段，通过将图像语义感知与高精地图中具有相同语义的地标对齐来估计姿态，具体地说，对于给定的一幅或多幅图像，首先通过深度学习方法得到高精地图中实体的语义分割结果，基于分割结果，利用类距离变换函数构建成本图，最小化成本可以定义为成本地图上地标的投影光度误差，在附加车轮里程计信息的情况下，通过滑动窗口方案中的位姿图优化计算最终位姿，最后，跟踪丢失恢复模块负责在跟踪阶段发生故障时重新初始化系统，该系统运行图如图1所示。
图1 基于高精地图的视觉定位运行效果
主要贡献：
- 通过利用语义分割和高精地图，提出了一个完整的视觉定位系统，包括初始化、跟踪和跟踪丢失恢复模块。
- 我们的解决方案可灵活处理单目摄像头和多摄像头系统。
- 我们在两个数据集上评估了我们的方法，并证明我们的方法在不同的驾驶场景中产生了积极的定位结果。
主要内容
图2，提出的视觉定位系统概述，基于先验的地图、低成本GPS、轮速里程计输入和摄像头信息，可以以厘米级精度估计6自由度姿态
A.高精地图
高精度地图在自主驾驶中，通常是一种简单且灵活的环境结构表达方式代表着驾驶场景，在车辆定位中，使用地图元素路标（ lanemarkings LA）、杆状物体（ pole-like objects PO）、标志牌（ signboards SB），这些元素由HD地图中连续有序的三维点集合来描述，图2的跟踪部分中的图形显示了上述语义元素，在定位系统中，可以根据当前车辆位置和给定的搜索半径查询地图元素，对于查询到的地标，我们以固定长度间隔采样点作为地标代表。
B.语义分割和后处理
为了找到高精地图元素与图像的对应关系，采用语义分割的方法提取图像的语义特征，我们提出了一个轻量级的深度学习网络，可以提供有效的分割结果，深度学习的主干是Resnet-18，并在Cityscape数据集上进行预训练，该网络是一个多头部结构，每个头部是高精地图中一个元素（LA、PO或SB）的二进制分割，用于定位。通过使用语义分割图进行非线性优化来实现车辆姿态估计，这里使用不同的后处理方法对高精地图中的不同元素进行语义分割，给定车道和极点的分割结果，使用腐蚀和膨胀操作生成梯度图像，对于标志地标，采用拉普拉斯变换提取边缘信息，然后利用形态学运算得到平滑的梯度图像。
图3，距离变换和形态学运算的比较。左：车道标线分割图像；中间：基于距离变换的代价或者成本图；右：形态学操作后的代价图。
图3显示了所提出方法使用的距离变换和形态学操作之间的图像差异，形态学运算生成的代价图更容易使姿态优化收敛到正确的结果，最后，将处理后的分割结果在[0,1]范围内进行转换。
C.初始化
初始化模块的目的是在地图坐标系中获得相对精确的姿态估计，以便进行后续的姿态跟踪步骤,我们以从粗到精的方式介绍了一种鲁邦而精确的初始化方法，具体而言，是由两个有效的GPS信息计算粗略的初始姿态Twb，由于车辆可能处于静止状态，两个GPS点的距离设置为中值，车辆的x和y平面坐标设置为第二个有效点，根据地图近地面元素搜索得到z坐标，此外，车辆的侧倾角θx和俯仰角θy设置为零，偏航角θz设置为两个选定测量的方向，为了获得较高的初始化成功率和更精确的初始姿势结果，通过在预定义网格中进行穷举位姿搜索来细化粗略的初始姿势，搜索和优化成本由所有语义地标的光度残差之和定义，可以表示为：
在等式1中 ，Pw是地图中元素{Ei}的3D世界坐标，Tbc是相对于车辆基线的相机外参，π是基于摄像机模型的投影函数，对于不同的姿态自由度，我们使用不同的搜索参数、搜索步长和搜索范围。例如，车辆横向位置的搜索步长和范围设置为0.2m和[-10m，10m]，这涵盖了车载GPS的误差容限，最后，将具有最小代价的姿势组合视为当前初始化帧的姿势，通过CUDA加速实现了高效的计算。
D.跟踪
在给定初始姿态后，进行跟踪阶段，基于语义特征和先验地图的估计车辆姿态，跟踪模块可分为三个步骤。首先，基于k时刻的姿态估计和其它传感器输入，如车轮里程测量值，预测k 1帧的车辆姿态。如果驾驶场景满足纵向约束设置，则执行全局地图步骤中的裁剪局部地图，否则，首先应用纵向位置校正过程。
- 从全局地图元素（LA、PO和SB）裁剪局部地图将使用当前粗略的车辆姿势在预定义的阈值距离内从全局地图查询，然后利用查询到的局部地图进行无漂移视觉定位，将地图元素E投影回图像点P。为了获得精确的姿势优化，P中的点在图像空间中均匀采样。
- 纵向位置校正如果驾驶场景不满足纵向约束条件，经过长时间后，纵向定位可能会出现显著漂移，这种纵向位置校正模块能够避免在恶劣环境条件下，特别是在长时间内，纵向定位的漂移问题。
其次，通过与高精地图元素的图像对齐来细化6自由度车辆姿态，基于图像语义分割和形态学操作，已经建立了代价图，通过非线性优化（Levenberg-Marquardt（LM））解决对准问题，如果场景中缺少垂直地标（如招牌或标杆），则通过计算θy、θz、ty和tz，首先估计θy、θz和ty，然后优化θy和tz。θx和tx不包括在内，因为当车辆在平地上行驶时，侧倾角通常很小，并且当车辆和搜索车道相互平行时，车辆的纵向位移不可用，此外，为了折衷优化中缺少的侧倾角度，车辆旋转使用大范围的通过暴力搜索进行微调。旋转的搜索间隔设置为0.5度。
最后，为了使规划模块获得更平滑的姿态，提高定位系统的鲁棒性，采用了带有滑动窗口的姿态图，优化窗口中包含跟踪良好的帧数据，如果滑动窗口的大小超过阈值，历史记录中的一帧将根据车辆状态从滑动窗口中剔除。例如，如果车辆里程测量值接近零，则使用第二个最新帧，否则使用最旧帧。在姿态优化中，因子图由两部分组成，第一部分是每帧的先验姿态因子，约束其视觉对齐的先验分布，另一个是车轮里程计系数，它建立相邻帧之间的连接，以确保平滑的姿势输出，位姿图优化的总残差如等式所示：
E.优化
关于损失函数梯度的优化细节在以下方程式中推导，相对于优化状态的误差雅可比矩阵通常用于加速非线性优化方法（如高斯-牛顿法或LM法）的过程：
为了支持多摄像机观测，优化状态是车辆姿态而不是相机姿态，相机的外参Tbc用于车辆和摄像机坐标系之间的转换，优化状态中不包括相机外参数。单个图像的优化结果如图4所示。
图4，将高精地图元素投影到图像的优化示例，初始姿势投影为红色，优化结果为绿色。
F.跟踪丢失恢复系统
跟踪系统可能在以下三种情况下丢失：
（1）车辆不在HD地图的范围内；
（2） 姿势优化失败的总数超过阈值；
（3） 严重遮挡的连续帧数超过阈值（例如，在语义地图元素完全不可见的交通堵塞情况下会发生这种情况）。
跟踪置信度计算模块将根据上述统计指标确定系统状态，当定位系统处于丢失状态时，跟踪丢失恢复模式被激活，丢失帧的姿势替换为从车轮里程计推断的备用姿势，即优化前的姿势，给定下一帧，为了激活跟踪阶段，系统再次进入初始化状态。
实验
本文在两个数据集上对所提出的算法进行了评估：
- 第一个数据集包含上海约30公里的高架结构化场景，由第三方地图供应商提供。地图元素包括车道标记、标志牌和标杆，由于矢量格式的地图表示，地图的存储大小为KB级。定位系统在多种情况下进行评估，包括各种天气条件、光照强度和不同路线。
- 第二个数据集是公共Kaist数据集，由于Kaist数据集不提供所提出算法所需的语义地图，因此利用激光雷达和惯性导航系统的立体相机数据和高精度定位姿态来构建语义地标地图，定性和定量实验结果用于评估该方法的准确性和鲁棒性。
A.定性分析结果
精确定位可确保地图元素在图像上的投影与语义感知完全一致（如图5（A））。
图 5 高精地图与图像语义分割对齐示例
图5列出了HD地图和图像语义分割之间的对齐示例。（b）-（g）的结果是（a）中使用的车辆姿态小扰动下的对准，角度扰动量为2度，平移扰动量为1米，我们可以看到，HD地标的投影随着俯仰、偏航、y和z的扰动而变化很大，相比之下，投影结果受侧倾角和车辆前进方向扰动的影响较小，此外，由于成像比例与车辆高度密切相关，车道标记投影将扩展到图像边界或收缩到具有错误车辆高度的图像中心，因此，如果没有标志牌或标杆，则优化阶段不包括侧倾角度和车辆纵向位置。
图6，上海数据集的定性结果，不同场景的投影效果
上海数据集和Kaist数据集的投影结果如图6和图7所示，例如，不同场景的结果如图6所示：（1-2）弯道；（3） 阳光明媚的日子里，长长的直道；（4） 多雨；（5） 挡风玻璃雨刮器阻挡部分图像；（6） 分流匝道；（7） 低照度；（8-9）交通堵塞。我们的视觉定位系统在这些场景中实现了稳健的结果。
图7，Kaist数据集的定性结果，左：矢量格式地标hdmap和投影结果，中间和右边是两个不同场景的投影结果
表中报告了几种数据序列的定位精度评估。在实验中，在序列3中使用宽摄像机（视场为120度），而在其他序列中使用具有42.5度视场的摄像机，平均旋转误差小于1度，横向误差和纵向误差约为20cm，如果通过从顺序中的任何帧初始化，在10帧内定位成功，则此帧将标记为初始化成功帧，初始化成功率可以通过手动标记和观察来计算，我们提出的定位初始化策略实现了大约90%的成功率，初始化失败主要是由于信号阻塞导致GPS信号差，此外，广角摄像机在横向上捕获了更多的语义元素，提高了序列3的定位精度。
B.定量评估结果
由于加密问题，第三方地图无法精确匹配全球导航卫星系统（GNSS）惯性导航系统Novatel的高精度轨迹。采用相对位姿误差（RPE）作为定位精度的评价指标，由于横向和纵向定位精度比其他指标更为关键，实验中报告了这两个误差。
图8 序列1的横向和纵向定位误差
C.场景变化挑战
高精地图通常无法及时更新，而我们提出的算法对小规模的城市环境变化具有鲁棒性，此外，我们的方法能够确定地图的变化区域，这对定位和地图应用具有重要意义。图9显示了在驾驶场景中改变的标志牌布局。我们的方法可以（1）实现鲁棒定位
（2）根据摄像机和地图之间的偏差报告地图中的更新区域。
图9，高精地图没有及时更新的例子，例如，场景中缺少几个交通标志（青色的矩形表示）
D .支持多相机
视野为42.5度的前摄像机和视野为195度的后鱼眼摄像机作为我们的多摄像机定位实验传感器设置，为了简化计算，将原始鱼眼图像转换为针孔图像，图10说明了使用两个摄像头和仅使用后摄像头模拟前摄像头禁用的定位结果，结果表明，即使前视摄像机失效，仍能获得成功的定位结果，因此，多摄像机设置提高了定位系统的鲁棒性和准确性。
图10，多相机（如前后）视觉定位示例。（a） 前后都有相机。（b）前相机在系统中禁用
总结
在本文中，我们提出了一种基于视觉的定位系统，该系统使用轮速里程计、普通汽车配备的消费级GPS、高精地图和摄像头。该系统能够处理单目和多摄像头传感器设置定位，并且证明了我们的系统对不同的环境条件和驾驶场景的变化具有鲁棒性，并获得了准确的定位结果。在未来的工作中，我们将把IMU引入定位系统，建立视觉惯性里程计和GNSS-IMU惯性导航系统，VIO系统和INS系统的姿态输出将与我们算法的定位结果有效融合，形成一个实用的低成本批量生产定位系统。
