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

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

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

whos *Map*
名称大小字节类属性

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

map = robotics.BinaryOccupancyGrid(simpleMap, 2)
 map = 具有属性的 BinaryOccupancyGrid ：GridSize：[26 27] 分辨率：2 XWorldLimits：[0 13.5000] YWorldLimits：[0 13] GridLocationInWorld：[0 0] 

show(map)

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

robotRadius = 0.2;

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

show(mapInflated)

### 构建PRM并设置参数

prm = robotics.PRM
 prm = 具有属性的PRM： 映射：[0x1 robotics.BinaryOccupancyGrid] NumNodes：50 ConnectionDistance：Inf 

prm.Map = mapInflated;

prm.NumNodes = 50;

prm.ConnectionDistance = 5;

### 找到构建PRM的可行路径

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

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 

show(prm)

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

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.Map = mapInflated;

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

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 

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.Map = mapInflated;

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 

show(prm)

posted @ 2018-06-23 17:53  jingsupo  阅读(2477)  评论(0编辑  收藏