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

介绍

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

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

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

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

导入的地图是:simpleMapcomplexMapternaryMap这将在变量名称中搜索包含字符串'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;

设置NumNodesConnectionDistance属性

 

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;

设置NumNodesConnectionDistance属性

 

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解决方案

show(prm)

 

posted @ 2018-06-23 17:53  jingsupo  阅读(2477)  评论(0编辑  收藏
我要啦免费统计