使用Robotics System Toolbox工具箱在已知环境中规划路径
%% 导入环境数据
filePath = fullfile(fileparts(which('PathPlanningExample')),'data','exampleMaps.mat');
load(filePath)
%%
% 将环境数据读入工具箱
map = robotics.BinaryOccupancyGrid(simpleMap, 2)
%%
% 显示环境
show(map)
%% 将机器人转化为质点需要考虑的形态参数
robotRadius = 0.2;
%%
% 环境中的障碍转化
mapInflated = copy(map);
inflate(mapInflated,robotRadius);
%%
% 显示转化后的环境
show(mapInflated)
%% 定义规划器
prm = robotics.PRM
%%
% 读入转化后的环境
prm.Map = mapInflated;
%%
% 设置规划器参数
prm.NumNodes = 50;
prm.ConnectionDistance = 5;
%% 设置起点终点
startLocation = [2 1];
endLocation = [12 10];
%%
% 路径搜索
path = findpath(prm, startLocation, endLocation)
%%
% 显示路径结果
show(prm)
%% 复杂环境情况
map = robotics.BinaryOccupancyGrid(complexMap, 1)
%%
show(map)
%%
mapInflated = copy(map);
inflate(mapInflated, robotRadius);
%%
show(mapInflated)
%%
prm.Map = mapInflated;
%%
prm.NumNodes = 20;
prm.ConnectionDistance = 15;
%%
show(prm)
%%
startLocation = [3 3];
endLocation = [45 35];
%%
path = findpath(prm, startLocation, endLocation)
%% 加入循环直到搜索到合适路径
while isempty(path)
% 不合适就增加节点
prm.NumNodes = prm.NumNodes + 10;
% 更新规划器
update(prm);
% 搜索
path = findpath(prm, startLocation, endLocation);
end
% 显示路径
path
show(prm)
%% 真实环境情况
map = robotics.OccupancyGrid(ternaryMap, 20)
%%
show(map)
%%
mapInflated = copy(map);
inflate(mapInflated, robotRadius);
%%
show(mapInflated)
%%
prm.Map = mapInflated;
prm.NumNodes = 60;
prm.ConnectionDistance = 5;
%
show(prm)
%%
startLocation = [7 22];
endLocation = [15 5];
% 同样加入循环
path = findpath(prm, startLocation, endLocation);
while isempty(path)
prm.NumNodes = prm.NumNodes + 10;
update(prm);
path = findpath(prm, startLocation, endLocation);
end
% 显示结果
path
show(prm)