基于PRM的路径规划算法实战 matble实现

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);