驭势科技研究成果入选学术顶会IROS 2024

近日，驭势科技团队关于自动驾驶车辆定位算法的最新研究成果《LiDAR-based HD Map Localization using Semantic Generalized ICP with Road Marking Detection》，创造性地解决了基于LiDAR的实时路标检测和高精地图配准所带来的挑战，成功入选国际顶级学术会议IROS 2024发表名单，标志驭势在自动驾驶中的精准定位能力已达到国际领先水平。

关于IROS

IROS由著名的IEEE（美国电气与电子工程协会）和日本机器人联合会联合主办，这两个组织在机器人及相关领域具有深厚的学术积淀和广泛的影响力，为IROS会议的学术水平提供了坚实保障。IROS每年都吸引了来自世界各地顶尖高校、科研机构及创新企业的大量人才参与。会议论文的评审过程严格，通常由该领域的资深专家学者进行评审，确保录用的论文具有较高的质量和创新性，能代表机器人领域的前沿研究成果。

本文介绍了一种使用广义语义ICP和道路标记检测的基于激光雷达的高精地图定位方法，并且在现实世界场景中进行了大量实验，证明了该方法的有效性和鲁棒性。

摘要

精准定位在自动驾驶研究中至关重要，GPS作为露天环境下的主要定位技术，在信号受阻时会变得不够稳定。在这种情况下，使用相机和LiDAR等传感器对环境的观测和感知信息进行定位，就成为了一项重要方法。尤其是针对机场道路这样的空旷环境，由于缺乏纹理信息和结构信息，检测出道路标记并使其与高精地图配准，最终完成车辆的自主定位，对于自动驾驶车辆在特殊场景实现稳定导航至关重要。

摄像头由于其图像包含丰富的环境纹理信息而广泛用于道路标记识别和检测，但受光照变化和鸟瞰视图车道表示中失真影响，在某些应用中表现不够鲁棒。相比之下，LiDAR对光照条件变化的敏感度较低，可提供精确的3D环境模型，且可从高反射材料中提取道路标记，但基于LiDAR的方法在平衡高密度点云和实时性能需求方面同样面临挑战。

为了解决这些挑战，本文提出了一种基于LiDAR的实时道路标记检测与地图配准方法：

首先引入了一种自适应分割技术来分割与道路标记相关的高反射率点，从而提升实时处理的效率。
接着通过汇总历史LiDAR扫描数据，构建一个空间-时间概率局部地图，从而提供了一个高密度的点云。
最终生成了一个LiDAR鸟瞰图（LiBEV）图像，并利用实例分割网络对道路标记进行精确的语义分割。

针对道路标记的配准，设计了一种语义广义迭代最近点（SG-ICP）算法。线性道路标记被建模为嵌入在二维空间中的1维流形，从而减小沿线性方向的约束影响，解决了欠约束问题，并在高精地图上实现了比ICP更高的定位精度。在真实场景中进行了大量实验，证明了系统的有效性和鲁棒性。

图1：在高精地图上的定位效果和检测到的道路标志

如图1所示，(a)可视化了该方法的高精地图定位，其中车辆定位轨迹用绿色标记，车辆当前位置由红色立方体表示。蓝色点云代表来自单帧LiDAR数据的地面点。这些地面点通过自适应分割来识别高反射点。随后通过连续数据帧汇聚形成更密集的点云。最后应用语义分割获得语义点云，将其与高精地图匹配以估计车辆的位置。(b)展示了使用本文方法提取的道路标记，包括虚线车道线、实线车道线、停车线、文字、箭头、菱形标志、三角标志、路缘以及人行横道。

主要贡献

提出了一种基于LiDAR的道路标记检测方法，用于在线环境感知。通过自适应分割高反射点和更新时空概率局部地图，平衡了点密度和实时性能。最终生成LiBEV图像，并使用实例分割网络在LiBEV图像上准确检测9种不同类型的道路标记。
提出了一种新颖的道路标记配准算法，用于自动驾驶车辆在高精地图上的定位。在线性道路标记表示为嵌入二维空间的1维流形。这种表示法可以为配准问题提供稳健而准确的解决方案，对欠约束维度的影响最小。与广泛使用的ICP相比，SG-ICP实现了更高的定位精度。
在真实场景中进行了全面实验，展示了系统的实时性能和定位精度。实验结果表明，该方法适用于各种类型的LiDAR传感器，并在不同的车辆速度和天气条件下表现出鲁棒性。

具体方法

图2：本文定位系统的流程图

基于LiDAR的实时道路标记检测

由于LiDAR点的分布稀疏，仅依赖单帧数据进行稳定且稳健的道路标记检测具有挑战性。为了解决这一限制，将连续的LiDAR扫描数据聚合到一个局部地图中，生成有利于有效道路标记检测的高密度点云。考虑到在线需求和高反射道路标记，聚合过程可以选择性地从地面平面中提取高强度点。这种方法确保构建一个用于道路标记检测的局部地图，用于道路标记检测，平衡了计算效率和信息丰富度。

高反射点分割

该过程旨在自适应识别高反射率的点，这些点通常与使用反光材料绘制的道路标记相关。为了确保在不同传感器和场景中的适应性，引入了一种自适应分割方法，设计用于分割出高反射点。这一增强整体上有助于系统更加鲁棒。

为了提高系统效率，仅考虑地面点，这些点从LiDAR扫描中提取。该方法基于高度信息分割地面点，并通过部分拟合地面平面来提取它们。然后，引入分割系数ρk来区分第k次扫描中的高反射点。具体来说，强度低于ρk的点将从扫描中排除。值得注意的是，分割系数ρk并非人为预设，而是通过卡尔曼滤波动态估计并持续更新。卡尔曼滤波的状态根据状态转移模型演变：

其中wk∼N(0,Qk)是过程噪声。测量模型由下式给出：

其中vk∼N(0,Rk)是测量噪声。在每次LiDAR扫描中，计算地面点的强度均值μk和方差σk。然后，创新计算的测量值确定为zk=μk+2σk。

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

1、假设附近的连续道路应具有相似的分割系数，因为地面材料在局部往往保持一致。

2、假设大多数LiDAR点位于常见的沥青路面，而道路标记点具有统计上较高的强度。

在大多数城市和机场道路环境中，这两个假设是成立的，确保了该方法的有效性。此外，分割这些高反射点对于优化效率至关重要，通过排除与道路标记无关的大量数据点，策略性地减轻计算负担。

概率局部地图更新

为了实现实时性，提出了一种新的概率更新局部地图的方法。该方法采用概率丢弃策略，根据计算的概率值选择性地移除地图中的每个点。局部地图中第i个点的概率pi计算如下：

其中k表示当前帧的索引，ki表示第i个点所属的帧。η是人工设置的参数，用于确定旧点的丢弃概率。随着η增加，旧点更有可能被保留，从而在概率局部地图中产生更高密度的点。

如公式所示，LiDAR传感器新观察到的点赋予更高的保留概率值。该策略有效确保了局部地图的时空一致性，减轻了随时间积累误差的影响。此外，与在固定窗口内聚合扫描的方法相比，方法确保了局部地图数据的更平滑过渡，从而生成更高质量的LiBEV图像。

LiBEV图像生成

LiBEV图像的生成涉及将局部地图划分为地面平面的网格单元，每个单元对应于LiBEV图像中的一个像素。在每个单元中，使用色彩图将包含点的最大强度值映射到对应像素的RGB值。

本文的实现利用了高效的实例分割网络，从生成的LiBEV图像中准确分割语义道路标记。随后，从局部地图中提取对应于分割像素的网格单元内的点。生成语义点云，每个点都标记有特定的道路标记类别。本方法支持最多9种类型的道路标记，包括虚线车道线、实线车道线、停车线、文本、箭头、菱形标志、三角形标志、路缘和人行横道，如图 1(b) 所示。相比仅关注车道线的方法，结合多样的语义道路标记显著提高了基于地图匹配的位置估计的鲁棒性。此外，由于标注图像语义分割比标注点云更便捷，所提出的方法将点云转换为图像，更有利于实际部署。

基于SG-ICP的高精地图道路标记配准

在SG-ICP中，道路标记被分为线条、线段和其他三类。实线车道线和路缘在其点云中呈线性分布且没有明显的端点，因此被归类为线条。虚线车道线、人行道和停车线也具有线性分布但有端点，因此被归类为线段。文本、箭头、菱形标志和三角形标志没有线性点云分布，因此被归类为其他。

线条由于缺少端点，而在线性方向约束缺失。线段的端点可以提供沿线性方向的约束，但由于端点估计不准确，仍会影响沿线段方向的配准精度。因此线性标记，沿其线性方向的约束对位姿估计的影响需要最小化，从而减轻整体位姿估计过程中的欠约束问题。对于其他标记，它们的点云不是线性分布的，因此通常在位姿估计上提供足够的约束。在本文算法中，三类不同标记的配准通过广义ICP（GICP）的目标函数组织为统一表示。

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

其中qmi和qLi分别表示来自高精地图元素和标记点云的一对对应点。它们的对应关系通过ICP算法中的最近邻搜索策略建立。Cmi和CLi分别表示地图点和标记点云点的协方差矩阵，在本文的语义GICP（SG-ICP）中适当构建，以减小欠约束方向的影响。

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

其中 pL(i,j) 表示第i个道路标记实例的第j个点，p~Li表示这些点的质心。然后，对CLi进行奇异值分解 (SVD)：

其中σ1和σ2满足σ1≥σ2。然后，构建一个矩阵Σi=diag(1,ϵ)，其中ϵ的值如下：

这三类道路标记的ϵ值不同，代表了沿线方向的不同约束。ϵ值越接近1.0，沿线方向的约束越强。最终协方差矩阵可以通过以下公式计算：

高精地图中的第i个语义元素表示为 {vmi,lmi,Pmi}，其中vmi、lmi和Pmi={pm(i,j),j=1,2,⋯,nmi} 分别表示地图元素的主方向、语义标签和点集。将基向量 e1=[1,0]T 旋转到方向vmi的旋转可以通过以下公式计算：

其中，

符号[r]×表示与向量r相关的反对称矩阵。对应于第 i 个语义元素的协方差矩阵计算为：

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

实验效果

实验设置

实验均在NVIDIA Jetson AGX Xavier上进行，LiDAR数据的采集频率设置为10Hz。车辆的全球定位结果使用实时动态（RTK）记录，并与LiDAR数据进行时间同步。RTK结果用作地面实况。

实验场景和对应的高精地图如图3所示，包括两个北京房山城市区域（第一个测试区域为0.30km x 0.25km的城市环形道路，第二个测试区域为全场2.0km的城市直线路段）、嘉善测试场0.20km内部道路及机场4.0km路段。方法参数中，卡尔曼滤波器的状态转移与测量模型初试方差设为0.1和2.0，点丢弃概率参数定为50.0。

图3：实验场景（上方）及其对应的高精地图（下方）

道路标记检测评估

通过精确率-召回率指标评估道路标记检测方法的性能。表I展示了方法支持的所有道路标记类型的精确率、召回率和F1评分。实验结果表明，我们的方法能够成功检测到9种不同类型的道路标记，并为LiDAR数据中的每个点分配语义标签，如图1（b）所示。实验结果证明，本文的方法在成功检测常见道路元素方面具有高精确率和召回率。

定位性能评估

提出的SG-ICP算法基于横向、纵向和航偏误差评估，涵盖了八个实验序列，跨越四个场景和七种不同的LiDAR配置，展示了方法的灵活性。选择广泛使用的ICP算法作为基准进行评估，对比结果如表II所示。SG-ICP算法在大多数序列中优于基于ICP的方法。尤其是由于SG-ICP计算过程中特别强调充分约束的方向，在横向和航向准确性方面具有明显优势。

图4展示了SG-ICP和ICP方法估算的轨迹与通过RTK获得的地面实况进行对比的可视化结果。值得注意的是，SG-ICP和ICP的显著定位误差分别用紫色和红色线标记，当估计距离误差超过2.0米或航偏误差超过5.0度时。图4表明，在所有序列中，SG-ICP方法减少了较大定位误差情况的出现。

图4：使用由RTK提供的真实轨迹，

对比了SG-ICP和ICP估计的轨迹

运行时间评估

在八个序列上进行实验时，记录每个子步骤的运行时间，详细信息如表III所示。值得注意的是，检测子步骤的运行时间分为CPU时间和GPU时间。CPU时间指由CPU处理步骤消耗的时间，包括高反射率点分割、概率局部地图更新和LiBEV图像生成。GPU时间指LiBEV图像实例分割的推理时间。配准子步骤仅由CPU处理。

图5：方法中每个子步骤的时间消耗情况

可以看出，当使用XAVIER车载处理器时，在各种场景和类型的LiDAR传感器下，整体方法的平均和最大运行时间分别始终低于50毫秒和200毫秒。因此，所提出系统的效率足以满足自动驾驶应用中的实时感知和定位需求。

鲁棒性评估

为展示本文方法的鲁棒性，将获得的结果与RTK提供的地面实况进行对比。表IV显示，随着行驶速度的增加，定位误差略有增加。这可以归因于随着行驶速度的增加，LiDAR传感器捕捉的点云数据更容易受到运动失真的影响。尽管在较高行驶速度下定位误差略有增加，所提出的方法仍能保持较高的定位准确性，这展示了该方法在不同车速下的鲁棒性。关于实时性能，如表IV所示，整体系统运行时间受行驶速度增加的影响很小，这进一步突显了系统在处理车速变化时的鲁棒性。

为了说明本文方法在不同天气条件下的鲁棒性，进行了不同环境下的实验。如图6所示，干燥和潮湿路面（晴天和雨天）的LiDAR点云强度分布通常存在显著差异。因此，雨天对基于强度的道路标记提取提出了相当大的挑战，特别是对于依赖固定强度阈值的方法。在晴天和雨天条件下生成的LiBEV图像如图7所示。显然，所提出的自适应阈值方法即使在不同天气条件导致显著不同强度分布的情况下，仍能稳定并准确地分割结果。

图6：在Fangshan1场景中，晴天和雨天的激光雷达强度分布情况

图7：在Fangshan1场景中，晴天和雨天的LiBEV图像。

绿色代表高强度，红色代表低强度

表V比较了在Fangshan1场景中使用1个Hesai-Pandar64 LiDAR在干燥和潮湿地面条件下的定位误差。尽管在湿地面上驾驶时LiBEV图像中的噪声增加导致定位误差增加，但横向误差仍能确保平均在0.10米以内，纵向误差在0.20米以内。这些结果展示了本文方法在应对具有挑战性的天气条件下的鲁棒性。