激光雷达定位与虚幻引擎模拟

这个示例使用:

这个例子展示了如何开发一个激光雷达定位算法,并对其使用虚幻引擎®合成激光雷达数据模拟环境。

最大的挑战之一在发展中定位算法和评估其性能在不同的条件下获取地面实况。虽然您可以使用昂贵的捕获地面实况,高精度的惯性导航系统(INS),虚拟仿真是一个性价比不错的备用方案。仿真允许您测试在不同的场景和传感器配置。它还使迭代和快速发展提供了精确的地面实况。

这个例子使用一个预先构建的一个停车场地图场景在虚幻引擎模拟环境开发和评估一个激光雷达定位算法基于正态分布的变换(无损检测)方法。这个例子假定已知车辆的初始姿势。

概述

激光雷达定位的过程是评估激光雷达姿势拍摄点云相对于一个已知的点云环境的地图。定位是一个关键的技术应用,如增强现实、机器人、自动驾驶。这个例子显示了一个激光雷达本地化工作流程与这些步骤:

加载一个预先构建的地图。
定位在一个给定的参考路径使用无损检测地图。
控制车辆在一个给定的参考路径使用无损检测定位估计的反馈。

在仿真环境中建立场景

停车车辆的停车位是一项具有挑战性的操作依赖于准确的定位。使用预先构建的大型停车场场景创建这个场景。这个示例使用记录获得的参考轨迹从一个场景交互式地选择一个锚点序列。首先,使用二维可视化参考路径场景的鸟瞰图。

%负荷参考路径data =负载(“ReferencePathForward.mat”);refPosesX = data.ReferencePathForward.refPosesX;refPosesY = data.ReferencePathForward.refPosesY;refPosesT = data.ReferencePathForward.refPosesT;sceneName =“LargeParkingLot”;hScene =图;helperShowSceneImage (sceneName);持有在散射(refPosesX (:, 2), refPosesY (:, 2), [],“填充”DisplayName =“参考路径”);xlim (40 [-60]) ylim hScene (60 [10])。位置= (100 100 1000 500);%调整图传说举行从

从预先构建的点云创建无损检测地图地图

这些步骤生成预先构建的点云地图:

记录点云和地面真理构成的激光雷达传感器,而自我车辆沿着已知环境中的路径。更多信息用于生成地图的路径对于这个示例,明白了使用大满贯构建入住率从3 d激光雷达数据映射。
点云预处理段,消除地面和自我,选择范围和夹点云。
所有记录的点云对齐真实使用已知的姿势pcalign。

这个示例使用地图已经预先构建的使用这些步骤。加载和可视化地图。

负载(“parkingLotPCMapPoints.mat”);图pcshow (ptCloudMapPoints)视图(2)标题(“Pointcloud地图”)包含(“X”)ylabel (“Y”)

重叠顶视图图像上的点云的地图场景的视觉检查它像特性现场是多么的相似。

hMapOnScene = helperSuperimposeMapOnSceneImage (“LargeParkingLot”,ptCloudMapPoints);

创建一个使用无损检测地图从上面的点云pcmapndt。无损检测可视化地图。

voxelSize = 1;ndtMap = pcmapndt (pointCloud (ptCloudMapPoints) voxelSize);图显示(ndtMap)视图(2)标题(“无损检测图”)包含(“X”)ylabel (“Y”)

本地化使用无损检测图

你可以开始本地化过程的初始估计姿势,可以选择使用submap周边地区,并开始姿势估计。这是更有效的比全球搜索在整个地图。在真实的场景中,您可以获得一个初始姿势估计门口使用外部映射环境传感器,如全球导航卫星系统(GNSS)或基准标记,如4月标签或QR码。车辆进入绘制环境,造成估计获得了以前的时间步的使用本地化作为当前时间步的初始姿态估计。

这些步骤总结在这个例子中给出的本地化工作流程:

的模拟,使用初始姿势估计选择感兴趣的submap在已知的无损检测地图,并获得实际的姿势估计本地化无损检测的点云的地图。
使用在前面获得的姿势估计时间步检查如果估计submap边缘太近,或如果它是在映射中。如果是这样,更新submap姿势估计周边地区使用selectSubmap。
找到实际的姿势估计本地化当前无损检测地图使用点云findPose。指定值低宽容名称-值参数的准确的结果。
重复步骤2和3为所有后续的步伐沿着参考轨迹。

的localizeUsingLidar模型包含掀背车沿着通过使用指定的参考路径模拟3 d车辆与地面块。激光雷达传感器安装在屋顶上的中心车辆使用模拟3 d激光雷达块。的本地化块和MATLAB函数helperLidarLocalizerNDT函数实现定位算法使用前面列出的步骤。运行仿真可视化定位估计和地面真理构成的汽车沿着参考轨迹。

关上(hScene)如果~ ispc错误(“虚幻引擎模拟只支持微软”金宝app…+ char (174) +“Windows”+ char (174) +“。”);结束%开放模式modelName =“localizeUsingLidar”;open_system snapnow (modelName)%运行仿真simOut = sim (modelName);

评估定位精度

量化的功效定位、测量偏差在平移和旋转估计相对于地面真理。由于车辆是平坦的地面上移动,这个例子是只关心运动XY -飞机。

hFigMetrics = helperDisplayMetrics (simOut);

控制车辆使用无损检测定位估计的反馈

指标如平移和旋转的偏差估计不是,足以保证一个定位系统的性能和精度。例如,定位系统的精度和性能变化会影响车辆控制器,控制器需要重新调整收益。因此,您必须有一个闭环验证框架,包含了下游组件。的localizeAndControlUsingLidar模型演示了这个框架,通过融合定位算法,车辆控制器,和合适的车辆模型。

模型主要有这些组件:

的本地化块是一个MATLAB函数块,封装了无损检测基于地图的定位算法实现使用helperLidarLocalizerNDT函数。这一块需要产生的激光雷达点云模拟3 d激光雷达块和最初的姿势被称为输入并产生定位估计。返回的估计 $(x y θ]$ 代表二维构成的激光雷达地图参考系。
的计划子系统加载一个预先计划的轨迹从工作空间使用refPoses,方向,曲率,和速度工作空间变量。路径平滑样条块用于计算refPoses,的方向,和曲率变量。计算速度分析器的块速度变量。
辅助路径分析仪块使用参考轨迹和当前对车辆满足适当的参考信号控制器。
的车辆控制器子系统控制车辆的转向和速度通过使用横向和纵向控制器生产转向和加速或减速命令,由横向控制器Stanley)和纵向控制器实现斯坦利块。子系统提供这些命令车辆模型来模拟车辆的动态仿真环境中使用车辆身体3自由度。

%指定车辆维centerToFront = 1.104;centerToRear = 1.343;frontOverhang = 0.828;rearOverhang = 0.589;vehicleWidth = 1.653;vehicleHeight = 1.513;vehicleLength = centerToFront + centerToRear + frontOverhang + rearOverhang;hatchbackDims = vehicleDimensions (vehicleLength vehicleWidth vehicleHeight,…FrontOverhang = FrontOverhang RearOverhang = RearOverhang);(hatchbackDims vehicleDims =。hatchbackDims长度。宽度hatchbackDims.Height];vehicleColor = (0.85 0.325 0.098);%负载工作空间变量为预先计划的轨迹refPoses = data.ReferencePathForward.Trajectory.refPoses;方向= data.ReferencePathForward.Trajectory.directions;曲率= data.ReferencePathForward.Trajectory.curvatures;速度= data.ReferencePathForward.Trajectory.velocities;:startPose = refPoses (1);%开放模式modelName =“localizeAndControlUsingLidar”;open_system snapnow (modelName)%运行仿真sim (modelName);close_system (modelName)

金宝app支持功能

helperDisplayMetricsdisaplays指标来评估质量的本地化。

函数hFig = helperDisplayMetrics (simOut) simTimes = simOut.logsout {1} .Values.Time;x = simOut.logsout {1} .Values.Data;是的= simOut.logsout {2} .Values.Data;偏航= simOut.logsout {3} .Values.Data;xTruth =挤压(simOut.logsout {4} .Values.Data (: 1:));yTruth =挤压(simOut.logsout {4} .Values.Data (:, 2:));yawTruth =挤压(simOut.logsout {5} .Values.Data (:, 3:));xDeviation = abs (x - xTruth);yDeviation = abs(是的- yTruth);yawDeviation = abs (helperWrapToPi (yawTruth -偏航)); lim = [-1 1]; hFig = figure(Name=“指标-绝对偏差”);次要情节(1,1)情节(simTimes xDeviation,线宽= 2);ylim (lim)网格在标题(“X”)包含(“时间(s)”)ylabel (“偏差(m)”次要情节(3、1、2)情节(simTimes yDeviation,线宽= 2);ylim (lim)网格在标题(“Y”)包含(“时间(s)”)ylabel (“偏差(m)”次要情节(3,1,3)情节(simTimes yawDeviation,线宽= 2);ylim网格([0π/ 20])在标题(“偏航”)包含(“时间(s)”)ylabel (“偏差(rad)”)结束

helperSuperImposeMapOnSceneImage添加点云的地图场景图像。

函数hFig = helperSuperimposeMapOnSceneImage (sceneName ptCloudAccum) hFig图(Name = =“点云地图”);他= helperShowSceneImage (sceneName);持有(hIm.Parent“上”)pcshow (ptCloudAccum);持有(hIm.Parent“关闭”)xlim(他。家长,35 [-10]);ylim(他。家长,20 [-23]);结束

helperWrapToPi包角范围 $(- - - - - - π π]$ 。

函数角= helperWrapToPi(角)idx =(角< -π)|(角>π);角(idx) = helperWrapTo2Pi角(idx) +π-π;结束

helperWrapTo2Pi包角范围 $(- - - - - - 2 π 2 π]$ 。

函数角= helperWrapTo2Pi(角)pos =(角度> 0);角=国防部(角,2 *π);角(角= = 0 & pos) = 2 *π;结束