使用地图层融合多个激光雷达传感器
占用图通过将连续的世界空间映射到离散的数据结构,为机器人应用程序提供了一种简单而健壮的表示环境的方法。个人网状细胞可以包含关于障碍物信息的二进制或概率信息。然而,自主平台可能会使用各种传感器,这些传感器可能需要组合在一起来估计平台的当前状态和周围环境的状态。
这个例子主要是集成各种传感器来估计不同地图层的环境状态和商店占用值。这个例子展示了如何multiLayerMap
对象可用于可视化、调试和融合从安装在自动驾驶汽车上的三个激光雷达传感器收集的数据。本例中的传感器读数是使用一组lidarPointCloudGenerator
(自动驾驶工具箱)对象,从附带的对象中捕获读数drivingScenario
(自动驾驶工具箱)对象。
每个激光雷达更新自己的validatorOccupancyMap3D
对象,使我们能够可视化由每个传感器单独创建的本地映射。这些局部映射可用于快速识别噪声或安装错误的来源,并有助于选择合适的融合技术。的multiLayerMap
包含第四个mapLayer
对象,该对象使用自定义回调函数来融合每个占用层中包含的数据。最后,当自动驾驶汽车沿着预先规划的路径行驶时,使用融合地图更新世界地图的相应子区域。
负载驱动场景
首先,创建一个drivingScenario
对象,并使用示例helper函数用几个建筑物填充场景。该功能还可以可视化场景。
scene = drivingScenario;groundTruthVehicle =车辆(场景,“PlotColor”,[0 0.4470 0.7410]);添加道路和建筑物的场景和可视化。exampleHelperPopulateScene(场景,groundTruthVehicle);
生成一个轨迹,该轨迹跟随场景中的主要道路waypointTrajectory
对象。
sampleRate = 100;速度= 10;T = [0 20 25 44 46 50 54 56 59 63 90].';wayPoints = [0 0 0;2000 00;200 50 0;200 230 0;215 245 0;260 245 0;290 240 0; 310 258 0; 290 275 0; 260 260 0; -15 260 0]; velocities = [ speed 0 0; speed 0 0; 0 speed 0; 0 speed 0; speed 0 0; speed 0 0; speed 0 0; 0 speed 0; -speed 0 0; -speed 0 0; -speed 0 0]; traj = waypointTrajectory(wayPoints,“TimeOfArrival”t...“速度”,速度,“SampleRate”, sampleRate);
创建模拟激光雷达传感器
要从驾驶场景中收集激光雷达读数,请创建三个lidarPointcloudGenerator
对象,使用示例帮助函数。这辆车已经配置成有两个前置窄视场(FOV)激光雷达和一个单一的宽视场后方激光雷达。两个前置传感器的重叠区域应该有助于快速登记和确认车辆前方的空闲空间,而后置传感器范围有助于绘制穿越区域。
lidarSensors = exampleHelperCreateVehicleSensors(scene, groundTruthVehicle);disp (lidarSensors)
{1×1 lidarPointCloudGenerator} {1×1 lidarPointCloudGenerator} {1×1 lidarPointCloudGenerator}
初始化自我中心地图
创建一个multiLayerMap
对象由三个对象组成occupancyMap
对象和一个泛型mapLayer
对象。每一个地方occupancyMap
由相应的激光雷达传感器更新。将所有地图的数据合并到mapLayer
对象,设置GetTransformFcn
属性的名称-值参数exampleHelperFuseOnGet
函数存储为句柄fGet
.的exampleHelperFuseOnGet
函数将所有三个映射数据值融合在一起getMapData
对每个函数进行运算,并使用值的对数赔率求和。
定义映射和参数。Res = 2;宽度= 100*2;高度= 100*2;为所有传感器读数定义相同的权重。权重= [1 1 1];为每个传感器创建maplayer。fLeftLayer =占用地图(宽度,高度,分辨率,“LayerName”,“FrontLeft”);fRightLayer = occuancymap(宽度,高度,分辨率,“LayerName”,“FrontRight”);rearLayer = occuancymap(宽度,高度,分辨率,“LayerName”,“后”);创建一个用于融合三层数据的get回调。fGet = @(obj,values,varargin)...exampleHelperFuseOnGet (fLeftLayer fRightLayer rearLayer,...obj,权重值,变长度输入宗量{:});创建一个通用的mapLayer对象,其getMapData函数融合所有数据%三层。fusedLayer = mapLayer(宽度,高度,“决议”res,“LayerName”,“FuseLayer”,...“GetTransformFcn”fGet,“DefaultValue”, 0.5);将图层组合成multiLayerMap。egoMap = multiLayerMap({fLeftLayer, fRightLayer, rearLayer, fusedLayer});%设置地图网格原点,使机器人位于中心。egoMap。GridOriginInLocal = -[diff(egoMap.XLocalLimits) diff(egoMap.YLocalLimits)]/2;
创建重建地图
创建一个空的世界地图。该映射使用来自融合层的数据定期更新。用这张图来说明激光雷达融合方法的工作情况。
创建一个空白的重建层,覆盖与世界地图相同的区域。reconLayer = occuancymap (400,400,res,...%的宽度,高度,决议“LayerName”,“FuseLayer”,“LocalOriginInWorld”-50年[-25]);
设置可视化
在重建的地图旁边绘制以自我为中心的图层。使用exampleHelperShowEgoMap函数显示每个本地映射。
设置显示窗口。axList = exampleHelperSetupDisplay(groundTruthVehicle,lidarSensors);显示重构层和子映射区域。显示(reconLayer“父”, axList {1});hG = findobj(axList{1},“类型”,“hggroup”);自我取向= hg .儿童;egoCenter = hgtransform(“父”、hG);egoOrientation。Parent = egoCenter;gridLoc = egoMap.GridLocationInWorld;xLimits = egoMap.XLocalLimits;yLimits = egoMap.YLocalLimits;矩形(“父”egoCenter,...“位置”,[gridLoc diff(xLimits) diff(yLimits)],...“EdgeColor”,“r”);在重建图旁边显示每个传感器构建的局部地图。。exampleHelperShowEgoMap(axList,egoMap,[0 0 0],{“FrontLeft激光雷达”,“FrontRight激光雷达”,“后激光雷达”,“融合”});
模拟传感器读数和建立地图
沿着轨迹移动机器人,同时用模拟激光雷达读数更新地图。
要运行驾驶场景,请调用exampleHelperResetSimulation
helper函数。这将重置模拟和轨迹,清除地图,并将自我中心地图移回轨迹的第一个点。
exampleHelperResetSimulation(场景,traj lidarSensors、egoMap reconLayer)
调用exampleHelperRunSimulation
函数执行模拟。
仿真循环的主要操作有:
得到轨迹中的下一个姿态
traj
并从四元数中提取z轴方向(theta)。移动
egoMap
到新时代[x y theta]
姿势。中检索传感器数据
lidarPointCloudGenerators
.使用传感器数据更新本地地图
insertRay
.方法更新全局映射
mapLayer
融合的结果。刷新可视化。
exampleHelperRunSimulation(场景,traj groundTruthVehicle、egoMap lidarSensors, reconLayer, axList)
显示的结果表明,右前传感器正在引入大量的噪声到融合地图。注意,右壁在整个轨迹中有更多的变化。您不希望完全丢弃该传感器的读数,因为传感器仍在检测前方的空闲空间。相反,在融合过程中减少这些传感器读数的权重,并重新创建完整的多层地图。然后,重置并重新运行模拟。
用一组不同的融合权重构造一个新的multiLayerMapupdatedWeights = [1 0.25 1];egoMap = exampleHelperConstructMultiLayerEgoMap(res,width,height,updatedWeights);%重新运行模拟exampleHelperResetSimulation(场景,traj lidarSensors、egoMap reconLayer) exampleHelperRunSimulation(场景,traj, groundTruthVehicle、egoMap lidarSensors, reconLayer, axList)
在再次模拟之后,注意关于地图的一些事情:
只有噪声传感器覆盖的区域仍然可以检测到很少噪声的自由空间。
当噪声仍然存在时,来自其他传感器的读数超过来自噪声传感器的读数。地图显示了传感器重叠区域的明显障碍边界(黑色方块)。
边界以外的噪声仍然存在,因为噪声激光雷达是唯一报告这些区域读数的传感器,但不连接到其他自由空间。
下一个步骤
这个例子展示了如何融合读数的简单方法。您可以根据以下建议进一步定制此融合: