利用虚幻引擎仿真环境设计激光雷达SLAM算法

这个例子使用了:

打开脚本

该示例展示了如何从3D仿真环境中记录合成激光雷达传感器数据，并使用记录的数据开发同步定位和映射(SLAM)算法。模拟环境使用虚幻引擎®Epic Games®。

简介

自动驾驶工具箱™集成了虚幻引擎仿真环境在Simulink®。金宝app金宝app与此仿真环境相关的Simulink块可以在drivingsim3d图书馆。这些块提供了以下能力:

在三维仿真环境中选择不同的场景
在场景中放置和移动车辆
在车辆上安装和配置传感器
基于车辆周围环境模拟传感器数据

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

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

记录和可视化仿真环境中的合成激光雷达传感器数据。
在MATLAB®中使用SLAM开发一个感知算法来构建地图。

在仿真环境中设置场景

首先，在仿真环境中设置一个场景，用于测试感知算法。用一个场景描述一个典型的城市街区，只有一辆车是被测试的车辆。您可以使用此场景在城市道路设置中测试算法的性能。

接下来，选择车辆在场景中要遵循的轨迹。的为虚幻引擎模拟选择路点示例描述了如何从场景中交互式地选择一系列路点并生成车辆轨迹。命令获取的已记录的驱动器段helperSelectSceneWaypoints函数，如路径点选择示例中所述。

载入记录驱动器段的参考路径xData = load(“refPosesX.mat”）;yData = load(“refPosesY.mat”）;yawData = load(“refPosesT.mat”）;设置模型使用的工作区变量refPosesX = xData.refPosesX;refPosesY = yData.refPosesY;refPosesT = yawData.refPosesT;显示场景图像上的路径sceneName =“USCityBlock”；hScene =图;helperShowSceneImage (sceneName);持有在散射(refPosesX(:，2)， refPosesY(:，2)， 7，“填充”）%调整轴限制Xlim ([-150 100]) ylim([-125 75])

的LidarSLAMIn3DSimulation金宝app配置了Simulink模型美国城市街区场景使用模拟三维场景配置块。该模型将车辆放置在现场使用模拟三维车辆与地面跟踪块。激光雷达传感器被连接到车辆使用模拟三维激光雷达块。在块对话框中，使用越来越多的标签来调整传感器的位置。使用参数选项卡来配置传感器的属性，以模拟不同的激光雷达传感器。在这个例子中，激光雷达安装在屋顶的中心。激光雷达传感器配置为典型的Velodyne®HDL-32E传感器。

关上(hScene)如果~ ispc错误([“3D模拟只支持微软”金宝appchar (174),“窗口”char (174),“。”]);结束打开模型modelName =“LidarSLAMIn3DSimulation”；open_system (modelName);snapnow;

该模型对合成的激光雷达数据进行记录和可视化。记录的数据可通过仿真输出获得，并可用于在MATLAB中创建算法原型。此外，该模型使用从工作空间(金宝app模型)块加载来自惯性导航传感器(INS)的模拟测量。INS数据是用一种insSensor对象，并保存在一个MAT文件。

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

模拟模型，记录传感器生成的合成激光雷达数据并保存到工作空间。
使用保存到工作空间的传感器数据在MATLAB中开发感知算法。感知算法使用SLAM构建周围环境的地图。
可视化构建的地图的结果。

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

的记录和可视化子系统将合成的激光雷达数据记录到工作空间到工作空间(金宝app模型)块。的可视化点云MATLAB函数块使用pcplayer对象以可视化流点云。的INS路径可视化MATLAB函数块可视化流INS数据。

模拟模型。流点云显示合成激光雷达传感器数据。现场显示合成INS传感器数据。一旦模型完成了仿真，就可以使用simOut变量保存一个结构，其中变量被写入工作区。的helperGetPointCloud函数将传感器数据提取到pointCloud对象。的pointCloud对象是用于保存激光雷达数据并在MATLAB中执行点云处理的基本数据结构。此外，INS数据是从MAT文件中加载的，该文件稍后将用于开发感知算法。INS数据是使用insSensor对象。INS数据已被处理为在世界坐标中包含[x, y, theta]位姿。

%更新模拟停止时间到参考路径完成时结束simStopTime = refPosesX(end,1);set_param (gcs,“StopTime”num2str (simStopTime));从MAT文件加载INS数据数据=负载(“insMeasurement.mat”）;insData = data.insMeasurement.signals.values;%运行模拟simOut = sim(modelName);根据记录的数据创建pointCloud数组ptCloudArr = helperGetPointCloud(simOut);

利用记录数据开发感知算法

合成的激光雷达传感器数据可用于开发、实验和验证不同场景下的感知算法。本例使用一种算法从流激光雷达数据构建环境的3D地图。这种算法是本地化等应用程序的构建块。它还可以用于为地理区域创建高清(HD)地图，然后用于在线本地化。映射构建算法封装在helperLidarMapBuilder类。本课程使用MATLAB中的点云和激光雷达处理功能。详情请参见点云处理．

的helperLidarMapBuilder类从激光雷达传感器获取传入的点云，并使用以下步骤逐步构建一个地图:

预处理点云:预处理每个来的点云以移除地平面和自我载具。
注册点云:使用正态分布变换(NDT)配准算法将传入点云与最后一个点云进行配准。的pcregisterndt函数执行注册。为提高登记的准确性和效率，pcdownsample用于在注册之前对点云进行下采样。初始转换估计可以极大地提高配准性能。在本例中，使用INS测量来实现这一点。
注册点云:使用从配准获得的估计转换将传入点云转换为地图的参照系。
更新视图集:将传入的点云和估计的绝对姿态作为视图添加到pcviewset对象。在当前视图和以前的视图之间添加一个连接，并在它们之间进行相对转换。

的updateMap方法helperLidarMapBuilder类完成了这些步骤。的helperEstimateRelativeTransformationFromINS函数根据模拟惯导系统传感器读数计算配准的初始估计。

这种算法在长序列上积累映射时容易漂移。为了减少漂移，典型的方法是检测循环闭包并使用图SLAM来纠正漂移。看到使用SLAM从激光雷达数据建立地图实例进行了详细的处理。的configureLoopDetector方法helperLidarMapBuilder类配置环路闭合检测。一旦配置了它，每次都会进行环路闭合检测updateMap使用以下函数和类调用:

pcviewset:管理与点云里程计相关的数据，如点云、姿势和连接。
scanContextDescriptor:从每个传入点云提取扫描上下文描述符。扫描上下文是一个用于循环闭合检测的二维全局特征描述符。
scanContextLoopDetector:管理扫描上下文描述符并检测循环闭包。它使用scanContextDistance计算扫描上下文描述符之间的距离，并选择最接近的特征匹配。

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

设置随机种子为例，再现性rng (0);创建一个激光雷达地图生成器mapBuilder = helperLidarMapBuilder(“DownsamplePercent”, 0.25,.．.“RegistrationGridStep”, 3.5,“详细”,真正的);配置映射生成器以检测循环闭包configureLoopDetector (mapBuilder.．.“LoopConfirmationRMSE”2,.．.“SearchRadius”, 0.15,.．.“DistanceThreshold”, 0.07);遍历点云阵列并逐步构建一个地图skipFrames = 5;numFrames = nummel (ptCloudArr);exitLoop = false;prevInsMeas = insData(1，:);为insMeas = insData(n，:);使用INS估计初始转换initTform = helperEstimateRelativeTransformationFromINS(insMeas, prevInsMeas);使用新的激光雷达框架更新地图updateMap(mapBuilder, ptCloudArr(n)， initTform);更新顶视图显示isDisplayOpen = updateDisplay(mapBuilder, exitLoop);如果需要，检查并退出exitLoop = ~isDisplayOpen;prevInsMeas = insMeas;结束snapnow;%关闭显示closeDisplay = true;updateDisplay (mapBuilder closeDisplay);

在视图Id 211和4之间发现循环关闭候选，RMSE为1.135051…接受

随着时间的推移，累积漂移逐渐增加，导致地图不可用。

一旦检测到足够的环路闭合，就可以使用位姿图优化来校正累积漂移。这是由optimizeMapPoses方法helperLidarMapBuilder类，它使用createPoseGraph创建一个姿态图和optimizePoseGraph(导航工具箱)优化姿态图。

在姿态图优化后，使用更新的姿态重新构建地图。这是由rebuildMap的方法helperLidarMapBuilder使用pcalign．

使用optimizeMapPoses而且rebuildMap修正漂移并重建地图。可视化姿态图优化前后的视图集。

在姿态图优化之前可视化视图集hFigViewset =图;hG = plot(mapBuilder.ViewSet);视图(hG。父母,2); title(“Viewset显示”）优化姿态图和重建地图optimizeMapPoses (mapBuilder);rebuildMap (mapBuilder);姿态图优化后的覆盖视图集(hG。父母,“上”）;情节(mapBuilder.ViewSet);(hG。父母,“关闭”）;传奇(hG。父母,“之前”，“后”）

优化姿态图…重建地图…完成

可视化使用记录数据计算的累积点云图。

close(hFigViewset) hFigMap = figure;pcshow (mapBuilder.Map)自定义轴标签和标题包含(“X (m)”) ylabel (“Y (m)”) zlabel (“Z (m)”)标题(“点云图”) helperMakeFigurePublishFriendly (hFigMap);

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

%关闭窗口关上(hFigMap) close_system (modelName)

金宝app支持功能

helperGetPointCloud提取一个数组pointCloud对象。

函数ptCloudArr = helperGetPointCloud(simOut)提取信号ptCloudData = simOut.ptCloudData.signals.values;创建pointCloud数组ptclouddarr = pointCloud(ptCloudData(:，:，:，1));为n = 2: size(ptCloudData,4) ptCloudArr(end+1) = pointCloud(ptCloudData(:，:，:，n));% #好< AGROW >结束结束

helperMakeFigurePublishFriendly调整数字，以便发布的截图是正确的。

函数helperMakeFigurePublishFriendly (hFig)如果~isempty(hFig) && isvalid(hFig)HandleVisibility =“回调”；结束结束

示例中使用的其他支持函金宝app数或类包括在下面。

helperLidarMapBuilder逐步建立一个激光雷达地图使用点云扫描。对每个点云进行处理，去除地平面和自我载体，并与前一个点云进行配准。然后通过对齐和合并点云逐步构建点云图。

helperEstimateRelativeTransformationFromINS估计INS数据的相对转换。

helperShowSceneImage显示虚幻场景的俯视图图像。

helperUpdatePolyline更新与helperShowSceneImage一起使用的折线位置。

另请参阅