ROS与Matlab语言入门教程-在复杂环境中进行路径规划

本例程演示如何使用PRM(Probabilistic Roadmap)路径规划器在给定地图计算两个位置之间的无障碍路径。PRM路径规划器通过在自由空间随机采样点并让他们互相建立连接，以在给定地图的自由空间构建路线图。一旦构建路线图，用户可以在地图上查询给定起点和重点的路径。

在本例程中，地图使用导入二进制数据以占有网格的形式描述，当在地图上的自由空间采样点时，PRM使用该二进制占有网格描述以推断自由空间。此外，PRM在计算地图上的无障碍路径时并不考虑机器人的尺寸。因此，为了允许计算无障碍路径时考虑机器人的尺寸和确保真实机器人避免碰撞，用户需要使用机器人的尺寸膨胀地图。为PRM路径规划器定义地图上的起始和停止位置，以寻找一条无障碍路径。

导入示例地图以规划路径

filePath = fullfile(fileparts(which('PathPlanningExample')),'data','exampleMaps.mat');
load(filePath)

导入的地图是：“simpleMap”和“complexMap”。
下面的指令搜索变量名中包含“Map”的变量。（及其输出结果）

whos Map
Name Size Bytes Class Attributes
complexMap 41x52 2132 logical
simpleMap 26x27 702 logical

通过“robotics.BinaryOccupancyGrid”类使用导入的简单地图并构建占有网格描述。为该地图设置分辨率为每米两个网格。

map = robotics.BinaryOccupancyGrid(simpleMap, 2)
map =
BinaryOccupancyGrid with properties:
GridSize: [26 27]
Resolution: 2
XWorldLimits: [0 13.5000]
YWorldLimits: [0 13]
GridLocationInWorld: [0 0]

使用“show”函数显示地图于“robotics.BinaryOccupancyGrid”对象。

show(map)

定义机器人尺寸和膨胀地图

为确保机器人不会碰撞到任何障碍物，用户需要在应用地图到PRM路径规划器之前，使用机器人的尺寸膨胀地图。

此处，机器人的尺寸可以假设为直径为0.2米的圆，那么用户可以使用“robotics.BinaryOccupancyGrid.inflate”函数，使用该尺寸膨胀地图。

robotRadius = 0.2;

正如之前描述，PRM不会考虑机器人的尺寸，因此考虑了机器的尺寸给PRM提供一个膨胀的地图。使用“inflate”函数之前，创建一个地图拷贝以保留原地图。

mapInflated = copy(map);
inflate(mapInflated,robotRadius);

显示膨胀的地图。

show(mapInflated)

构建PRM和设置参数

现在，用户需要定义一个路径规划器。创建一个“robotics.PRM”对象并定义关联属性。

prm = robotics.PRM

将膨胀的地图赋给PRM对象：

prm.Map = mapInflated;

定义PRM构建过程中使用的PRM点的数目，PRM使用给定地图上的给定数目节点构建路线路。基于机器人的尺寸和输入地图的复杂性，这是最基本的属性之一，为了获得地图上两点之间的解决方案。大量的节点创建一个稠密的路线图并增加发现路径的可能性。然而，拥有更多的节点增加创建路线路和找到解决方案的计算时间。

prm.NumNodes = 50;

定义地图上两个节点之间允许的最大距离，PRM通过该距离（更短）连接地图上所有分立的节点，这是另一个调整的属性，在更大或复杂的输入地图的情况下。一个大的连接距离两个节点之间的连接性以更容易的寻找路径，但是却也增加了路线图创建的计算时间。

prm.ConnectionDistance = 5;

在构建的PRM寻找可行的路径

定义地图上的开始和结束位置给路径规划器使用。

startLocation = [2 1];
endLocation = [12 10];

使用“robotics.PRM.findpath”函数寻找开始和结束位置的路径，解决方案是从开始位置到结束位置的一系列的陆标。注意，由于PRM算法的随机性，“path”将会不一样。

path = findpath(prm, startLocation, endLocation)
path =
2.0000 1.0000
1.9569 1.0546
3.2389 6.6106
11.4632 10.5857
12.0000 10.0000

显示PRM解决方案。

show(prm)

使用PRM为一个庞大而复杂的地图

使用导入的复杂地图，该地图描述了一个大范围复杂的地板平面图，构建了一个给定分辨率（每米一个网格）二进制占有网格描述。

map = robotics.BinaryOccupancyGrid(complexMap, 1)
map =
BinaryOccupancyGrid with properties:
GridSize: [41 52]
Resolution: 1
XWorldLimits: [0 52]
YWorldLimits: [0 41]
GridLocationInWorld: [0 0]

显示地图：

show(map)

根据机器人尺寸膨胀地图

复制并考虑机器人的尺寸因素膨胀地图以壁障。

mapInflated = copy(map);
inflate(mapInflated, robotRadius);

显示膨胀的地图：

show(mapInflated)

关联已有PRM对象和新的地图并设置参数

用新膨胀的地图更新PRM属性并定义其它属性：

prm.Map = mapInflated;

设置“”和“”的属性

prm.NumNodes = 20;
prm.ConnectionDistance = 15;

显示PRM图：

show(prm)

在构建的PRM寻找可行的路径

定义地图上的开始和结束位置以寻找无障碍路径。

startLocation = [3 3];
endLocation = [45 35];

在开始位置和结束位置之间寻找解决方案，对于复杂的地图，对于给定节点数量，可能没有可行的路径（返回空路径）。

path = findpath(prm, startLocation, endLocation)
path =
[]

用于此时用户在大范围且复杂的地图上规划路径，可能需要更大数量的节点。然而，经常不清楚需要多少节点才足够。调整节点的数量以确保在开始和结束位置之间存在一条可行的路径。

path =
3.0000 3.0000
4.2287 4.2628
7.7686 5.6520
6.8570 8.2389
19.5613 8.4030
33.1838 8.7614
31.3248 16.3874
41.3317 17.5090
48.3017 25.8527
49.4926 36.8804
44.3936 34.8592
45.0000 35.0000

显示PRM解决方案：

show(prm)