0. 简介

精确且长期稳定的定位对于停车场内的行车任务，如自动驾驶或自动代客泊车等，至关重要。现有方法依赖于固定且内存效率低下的地图，缺乏强大的数据关联方法，不适用于精确的定位或长期地图维护。《LESS-Map: Lightweight and Evolving Semantic Map in Parking Lots for Long-term Self-Localization》一文提出了一种基于地面语义特征、利用低成本摄像头的新型地图制作、定位和地图更新系统，提出了一种精确且轻量级的参数化方法，用于建立改进的数据关联，实现厘米级别的精确定位。此外还提出了一种新颖的地图更新方法，通过对参数化的语义特征实现高质量的数据关联，允许在重新定位过程中进行连续地图更新和细化，同时保持厘米级别的准确性。

1. 主要贡献

现有方法缺乏对地面语义信息的强大参数化，导致在制图和定位过程中缺乏强大的数据关联，这个不足会对定位的准确性产生不利影响，同时对地图的长期更新能力有所限制。因此，在这项工作中，我们提出了一种用于地面语义特征的准确且轻量级的参数化方法，以帮助提高制图和定位的准确性和效率。此外提出了一种新颖的地图更新方法，允许在重新定位过程中进行连续地图更新和细化，同时保持厘米级别的准确性。本工作的贡献总结如下：

1. 提出了一种用于地面语义特征的新型参数化方法，以实现更高效的位姿估计和更强大的数据关联。

2. 提出了一种轻量级的地图维护方法，可以通过后续车辆的定位结果进行更新，实现厘米级别的定位精度。

3. 提供了一个完整而实用的地图制作、定位和地图更新流程，通过实际实验在室外和室内场景中表现出可靠性。

2. 系统概述

我们利用四个环视鱼眼摄像头、惯性测量单元（IMU）和轮子编码器来捕捉周围环境并估计车辆的轨迹。所提出的算法包括两个主要组成部分：建图和定位，如图2所示。

图2. 提出算法的概述。我们利用四个周围摄像头提取准确的参数化地面特征。系统主要分为两个部分。第一个（左侧）部分是建图部分，当首次访问环境时，它创建一个全局地图。第二个（右侧）部分是定位和地图更新部分，它进行定位并使用新捕获的环境数据更新先前的地图。

建图：当车辆首次进入环境时，一个基本任务是构建一个全局地图，作为后续定位和地图更新的基础。我们从环视摄像头中提取地面语义特征，并将其投影到三维空间中。通过估计这些观测的不确定性来增强地图融合。然后，我们使用第III-C节中详细介绍的方法对这些特征进行参数化。结果，全局地图通过姿态估计和闭环的精化而生成。

定位和地图更新：在构建全局地图之后，当车辆返回环境时，可以利用新捕获的环境数据进行定位和地图更新。考虑到环境的动态性和可能进入未映射区域的可能性，我们构建了一个因子图来融合里程计和定位结果，得到一个经过微调的全局轨迹和一个新生成的地图。然后，新生成的地图与现有地图无缝合并，确保地图更新过程的稳健性和一致性。

3. 义分割和云投影

我们采用一个强大的分割网络[23]，从原始的鱼眼图像中有效地检测地面、车道线和道路标记。在语义分割之后，我们将鱼眼图像转换为车辆坐标系内的语义点云。这个转换过程包括对鱼眼图像进行去畸变处理和执行逆透视映射（IPM）[24]。
鱼眼相机使用实用的鱼眼相机模型[25]进行建模，该模型将相机的投影模型表示如下：

在图像中，u和v是像素坐标，u_c和v_c是以相机光轴为原点的像素坐标，u_0和v_0是像素坐标系中光轴中心的坐标，λ是一个比例因子，d(ρ)表示畸变多项式d(ρ) = a_0 + a_2ρ^2 + a_3ρ^3 + a_4ρ^4，S是拉伸矩阵。因此，与鱼眼图像中像素坐标u、v对应的无畸变坐标u^′、v^′可以表示为：

在获得无畸变像素坐标u^′、v^′之后，可以使用预先校准的逆透视矩阵H 来获取车辆坐标系中的点云(x_b, y_b)。

为了方便后续的局部地图融合，我们在投影过程中计算了一个置信度指标，用于评估投影语义点云的观测质量。根据上述计算，车辆中心点 (x_b, y_b) 到距离 \sqrt{x^2_b + y^2_b}可以表示为像素坐标 (u, v) 的函数 f(u, v)。该函数的梯度||∇f|| 表示像素在物理空间中的大小。较大的||∇f|| 表示像素在物理空间中存在更大的不确定性。由于 ||∇f|| 的大小与相机模型相关，我们使用一个归一化函数为每个像素定义了一个置信度指标：

其中a和b是与因素相关的两个参数，例如图像大小、畸变模型和相机安装位置。这个置信度测量表示相机投影模型在特定区域内的观测质量。图3展示了这个置信度测量的可视化结果。值得注意的是，由于给定相机的投影映射保持不变，上述步骤只需要在相机初始化期间计算一次，不需要重复计算。

图3. 云投影结果。(a) 鱼眼相机拍摄的原始图像。(b) 语义分割的结果。© 通过IPM投影获得的点云，用置信度测量进行渲染。(d) 由四个鱼眼相机的结果拼接而成的完整点云。

4. 轮廓参数化

为了充分利用地面的语义结构，我们提出了一种自适应参数化方法来编码地面的语义信息。该方法的步骤如图4所示。

图4. 我们的地面语义特征参数化过程。(a) 点云中的不同轮廓。(b) 轮廓点的轮廓法向量（绿线）。(c) 通过区域生长算法获得的线簇。(d) 从线簇点云中提取的线特征。

我们采用边界跟随方法提取带有语义标签的图像中的道路标记的轮廓。利用第3节中详细介绍的方法，我们在车辆坐标系中获得轮廓点云P^t_b。此外，还在P^t_b中存储了每个轮廓的邻接关系、标签（索引）和每个点的置信度。为了消除在噪声环境中可能存在的琐碎或不可靠的特征，我们过滤掉置信度低和面积小的轮廓。轮廓聚类结果如图4(a)所示。

在生成了P^t_b之后，我们计算每个轮廓点的二维轮廓法向量，因为通过IPM投影获得的点云都位于同一平面上。这个轮廓法向量被定义为从轮廓的内部指向外部、垂直于轮廓边缘的向量。我们利用一种基于表面的边缘估计算法[26]来估计轮廓法向量。估计结果如图4(b)所示。然后，根据我们得到的邻接关系，我们在获得轮廓法向量后使用区域生长算法来对轮廓点进行聚类。具有一致轮廓法向量的点被聚类到同一组中。聚类结果如图4(C)所示，展示了将道路标记分割成多个线云的过程。随后，对于每个线簇，我们计算其方向和质心以获得6自由度线的系数。此外，还获得了每个线簇的一组端点。提取的线特征如图4(d)所示。我们使用线簇中所有点的平均置信度来表示线的置信度。
总之，我们将一帧原始点云视为一组线，表示为L_t = {l_1, l_2, …, l_k}。每条线由6自由度系数coef_i、轮廓法向量e_i、两个端点p_{d1}和p_{d2}、聚类标签k_i和观测置信度c_i来描述。

5. 里程计

里程计估计了两个连续相机拍摄之间的姿态变换。在第4节中，基于我们提取的特征，我们利用P^t_b，L_t和P^{t−1}_b，L_{t−1}来估计相对姿态变换^{t−1}_t T。这个估计是通过点对线ICP方法进行的，为了提高准确性和效率，我们实施了一些修改。

5.1 轮廓法线辅助点线匹配(很好的关联性思路)

我们使用轮廓法线向量来寻找更好的相关性，只匹配具有一致轮廓法线方向的点。对于P^t_b中的每个点p^t_{bi}，我们首先进行最近邻搜索，找到其在P^{t−1}_b中最近的点p^{t−1}_{bj}。然后，我们将我们的相关性搜索限制在与p^{t−1}_{bj}相同轮廓的线上。具体来说，我们搜索具有最短点对线距离并与p^t_{bi}的轮廓法线向量对齐的线l^{t−1}_m ∈ L_{t−1}。这条线，表示为l^{t−1}_m，然后被视为p^t_{bi}的点对线对应关系。

5.2 使用拟合线特征进行点对线距离计算

一旦建立了p^t_{bi}和l^{t−1}_m之间的关联，我们直接利用p^t_{bi}和l^{t−1}_m之间的点对线距离来制定一个优化问题，以减轻使用两个最近点计算点对线距离可能产生的潜在噪声。

如图5所示，基于最近邻搜索的数据关联通常需要大量迭代，因为点对点匹配存在不准确性。然而，借助轮廓法线向量的辅助，我们可以实现更准确的数据关联，以更高的效率达到可比较的准确性水平。此外，我们利用IMU预积分[27]来进一步优化里程计的结果。

图5. 不同方法得到的一对相邻帧的匹配结果。(a) 通过最近搜索得到的匹配结果。(b) 通过我们的方法得到的匹配结果

6. 本地建图

我们通过里程计获得连续帧之间的变换。当机器人的姿态变化超过预定阈值时，生成一个关键帧。由于语义信息的限制以及个别测量中存在的碎片化、低置信度观测，我们采用概率滤波来融合两个关键帧之间的所有观测，从而得到更好的融合关键帧。

我们将每个普通帧定义为 F^i = { ^k_i T, L^iP^i_b }， 其中 ^k_i T 表示 F^i 与前一个关键帧 F_k 之间的姿态变换。我们将每个普通帧中的线特征与关键帧关联起来，将多个普通帧中的相同线特征合并成一个批次，表示为 b^k_j = (l^k_j , {l^{i_1}_{m_1} , l^{i_2}_{m_2} , … l^{i_n}_{m_n} })，其中l^{i_1}_{m_1} 表示从第 i_1 个普通帧获得的第 m_1 个线特征，l^k_j 表示从 b^k_j 中的所有线中获得的融合线特征。我们将关键帧定义为 F_k = { ^w_k T, B^k}，其中 ^w_k T 是与关键帧对应的全局姿态，B_k = {b^k_1 , b^k_2 , …, b^k_n} 表示关键帧中的所有批次。 我们根据轮廓法线、线方向和线与线之间的距离来判断一条线是否属于一个批次。此外，每个批次 b^k 都被赋予一个表示其稳定性的置信度分数 p^k。该分数通过贝叶斯滤波器按以下公式进行增量更新：

在这里，p(b^k |z_{l^{i_n}_{m_n}} )代表从第3节的计算中得到的线段l^{i_n}_{m_n} 的置信度分数。我们对b^k_l 中观察到的最佳线段进行平均，得到l^k_j ，从而形成一组用于关键帧注册任务的线段集合L^k = {l^k_1 , …l^k_n}。这种地图融合方法保留了关键帧中高质量和最稳定的观测结果，同时保持了地面语义的完整结构。

7. 环路闭合检测和全局优化

为了实现环路闭合检测，我们在关键帧之间进行注册。在生成新的关键帧时，我们通过点到线的注册将关键帧与附近的关键帧对齐，以识别潜在的环路闭合。然而，仅依靠注册的收敛有时会由于局部最优而导致错误的收敛。因此，在完成当前关键帧F_i与候选关键帧F_{dst}之间的注册后，我们在局部地图处理过程中实施类似的方法来评估对齐结果。我们尝试将F_i合并到F_{dst}中，并计算比率：

其中，len_j表示F_i中的线特征l_j的长度，c_i表示l_i的置信度。而s的定义如下：

在这里，p描述了F_i和F_{dst}之间的重叠比例。如果p超过了某个阈值，就认为配准已经收敛，表示存在一个闭环。
我们构建了一个用于全局位姿优化的因子图，包括三种类型的因子：（1）里程计因子，（2）IMU预积分因子，和（3）闭环因子。我们使用iSAM2进行全局优化。在完成全局优化后，我们可以使用优化后的位姿将局部地图合并到全局地图中。由于地图是随时间累积的一组批次，我们只需要存储每个批次的一些向量化描述，用于离线地图存储，例如6自由度参数、轮廓法线、端点和批次置信度。这种方法显著减少了地图的存储量。

8. 定位和地图更新

完成建图过程后，当车辆重新进入相同的环境时，我们实施定位和更新程序。在定位过程中，除了先前的地图用于引导外，我们的算法还可以适应环境可能发生变化或车辆进入先前地图未覆盖区域的情况。
在对地面语义信息进行参数化后，我们并行运行两个模块：基于帧间配准的里程计模块和基于先前地图的定位模块。里程计模块与前面提到的模块一致。在定位模块中，我们根据预估的姿态加载周围的先前地图，并进行帧到地图的配准，以获取车辆在全局坐标系中的姿态和位置。
为了估计车辆是否遇到环境变化或到达先前地图的边缘，我们在定位模块中进行有效性评估。在这种情况下，由于车辆当前的观测无法与先前地图建立有效的对应关系，我们通过帧到地图的配准评估当前帧与地图帧之间的重叠程度。重叠程度使用公式（6）进行量化。地图边缘处重叠变化的可视化如图6所示。
当计算得到的重叠值低于预定义的阈值时，定位结果被视为无效。这个有效性判断然后被发送到图优化组件。

图6. 显示地图边缘处的重叠值的插图。 (a) 到 (d) 表示车辆从地图覆盖区域移动到地图边缘时重叠比例的变化。(a) 完全重叠；(b) 和 (C) 部分重叠；(d) 未重叠

在获得定位和里程计结果之后，我们维护一个如图7所示的因子图。该因子图包含三种关键类型的因子：里程计因子、IMU因子和定位因子。里程计因子是基于从里程计模块获得的帧间结果构建的，而定位因子是基于定位模块提供的帧到地图结果构建的。值得注意的是，只有在定位有效时才会加入这些定位因子。

图7. 因子图的示意图