【论文阅读】AVP-SLAM Semantic Visual Mapping and Localization for Autonomous Vehicles in the Parking Lot

文章：AVP-SLAM: Semantic Visual Mapping and Localization for Autonomous Vehicles in the Parking Lot
作者：Tong Qin, Tongqing Chen, Yilun Chen, and Qing Su

前言：去年笔者之前在自研探索AVP方案时，阅读到秦通博士的一篇论文AVP-SLAM: Semantic Visual Mapping and Localization for Autonomous Vehicles in the Parking Lot，在 自研过程中起到了一定的帮助，今日便对此做一篇【论文阅读】，以丰富自己弹药库。本文仅做学术分享，如有侵权，请联系删除。

AVP-SLAM：自动驾驶在停车场内的语义视觉建图与定位

摘要

    自动代客泊车是自动驾驶汽车的一种特殊应用程序。在这项任务中，车辆需要在狭窄、拥挤和无GPS信号的停车场中导航。准确的定位是非常重要的。传统的基于视觉 的方法会由于无纹理区域、重复的结构和外观变化而导致跟踪丢失。本文利用鲁棒的语义特征来构建地图，并实现停车场车辆的定位。语义特征包括在停车场通常出现的引导标志、停车线、减速带等。与传统特征相比，这些语义特征对视角和照明变化具有长期稳定性和鲁棒性。本文采用四个环视相机来增加感知范围。在IMU（惯性测量单元）和轮速计的辅助下，该系统生成一个全局视觉语义图。这张地图进一步用于车辆的厘米级定位。本文分析了该系统的准确性和召回率，并与实际实验中的其他方法进行了比较。此外，还验证了该系统的实用性。

一、引言

    近年来，对自动驾驶的需求越来越大。对自动驾驶而言，准确的定位是重要的前提条件。感知，预测，规划和控制都是基于准确的定位。为了实现鲁棒的定位，车辆装备了各种传感器 ，例如GPS，相机，激光雷达，IMU，轮速计等。过去几十年出现了大量的定位方法，有基于视觉的，基于视觉-惯导的，基于激光雷达的。但是对于量产车辆，低成本方案（装备相机和IMU）更受青睐。除了定位以外，建图对于自动驾驶而言也是十分重要。对于一些私人领域，比如封闭的厂区，停车场等，这些都是没有先验地图的。车辆需要自己建图。所以对自动驾驶而言，建图与定位更加重要。
    自动代客泊车是自动驾驶中的一个具体应用，车辆需要导航进入停车场并自动泊车指定库位。因为停车场通常是狭窄拥挤的，准确的定位就变得至关重要。任何定位误差都可能造成不可预估的损失。为了避免装备高昂的传感器，很多研究都聚焦在视觉-惯导方案上。然而，停车场这样的场景对传统的视觉定位方案而言是一个巨大的挑战。一方面，室内或地下停车场大部分都是无纹理的墙体，柱子和地面。特征检测和匹配是不可靠的。传统的视觉方案容易有跟踪失败的问题。另一方面，不同的车辆在不同的时间停放在不同的位置，这样也造成了巨大的挑战。在长时间内，不可能通过基于外观的地图来重新定位车辆。
    为了解决这个问题，本文采用了一种新的特征，即语义特征。语义功能包括引导标志、停车线和减速带，这些标志通常出现在停车场。与传统的几何特征相比，这些语义特征对视角和光照的变化具有长期的稳定性和鲁棒性。在本研究中，提出了一种基于语义特征的建图和定位系统，使车辆能够在停车场自动导航。这篇论文的贡献总结 如下：

• 提出一个用于视觉SLAM框架的新型的语义特征类型。
• 提出一个在停车场内的自动驾驶的建图定位系统。
• 基于提出的方案实现了真实世界的自动泊车应用。

二、文献评论

    在过去的几十年里，有大量关于基于视觉的定位的研究工作。根据输出类型，将它们分为相对定位和全局定位。同时，根据特征类型，将其分为传统的方法和基于道路的方法。
    相对定位–即里程计，在起点初始化一个坐标系，求解车辆在这个局部坐标系下的相对位姿。
    全局定位–拥有一个固定的坐标系，它通常是在一个先验地图上进行定位。
    传统特征方法–探索几何特征，例如自然环境中的稀疏点，线和稠密平面。在视觉里程计算法中，角特征点被广泛应用。相机位姿和特征点被同时优化求解。特征通过被其周围色斑描述以保证特殊性，例如SIFT,SURF,ORB,BRIEF描述符等。不同于视觉里程计，这些方法都是预先建立一个视觉地图，然后基于先验地图重定位。传统的基于特征的方法会因为长期的照明、视角和外观的变化而受影响。
    基于道路特征的方法–采用自动驾驶场景中常见的道路标识。地面标识包括车道线，路沿，标志灯。这些方法通过感知的路面标识与先验地图的匹配进行车辆定位。与传统方法相比，这些标识在长时间的光照和视角变化下更加鲁棒。对全局定位而言，一个准确的先验地图特别重要。这个先验地图通常由其他传感器生成（例如激光雷达，GNSS等）。
    在本文中，处理了一个比上述方法更具挑战性的场景。那是地下停车场，那里的环境很窄，光线弱，无GPS信号，没有先验地图。我们采用了一种新的语义特征来进行定位和建图。语义特征（引导标志、停车线和减速带）通过卷积神经网络进行检测。从建图过程中生成了一个全局语义地图。然后利用该地图车辆实现厘米级定位。

三、系统概要

    该系统采用四个环视相机来增加感知范围。这是目前高配置汽车的一种常见设置。该系统还使用了一个IMU（惯性测量单元）和两个轮速计。IMU和轮速计完成里程计，提供相对姿态，但存在累积误差。所有传感器的内参数和外参数参数都提前进行了离线校准。
    该框架由两部分组成，如图所示。

    第一部分是建图，它为环境构建了一个全局语义地图。环视相机的四张图像被投射到鸟瞰图中，并合成成一幅全向图像。神经网络检测语义特征，包括车道、停车线、引导标志和减速带。根据里程计，将语义特征投影到一个全局坐标中。由于里程计长期运行时会存在漂移，通过闭环检测进行局部地图匹配，以减少累积误差。
    第二部分是定位。与建图部分相同，从鸟瞰图像中提取语义特征。通过将语义特征与先前构建好的地图进行匹配来实现车辆定位。最后，EKF（扩展卡尔曼滤波器）将视觉定位结果与里程计融合，保证了系统具有平滑的输出，并在无纹理区域生存。

四、方法论

A、IPM图像

    在AVP-SLAM中使用了四个环视相机，在量产车上是常见的。它们装备了鱼眼镜头并向下。
    每个相机的内参和外参都提前进行离线 标定。逆透视变换（Inverse Perspective Mapping，IPM）就是将每一个 像素点投影在以车辆中心为原点的自车坐标系下的地面平面上（$z=0$）。这个投影过程可以用下面公式计算

其中，${\pi }_c(\cdot )$是相机的畸变和投影模型。${\pi }_c(\cdot {)}^{-1}$是将像素转换为空间点的反投影。$[R_c{t}_c]$是每个相机的外参。$[uv]$是在图像坐标系下的像素位置。$[x^v{y}^v]$是车辆坐标下的特征点坐标。$\lambda$是一个标量。$({)}_{col:i}$一个矩阵的第$i$列。
    在逆透视变换后，将四个图像合并为一个图像。

下面等式计算重投影

其中$K_{ipm}$是合成的IPM图像的内参。$[u_{ipm}{v}_{ipm}]$是合成的IPM图像上的像素坐标。
    该合成图像包含全方位信息，显著增加了感知范围。它在狭窄的停车场是非常有用的，即使那里的遮挡经常发生。

B、特征检测

    本文采用卷积神经网络方法检测语义特征（Convolution Neural Network，CNN）。有很多语义分割网络可以用于语义分割，本文采用的是U-Net。通过这个网络训练大量从停车场采集到的图像，将要素分类成车道，库位线，引导表示，减速带，可通行空间，障碍物和墙体。

其中，库位线，引导标识，减速带用于定位；库位线也用于库位检测；可通行空间与障碍物用于规划。

C、局部建图

    完成图像的语义分割后，将一些特征从图像坐标系转换成3D坐标系，

    基于里程计，这些特征从自车坐标系转换成世界坐标系下

其中，$[R_o{t}_o]$是里程计的位姿值。这些点聚合成一个局部地图。每30米维护一个局部地图。

D、闭环检测

    因为里程计运行长时间后会存在漂移，所以需要进行闭环检测以消除累计误差。将最新的局部地图与其周围的其他局部地图进行ICP匹配比较。如果两个局部地图匹配成功，那么得到这两个局部地图的相对位姿。这个相对位姿将在全局位姿图优化中用于消除漂移。
    例如，上面两个局部地图，如果我们直接合并，因为存在漂移，效果很不好

但是经过闭环检测后，再进行合并，会消除漂移量

E、全局优化

    当闭环出现时，全局位姿图优化可以消除累计误差并维护整个位姿图的平滑连续。在这个位姿图中，每个节点是指每个局部地图的位姿，包含三个轴的旋转$r=[r_x{r}_y{r}_z{]}^T$和平移$t=[t_x{t}_y{t}_z{]}^T$。这有两个边，一个是里程计的边，根据里程计的测量值约束两个相邻局部地图；另一个是回环边，约束闭环的局部地图。位姿图的优化可以用下面的代价函数来公式化表示

其中，$X=[r_0,t_0,\cdots ,r_t,t_t{]}^T$是所有局部地图的位姿。$z_{t,t+1}^o$表示局部地图$t$到局部地图$t+1$在里程计下的相对位姿。$L$表示所有的闭环对。$z_{i,j}^l$表示闭环时帧$i,j$的相对位姿。函数$f(\cdot )$计算两个局部地图的相对位姿。最后利用高斯-牛顿法进行优化。
    完成全局位姿图优化后，更新所有的位姿并将所有对应的局部地图合并便得到一张连续的全局地图。

F、定位

    基于生成的语义地图，当车辆再次来到这个停车场时能够完成定位。与建图过程一样，环视图像合并成一张IPM图像。在这张IPM图像上进行语义特征检测并将结果转换成自车坐标系下。然后通过与先验地图的语义特征进行匹配即可计算出当前车辆位姿。

计算方法用ICP方法，如下面公式

其中$r,t$是指当前帧时的车辆位姿。$S$是指当前帧特征点的集合。$[x_k^v{y}_k^{v}0]$是指当前帧特征点在自车坐标系下的位置。$[x_k^w{y}_k^w{z}_k^w]$是指这个特征在全局坐标系下距离地图中最近的点的位置。
    对于ICP而言，一个好的初始值是至关重要的。在最开始有两个策略可用于初始化。一是在地图上标记停车场的入口。这样车辆在停车场入口处可直接初始化。二是在进入地下停车场时，可以利用GPS作为初始位姿。在车辆在地图上完成定位后，将不再使用GPS。在初始化后，里程计的预测被用作初始猜测值。
    在纹理区域的定位是准确的。虽然环视尽可能地增加了感知范围，但在停车场中存在着一些非常无纹理的区域。为了克服这一问题，最后采用了EKF框架，将里程计与视觉定位结果结合起来。在该滤波器中，使用里程计进行预测，并使用视觉定位结果进行更新。该滤波器不仅提高了系统的鲁棒性，而且使估计的轨迹平滑。

G、库位检测

    因为库位线和库位线角点在IPM图像中检测出来，那么将容易自动完成库位检测。角点被用作预测库位的位置。如果库位线与预测的库位匹配的很好，那么预测结果就可以认为是正确的。