基于视觉的语义匹配见多了，那基于雷达的呢？

论文题目：

LiDAR-based HD Map Localization using Semantic Generalized ICP with Road Marking Detection

论文作者：

Yansong Gong, Xinglian Zhang, Jingyi Feng, Xiao He and Dan Zhang

作者单位：北京驭势科技有限公司

导读：

本文为驭势科技最新的车辆定位算法论文，介绍了一种使用广义语义ICP和道路标记检测的基于激光雷达的高精地图定位方法，并且在现实世界场景中进行了大量实验，证明了该方法的有效性和鲁棒性。©️【深蓝AI】编译

1.摘要

在GPS信号不可用的场景下，强大的环境感知与定位系统对于自动驾驶而言变得至关重要。本文提出了一种基于激光雷达的在线定位系统，该系统融合了道路标记检测以及在高精地图上的配准功能。该系统使用了一种具备实时性能的道路标记检测方法，其中首先引入了一种自适应分割技术来提取与道路标记相关的高反射率点，从而提升实时处理的效率。接着，通过汇总历史激光雷达扫描数据，构建了一个时空概率局部地图，提供了一个密集的点云。最终，生成了一个激光雷达鸟瞰图（LiDAR Bird’s-Eye View, LiBEV）图像，并利用实例分割网络对道路标记进行精确标注。针对道路标记的配准，设计了一种SG-ICP（Semantic Generalized Iterative Closest Point, SG-ICP）算法。线性道路标记被建模为嵌入在二维空间中的1维流形，缓解了沿着线性方向约束的影响，解决了欠约束问题，并且在HD地图上实现了比传统的ICP算法更高的定位精度。在真实世界环境中进行了大量实验，证明了本文系统的有效性和鲁棒性。

2.介绍

精准定位在自动驾驶研究中至关重要，尽管GPS是露天环境下的主要定位技术，但在信号受阻时（如高架桥下）会变得不稳定。因此，使用摄像头和LiDAR等传感器的环境感知就成了处理问题的关键，尤其在GPS受限区域。道路标记检测是确保稳定感知的核心技术，它通过关联标记与高精地图来估算车辆位置。

摄像头因其丰富的环境纹理信息而广泛用于道路标记识别，但受光照变化和鸟瞰视图失真影响。LiDAR则较少受光照影响，提供精确的3D环境模型，且可从高反射材料中提取道路标记，但需平衡点云密度与实时处理需求。

在这里插入图片描述
图1｜在高精地图上的定位效果和检测到的道路标志©️【深蓝AI】编译

为了应对上述视觉匹配的难点和挑战，本作研究者提出了一种基于LiDAR的实时道路标记检测与地图配准方法，如图1所示，（a）板块是本文的方法中高精地图定位被可视化展示，其中车辆定位的轨迹用绿色标记，而车辆当前的姿态由一个红色立方体表示。蓝色的点云代表来自单帧激光雷达数据的地面点。这些地面点通过自适应分割来识别高反射点。随后，它们通过连续的数据帧汇聚形成更密集的点云。最后，应用语义分割获得语义点云，然后将其与高精地图配准，以估计车辆的姿态。（b）板块则为使用本文的方法提取的道路标记被可视化展示，包括虚线车道、实线车道、停车线、文字、箭头、菱形标志、三角标志、路缘石以及人行横道。本文的主要贡献有：

●提出了基于LiDAR的道路标记检测方法，用于在线环境感知，其中通过自适应分割高反射率点和更新时空概率局部地图来平衡点密度和实时性能。最终生成LiBEV图像，使用实例分割网络在LiBEV图像上能够准确检测出9种不同类型的道路标记。

●提出了一个新的道路标记配准算法，用于在HD地图上定位自动驾驶车辆，其中线性道路标记被表示为嵌入在二维空间中的1维流形。这种表示法可以为配准问题提供一个稳健且准确的解决方案，对欠约束维度的影响最小。与传统的ICP相比，SG-ICP实现了更高的定位精度。

●在真实世界场景中进行了全面的实验，展示了本文系统的实时性能和定位精度。实验结果表明，该方法适用于各种类型的LiDAR传感器，以及在不同的车速和天气条件下表现出的鲁棒性。

3.相关工作

在城市NOA领域，道路标记检测凭借其对车辆导航的引导作用成为环境感知的核心。激光雷达因能高效区分逆反射材料制成的标记与普通路面而成为主流检测工具，其原理在于标记物对近红外激光脉冲有较高反射率。此技术广泛用于构建高精地图，通过离线聚合高密度点云来详尽描绘环境，但处理速度慢，不适于实时应用。

实时检测中，单次激光雷达扫描的强度数据经阈值分割用于识别标记，如AnnieWAY在DARPA挑战中所展示。极坐标网格和改进的Otsu阈值算法也被用于标记检测，但单次扫描的稀疏性使检测结果不够稳定。多帧累积技术改善了这一点，通过固定阈值提取车道标记，且后续研究扩展至更多高反射目标，提升定位精度。然而，固定阈值策略在变动态势下表现欠佳。
近年来，深度学习在道路标记检测中崭露头角。LLDN-GFC等方法利用点云的车道线特征，如空间分布特性，优化了检测过程并降低计算需求。尽管如此，其专注车道线而忽略其他标记的局限性，可能影响车辆定位的全面性，特别是在横向约束方面存在潜在缺陷。

4.本文算法

针对先前研究中确定的局限性，本文提出了一种基于LiDAR的道路标记检测系统，用于实时环境感知。此外，本文还引入了一种新的道路标记配准算法，以提高使用高精地图的自动驾驶汽车的定位准确性。本文算法的流程图如图2所示。

在这里插入图片描述
图2｜本文算法的流程图©️【深蓝AI】编译

4.1 基于激光雷达的实时道路标线检测

◆高反射点分割：

此处理旨在适应性地识别高反射率的点，这些点通常与使用逆反射材料涂绘的道路标线相关联。为了确保在不同传感器和场景下都能适用，本文引入了一种自适应分割方法，专门设计用于提取高反射率的点，以构建一个更加稳健的系统。

该方法基于高度信息对地面点进行分割，并通过部分拟合地面平面来提取它们。随后，引入了一个分割系数 $\rho_k$ 来区分第 $k$ 次扫描中的高反射率点。具体而言，强度低于 $\rho_k$ 的点将从扫描中排除。值得注意的是，分割系数 $\rho_k$ 并非人为预设，通过卡尔曼滤波器动态估计并持续更新，状态转移模型为：

$\rho_k = \rho_{k-1} + w_k, \qquad (1)$

其中 $w_k \sim N(0, Q_k)$ 是过程噪声。测量模型则由下式给出：

$z_k = \rho_k + v_k, \qquad (2)$

其中 $v_k \sim N(0, R_k)$ 是测量噪声。在每次激光雷达扫描中，计算地面点强度的均值 $\mu_k$ 和方差 $\sigma_k$ 。然后测量值确定为 $z_k = \mu_k + 2\sigma_k$ 。

该自适应方法基于两点假设以确保其在城市道路环境中的有效性：

1）相邻路段的分割系数相似，源于地面材质一致性。

2）大部分激光雷达点反映普通路面，而路标点有更高强度。

聚焦高反射点优化效率，剔除无关数据，减轻计算负担。

◆概率局部地图更新：

为了实现实时性，引入了一种新的概率局部地图更新方法。该方法采用概率性丢弃策略，其中地图上的每个点都会基于计算出的概率值被选择性地移除。分配给局部地图中第 $i$ 个点的概率，记作 $p_i$ ，通过下式计算：

$p_i = \frac{1}{1 + \left(\frac{|k - k_i|}{\eta}\right)^2} \qquad (3)$

其中 $k$ 表示当前帧的索引，而 $k_i$ 代表第 $i$ 个点源自的帧数。是一个人工设定的参数，用于确定丢弃旧点的概率。当 $\eta$ 增大时，旧点更可能被保留，从而在概率局部地图中形成更高的点密度。

从公式3可以看出，激光雷达传感器新观察到的点被赋予较高的保留概率值。这种策略保证了时空一致性，缓解了时间累积误差。此外，与使用固定窗口内扫描的聚合方法相比，本文方法确保了局部地图数据更为平滑的过渡。

◆LiDAR 增强视图（LiBEV）图像生成：

生成LiBEV图像时，局部地图被分割为网格，各网格代表图像像素；RGB值基于色彩映射和点云强度确定。采用CenterMask网络精确分割LiBEV图像中的道路标记，接着从对应网格提取点，形成语义点云。此法支持9类标记，如虚线、实线、文字等，提升地图匹配定位的稳定性。语义分割图像比点云标注便捷，利于实际部署。

4.2 基于SG-ICP的高精地图（HD Map）道路标记配准

在SG-ICP中，道路标记按特性分为线、线段与其他三类。实线车道与路缘无明确端点，归为线；虚线、人行道等有端点，属线段；非线性分布的文字、箭头等标记为其他。线因无端点而在线性方向约束缺失；线段的端点虽提供约束，但端点误差影响配准精度；线性标记需减小其对姿态估计的影响，以缓解约束不足。非线性分布的其他标记则自然提供充足约束。本算法统一处理三类标记的配准，采用GICP目标函数。

广义迭代最近点（GICP）算法将概率模型融入优化过程中，定义如下：

$T^* = \underset{T}{\arg\min} \left( \sum_{i=1}^{n} (q_{mi} - T \cdot q_{Li})^T (C_{mi} + R C_{Li} R^T)^{-1} (q_{mi} - T \cdot q_{Li}) \right) (4)$

其中 $q_{mi}$ 和 $q_{L_i}$ 分别代表一对对应的点，前者属于高精地图元素，后者属于标记的点云。这些对应点是通过ICP算法中的最近邻搜索策略建立的。 $C_{mi}$ 和 $C_{L_i}$ 分别表示来自地图和标记点云的点的协方差矩阵，它们在本文的语义GICP（SG-ICP）算法中被适当地构建，以缓解欠约束方向的影响。

在本文的SG-ICP算法中，概率模型是通过利用语义道路标记内在的语义和几何属性专门设计的。对于位于第 $i$ 个检测到的道路标记实例上的点，协方差矩阵 $\tilde{C}_{Li}$ 被估计为：

$\tilde{C}_{Li} = \frac{1}{n_i - 1} \sum_{j} (p_{L(i,j)} - \tilde{p}_{Li}) \cdot (p_{L(i,j)} - \tilde{p}_{Li})^T\qquad(5)$

其中 $p_{L(i,j)}$ 表示第 $i$ 个道路标记实例的第 $j$ 个点， $\tilde{p}_{Li}$ 表示这些点的质心。接着，对 $\tilde{C}_{Li}$ 进行奇异值分解（SVD）。

$\tilde{C}_{Li} = U_i \tilde{\Sigma}_i V_i^T, \tilde{\Sigma}_i = \begin{bmatrix} \sigma_1^2 & 0 \\ 0 & \sigma_2^2 \end{bmatrix}\qquad(6)$

$\sigma_1$ 和 $\sigma_2$ 满足 $\sigma_1 \geq \sigma_2$ 。然后，构建矩阵 $\Sigma_i = \text{diag}(1, \epsilon)$ ，其中满足：

$\epsilon = \begin{cases} 1e-6, & \text{如果标记被分类为线条;} \\ 1e-1, & \text{如果标记被分类为线段;} \\ 1, & \text{如果标记被分类为其他。} \end{cases} (7)$

这三种道路标记类别具有不同的 $\epsilon$ 值，代表着沿直线方向的不同约束程度。 $\epsilon$ 的值越接近1.0，表示沿直线方向的约束越强。与第 $i$ 个道路标记对应的最终协方差矩阵可以通过以下方式计算：

$C_{Li} = U_i \Sigma_i V_i^T\qquad(8)$

在高精地图中的第 $i$ 个语义元素表示为 ${v_{mi}, l_{mi}, P_{mi}\}$ ，其中 $v_{mi}$ ， $l_{mi}$ 和 $P_{mi} = \{p_{m(i,j)}, j = 1, 2, \ldots, n_{mi}\}$ 分别表示主要方向、语义标签和地图元素的点集。将基向量 $e_1 = [1, 0]^T$ 旋转到方向 $v_{mi}$ 的旋转矩阵可以按以下方式计算：

$R_{vi} = \cos(\theta)I + (1 - \cos(\theta))rr^T + \sin(\theta)[r]_\times\qquad(9)$

其中

$[e_1]_\times v_{mi}, \quad \theta = \arccos(e_1^T v_{mi})\qquad(10)$

符号 $[r]_\times$ 表示与向量 $r$ 关联的反对称矩阵。与第 $i$ 个语义元素对应的协方差矩阵由以下方式计算：

$C_{mi} = R_{vi} \Sigma_i R_{vi}^T\qquad(11)$

最后，可以在语义点云和共享相同语义标签的地图元素的最近点之间建立关联。

5.实验

5.1 实验配置

实验基于NVIDIA Jetson AGX Xavier平台，激光雷达采样率10Hz，利用RTK获取并同步车辆定位数据作为真值。图3示出实验场景及对应高精地图，包括两个房山城市区域（总面积0.30km x 0.25km，全长2.0km）、嘉善测试场0.20km内部道路及机场4.0km路段。方法参数中，卡尔曼滤波器的状态转移与测量模型初试方差设为0.1和2.0，点丢弃概率参数定为50.0。

在这里插入图片描述
图3｜实验场景（上方）及其对应的高精地图（下方）。(a) Fangshan1 (b) Jiashan © Fangshan2 (d) Airport©️【深蓝AI】编译

5.2 道路标记检测评估

本小节通过精确度和召回率指标评估了道路标记检测方法的性能。实验中，80%的手动标注LiBEV数据用于训练，剩余20%用于测试。手动注解作为真实值，用于评估检测方法的精确度和召回率。当检测实例与标注实例的交并比（IoU）超过0.5且共享同一语义标签时，判定为真阳性；反之，未找到匹配实例则为假阳性，而未检测到真实存在的实例为假阴性。

表I概览了9类道路标记检测的精确度、召回率与F1分数，证实了方法对典型道路特征高精度识别的能力。尽管路缘和人行横道的精确度稍逊，受其与车道线视觉相似性的限制，但HD地图配准有效抑制了误检影响。该方案不仅成效显著，且运行高效，满足实时车辆感知需求，详述见D节。

5.3 定位性能评估

SG-ICP算法通过横向、纵向及偏航误差评估，跨八组实验序列，涵盖四场景七配置，彰显其适应性。对比通用ICP算法（表II），SG-ICP多数序列表现更优，尤其在侧向与偏航精度上，得益于其对充分约束方向的强化处理。

图4展示了基于SG-ICP和基于ICP两种方法估计的轨迹与通过RTK获取的真实值的对比可视化。值得注意的是，SG-ICP和ICP的显著定位误差分别用紫色和红色线条标记，其中估计距离误差超过2.0米或偏航误差超过5.0度。从图4可以看出，在所有序列中，SG-ICP相较于ICP显著减少了大量定位误差的发生。

总的来说，所提出的方法在多种环境场景和不同类型激光雷达传感器下实现了厘米级的横向定位精度。测试的传感器包括VLP-32C、Hesai-Pandar64和Hesai-XT16传统机械激光雷达，还包括HAP固态激光雷达。全面说明了该方法在不同场景和传感器类型下的鲁棒性和适应性。此外，如表II所示，纵向误差略大于横向误差，这一点值得关注。在自动驾驶发生的城区道路场景中，大部分道路标记沿纵向呈线性形状。尽管如此，本文的方法保证了纵向误差保持在0.20米以下，确保了其在自动驾驶应用中的有效性。

5.4 运行时间评估

在八个序列进行的实验中，本文方法每个子步骤的运行时间在表III中，箱型图可在图5中。值得注意的是，检测子步骤的运行时间分为CPU时间和GPU时间。CPU时间指的是CPU处理步骤所消耗的时间，包括高反射点分割、概率局部地图更新和LiBEV图像生成。GPU时间指的是LiBEV图像实例分割的推断时间。配准子步骤仅由CPU处理。可以看到，当使用车载处理器XAVIER时，整个方法的平均运行时间和最大运行时间在各种场景和激光雷达传感器类型下始终低于50毫秒和200毫秒。

此外，值得一提的是，在S1序列上的运行时间仅比S2序列长8.35毫秒，尽管S1的数据量是S2的两倍（如表II的激光雷达类型列所示）。说明运行时间并没有随着点云数据量的增加而线性增长，因为通过概率丢弃策略，聚合局部地图点的数量大幅减少。

5.5 鲁棒性评估

为了测试鲁棒性，本文在不同的车速下评估了定位误差，如表IV所示。在Fangshan1场景中，使用1台Hesai-Pandar64激光雷达，车辆分别以20、40和60公里/小时的速度行驶。然后，将得到的结果与RTK提供的真实值进行对比。

如表IV所示，尽管随着更高的驾驶速度，定位误差略有上升，但本文方法始终保持较高的定位精度水平。这证明了该方法在不同车辆速度下的鲁棒性。关于实时性能，如表IV所示，整体系统运行时间几乎不受车速增加的影响，表明了系统在处理车速变化时的鲁棒性。

为了展示不同天气条件下的鲁棒性，本文在不同环境下进行了实验。如图6所示，干路面和湿路面上雷达点云的强度分布（晴天和雨天）表现出显著差异。因此，雨天对基于强度的道路标记提取构成重大挑战，尤其是对于依赖固定强度阈值的方法。图7展示了在晴天和雨天条件下生成的LiBEV图像。很明显，本文方法即使在由不同天气条件引起的不同的强度分布情况下，也能持续提供稳定和准确的分割结果。表V呈现了在Fangshan1场景中，使用1台Hesai-Pandar64激光雷达，在干地和湿地面上定位误差的对比。尽管在湿地上定位误差增加，但仍能确保平均横向误差在0.10米以内，纵向误差在0.20米以内。这些结果证明了本文方法在不同天气条件时的鲁棒性。

6.结论

本文介绍了一种基于激光雷达的在线环境感知与定位系统，利用自适应分割技术提高道路标记检测的效率，并设计了SG-ICP来实现语义信息的配准，再城市环境中的大量实验说明了该系统的有效性和鲁棒性。然而，本文的方法无法应用于地面无道路标记的道路上。在未来的工作中，本文将探索有效利用地面以上信息的方法，以提高定位的鲁棒性。

编译｜蒙牛二锅头

审核｜Los

移步公众号【深蓝AI】，第一时间获取自动驾驶、人工智能与机器人行业最新最前沿论文和科技动态👇

深蓝AI·赋能AI+智驾+机器人