基于PRM(probabilistic roadmaps)算法的机器人路线规划算法matlab仿真
目录
一、理论基础
地图和机器人的模型如下：
1.使用一个2*2的网格大小(gridsize)和5度的角分辨率(angular resolution)，创建机器人的构型空间(Configurationspace)。请简单说明，并输出构型空间的视图。
机器人的初始状态：坐标(0,45)，90度
2.使用谈心搜索算法计算从(50,50)到(750, 250)的最短路径。请在图中标明并且输出最短路径的长度。
3.使用中轴变换计算最安全的路径(最安全的路径是指离墙最远的路径)。请在图中标明并且输出最短路径的长度。
4.使用PRM（probabilistic roadmaps）算法计算从(50,50)到(750, 250)的最短路径。分别使用50、100、500个样本点。请在图中标明并输出这些路径的长度。
使用一个2*2的网格大小(grid size)和5度的角分辨率(angular resolution)，创建机器人的构型空间(Configuration space)。请简单说明，并输出构型空间的视图。
机器人的初始状态：坐标(0,45)，90度
这里，如果按1*1的方格，算法将及其复杂，数据量极大，我们这里将网格进行分割。300*800的空间，分割为10*10一个方格。
PRM算法是通过对空间进行大量采样来构建路线图，用于后续的特定查询。路线图是无向连接图，车辆或机器人可以路线图上由任意一点移动到其他点。点与点连接线最简单的就是直线。路线图构建好之后可以采用经典的A*算法来搜索路径。PRM算法构建路线图过程如下所示。
二、MATLAB仿真程序
clc;
clear;
close all;
warning off;
addpath 'func\'
%转弯分辨率
ang = 5/180*pi;
W = 800;
H = 300;
K = 10;
Scale = max(W,H)/K;
[MAPs,Start,Ends,cc,MAPpoint] = func_wall(Scale,K);
%显示方格场景图
func_Map2fig(MAPs,Start,Ends,H/K,W,H,K);
S1 = Start;
S2 = 0;
S3 = inf;
S4 = [];
S5 = [];
Paths = ['R','L','D','U'];
%开始贪心算法搜索，
while ~max(ismember(S1,Ends))&&~isempty(S1)
[temp,kj] = min(S2+S3);
%搜索路径值用来判断往哪走
[path1,path2,path3] = func_search(S1(kj),S2(kj),MAPs,Ends);
S4 = [S4;S1(kj)];
S5 = [S5;S2(kj)];
%判决
if kj>1&&kj<length(S1)
S1=[S1(1:kj-1);S1(kj+1:end)];
S2=[S2(1:kj-1);S2(kj+1:end)];
S3=[S3(1:kj-1);S3(kj+1:end)];
else
S1=[S1(kj+1:end)];
S2=[S2(kj+1:end)];
S3=[S3(kj+1:end)];
end
for jj=1:length(path3)
if ~isinf(path1(jj))
if ~max([S1;S4]==path3(jj))
MAPpoint{path3(jj)}=Paths(jj);
S1 = [S1; path3(jj)];
S2 = [S2; path1(jj)];
S3 = [S3; path2(jj)];
elseif max(S1==path3(jj))
i=find(S1==path3(jj));
if S2(i)>path1(jj)
S2(i)=path1(jj);
S3(i)=path2(jj);
MAPpoint{S1(i)}=Paths(jj);
end
else i=find(S4==path3(jj));
if S5(i)>path1(jj)
S5(i)=path1(jj);
MAPpoint{S4(i)}=Paths(jj);
end
end
end
end
if isempty(S1)
break;
end
end
Pathss=func_check(Ends,MAPpoint);
figure(1);
plot(Pathss(:,1)+0.5,Pathss(:,2)+0.5-(W-H)/K,'color',[0 1 0],'LineWidth',2);
title('贪婪算法');
X=Pathss(:,1)+0.5;
Y=Pathss(:,2)+0.5-(W-H)/K;
%输出长度
d=0;
for i = 1:length(X)-1
d = d+K*sqrt((X(i)-X(i+1))^2 + (Y(i)-Y(i+1))^2);
end
d
三、仿真结果
A16-73
