不同复杂环境下的路径规划

佚名 6年前 (2018-06-23) 人工智能 1824人围观抢沙发百度已收录

介绍

此示例演示如何使用概率路线图（PRM）路径规划器来计算给定地图上两个位置之间的无障碍路径。PRM路径规划器使用自由空间中的随机采样节点并将它们相互连接，从而在给定地图的自由空间中构建路线图。路线图构建完成后，您可以查询从给定开始位置到地图上给定结束位置的路径。

在这个例子中，地图被表示为使用导入数据的占用网格地图。在对地图的空闲空间中的节点进行采样时，PRM使用此二进制占用网格表示来推断空闲空间。此外，PRM在计算地图上的无障碍路径时没有考虑机器人尺寸。因此，您应该根据机器人的尺寸对地图进行充气，以便计算无障碍路径，从而解决机器人的尺寸问题，并确保避免实际机器人的碰撞。定义地图上的开始和结束位置，以便PRM路径规划器查找无障碍路径。

SRE实战互联网时代守护先锋，助力企业售后服务体系运筹帷幄！一键直达领取阿里云限量特价优惠。

导入用于规划路径的示例地图

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

导入的地图是：simpleMap，complexMap和ternaryMap。这将在变量名称中搜索包含字符串'Map'的变量。

whos *Map*

名称大小字节类属性

  complexMap 41x52 2132逻辑               
  simpleMap 26x27 702逻辑               
  三元图501x501 2008008 double

使用导入的simpleMap数据并使用robotics.BinaryOccupancyGrid该类构建占位网格表示。这张地图的分辨率设置为每米2格。

map = robotics.BinaryOccupancyGrid(simpleMap, 2)

map = 

  具有属性的
 BinaryOccupancyGrid ：GridSize：[26 27] 
             分辨率：2 
           XWorldLimits：[0 13.5000] 
           YWorldLimits：[0 13] 
    GridLocationInWorld：[0 0]

使用对象show上的函数显示地图robotics.BinaryOccupancyGrid

show(map)

定义机器人尺寸并填充地图

为确保机器人不与任何障碍物碰撞，应在将机器人提供给PRM路径规划器之前，按照机器人的尺寸对地图充气。

这里机器人的尺寸可以假定为半径为0.2米的圆。然后，您可以使用该inflate函数通过此维度膨胀地图。

robotRadius = 0.2;

如前所述，PRM不考虑机器人的尺寸，因此为PRM提供膨胀的地图考虑了机器人尺寸。在使用该inflate功能保留原始地图之前，创建地图的副本。

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

显示膨胀的地图

show(mapInflated)

构建PRM并设置参数

现在你需要定义一个路径规划器。创建一个robotics.PRM对象并定义关联的属性。

prm = robotics.PRM

prm = 

  具有属性的PRM：

                   映射：[0x1 robotics.BinaryOccupancyGrid] 
              NumNodes：50 
    ConnectionDistance：Inf

将充气地图分配给PRM对象

prm.Map = mapInflated;

定义在PRM构建过程中要使用的PRM节点的数量。PRM使用给定地图上给定数量的节点构建路线图。根据输入地图的维度和复杂性，这是要在地图上的两个点之间获得解决方案的主要属性之一。大量节点创建密集的路线图并增加找到路径的可能性。但是，拥有更多节点会增加创建路线图和寻找解决方案的计算时间。

prm.NumNodes = 50;

定义地图上两个连接节点之间允许的最大距离。PRM连接地图上以此距离（或更少）分隔的所有节点。这是调整较大和/或复杂输入地图的另一个属性。较大的连接距离会增加节点之间的连通性，从而更容易找到路径，但可以增加路线图创建的计算时间。

prm.ConnectionDistance = 5;

找到构建PRM的可行路径

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

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

使用该findpath功能搜索开始和结束位置之间的路径。解决方案是从开始位置到结束位置的一组路标。请注意，path由于PRM算法的概率性质，这将会有所不同。

path = findpath(prm, startLocation, endLocation)

路径= 

    2.0000 1.0000 
    1.9569 1.0546 
    1.8369 2.3856 
    3.2389 6.6106 
    7.8260 8.1330 
   11.4632 10.5857 
   12.0000 10.0000

显示PRM解决方案。

show(prm)

将PRM用于大型和复杂的地图

使用导入的complexMap数据代表一个庞大而复杂的平面图，并以给定的分辨率构建二进制占用网格表示（每米1个单元）

map = robotics.BinaryOccupancyGrid(complexMap, 1)

map = 

  具有属性的
 BinaryOccupancyGrid ：GridSize：[41 52] 
             解析：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;

设置NumNodes和ConnectionDistance属性

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

显示PRM图

show(prm)

找到构建PRM的可行路径

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

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

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

path = findpath(prm, startLocation, endLocation)

path = 

     []

由于您正在规划大型复杂地图上的路径，因此可能需要大量节点。但是，通常不清楚有多少节点就足够了。调整节点的数量以确保开始和结束位置之间有可行的路径。

while isempty(path)
    % No feasible path found yet, increase the number of nodes
    prm.NumNodes = prm.NumNodes + 10;

    % Use the |update| function to re-create the PRM roadmap with the changed
    % attribute
    update(prm);

    % Search for a feasible path with the updated PRM
    path = findpath(prm, startLocation, endLocation);
end
% Display 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)

使用PRM与概率占用网格

robotics.OccupancyGrid使用导入的ternaryMap数据构造一个对象。的ternaryMap使用概率，其中占用的自由空间中的概率是0，对于占用空间是1和用于未知空间为0.5表示的环境。这里使用每米20个单元的分辨率。

map = robotics.OccupancyGrid(ternaryMap, 20)

map = 

  OccupancyGrid属性：占有

        阈值：0.6500 
            FreeThreshold：0.2000  概率饱和度：[0.0010 0.9990] 
                 GridSize：[501 501] 
               分辨率：20 
             XWorldLimits：[0 25.0500] 
             YWorldLimits：[0 25.0500] 
      GridLocationInWorld：[0 0]

显示地图

show(map)

基于机器人尺寸填充地图

复制并膨胀地图以考虑机器人的尺寸以避开障碍物

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

显示膨胀的地图

show(mapInflated)

将现有的PRM对象与新的映射关联并设置参数

用新膨胀的地图更新PRM对象并定义其他属性。PRM使用FreeThresholdOccupancyGrid对象来确定无障碍空间并计算该无障碍空间内的路径。未知单元格的值ternaryMap是0.5，而FreeThresholdOccupancyGrid对象上的默认值mapInflated是0.2。因此，PRM不会在未知地区规划一条路径。

prm.Map = mapInflated;

设置NumNodes和ConnectionDistance属性

prm.NumNodes = 60;
prm.ConnectionDistance = 5;

% Display PRM graph
show(prm)

找到构建PRM的可行路径

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

startLocation = [7 22];
endLocation = [15 5];

% Search for a solution between start and end location.
path = findpath(prm, startLocation, endLocation);
while isempty(path)
    prm.NumNodes = prm.NumNodes + 10;
    update(prm);
    path = findpath(prm, startLocation, endLocation);
end

% Display path
path

路径= 

    7.0000 22.0000 
    7.9059 22.1722 
   10.6881 22.3734 
   11.7508 19.3716 
   13.7982 17.1659 
   17.5826 14.6769 
   16.8227 9.9964 
   15.1329 8.3100 
   14.9489 5.7648 
   15.0000 5.0000

显示PRM解决方案