一、前言

此示例演示如何记录来自 3D 仿真环境的合成激光雷达传感器数据，以及如何使用记录的数据开发同步定位和映射 （SLAM） 算法。

自动驾驶工具箱在Simulink中集成了虚幻引擎模拟环境。与此仿真环境相关的 Simulink 模块可以在库中找到。这些块提供了以下功能：

• 在3D模拟环境中选择不同的场景

• 在场景中放置和移动车辆

• 在车辆上安装和配置传感器

• 根据车辆周围环境模拟传感器数据

这款功能强大的仿真工具可用于在开发、测试和验证自动驾驶算法性能时补充真实数据，从而可以测试在现实世界中难以重现的场景。

在此示例中，您将使用从模拟环境生成的合成激光雷达数据评估激光雷达感知算法。该示例将引导您完成以下步骤：

• 记录和可视化来自仿真环境的合成激光雷达传感器数据。

• 开发一种感知算法，使用 MATLAB 中的 SLAM 构建地图。

二、在模拟环境中设置场景

首先，在模拟环境中设置一个可用于测试感知算法的场景。使用描绘典型城市街区的场景，其中一辆车辆是受测车辆。您可以使用此场景在城市道路环境中测试算法的性能。

接下来，选择车辆在场景中遵循的轨迹。为虚幻引擎模拟选择航点（自动驾驶工具箱）示例介绍了如何以交互方式从场景中选择一系列航点并生成车辆轨迹。此示例使用使用该函数获得的记录驱动器段，如航点选择示例中所述。

Simulink 模型使用模拟 3D 场景配置（自动驾驶工具箱）模块配置了美国城市街区（自动驾驶工具箱）场景。该模型使用具有地面跟随的模拟 3D 车辆（自动驾驶工具箱）块将车辆放置在场景中。激光雷达传感器使用模拟 3D 激光雷达（自动驾驶工具箱）模块连接到车辆上。在块对话框中，使用“安装”选项卡调整传感器的位置。使用参数选项卡配置传感器的属性，以模拟不同的激光雷达传感器。在此示例中，激光雷达安装在屋顶的中心。激光雷达传感器配置为对典型的传感器进行建模。

该模型记录并可视化合成激光雷达数据。记录的数据可通过仿真输出获得，并可用于在 MATLAB 中对算法进行原型设计。此外，该模型使用“从工作区”（Simulink）模块从惯性导航传感器（INS）加载模拟测量值。INS数据是通过使用自动驾驶工具箱对象获得的，并保存在MAT文件中。

示例的其余部分遵循以下步骤：

1. 模拟模型以记录传感器生成的合成激光雷达数据并将其保存到工作空间。

2. 使用保存到工作区的传感器数据在 MATLAB 中开发感知算法。感知算法使用 SLAM 构建周围环境的地图。

3. 可视化构建地图的结果。

三、记录和可视化合成激光雷达传感器数据

记录和可视化子系统使用目标工作空间 （Simulink） 块将合成激光雷达数据记录到工作空间。可视化点云 MATLAB 功能块使用对象来可视化流式点云。可视化 INS 路径 MATLAB 功能块可视化流式 INS 数据。

模拟模型。流点云显示显示合成激光雷达传感器数据。场景显示显示合成的 INS 传感器数据。模型完成模拟后，变量将保存一个结构，其中包含写入工作区的变量。该函数将传感器数据提取到点云对象数组中。点云对象是用于在 MATLAB 中保存激光雷达数据和执行点云处理的基本数据结构。此外，INS数据是从MAT文件加载的，该文件稍后将用于开发感知算法。INS数据是使用自动驾驶工具箱对象获得的。INS 数据已经过处理，以包含世界坐标中的 [x， y， theta] 姿势。

四、使用记录的数据开发感知算法

合成激光雷达传感器数据可用于在不同场景中开发、试验和验证感知算法。此示例使用一种算法根据流式激光雷达数据构建环境的三维地图。这种算法是本地化等应用程序的构建块。它还可用于为地理区域创建高清 （HD） 地图，然后可用于在线本地化。地图构建算法封装在类中。此类使用 MATLAB 中的点云和激光雷达处理功能。MapBuilder

该类从激光雷达传感器获取传入点云，并使用以下步骤逐步构建地图：

1. 预处理点云：对每个传入点云进行下采样。为了提高配准的准确性和效率，下采样用于在配准前对点云进行降采样。

2. 注册点云：使用广义迭代最近点 （G-ICP） 配准算法将传入点云与最后一个点云进行注册。函数执行注册。初始转换估计可以显著提高配准性能。在此示例中，INS 测量用于完成此操作。

3. 对齐点云：使用从配准获得的估计变换将传入点云转换为地图的参考框架。

4. 更新视图集：将传入点云和估计的绝对姿势添加为对象中的视图。在当前视图和上一个视图之间添加连接，并在它们之间进行相对转换。

类的方法完成这些步骤。该函数根据模拟的 INS 传感器读数计算注册的初始估计值。

这种算法在长序列上累积映射时容易受到漂移的影响。为了减少漂移，通常检测环路闭合并使用图形SLAM来校正漂移。该类的方法配置循环闭合检测。配置后，每次调用时都会使用以下函数和类进行循环闭合检测：

• 管理与点云测程相关的数据，如点云、姿势和连接。

• 从每个传入点云中提取扫描上下文描述符。扫描上下文是用于环闭合检测的二维全局特征描述符。

• 管理扫描上下文描述符并检测循环闭包。计算扫描上下文描述符之间的距离，并选择最接近的特征匹配项。

然后，该示例使用点云配准来接受或拒绝闭环候选项，并查找闭环转换。

累积漂移随着时间的推移逐渐增加。

一旦检测到足够的闭环，就可以使用姿势图优化来校正累积的漂移。这是通过类的方法完成的，该方法使用创建姿势图，并使用导航工具箱来优化姿势图。优化姿势图后，使用更新的姿势重建地图。校正漂移并重新构建地图。可视化姿势图优化前后的视图集。

要评估构建地图的准确性，请计算姿态图优化前后估计轨迹与地面实况轨迹之间的均方根误差 （rmse）。可视化使用记录数据计算的累积点云图。

通过改变场景、在场景中放置更多车辆或更新传感器支架和参数，可以在不同场景下对感知算法进行压力测试。此方法可用于增加在现实世界中难以重现的方案的覆盖范围。

五、程序

使用Matlab R2022b版本，点击打开。（版本过低，运行该程序可能会报错）

程序有偿获取：评论区下留言，博主看到会私信你。