针对自主泊车的多相机视觉惯导同时定位与建图方案

作者 | dianyunPCL  编辑 | 点云PCL

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【全栈算法】技术交流群

摘要

本文介绍了一种视觉惯导同时定位与建图（SLAM）系统，用于自动驾驶车辆的定位和建图关键能力。该系统通过使用多个摄像头和外部（例如轮速）里程计传感器扩展了Kimera，并在真实环境中获得准确且稳健的里程计估计。此外提出了一种有效的闭环方案，避免了基于透视n点方法的常见替代方案的缺点，并且适用于单目摄像头，最后开发了一种用于自由空间的稠密3D建图方法，将自由空间检测的分割网络与基于单应性的稠密建图技术相结合。在仿真环境中收集的多个真实数据集上对系统进行了测试，涵盖了室内和室外停车场场景。实验结果表明，该多相机系统在性能上优于最先进的开源视觉惯性SLAM方案（Vins-Fusion、ORBSLAM3），在超过8公里的行驶距离（所有数据集的组合）上，平均轨迹误差不超过轨迹长度的1%。

主要贡献

在这篇系统论文中，作者开发了一个多摄像头VI-SLAM方案，可以进行高效且全局一致的轨迹估计，并构建了围绕车辆的稠密三维自由空间地图，实现障碍物避让和导航。该系统基于Kimera的基础上进行了扩展，具体包括：

(i) 支持多摄像头和外部里程计信息；

(ii) 实现稳健的单目或多摄像头闭环检测；

(iii) 针对自主泊车应用实现了高效的地平面建图；

对Kimera的前端和后端进行了修改，改进了跟踪和因子图优化，并使用高度并行化的架构支持多传感器融合。实现了几种闭环检测方法，包括基于透视n点(PnP)方法的单目闭环检测技术，证明其优于流行的方法。最后，我们的SLAM系统与快速语义分割网络Kimera-Semantics结合使用，创建了机器人周围的自由空间的3D地图。

主要内容

A. 硬件架构和数据收集

硬件架构

用于实验的实际平台是一辆改装的Lincoln MKZ轿车，配备了定制的发动机控制单元和定制的车载传感器，包括四个单目鱼眼摄像头、一个IMU和一个车载轮式里程计系统，该系统利用轮式编码器和其他专有传感器来估计车辆的运动，传感器都是消费级别的，只有IMU不同。图1展示了车上传感器组件的排列方式。

图1: (a) 福特测试平台和传感器设置的示意图。(b) 来自室外数据集的四个示例图像；前两个图像来自车上的前置和右侧摄像头，下面两个图像是语义分割网络输出的用于地图模块的识别自由空间道路的结果。(c) 在美国密歇根州底特律收集的车辆室外轨迹样本。图中的轨迹平均长度为450米。

数据收集

为了评估，使用了模拟数据集和使用上述车辆收集的真实数据集。模拟数据集是在TESSE仿真环境中记录的。场景是一个室外城市区域，数据来自具有真实动力学模型的模拟车辆，语义标签以像素级别从模拟器中提取，用于替代分割网络，并且与道路相关的标签用于Kimera-Semantics中的自由空间重建。对于真实世界的数据集，我们在5个不同的室内外位置收集了22个数据集，历时5个月。数据集中车辆的速度、轨迹、环境、障碍物和天气各不相同。一些数据集中一个或多个摄像头会有长时间的遮挡，另一些数据集中车辆会在交通拥堵时长时间停车，市场上有许多行人，并且超出VIO模块时间范围的长直线运动使得尺度估计变得困难。对于室内数据集，由于停车场内的GPS不可靠，我们使用了福特的专有轮式里程计运动估计作为基准数据，因为它在低速和中速下证明非常准确。

B. 软件架构 

所提出的系统对现有的Kimera-VIO和Kimera-Semantics管道进行了几项重大修改，如图2系统架构

图2：所提出的系统架构概述。输入包括来自车辆四个方向的RGB单目图像以及单个IMU，修改后的Kimera-VIO并行处理所有摄像头输入，并生成稳健的状态估计，该估计结果传递给Robust Pose Graph Optimization (RPGO) 模块进行闭环检测和修正，同时，语义分割网络在图像中识别地平面，该信息由修改后的Kimera-Semantics模块用于生成自由空间的三维重建。

多摄像头Kimera-VIO

之前Kimera只能使用立体或RGB-D相机进行紧耦合的视觉惯导测距,所提出的Kimera版本经过修改，可以接受单目图像数据，并与IMU数据进行耦合。单目VIO前端被分成两部分；IMU数据进行预积分，并根据标准方法在两个端点进行插值处理。然后，使用OpenCV 中的工具对图像数据进行处理，以检测特征点（goodFeaturesToTrack），并在帧之间进行特征点跟踪。然后，在跟踪的特征点上应用5点RANSAC算法，以去除每个关键帧上的异常值；关键帧的触发取决于跟踪到的图像特征的数量和质量。除了视觉和惯导数据外，我们还使用车轮里程计测量数据。为实现这个目标，将车载轮式里程计传感器提供的相对位姿链接在一起，以估计关键帧之间的相对位姿，然后将其作为基于因子图的VIO后端中的相对位姿因子使用。特征点、预积分的IMU测量和里程计测量随后被发送到VIO后端模块，该模块使用所有可用的测量数据进行固定滞后平滑处理。

闭环优化

尽管VIO后端可以生成局部一致的轨迹，但我们的目标是获得全局一致的轨迹和地图估计。所提出的架构将VIO的运动估计传递给鲁棒位姿图优化模块，该模块检测闭环并相应地优化轨迹。为了检测闭环，Kimera使用视觉词袋（visual Bag-of-Words）来检测相似的图像。

PnP闭环位姿计算

在PnP方法中，ORB描述符在每个关键帧中提取，并与每个跟踪的特征点关联起来。同时，VIO后端的3D地标数据与每个图像一起发送到闭环检测模块。我们使用ORB描述符在后端优化的地标和2D特征之间获得潜在的对应关系，并使用带有RANSAC的PnP算法找到内点对应关系。然后，RANSAC内点用于生成Kimera-RPGO中闭环因子的帧之间的相对位姿[10]。虽然这种方法相当流行，并且在其他管道中被使用，但我们的实验结果显示得到的位姿不是非常准确。这是因为2D和3D特征之间通常只有很少的匹配（主要是由于基于3D地标的地图的稀疏性），因此在RANSAC之后只有很少的内点。

无尺度闭环位姿计算

为了解决PnP方法的问题，我们考虑使用仅使用2D图像数据来计算相对闭环位姿的闭环检测模块的一个版本。通过使用ORB描述符匹配建立的许多2D-2D对应关系，我们可以直接使用标准的5点RANSAC方法获得无尺度的位姿。在这种情况下，由于没有平移的尺度因子，我们修改了与闭环因子相关的噪声模型的信息矩阵，使其在平移向量的方向上携带零信息。然后，位姿图优化只使用闭环因子中的旋转部分和平移的方向，而不考虑大小，这两者都非常准确。

通过Kimera-Semantics进行自由空间建图

在我们之前的工作中，我们使用Kimera-Semantics从立体或深度相机数据生成密集的语义注释的3D网格,由于本文中的系统使用多个非重叠的单目摄像头，因此无法简单地使用立体重建来生成密集的深度图。由于我们只关注自由空间建图，并且道路可以被假设为局部平面，我们首先使用CNN对图像进行像素级二值分类，检测出地面平面，然后使用单应性变换将地面平面映射到3D平面上[36，第13章]。单应性矩阵是在每个摄像头的相机标定期间计算得到的。使用单应性矩阵，我们将属于地面平面的每个像素映射到一个3D点，然后将相应的3D点云传递给Kimera-Semantics，该模块执行射线投射以推断基于体素的3D地图，然后通过Marching Cubes算法提取纹理化的3D网格。

实验

表格I显示了我们对Kimera-VIO进行单目扩展的性能情况，比较了每个独立摄像头的性能。我们使用一次只运行一个摄像头的方式运行了每个数据集，所有数据集都使用相同的IMU数据。

表格I：每个摄像头的VIO准确性,每个数据集的最佳结果用绿色突出显示,虚线用于表示跟踪失败.

表格II显示了Kimera的多摄像头配置的性能，在1摄像头配置中，仅使用左摄像头,在2摄像头配置中，同时使用左摄像头和右摄像头,然后添加了前摄像头，最后是后摄像头。

表格II：多摄像头VIO的准确性

表格III展示了Kimera-VIO在单摄像头配置下与两个最先进的单目VIO系统（Vins-Fusion 和Open-Vins）的估计性能对比。结果显示了没有闭环的VIO系统的表现，由于这些竞争对手不支持多摄像头环绕视图，为了公平起见，比较是以单目模式进行的。

表格IV展示了对这些不同方法进行闭环处理的割离研究的结果。显示了每种方法对SLAM系统的估计误差和漂移的影响，最佳结果以绿色突出显示。

图3比较了三种方法生成的闭环候选项的旋转和平移误差的直方图。显然，无尺度方法具有最低的误差。

图3：所提出的闭环检测方法的直方图

表格V报告了Kimera（单目）在闭环检测下与Vins-Fusion和ORB-SLAM3的VIO性能。这三个流程都使用视觉词袋生成闭环候选项。

表格V：Kimera、Vins-Fusion和ORB-SLAM3的姿态估计准确性（包括闭环检测）。

表格VI显示了模拟数据集的几何重建准确性，其中存在地面真值地图，系统使用地面真实姿态和Kimera的姿态进行评估，在使用Kimera的情况下，我们使用1个摄像头配置，并且没有使用车轮里程计。

图4展示了使用提出的基于单应性矩阵的方法对Ford数据集进行的多个自由空间重建结果。这些地图是使用Kimera-VIO的4个相机和外部里程计获得的姿态生成的。

图4：在几个Ford数据集上，通过提出的自由空间见图方法生成的3D重建结果。所有四个相机都用于重建，并且Kimera的视觉惯导里程计使用了所有四个相机和外部里程计。每个重建结果上都绘制了估计轨迹的颜色映射，较冷的颜色表示较低的ATE RMSE。

总结

我们对Kimera进行了重要的修改，以支持单目和多相机输入数据，并整合了外部轮式里程计输入。我们修改了回环闭合模块以利用单目输入完善SLAM系统。此外，还修改了Kimera-Semantics，以实现用于自主泊车应用的高效自由空间映射，该方法适用于具有非重叠视场的相机，并且不需要进行深度估计的学习。我们在模拟车辆数据和在福特汽车公司的测试车上收集的真实世界数据集上对该系统进行了测试。所提出的系统具有较小的轨迹和地图误差，并且始终优于最先进的开源VIO和VI-SLAM系统。多相机系统的真实世界结果还有改进的空间，特别是在外参标定方面，后端因子图中的自动标定可能可以缓解其中的一些问题。

（一）视频课程来了！

自动驾驶之心为大家汇集了毫米波雷达视觉融合、高精地图、BEV感知、传感器标定、传感器部署、自动驾驶协同感知、语义分割、自动驾驶仿真、L4感知、决策规划、轨迹预测等多个方向学习视频，欢迎大家自取（扫码进入学习）

（扫码学习最新视频）

视频官网：www.zdjszx.com

（二）国内首个自动驾驶学习社区

近1000人的交流社区，和20+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、Occpuancy、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！

（三）【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称