使用距离传感器创建以自我为中心的占用地图

打开实时脚本

占用地图通过将连续的世界空间映射到离散的数据结构，为机器人应用提供了一种简单而健壮的环境表示方法网状细胞可以包含二进制或概率信息，其中0表示空闲空间，1表示已占用空间。随着时间的推移，您可以使用传感器测量来建立这些信息，并有效地将它们存储在地图中。这些信息对于更高级的工作流程也很有用，比如碰撞检测和路径规划。

这个例子展示了如何创建一个以自我为中心占用地图，用于跟踪机器人周围环境，并可在环境中高效移动。通过在环境中规划路径生成轨迹，并指示机器人的运动。当机器人四处移动时，地图将使用来自模拟激光雷达和地面真实地图的传感器信息进行更新。

加载一个预建的地面真实占用地图

从先前生成的数据文件创建非自我中心地图，该文件被视为模拟激光雷达的地面真相。加载地图，地图数据，其中包含数据字段作为概率矩阵，并将其转换为二进制值。

创建一个binaryOccupancyMap对象，并指定映射的分辨率。

%加载保存的地图信息负载mapData_rayTracingTrajectorybinaryMatrix = mapData。数据> 0.5;worldMap = binaryOccupancyMap (binaryMatrix mapData.Resolution);

设置地图左下角的位置，相对于世界的原点。

worldMap.LocalOriginInWorld=mapData.GridLocationInWorld；

绘制地面真相。本例设置了一个子图，用于并排显示两张地图。

集(gcf,“可见的”，“上”)worldAx=子批次（1,2,1）；worldHandle=show（世界地图，“家长”，worldAx）；暂停全部的

创建一个模拟激光雷达

创建一个范围传感器对象，可用于从模拟中收集激光雷达读数。您可以在范围传感器更准确地表示特定型号的激光雷达，或添加传感器噪声来测试解决方案的稳健性。对于本例，设置(最小最大)范围和噪声参数。创建对象后，通过提供(x yθ)相对于世界框架的姿势。示例助手绘制机器人，激光雷达读数高于世界地图。

%创建范围传感器激光雷达= rangeSensor;激光雷达。范围= [0 5];激光雷达。RangeNoise = 0;Pos = [0 0 0];%在世界地图上显示激光雷达读数[范围，角度]=激光雷达(pos, worldMap);hSensorData = exampleHelperPlotLidar(worlddax, pos，范围，角度);%在世界地图中显示机器人hRobot = exampleHelperPlotRobot(worlddax, pos);

初始化自中心映射

创建一个occupancyMap对象表示以自我为中心的映射。通过设置网格原点相对于局部原点偏移GridOriginInLocal财产到一半的差别是世界的极限。这将改变地图的边界，使局部原点居中。

%默认情况下，GridOriginLocal=[0]电子地图=职业地图（10,10，worldMap.Resolution）；%偏移GridOriginInLocal，使“本地帧”位于地图窗口中心%egoMap。GridOriginInLocal = -[diff(egoMap.XWorldLimits) diff(egoMap.YWorldLimits)]/2;

将以自我为中心的地图放在副情节中的世界地图旁边。

%更新局部图localAx =情节(1、2、2);显示(egoMap“家长”，localAx）；持有全部的localMapFig=plot（localAx，egoMap.LocalOriginInWorld+[0 1]，egoMap.LocalOriginInWorld+[0 0]，的r -，“线宽”,3);

规划点间路径

我们现在可以使用地面真实地图来规划两个自由点之间的路径。创建一个世界地图的副本，并根据机器人的大小和需要的间隙膨胀它。这个例子使用了一个类似汽车的机器人，它具有非完整的约束条件，由stateSpaceDubins对象。路径规划器使用该状态空间对机器人的可行状态进行随机采样。最后，创建一个职业地图对象，它使用贴图通过检查相应单元格的占用情况来验证生成的状态以及连接这些状态的运动。

%复制世界地图并将其充气。binaryMap=BinaryOccupencyMap（世界地图）；充气（二进制映射，0.1）；%创建状态空间对象。stateSpace = stateSpaceDubins;减小转弯半径以更好地适应地图和障碍物的大小%密度。stateSpace.MinTurningRadius=0.5；创建状态验证器对象。validator=validatorockincymap（stateSpace）；validator.Map=binaryMap；validator.ValidationDistance=0.1；

使用RRT*规划算法，作为一个plannerRRTStar对象，并将状态空间和状态验证器指定为输入。为规划器指定开始和结束位置并生成路径。

%使用先前创建的StateSpace和StateValidator对象创建规划器planner=PlannerRTStar（状态空间，验证器）；planner.MaxConnectionDistance=2；planner.MaxIterations=20000；%为随机生成的路径设置种子，以获得可重复的结果。rng (1,“旋风”）%设置开始和结束点。起始点=[-6-50]；目标点=[8 7 pi/2]；%在起点和终点之间规划一条路径。路径=计划（规划器、起始点、目标点）；插值（路径、大小（路径状态，1）*10）；绘图（worldAx、路径状态（：，1）、路径状态（：，2），“b -”）;

生成沿着路径的轨迹

规划器生成一组状态，但是要执行一个轨迹，则需要状态的时间。这个例子的目标是让机器人以恒定的线速度沿路径移动０．５m / s。要获得每个点的时间戳，计算点之间的距离，将它们相加，然后除以线速度，得到单调递增的时间戳数组tStamps。

%获取每个航路点之间的距离pt2ptDist =距离(stateSpace path.States (1: end-1,:), path.States(2:最终,:))

pt2ptDist=129×10.2000 0.2000 0.2000 0.2000 0.2000 0.2000 0.2000 0.2000 0.2000 0.2000 ⋮

linVel = 0.5;% m / stStamps = cumsum (pt2ptDist) / linVel;

生成一个最终的模拟轨迹航路点轨迹，指定路径状态、相应的时间戳和10Hz的期望采样率。

traj=航路点轨迹（路径状态，[0；tStamps]，“SampleRate”10);

模拟传感器读数并构建地图

最后，沿着轨迹移动机器人，同时用模拟激光雷达读数更新地图。

要初始化模拟，请重置轨迹，将局部原点设置为轨迹上的第一个xy点，然后清除贴图。

重置(traj);: robotCurrentPose = path.States (1);移动(egoMap robotCurrentPose (1:2));setOccupancy (egoMap repmat (egoMap.DefaultValue egoMap.GridSize));

模拟回路的主要操作包括：

在这个轨迹上做下一个姿势特拉伊并从四元数中提取z轴方向。
移动egoMap变成新的[xy]姿势。
从激光雷达检索传感器数据。
使用传感器数据更新本地地图插入射线．
刷新可视化。

虽然~isDone（traj）%机器人沿轨迹递增[pts, quat] = step(traj);%从四元数获取方向角rotMatrix = rotmat(皮疹,“点”）;orientZ = rotm2eul (rotMatrix);%将机器人移动到新位置机器人当前姿势=[pts（1:2）方向Z（1）]；移动（电子地图，机器人当前姿势（1:2），“移动类型”，“绝对的”）;从激光雷达获取传感器信息并将其插入到egoMap中[范围、角度]=激光雷达（机器人当前姿态、世界地图）；插入射线（电子地图、机器人当前姿态、范围、角度、激光雷达范围（2））；%更新以地图为中心的绘图显示(egoMap“家长”localAx,“快速更新”, 1);%更新方向向量集(localMapFig,“XData”, robotCurrentPose (1) + [0 cos (robotCurrentPose (3))),“伊达塔”robotCurrentPose(2) +[0罪(robotCurrentPose (3))));%更新世界图例如HelperUpdateRobot和激光雷达（机器人、传感器数据、机器人当前姿态、范围、角度）；%调用drawnow将更新推到图形刷新屏幕限制结束

请注意，机器人在环境中行驶，模拟传感器读数，并在运行过程中构建占用地图。