主要内容

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

这个例子使用了:

打开脚本

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

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

本示例使用Epic Games®的虚幻引擎模拟环境，从停车场景中的已知初始姿态开发和评估激光雷达定位算法。

在仿真环境中设置场景

将车辆停在停车位是一项具有挑战性的操作，依赖于准确的定位。使用预建的大型停车场场景来创建这样的场景。的为虚幻引擎模拟选择路点示例描述了如何从场景中交互式地选择路径点序列，以及如何生成参考车辆轨迹。本例使用使用链接示例中描述的方法获得的已记录的参考轨迹。首先，在场景的二维鸟瞰图中可视化参考路径。

负载参考路径%数据=负载(“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 ([0 40]) ylim([-20 10])传说保持从

记录和可视化传感器数据

方法建立一个简单的模型，其中一辆掀背车沿着指定的参考路径移动模拟三维车辆与地面跟踪块。在车辆的顶部中心安装激光雷达模拟三维激光雷达块。记录并可视化传感器数据。记录的数据被用来开发一个定位算法。

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

记录的传感器数据返回simOut变量。

利用记录数据开发算法

在本例中，您将开发一个基于点云配准的算法。点云配准是一种常用的定位技术，通过估计两点云之间的相对运动来获得定位数据。在长序列上积累这样的相对运动可能导致漂移，可以使用环路闭合检测和姿态图优化来纠正漂移，如图所示使用SLAM从激光雷达数据建立地图的例子。由于本例使用较短的引用路径，因此省略了循环闭合检测。

提取激光雷达传感器数据和地面提供的真实位置和方位模拟三维激光雷达块。在世界(场景)坐标系中提供了地面真实位置和方位。方法从真实数据中提取已知的初始姿态helperPoseToRigidTransform函数。

close_system (modelName);提取激光雷达传感器数据ptCloudArr = helperGetPointClouds(simOut);提取基本真理[lidarLocation, lidarOrientation, simTimes] = helperGetLocalizerGroundTruth(simOut);计算初始姿态initPose = [lidarLocation(1，:) lidarOrientation(1，:)];initTform = helperPoseToRigidTransform(initPose);

利用提取的传感器数据，提出一种激光雷达定位算法。使用一个pcviewset对象来处理和存储里程表数据。pcviewset将里程计数据组织到一组视图中，以及视图之间的关联连接，其中:

  • 每个视图都有一个描述到某个固定参考系的刚性变换的绝对姿态。

  • 每个连接都有一个描述两个连接视图之间刚性转换的相对姿态。

定位估计以相对于场景参考系的每个视图的绝对姿态的形式保持。

使用一个pcplayer对象在注册时显示循环中的流点云数据。将视角转换为俯视图。橙色长方体和路径表示算法估计的定位位置。绿色的路径显示了事实。

创建一个视图集vSet = pcviewset;absPose = initTform;relPose = rigid3d;viewId = 1;定义激光雷达传感器安装位置之间的刚性转换%和车辆参考点。lidarToVehicleTform = helperPoseToRigidTransform(single([0 0 0 -1.57 0 0 0]));处理点云帧ptCloud = helpprocesspointcloud (ptCloudArr(1));初始化累积的点云图ptCloudAccum = pctransform(ptCloud, absPose);将第一个视图添加到视图集vSet = addView(vSet, viewId, absPose，“PointCloud”, ptCloud);%配置显示Xlimits = [0 50];Ylimits = [-25 10];Zlimits = [-30 30];玩家= pcplayer(xlimits, ylimits, zlimits);estimatePathHandle = [];truthPathHandle = [];指定车辆尺寸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];初始化参数skipFrames = 5;为积累足够的运动而跳过的帧数prevViewId = viewId;prevPtCloud = ptCloud;在激光雷达传感器帧上循环并定位为viewId = 6: skipFrames: nummel (ptCloudArr)%进程帧ptCloud = helpprocesspointcloud (ptCloudArr(viewId));将当前帧注册到前一帧relPose = pcregistericp(ptCloud, prevPtCloud，“MaxIterations”现年40岁的.．.“指标”，“pointToPlane”）;由于运动限制在2-D平面，丢弃沿Z方向的运动到防止噪音累积。relPose.Translation(3) = 0;更新绝对位姿height = abpos . translation (3);abpose = rigid3d(relPose。胡扯。T);abpos . translation (3) = height;添加新的视图和连接到以前的视图vSet = addView(vSet, viewId, absPose，“PointCloud”, ptCloud);vSet = addConnection(vSet, prevViewId, viewId, relPose);累积的点云图ptCloudAccum = pccat([ptCloudAccum, pctransform(ptCloud, absPose)]);计算地面真相和估计位置localizationEstimatePos = abpos . translation;localizationTruthPos = lidarLocation(viewId，:);更新累计点云图视图(球员,ptCloudAccum);设置视角为俯视图视图(球员。轴,2);将传感器当前的绝对位姿转换为车架absVehiclePose = rigid3d(lidarToVehicleTform。胡扯。T);以当前绝对姿态绘制车辆helperDrawVehicle(球员。轴,absVehiclePose, vehicleDims,“颜色”, vehicleColor);绘制定位估计和地面真值点helperDrawLocalization(球员。轴,.．.localizationEstimatePos, estimatePathHandle, vehicleColor，.．.localizationTruthPos, truthPathHandle， [0 1 0]);prevPtCloud = ptCloud;prevViewId = viewId;结束

放大到轨迹的尾部，检查定位估计与地面真实值的比较。

xlim(球员。轴,[0 15]); ylim(player.Axes, [-15 0]); zlim(player.Axes, [0 15]); snapnow;%接近玩家隐藏(球员);

基于点云配准的定位算法的一个有用结果是被遍历环境的地图。您可以通过将所有点云组合到一个公共参考框架中来获得该地图。的pccat函数在上面循环的每次迭代中都使用pctransform，对注册点云进行增量组合。或者，您可以使用pcalign函数将所有点云对准公共参考框架在一个镜头结束。

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

hMapOnScene = helperSuperimposeMapOnSceneImage(sceneName, ptCloudAccum);snapnow;%关闭图形关闭(hMapOnScene);

上面描述的定位算法封装在helperLidarRegistrationLocalizer助手类。这个类可以作为一个框架来开发使用点云配准的本地化管道。

  • 使用“ProcessFcnHandle”而且“ProcessFcnArguments”名称-值对参数用于配置在注册之前如何处理点云。

  • 使用“RegisterFcnHandle”而且“RegisterFcnArguments”名称-值对参数用于配置点云的注册方式。

评估定位精度

为了量化定位的效果，测量平移和旋转估计与地面真实值的偏差。由于车辆是在平坦的地面上移动，本例只考虑在地面上的运动x - y飞机。

hFigMetrics = helperDisplayMetrics(vSet, lidarLocation, lidarOrientation, simTimes);

循环模拟

虽然像平移偏差和旋转估计这样的度量是必要的，但本地化系统的性能可能会产生下游影响。例如，定位系统精度或性能的变化可能会影响车辆控制器，需要重新调整控制器增益。因此，拥有一个包含下游组件的闭环验证框架至关重要。的localizeAndControlUsingLidar模型通过结合定位算法、车辆控制器和合适的车辆模型来演示该框架。

该模型有以下主要组件:

  • 本地化块是一个MATLAB函数块，封装了定位算法-实现使用helperLidarRegistrationLocalizer类。此块取激光雷达点云生成的模拟三维激光雷达块和初始已知姿态作为输入，并产生定位估计。该估算返回为$(x, y， \theta)$，表示激光雷达在地图参考系中的二维姿态。

  • 计划子系统从工作空间加载预先计划的轨迹refPoses，方向，曲率而且速度工作空间变量。的路径平滑样条块用于计算refPoses，方向而且曲率变量。的速度分析器块计算速度变量。

  • 辅助路径分析器块使用参考轨迹和当前姿态将适当的参考信号馈送到车辆控制器。

  • 车辆控制器子系统通过使用横向和纵向控制器来产生转向和加速或减速命令来控制车辆的转向和速度。的横向控制人Stanley而且纵向控制器斯坦利块用于实现这一点。将这些命令输入车辆模型，以在仿真环境中使用车身3DOF块。

关闭(hFigMetrics);为预先计划的轨迹加载工作空间变量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 (modelName);snapnow;%运行模拟sim (modelName);close_system (modelName);

通过这种设置，可以快速迭代不同的场景、传感器配置或参考轨迹，并在进行实际测试之前改进定位算法。

  • 若要选择不同的场景，请使用模拟三维场景配置块。从现有的预建场景中选择或在虚幻编辑器中创建自定义场景。

  • 要创建不同的参考轨迹，请使用helperSelectSceneWaypoints工具，如为虚幻引擎模拟选择路点的例子。

  • 若要更改传感器配置，请使用模拟三维激光雷达块。的越来越多的TAB提供了指定不同传感器安装位置的选项。的参数TAB提供了修改传感器参数的选项，如检测范围、视野和分辨率。

金宝app支持功能

helperGetPointClouds提取一个数组pointCloud包含激光雷达传感器数据的对象。

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

helperGetLocalizerGroundTruth提取地面真相位置和方向。

函数[lidarLocation, lidarOrientation, simTimes] = helperGetLocalizerGroundTruth(simOut) lidarLocation = squeeze(simOut.lidarLocation.signals.values)';lidarOrientation = squeeze(simOut.lidarOrientation.signals.values)';simTimes = simOut.lidarLocation.time;忽略前2帧lidarLocation(1:2，:) = [];lidarOrientation(1:2，:) = [];simTimes(1:2，:) = [];结束

helperDrawLocalization在坐标轴上绘制定位估计和地面真实值。

函数[estHandle,truthHandle] = helperDrawLocalization(axesHandle，.．.est, estHandle, estColor, truth, truthHandle, truthColor)创建散点对象并调整图例如果isempty(estHandle) || isempty(truthHandle) markerSize = 6;持有(axesHandle“上”）;estHandle = scatter3(axesHandle, NaN, NaN, NaN, markerSize, estColor，“填充”）;truthHandle = scatter3(axesHandle, NaN, NaN, NaN, markerSize, truthColor，“填充”）;%的传说(axesHandle{“分”,“估计”,“真理”},…% '颜色'，[11 11 1]，'位置'，'东北');持有(axesHandle“关闭”）;结束estHandle.XData(end+1) = est(1);estHandle.YData(end+1) = est(2);estHandle.ZData(end+1) = est(3);truthHandle.XData(end+1) = truth(1);truthHandle.YData(end+1) = true (2);truthHandle.ZData(end+1) = truth(3);结束

helperSuperimposeMapOnSceneImage在场景图像上叠加点云图

函数hFig = helpsuperimposemaponsceneimage (sceneName, ptCloudAccum) hFig = figure(“名字”，“点云图”）;he = helpshowsceneimage (sceneName);(他。父母,“上”) pcshow (ptCloudAccum);(他。父母,“关闭”) xlim(他。父母,[-10 50]); ylim(hIm.Parent, [-30 20]);结束

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

函数hFig = helperDisplayMetrics(vSet, lidarLocation, lidarOrientation, simTimes) absPoses = vSet. views . absolutepose;translationestimate = vertcat(abpos . translation);rotationestimate = pagetranspose(cat(3, abspoes . rotation));viewIds = vSet.Views.ViewId;viewTimes = simTimes(viewIds);xEst = translationestimate (:， 1);yEst = translationestimate (:， 2);yawEst =欧拉(四元数(旋转估计，“rotmat”，“点”)，“ZYX股票”，“点”）;yawEst = yawEst(:， 1);xTruth = lidarLocation(viewIds, 1);yTruth = lidarLocation(viewIds, 2);yawTruth = lidarOrientation(viewIds, 3);xDeviation = abs(xEst - xTruth);yDeviation = abs(yEst - yTruth);yawDeviation = abs(helpwraptopi (yawTruth - yawEst));hFig = figure(“名字”，“度量—绝对偏差”）;subplot(3,1,1) plot(viewTimes, xDeviation，“线宽”2);Ylim([0 1])网格在标题(“X”)包含(“时间(s)”) ylabel (“偏差(m)”) subplot(3,1,2) plot(viewTimes, yDeviation，“线宽”2);Ylim([0 1])网格在标题(“Y”)包含(“时间(s)”) ylabel (“偏差(m)”) subplot(3,1,3) plot(viewTimes, yawDeviation，“线宽”2);Ylim ([0 pi/20])网格在标题(“偏航”)包含(“时间(s)”) ylabel (“偏差(rad)”）结束

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;结束

另请参阅

功能

块

相关的话题

自动驾驶工具箱文档

金宝app

用Simulink实现自适应巡航控制器金宝app

用Simulink实现自适应巡航控制器金宝app

下载技术文件