clc;clear;close all;
img=imread('map4.bmp');
mapgray=im2gray(img);
map=im2bw(img,0.5);
startPoint=[10 10];%出发点
endPoint=[490 490];%目标点
range=20;%距离阈值
k1=45;%传统PRM算法的随机采样点数
k2=90;%传统PRM算法的随机采样点数
n=100;%重复实验次数
fprintf("对比性100次重复试验结果:\n")
%传统PRM算法重复性实验k=45
successPath = 0; % 总成功次数
pathLength = 0; % 总的路径长度
totTime = 0; % 总的规划时间
for i=1:n
tic;
drop=[startPoint;endPoint];
for i=1:k1
x=double(int32(rand(1,2) .* size(map)));
if feasiblePoint(x,map)
drop=[drop;x];
end
end
%构建无向路径图
edges=undirectedpath(drop,map);
%A*算法搜寻路径
[pathFound,len,path]=astart(startPoint,endPoint,drop,edges);
%统计成功次数、路径长度
if pathFound
successPath = successPath + 1;
pathLength = pathLength + len;
totTime = totTime + toc;
end
end
successRate = 100 * successPath / n; % 成功率
avgPathLength = pathLength / successPath; % 平均路径长度
avgPlanTime = totTime / successPath; % 平均规划时间
fprintf('传统PRM算法 ')
fprintf('成功次数:%-3d 成功率:%3.0f%% 平均路径长度:%f 平均规划时间:%f\n', ...
successPath, successRate, avgPathLength, avgPlanTime);
%传统PRM算法重复性实验k=45
successPath = 0; % 总成功次数
pathLength = 0; % 总的路径长度
totTime = 0; % 总的规划时间
for i=1:n
tic;
drop=[startPoint;endPoint];
for i=1:k2
x=double(int32(rand(1,2) .* size(map)));
if feasiblePoint(x,map)
drop=[drop;x];
end
end
%构建无向路径图
edges=undirectedpath(drop,map);
%A*算法搜寻路径
[pathFound,len,path]=astart(startPoint,endPoint,drop,edges);
%统计成功次数、路径长度
if pathFound
successPath = successPath + 1;
pathLength = pathLength + len;
totTime = totTime + toc;
end
end
successRate = 100 * successPath / n; % 成功率
avgPathLength = pathLength / successPath; % 平均路径长度
avgPlanTime = totTime / successPath; % 平均规划时间
fprintf('传统PRM算法 ')
fprintf('成功次数:%-3d 成功率:%3.0f%% 平均路径长度:%f 平均规划时间:%f\n', ...
successPath, successRate, avgPathLength, avgPlanTime);
%基于角点的PRM算法重复性实验
successPath = 0; % 总成功次数
pathLength = 0; % 总的路径长度
totTime = 0; % 总的规划时间
tic;
for i=1:n
tic;
drop=[startPoint;endPoint];
%%角点检测
H = corner(mapgray,'MinimumEigenvalue',45);
%删除距离较近的角点
newH = deletePoints(H, range);
%产生可行节点
drop=cornerPoint(drop,newH,range,map);
%构建无向路径图
edges=undirectedpath(drop,map);
%A*算法搜寻路径
[pathFound,len,path]=astart(startPoint,endPoint,drop,edges);
%统计成功次数、路径长度
if pathFound
successPath = successPath + 1;
pathLength = pathLength + len;
totTime = totTime + toc;
end
end
successRate = 100 * successPath / n; % 成功率
avgPathLength = pathLength / successPath; % 平均路径长度
avgPlanTime = totTime / successPath; % 平均规划时间
fprintf('基于角点的PRM算法 ')
fprintf('成功次数:%-3d 成功率:%3.0f%% 平均路径长度:%f 平均规划时间:%f\n', ...
successPath, successRate, avgPathLength, avgPlanTime);