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

这个例子使用了:

打开实时脚本

本示例展示了如何使用来自虚幻引擎®仿真环境的合成激光雷达数据开发和评估激光雷达定位算法。

开发定位算法并评估其在不同条件下的性能的最大挑战之一是获得地面真相。虽然您可以使用昂贵的高精度惯性导航系统(INS)获取地面真相，但虚拟仿真是一种具有成本效益的替代方案。模拟使您能够在各种场景和传感器配置下进行测试。它还支持快速开发迭代，并提供精确的基础事实。

本例使用虚幻引擎仿真环境中预构建的停车场场景地图，开发和评估基于正态分布变换(NDT)方法的激光雷达定位算法。本例假设车辆的初始姿态已知。

概述

激光雷达定位是相对于环境的已知点云图估计捕获点云的激光雷达姿态的过程。本地化是增强现实、机器人和自动驾驶等应用的关键技术。这个例子展示了激光雷达定位工作流的以下步骤:

加载一个预先构建的地图。
使用NDT映射在给定的参考路径上进行本地化。
使用NDT定位估计反馈沿给定参考路径控制车辆。

在仿真环境中设置场景

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

负载参考路径%数据=负载(“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([-60 40]) ylim([10 60])位置= [100 100 1000 500];%调整大小传说举行从

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

以下是生成预构建点云图的步骤:

记录激光雷达传感器的点云和地面真实姿态，同时自我车辆在环境中沿着已知路径移动。有关用于生成本例映射的路径的详细信息，请参见使用SLAM从三维激光雷达数据建立占用地图．
预处理点云以分割和移除地面和自我车辆，并将点云剪辑到车辆周围的选择半径。
将所有记录的点云使用已知的真实姿态对齐pcalign．

本例使用了一个使用这些步骤预构建的映射。加载和可视化地图。

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

将点云图叠加在场景的俯视图图像上，以直观地检查它与场景中的特征的相似程度。

hMapOnScene = helperSuperimposeMapOnSceneImage(“LargeParkingLot”, ptCloudMapPoints);

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

voxelSize = 1;ndtMap = pcmapndt(pointCloud(ptCloudMapPoints)，voxelSize);figure show(ndtMap) view(2)“无损检测图”)包含(“X”) ylabel (“Y”）

使用NDT地图进行本地化

你可以从姿态的初始估计开始定位过程，你可以用它来选择周围的子地图区域并开始姿态估计。这比在整个地图中进行全局搜索更有效。在现实场景中，您可以使用外部传感器(如全球导航卫星系统(GNSS))或基准标记(如april标签或QR码)在映射环境的入口获得初始姿态估计。当车辆进入映射环境时，使用前一个时间步的定位获得的位姿估计作为当前时间步的初始位姿估计。

以下步骤总结了本例中的本地化工作流程:

在模拟开始时，使用初始姿态估计在已知的NDT图中选择感兴趣的子图，并通过对NDT图中的点云进行本地化来获得实际姿态估计。
使用在前一个时间步中获得的姿态估计来检查估计是否太接近子映射边缘，或者它是否在子映射之外。如果是，更新子地图到一个区域周围的姿态估计使用selectSubmap．
通过在NDT地图中使用本地化当前点云来查找实际的姿态估计findPose．属性指定较低的值宽容用于精确结果的名称-值参数。
重复步骤2和3的所有后续时间步沿参考轨迹。

的localizeUsingLidar模型包含沿指定参考路径移动的掀背车模拟三维车辆与地面跟踪块。激光雷达传感器安装在车辆的顶部中心使用模拟三维激光雷达块。的本地化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函数块，封装了基于NDT地图的定位算法实现使用helperLidarLocalizerNDT函数。此块取激光雷达点云生成的模拟三维激光雷达块和初始已知姿态作为输入，并产生定位估计。该估算返回为 $［ x y θ ］$ ，表示激光雷达在地图参考系中的二维姿态。
的计划子系统从工作空间加载预先计划的轨迹refPoses，方向，曲率,而且速度工作空间变量。路径平滑样条块用于计算refPoses，的方向,而且曲率变量。Velocity Profiler块计算速度变量。
辅助路径分析器块使用参考轨迹和当前姿态将适当的参考信号馈送到车辆控制器。
的车辆控制器子系统控制车辆的转向和速度，通过使用横向和纵向控制器来产生转向和加速或减速命令，由横向控制器斯坦利和纵向控制器斯坦利块实现。子系统将这些命令输入到车辆模型中，使用车身3DOF模块在仿真环境中模拟车辆的动力学。

指定车辆尺寸centerToFront = 1.104;centerToRear = 1.343;frontOverhang = 0.828;rearOverhang = 0.589;vehicleWidth = 1.653;vehicleHeight = 1.513;vehicllength = centerToFront + centerToRear + frontOverhang + rearOverhang;hatchbackDims = vehicleDimensions(vehicllength,vehicleWidth,vehicleHeight，.．.FrontOverhang = FrontOverhang RearOverhang = RearOverhang);vehicleDims = [hatchbackDims.]hatchbackDims长度。宽度hatchbackDims.Height];vehicleColor = [0.85 0.325 0.098];为预先计划的轨迹加载工作空间变量refposed = data. referencepathforward . trajectory . refposed;directions = data.ReferencePathForward.Trajectory.directions;曲率= data. referencepathforward . trajectory .曲率;velocity = data. referencepathforward . trajectory . velocity;startPose = refpose (1，:);%开放模式modelName =“localizeAndControlUsingLidar”；open_system snapnow (modelName)%运行模拟sim (modelName);close_system (modelName)

金宝app支持功能

helperDisplayMetrics显示用于评估本地化质量的指标。

函数hFig = helperDisplayMetrics(simOut) simTimes = simOut.logsout{1}.Values.Time;xEst = simOut.logsout{1}.Values.Data;yEst = simOut.logsout{2}.Values.Data;yawEst = simOut.logsout{3}.Values.Data;xTruth = squeeze(simOut.logsout{4}.Values.Data(:，1，:));yTruth = squeeze(simOut.logsout{4}.Values.Data(:，2，:));yawTruth = squeeze(simOut.logsout{5}.Values.Data(:，3，:));xDeviation = abs(xEst - xTruth);yDeviation = abs(yEst - yTruth);yawDeviation = abs(helpwraptopi (yawTruth - yawEst)); lim = [-1 1]; hFig = figure(Name=“量度-绝对偏差”）;subplot(3,1,1) plot(simTimes, xDeviation,LineWidth=2);ylim (lim)网格在标题(“X”)包含(“时间(s)”) ylabel (“偏差(m)”) subplot(3,1,2) plot(simTimes, yDeviation,LineWidth=2);ylim (lim)网格在标题(“Y”)包含(“时间(s)”) ylabel (“偏差(m)”) subplot(3,1,3) plot(simTimes, yawDeviation,LineWidth=2);Ylim ([0 pi/20])网格在标题(“偏航”)包含(“时间(s)”) ylabel (“偏差(rad)”）结束

helperSuperImposeMapOnSceneImage在场景图像上叠加点云图。

函数hFig = helperSuperimposeMapOnSceneImage(sceneName, ptCloudAccum) hFig = figure(Name=“点云图”）;he = helpshowsceneimage (sceneName);(他。父母,“上”) pcshow (ptCloudAccum);(他。父母,“关闭”) xlim(他。父母,［-10 35]); ylim(hIm.Parent, [-23 20]);结束

helperWrapToPi将角度包装到范围 $［ - π π ］$ ．

函数angle = helpwraptopi (angle) idx = (angle < -pi) | (angle > pi);angle(idx) = helperWrapTo2Pi(angle(idx) + pi) - pi;结束

helperWrapTo2Pi将角度包装到范围 $［ - 2 π 2 π ］$ ．

函数angle = helpwrapto2pi (angle) pos = (angle>0);角度= mod(角度，2*pi);角度(Angle ==0 & pos) = 2*pi;结束

另请参阅

功能

pccat|pctransform|pcalign|pcviewset|pcplayer

块

模拟三维场景配置|模拟三维车辆与地面跟踪|模拟三维激光雷达