主要内容

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

打开实时脚本

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

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

加载一个预先构建的Ground-Truth占用地图

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

创建一个binaryOccupancyMap对象的二进制矩阵，并指定映射的分辨率。

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

将地图左下角的位置设置为世界原点。

worldMap。LocalOriginInWorld = mapData.GridLocationInWorld;

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

集(gcf,“可见”，“上”) worlddax = subplot(1,2,1);worldHandle = show(worldMap，“父”, worldAx);持有所有

创建一个模拟激光雷达

创建一个rangeSensor对象，该对象可用于从模拟中收集激光雷达读数。属性上的各种属性rangeSensor为了更准确地表示激光雷达的特定模型，或添加传感器噪声来测试解决方案的鲁棒性。对于本例，设置(最小最大)范围和噪声参数。创建对象后，通过提供[x y theta]姿态相对于世界框架。示例助手绘制机器人，激光雷达读数超过worldMap．

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

{

初始化一个自我中心地图

创建一个occupancyMap对象表示以自我为中心的映射。将local-origin设置为网格的中心位置。将网格原点移动边界宽度的一半。

%缺省情况下，GridOriginInLocal = [0 0]egoMap = occuancymap (10,10,worldMap.Resolution);%偏移GridOriginInLocal，使“本地帧”位于“地图窗口”的%中心egoMap。GridOriginInLocal = -[diff(egoMap.XWorldLimits) diff(egoMap.YWorldLimits)]/2;

在子图中，把自我中心地图画在世界地图旁边。

%更新本地绘图localAx = subplot(1,2,2);显示(egoMap“父”, localAx);持有所有localMapFig = plot(localAx,egoMap。LocalOriginInWorld+[0 1]， egoMap。LocalOriginInWorld + [0 0),的r -，“线宽”3);

规划点间路径

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

复制世界地图并充气。binaryMap = binaryoccuancymap (worldMap);充气(binaryMap, 0.1);创建一个状态空间对象。stateSpace = stateSpaceDubins;减小转弯半径，以更好地适应地图和障碍物的大小%的密度。stateSpace。MinTurningRadius = 0.5;创建一个状态验证器对象。validator = validatorOccupancyMap(stateSpace);验证器。Map = binaryMap;验证器。ValidationDistance = 0.1;

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

使用之前创建的StateSpace和创建我们的计划% StateValidator对象。planner = plannerRRTStar(stateSpace，验证器);计划。MaxConnectionDistance = 2;计划。MaxIterations = 20000;为可重现结果的随机生成路径设置种子。rng (1,“旋风”）设置起始点和结束点。startPt = [-6 -5 0];goalPt = [8 7 pi/2];规划起点和目标点之间的路径。。path = plan(planner, startPt, goalPt);插入(路径,大小(path.States 1) * 10);情节(worldAx path.States (: 1), path.States (:, 2),“b -”）;

沿着路径生成一个轨迹

计划器生成了一组状态，但是要执行轨迹，则需要为这些状态设置时间。本例的目标是以恒定的线速度沿路径移动机器人0．5m / s。为了得到每个点的时间戳，计算点与点之间的距离，把它们累加起来，然后除以线速度，得到一个单调递增的时间戳数组，tStamps．

获取每个路点之间的距离pt2ptDist =距离(stateSpace,path.States(1:end-1，:)，path.States(2:end，:)))
pt2ptDist =129×10.2000 0.2000 0.2000 0.2000 0.2000 0.2000 0.2000
linVel = 0.5;% m / stStamps = cumsum(pt2ptDist)/linVel;

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

traj = waypointTrajectory(路径。州,[0;tStamps),“SampleRate”10);

模拟传感器读数并绘制地图

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

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

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

仿真循环的主要操作有:

  • 得到轨迹中的下一个姿态traj并从四元数中提取z轴方向(theta)。

  • 移动egoMap到新的[x y]姿势。

  • 从激光雷达中检索传感器数据。

  • 使用传感器数据更新本地地图insertRay．

  • 刷新可视化。

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

而~结束(traj)%沿轨迹增加机器人[pts, quat] = step(traj);从四元数获取方向角rotMatrix = rotmat(quat，“点”）;orientZ = rotm2eul(rotMatrix);移动机器人到新的位置robotCurrentPose = [pts(1:2) orientZ(1)];移动(egoMap robotCurrentPose (1:2),“MoveType”，“绝对”）;从激光雷达中检索传感器信息并将其插入到egoMap中。[range, angles] = lidar(robotCurrentPose, worldMap);insertRay (egoMap robotCurrentPose、范围、角度、lidar.Range (2));更新以egomap为中心的图形显示(egoMap“父”localAx,“FastUpdate”1);更新方向向量集(localMapFig,“XData”， robotCurrentPose(1)+[0 cos(robotCurrentPose(3))]，“YData”， robotCurrentPose(2)+[0 sin(robotCurrentPose(3))]);%更新世界图exampleHelperUpdateRobotAndLidar(hRobot, hSensorData, robotCurrentPose，范围，角度);调用drawnow将更新推到图形drawnowlimitrate结束

{