基于matlab使用激光雷达数据构建地图并使用SLAM算法估计车辆轨迹（附源码）

news2024/9/23 19:24:44

一、前言

此示例演示如何处理来自安装在车辆上的传感器的 3-D 激光雷达数据，以逐步构建地图并使用同步定位和映射（SLAM）估计车辆的轨迹。除了 3D 激光雷达数据外，惯性导航传感器（INS）还用于帮助构建地图。以这种方式构建的地图可以促进车辆导航的路径规划，也可以用于本地化。

二、概述

根据激光雷达数据构建地图（自动驾驶工具箱）示例使用 3D 激光雷达数据和 IMU 读数逐步构建车辆穿越环境的地图。虽然此方法可构建局部一致的地图，但它仅适用于绘制小区域的地图。在较长的序列中，漂移累积成一个显著的误差。为了克服此限制，此示例识别以前访问过的地方，并尝试使用图形 SLAM 方法校正累积的漂移。

三、加载和浏览记录的数据

此示例中使用的数据是 Velodyne SLAM 数据集的一部分，表示近 6 分钟的记录数据。将数据下载到临时目录。

注意：此下载可能需要几分钟时间。

使用该函数以时间表的形式从创建的文件夹中读取数据。激光雷达捕获的点云以PNG图像文件的形式存储。提取变量中的点云文件名列表。要从图像文件中读取点云数据，请使用该函数。此函数采用图像文件名并返回对象。INS 读数直接从配置文件中读取并存储在变量中。

可视化点云。车辆穿越由两个环路组成的路径。在第一个循环中，车辆进行一系列转弯并返回起点。在第二个循环中，车辆沿着另一条路线进行一系列转弯，然后再次返回起点。

四、使用里程计构建地图

首先，使用根据激光雷达数据构建地图（自动驾驶工具箱）示例中介绍的方法构建地图。该方法包括以下步骤：

对齐激光雷达扫描：使用点云配准技术对齐连续的激光雷达扫描。此示例注册扫描。通过连续组合这些变换，每个点云都变换回第一个点云的参考系。
合并对齐的扫描：通过组合所有转换后的点云来生成地图。

这种增量构建地图并估计车辆轨迹的方法称为里程计。

使用对象跨多个视图存储和管理数据。视图集由一组连接的视图组成。

每个视图存储与单个视图关联的信息。此信息包括视图的绝对姿势、在该视图捕获的点云传感器数据以及视图的唯一标识符。将视图添加到视图集。
要在视图之间建立连接。连接存储诸如连接视图之间的相对转换、计算此度量所涉及的不确定性（表示为信息矩阵）以及关联的视图标识符等信息。
使用可视化视图集建立的连接。这些连接可用于可视化车辆经过的路径。

视图集对象现在包含视图和连接。在“视图”表中，该变量指定每个视图相对于第一个视图的绝对姿势。在的表中，变量指定连接视图之间的相对约束，变量为每个边指定与连接关联的不确定性。

现在，使用创建的视图集构建点云图。使用将视图绝对姿势与视图集中的点云对齐。指定格网大小以控制地图的分辨率。映射将作为对象返回。

请注意，使用此方法遍历的路径会随时间漂移。虽然沿着第一个环路回到起点的路径似乎是合理的，但第二个环路从起点明显漂移。累积漂移导致第二个环路在距离起点几米的地方终止。

仅使用测程法构建的地图是不准确的。显示带有遍历路径的构建点云图。请注意，第二个循环的映射和遍历路径与第一个循环不一致。

五、使用姿势图优化纠正漂移

图形SLAM是一种广泛使用的技术，用于解决里程计中的漂移。图形SLAM方法以增量方式创建一个图形，其中节点对应于车辆姿势，边缘表示约束连接姿势的传感器测量值。这样的图称为姿势图。姿势图包含由于测量中的噪声或不准确而对相互矛盾的信息进行编码的边缘。然后优化构建图中的节点，以找到能够最佳解释测量值的车辆姿势集。这种技术称为姿势图优化。

若要从视图集创建姿势图，可以使用函数。此函数为每个视图创建一个节点，并为视图集中的每个连接创建一个边。要优化姿势图，您可以使用优化姿势图（导航工具箱）功能。

有助于图形SLAM在校正漂移方面的有效性的一个关键方面是准确检测循环，即以前访问过的地方。这称为闭环检测或位置识别。向与环闭合对应的姿态图添加边，可以为连接的节点姿态提供相互矛盾的测量，这可以在姿态图优化期间解决。

可以使用描述符来检测环路闭合，这些描述符表征激光雷达传感器可见的局部环境描述符就是这样一种描述符，可以使用函数从点云计算。此示例使用对象来管理与每个视图对应的扫描上下文描述符。它使用对象函数通过两阶段描述符搜索算法检测循环闭包。在第一阶段，它计算环键子描述符以查找潜在的循环候选者。在第二阶段，它通过阈值扫描上下文距离将视图分类为循环闭包。