Robotics System Toolbox路径规划

使用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)