基于语义似然与高精度地图匹配的智能车辆同时定位与检测

文摘 2024-11-20 07:10 上海

作者 | 一点人工一点智能

点击下方卡片，关注“3D视觉之心”公众号

第一时间获取3D视觉干货

摘要：车载传感器数据与高精度地图的精确匹配是提升智能车辆感知与定位的关键。提出基于语义似然模型(SLM)的高精度地图匹配新算法，实现智能车辆同时定位与目标检测任务。

首先通过U-Net提取路面语义目标，利用核密度估计建立SLM。基于粒子滤波框架，利用位姿变换将高精度地图上目标抽样点映射至SLM中，计算该抽样点与传感器数据的匹配程度对每个粒子的权重更新，实现智能车辆的高精度定位。最后利用定位结果完成地图上的要素目标到图像的映射，实现目标的精准检测。

利用在校园道路与城市道路环境下采集的数据对所提算法进行验证，实验结果表明，算法的平均定位误差约为14cm，路面路标检测结果平均交并比(MIoU)均大于80。较之深度神经网络等当前最佳(SOTA)的检测方法，所提算法引入高精度地图的先验信息可显著提升智能车辆定位与目标检测性能。

引言

感知、决策与控制构成了智能车辆的三大技术模块[1-2]。其中，感知是决策与控制的基础，其核心是通过各类传感器数据来计算车辆周围的环境信息(包括动静态目标)与自车的位置、姿态等状态信息。目前，感知仍然是智能车辆的核心瓶颈技术问题，例如，复杂环境下的智能车辆定位问题在2020年被中国科协列为十大工程技术难题[3]。

智能车辆感知任务中，车道线和路面标志检测是重要的基础任务，在智能车辆辅助驾驶与高级版(如L3和L4级)中具有重要的作用。传统检测方法一般采用颜色模型进行阈值分割[4]或利用梯度算子进行边缘提取[5]以实现目标检测。然而此类方法鲁棒性较差，效果会随光照情况而受到影响，其次对所检测的结果无法分辨类别。

深度学习方法因其出色的泛化能力和鲁棒性，在路面标志检测方面逐渐取代传统方法。如Neven等[6]借助LaneNet提取车道线并利用H-Net对车道线进行参数拟合，通过曲线拟合方式对车道线进行表征，能够有效实现车道线遮挡的检测问题且可获得车道线的拓扑信息，但参数拟合形式只可用于描述车道线，无法表示路面中其他非线状信息；而Jang等[7]提出RMnet，将道路标志作为语义信息实现道路标志提取，解决了路面多种类别语义的提取问题，但存在误检测和部分缺失的情况。

总体而言，基于网络的路面标志检测方法主要存在因遮挡导致检测失败以及无法有效检测线与块状等不同特性的路面标志等问题。目前，最常用的智能车辆定位技术是基于卫星导航系统的定位方法，但在实际应用中存在卫星信号盲区和多路径等问题，导致无法获取有效的定位信息。为此，研究人员将卫星导航系统与惯性系统相融合，提高定位稳定性和精度。然而，在长时间卫星信号盲区条件下，惯性传感器存在严重的累积误差，导致系统定位性能失效。

近年来，研究人员提出基于同时定位与建图(simultaneous localization and mapping， SLAM)的定位方案，其主要有基于激光传感器和视觉传感器的SLAM定位方法。激光雷达可以获取精确的场景点云信息，使激光SLAM定位方法定位精度高，但目前激光点云传感器成本较高，难以在智能车辆上大规模推广应用；视觉传感器因其价格低廉，能够获取场景丰富的纹理信息，便于在智能车辆上进行安装，受到广泛关注。

ORB-SLAM[8]是近年来开源的经典视觉SLAM定位方法，其通过提取图像中的旋转不变简述特征(oriented FAST and rotated BRIEF， ORB)作为特征点进行匹配，并通过光束平差(bundle adjustment， BA)优化实现较高精度的定位。然而ORB特征对光照和动态目标比较敏感，特别是在特征贫瘠的场景中难以提取有效特征点，导致出现定位误差大甚至定位失效的情况。

高精度地图(high-definition map， HD map)近年来在智能车辆领域得到了广泛的应用与发展，为智能车辆的高精度定位、感知乃至规划、决策等提供了新的解决思路。特别是面向智能网联汽车的发展需求，高精度地图的数据组织与结构得到了极大的拓展，发展出面向不同功能的地图图层[9]。

基于高精度地图的智能车辆定位的基本思路是将车载传感器获取的场景信息与地图中存储的信息进行匹配，并在匹配的基础上，通过PnP或迭代最近点(ICP)算法完成车辆定位。如胡兵等[10]对地面进行加速鲁棒特征(speeded-up robust features， SURF)提取并结合全球定位系统(global positioning system， GPS)构建路面SURF特征地图，在定位阶段利用迭代最近点获得定位结果，但该方法获取地图较为复杂且特征提取易受到光照和遮挡的影响导致匹配失败。

Cui等[11]对车道线进行标注，抽取部分点的GPS值保存作为地图数据，并利用颜色阈值分割和边缘分割等方法将相机所提取的车道线与地图点进行ICP匹配从而获得车辆位置，但在大部分场景下车道线差异较小，使得车辆在纵向上的定位误差较大。

王相龙等[12]提出了一种路面指纹地图，利用卷积神经网络检测道路裂纹并使用ORB进行匹配得到车辆的精确位置，然而道路裂纹过于稀疏，大部分情况下路面并没有足够的裂纹用于定位，且裂纹的生长和加深若无法在地图中进行及时更新将导致定位失败。

Lin等[13]利用模板匹配方法，将数字地图中所记录的路面标识及车道线图像信息与当前所采集的图像信息进行模板匹配以矫正GPS定位误差，避免了地图数据与图像数据之间的对应点选择问题，但该方法一方面所需存储的数据较大，另一方面此类地图无法适用较大场景。

Xiao等[14]提出基于单目相机的语义分割与矢量地图匹配的最小二乘定位算法MLVHM，解决了车辆前后定位不准确及场景局限等问题，但点对之间的选取和对应关系依旧是其存在的关键性问题。在这些工作中，目标检测与车辆定位被分解成两个单独的模块。但实际上，这两个模块是相互耦合的。一方面，通过高精度定位信息可以从高精度地图中完成目标检测；另一方面，更好的目标检测结果可以进一步提升定位的精度与稳定性。

针对以上问题，提出基于语义似然概率模型(SLM)的高精度地图匹配新算法，并在此基础上实现智能车辆同时定位与检测。论文的创新点为：①与基于点与点或点与线的地图匹配算法不同，本文提出了基于点与似然对应的地图匹配算法，并提出基于核密度估计(KDE)的SLM构建方法；②提出基于地图与传感器的紧耦合粒子滤波的车辆定位算法，通过SLM中的似然概率完成粒子的权重更新与最优估计，并同时完成目标检测。

算法

算法流程如图1所示，主要包括以下4个步骤：

①高精度地图表征与要素抽样；

②利用U-Net[15]对图像中的路标进行语义提取，并进行逆透视变换与坐标变换；

③ 利用KDE方法构建语义似然概率模型；

④ 基于SLM与地图融合的粒子滤波框架构建，并同时完成智能车辆定位与路标检测。

图1 算法流程图

1.1 高精度地图表征与要素抽样

本文利用车道级高精度地图完成道路场景的表征，所采用的地图遵循ASAM[16]定义的高精度地图格式。高精度地图表征及其抽样如图2所示。其中，车道级高精度地图包含了道路各个语义目标(包括实线、虚线、路面标志、路面设施等)的高精度位置信息。以图2中虚线为例，在高精度地图中会存储虚线中每个小线段的两个端点的高精度坐标。针对井盖目标，也会存储其对应的中心点、半径、几何形状等精确信息。

图2 高精度地图表征及其抽样

在高精度地图表征的基础上，可在道路要素目标上进行点抽样，如图2(d)所示红色抽样点。车道线抽样按照所设定的间距进行等间隔抽样。地图中的道路标志抽样可按照对应语义的边、角点进行抽样；而对于圆形语义可以按照边缘点进行抽样，且对中心点进行平均抽样。通过抽样，可利用抽样点集合的形式对要素目标进行精确表征。

1.2 路面语义检测与逆投影变换

提取图像中的路面语义信息可利用卷积神经网络方法，本文提出将路面的语义特征进行核密度估计以表征该语义目标，这一方法可降低因错误分割而导致的匹配失效问题。同时，考虑到车载计算资源有限，且需要较高的定位实时性，故所使用的语义分割方法不宜过于复杂，因此，本文采用参数量较小的U-Net方法[15]完成12类路面语义分割，这些语义目标包含车道线、箭头、减速带、停止线、斑马线以及井盖等。为了消除前视摄像机成像的透视效应，利用逆透视变换(inverse perspective mapping， IPM)将U-Net所分割出的语义逆投影至车身坐标系。如图3所示，定义车身坐标系原点为右后轮与地面接触点，车辆前进方向为x轴方向，其垂直方向为y轴方向。前视图像点通过IPM变换(即变换单应矩阵H)映射至车身坐标系下对应点如下：

图3 车身坐标系与图像坐标系转换

式中：IPM变换中的单应矩阵H可以通过离线标定的方式获得。车身坐标系下的点与高精度地图下的点之间满足2D欧氏变换关系，可通过航向角θ与位移来表征，此即为车辆的定位信息。记此变换为：

从而，前视图像点与高精度地图之间的坐标变换关系可以表征为如下形式：

1.3 构建SLM

为解决离散地图抽样点与语义点之间匹配困难的问题，本文考虑利用KDE方法对所提取的语义信息进行概率建模，将基于车身坐标系下的语义点获得光滑连续的概率密度函数，在匹配时将地图点投影到车身坐标系，同时将点对之间的匹配转换为求解其投影点的概率最大化问题。

本方法首先从语义中所包含的N个点获得每个点对应的高斯核函数，随后对核函数进行叠加并归一化获得描述该语义信息的概率密度函数。x为从地图中映射到语义坐标系下的坐标，X_n={x₁， x₂， …， x_N}为独立同分布的N个语义点，对该单一地图点与语义之间相似性估计如下式，即SLM：

为解决离散的地图抽样点与语义点之间匹配困难的问题，考虑利用KDE方法将上式中K(x)定义为在二维空间上的非负核函数，h为核函数的带宽矩阵或称为核函数的窗口，K_h(x)表示在窗口h下的缩放核函数，高斯核函数表达式如下：

式中：n表示维度。所有坐标系均为平面坐标系，其语义以二维点对的形式存在，故本文取为n=2，SLM通过引入核函数来刻画所有语义点所对应的似然概率，如图4所示。

图4 从语义目标获得SLM

1.4 基于SLM的同时定位与检测

在贝叶斯框架下，通过历史状态与观测可将智能车辆定位问题转化为如下最大后验估计问题：

式中：表示智能车辆状态；z_k为观测变量，由各类传感器组成；P为后验概率。本文提出基于粒子滤波的改进算法对式(6)进行求解。首先，基于蒙特卡洛方法对后验概率按照不同权重来近似，同时利用一阶马尔可夫过程假设，将后验概率重新写作：

为了简便计算过程，将提议分布定义为

提议分布反映了车辆的先验概率。考虑相邻时刻内车辆可视为匀速运动，因此其状态转移符合如下式：

式中：根据车辆的历史时刻所计算得到的状态信息获得。通过式(8)定义的提议分布，可将粒子i的权重按照下式进行更新：

式中：为似然函数，反映了各种传感器数据的观测与粒子之间的一致性程度。本文传感器数据包括GPS数据、前视图像数据以及高精度地图数据，构成了粒子滤波算法的两类观测：

式中：Gk为GPS数据，构成了粒子滤波器的第一类观测；Lk为前视图像与高精度地图组合而成的滤波器第二类观测。假定这两类观测是独立的，则可将似然函数写作：

式中：表示地图抽样点经过变换后映射至语义似然模型中的概率。本文包含12类语义目标，对应了12个语义概率模型。假定这12类语义目标检测过程中也都是相互独立的，则可将似然函数进一步分解成：

式中：表示k时刻下的第j类语义目标对应的观测。以第一类语义目标对应的似然为例，设地图上该语义目标的抽样点集合为UM：

可进一步写作：

式中：L(x)即为式(4)。同样地，利用上述方法求解其对应的似然概率。从而，可以利用所有抽样点的似然概率完成对粒子权重的更新。最终，可通过对权重进行归一化，并利用归一化后的权重求解最佳状态，如下式所示：

式中：为经过归一化后的粒子权重[17]；即为粒子滤波所计算的车辆位姿信息。

基于车辆的位姿信息可以同时完成目标检测工作，其基本思路是利用式(3)将高精度地图中的语义目标通过算法所获得的车辆位姿投影到图像坐标系中，如下式所示：

式中：为地图中的路面交通标志所存在的坐标点。我国对于路面交通标志有明确的国家标准(GB 5768.2—2022)[18]，因此可基于该标准以及地图中所保存的对应交通标志的位置信息得到精确的语义坐标，而中包含车辆的位置及航向角信息，利用该车辆位姿可将该路面交通标志映射到图像平面上，从而实现对目标的精准检测。需要指出的是，这种检测方法充分利用了高精度地图的先验信息，因此可以实现超视距、遮挡情况等检测。

实验结果与分析

实验中选取两个典型场景对算法进行验证，其一是某校园场景中的某路段，长度约为2.5 km，该场景下行人较多。其二是武汉临江大道部分路段，长度约为5.6 km。校园道路作为人流量较大的低速(限速25 km/h)场景代表；而武汉临江大道双向六车道作为高车流量的中高速(限速70 km/h)场景代表。

上述场景为实际行驶中较为典型的两类场景，可对算法进行综合对比和校验。实验中，由实验室开发的智能车辆平台搭载包括FLIR工业灰点相机、高精度组合惯导、普通GPS接收器等传感器。车载计算单元为NVIDIA Jetson TX2，用于完成语义提取、检测与定位计算等任务。

搭载的前视摄像机向下倾斜30°，高约110 cm，分辨率为 2 048 像素×1 536 像素。利用Zhang标定法[19]对相机的内外参数进行精确标定。利用厘米级高精度卫星-惯导组合定位系统的定位结果作为真值来验证定位算法的性能。普通GPS接收器仅作为初始粗定位用于确定所在地图范围。实验中所用地图为满足ASAM标准的车道级高精度地图[15]。

为提升U-Net语义提取性能，本文利用新的数据集对其进行重新训练。训练数据集来自两个部分，其一为实地采集的 1 815 张前视图像(分辨率为 2 048 像素×1 536 像素)，并通过数据增强的方式进行数据集扩充；另一部分来自百度Apollo部分数据集，共形成了用于训练的12类语义类别数据集，其包含各类语义分布如图5所示。

利用这些数据集对U-Net进行训练，并将训练好的模型部署至车载NVIDIA Jetson TX2平台上完成语义目标提取。为定量描述本文所提出算法性能，将高精度组合惯导所获得的定位坐标作为真值计算车辆相对定位结果的平均偏移量，并分析其误差分布以衡量本文算法的定位性能；每个类别采用交并比(intersection over union， IoU)对检测结果进行分析。此外，为综合衡量分割效果，对各个类别的交并比求均值，即平均交并比(mean intersection over union， MIoU)。

图5 数据集各类别语义分布

2.1 校园道路下的定位结果与分析

校园场景选取了一段包含井盖及车道线和弯道的路面场景，如图6所示。此类园区场景道路较窄，且相对而言类似箭头等显著的路面语义标志较少，但该场景中往往存在下水道井盖或通信井盖即圆或方形井盖，因此本文方法对该场景依旧有较好的定位能力。该类场景由于道路较窄以及行人对车辆的阻挡等原因导致车辆无法维持相对稳定的匀速运动，在各种滤波算法进行预测过程中易出现较大的偏差导致无法匹配。

而本文算法对匹配效果的衡量是基于似然概率值，利用SLM的衰减旁瓣的范围可保证在出现偏移的情况下依然可通过粒子滤波算法实现精准匹配。此外，井盖与路面之间相似性较高，因此使用文献[11]中的方法对其进行定位易出现特征提取失效的问题，本文所采用的U-Net方法对语义进行提取解决了这一问题。

由于园区内道路通常围绕建筑物铺设道路，所以存在较多弯道，如图6(d)所示，在本实验场景中，弯道存在一处井盖语义，且包含少部分车道线，由于较大的转弯角度，常用的定位方法往往在此场景中无法获得有效定位。在整个校园场景中，路段的总长度约为2.5 km，选取其中共47处包含井盖等横向语义信息的定位点用于计算本文算法定位误差。

图6 校园实验场景

实验过程中，首先对该路段高精度地图进行抽样，同时利用U-Net提取路面语义标志，并将语义进行SLM建模，如图7所示。综合考虑车辆速度，对SLM建模的窗口大小设置为100 像素×100 像素。图7(a)表示该路段的高精度地图进行抽样获得抽样点；图7(b)表示从检测的两类语义目标通过KDE算法计算得到的SLM模型，在图像中不同的灰度值表示其概率大小；图7(c)为地图抽样点与SLM之间通过粒子滤波获得的最佳匹配结果示意图。

图7 地图抽样点与SLM匹配过程

在该场景下，利用本文算法得到的定位结果如表1与图8所示。图中：CDF为累积分布函数，用于衡量误差在不同区间内的占比。同时，对文献[11]与[13]中的定位算法的定位结果进行对比分析。其中，图8为3种算法经过多次定位测试后的定位误差的累积分布图，横轴为误差水平，纵轴为误差分布。

表1 校园场景下定位误差对比

图8 校园场景下的定位误差经验累积分布

由表1可见，在校园场景下，本文算法的各项指标均优于文献[11]与[13]中的算法；同时，由图8可见，本文算法定位误差90%以上集中在40.0 cm以下。

文献[11]中利用颜色阈值对车道线进行提取并与预先制作好的地图进行ICP匹配获得定位结果，在校园场景下，车道线匹配易出现前后漂移，尤其是在该场景中存在较长的路段为直线路段，即使一直存在车道线也将使误差无法降低。因此其定位误差存在较大的波动，定位不够稳定，且定位精度也较低，平均误差达到54.7 cm。

文献[13]中利用模板匹配方法进行定位，相比于文献[11]中方法可大幅降低误差波动，但在校园路段中，路面语义标志大部分以井盖的形式出现，井盖与柏油路之间的颜色差异较小，因此模板匹配在语义附近将无法获得更精确的匹配结果，其定位误差达到30.4 cm，结果依然不够理想。

本文对所提取的车道线和井盖分别进行概率建模，并在此基础上与地图抽样点进行匹配，结合车道与路面标志的约束，得到较为精准的定位结果，定位误差维持在较为稳定的范围内，定位误差最大不超过50.0 cm，且平均误差为14.4 cm。实验过程中，在嵌入式主板TX2上对路面的实时检测耗时平均为23 ms，而基于所检测的语义目标进行坐标转换以及每次匹配过程约为10 ms，故整体定位过程共花费平均时长约33 ms，可满足对车辆实时定位的要求。

2.2 城市道路场景下的定位结果与分析

第2个实验场景为武汉市临江大道某段双向六车道道路场景，如图9所示。该道路场景中包含较丰富的路面语义信息，但车辆受到车流的影响易出现路面交通标志类别误识别的情况，这一种情况也易导致在实际匹配过程中无法找到对应类别从而误匹配。

本文算法考虑多种道路标志所产生的约束，将其多类约束引入与地图的匹配过程中，即箭头等交通标志仅作为其中一类信息提供匹配结果，当误识别的语义无法产生匹配约束时，可通过其他分割较准确的语义信息进行匹配提供约束。图9(b)展示了部分路段对应的高精度地图。图9(c)和9(d)分别展示了该路段采集的前视图像数据。

图9 城市道路场景

对于城市路段，本文算法定位结果如表2和图10所示。为了进行对比分析，在实验中分别利用同样的测试数据对文献[11]和[13]中的定位算法进行测试。其中，图10为3种算法经过多次定位测试后的定位误差的累积分布图。

表2 城市道路场景下误差对比

图10 城市公路场景定位误差经验累积分布

在城市道路场景下，尽管路面语义信息较为丰富，但所行驶的路段车道较多，而文献[11]中仅考虑车道线约束，无法有效利用箭头等标志提供纵向定位；同时，不同车道之间也易产生横向的定位偏移，使误差存在大幅波动。最后计算其平均定位误差为45.9 cm。

相较于园区环境，其误差降低了大约10.0 cm，且最低误差仅2.2 cm，这一现象主要是因为车道线在本次实验中有较多虚线段，而虚线段的端点在一定范围内为此方法提供了纵向约束，然而，一旦相机所采集图像中虚线车道线相似度较高时，此方法依然无法解决其定位中纵向偏移较大的问题。文献[13]中方法则表现较为稳定，但依然存在问题，即该路段栅栏或其他物体的影子将影响其模板匹配结果，因此其定位误差相对较高，平均误差为24.2 cm。

本文方法对路面语义进行概率建模，在一定程度上降低了由于光照影响下U-Net出现误检测所带来的匹配误差。本文算法平均定位误差为14.0 cm，且有近90%的情况下误差小于20.0 cm，相较于其他两种算法，本文算法定位精度提升明显，定位误差稳定在较低水平。

2.3 目标检测结果与分析

为测试本文算法目标检测性能，在上述两个场景下，利用本文算法对场景中的语义目标进行检测，其结果如表3和图11所示。校园场景下光照稳定，因此图像中的语义特征较为明显，通过U-Net方法所分割的语义信息比较完整，如图11(b)所示，但其检测结果依然存在语义信息部分缺失的情况，如图中圆形井盖以及转弯箭头无法做到完整分割，而本文借助高精度地图，将地图中的先验信息进行提取并利用上文的映射关系将地图中的语义映射到图像中，大幅提高了检测精度，如图11(c)所示。

在公路场景下，环境较复杂，图像中所表现出的语义信息易受到遮挡，直接导致U-Net检测结果出现误检及缺失情况，如图11(e)所示，图中箭头、双黄线均无法完整分割，而相邻车道中的直行箭头则完全无法检测出来，同时该图像中的大部分语义被误识别为其他类别。

在本文算法下，错误的检测同样有可能导致地图与图像之间匹配精度降低，但由于对所提取的不同类别语义进行SLM建模，所以大大降低了对原始检测结果的依赖。对于算法中所使用的U-Net检测结果，同时借助高精度地图，本文算法大幅度提高了其检测精度，如图11(f)所示。

表3 语义检测结果的IoU对比

图11 语义检测结果对比

本文算法借助高精度地图的先验信息，能够提升检测性能，不同于卷积神经网络(CNN)方法对语义目标的检测和分割，本文算法所产生语义检测及分割误差的主要原因在于地图到图像之间映射关系的误差，因此在匹配结果较好的情况下，其误差维持在较小水平。

此外，本文方法不完全依赖图像中所能观察到的信息，在检测过程中地图提供了整个环境中的路面标志，因此当车辆或行人对相机进行遮挡时，本文算法依旧能对当前路标进行精确检测。总体而言，本文算法对路面标志检测的MIoU可达到86.1%，优于U-Net等其他检测方法。

结语

本文提出了一种基于语义似然模型的高精度地图匹配新方法，从而将高精度地图匹配问题转化为最大化后验概率问题，并在粒子滤波的框架下同时完成智能车辆地图定位与目标检测两个任务。

与传统的基于点-点或点-线距离匹配的算法相比，本文算法引入似然概率模型来描述数据与地图之间的匹配程度，可以实现传感数据与高精度地图的紧耦合，从而将定位与检测两个任务进行深度融合，提升定位与检测的整体性能。

实验部分分别用校园路段与城市路段对算法进行验证，结果表明算法通过似然概率模型融合高精度地图，可以有效提升定位与检测算法的性能。

【3D视觉之心】技术交流群

3D视觉之心是面向3D视觉感知方向相关的交流社区，由业内顶尖的3D视觉团队创办！聚焦三维重建、Nerf、点云处理、视觉SLAM、激光SLAM、多传感器标定、多传感器融合、深度估计、摄影几何、求职交流等方向。扫码添加小助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）

扫码添加小助理进群

【3D视觉之心】知识星球

3D视觉之心知识星球主打3D感知全技术栈学习，星球内部形成了视觉/激光/多传感器融合SLAM、传感器标定、点云处理与重建、视觉三维重建、NeRF与Gaussian Splatting、结构光、工业视觉、高精地图等近15个全栈学习路线，每天分享干货、代码与论文，星球内嘉宾日常答疑解惑，交流工作与职场问题。

http://mp.weixin.qq.com/s?__biz=MzkyMDY0OTc1NA==&mid=2247516931&idx=3&sn=381de2551a1f8ce201611911fc61ce8f

3D视觉之心

3D视觉与SLAM、点云相关内容分享