基于PRM(probabilistic roadmaps)算法的机器人路线规划算法matlab仿真
2023-09-11 14:15:33 时间
目录
一、理论基础
地图和机器人的模型如下:
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
相关文章
- (NO.00003)iOS游戏简单的机器人投射游戏成形记(六)
- V-rep学习笔记:机器人模型创建2—添加关节
- 青鸟IT汇微信公众号新增智能机器人
- 大负载机器人为什么要用平衡缸
- AI:2020年6月22日北京智源大会演讲分享之机器感知专题论坛—15:30-16:10孙富春教授《机器人视触觉传感器与主动感知》
- 水下机器人双机械手系统动态建模与控制仿真(Matlab代码实现)
- 基于MATLAB中雷达和视觉合成数据的目标级传感器融合(Matlab代码实现)
- 【MATLAB】数学建模没有基础怎么办,看过来一篇文章带你入门 matlab
- 【MATLAB】MATLAB 仿真 — 基于matlab的QPSK系统仿真
- 【人工智能 AI】Robotic Process Automation (RPA) 机器人流程自动化 (RPA)
- 【数字信号处理】卷积编程实现 ( Matlab 卷积和多项式乘法 conv 函数 | 使用 matlab 代码求卷积并绘图 )
- 【MATLAB】matlab 文档使用 ( 文档查询 | 文档层次 | 自带搜索工具 | 帮助命令 | 学习导引 )
- Matlab使用笔记(九):matlab实现交通流仿真/车感知/城市交通交叉路口
- 机器人控制算法五之 机器人由空间变换transform建立XML模型
- Matlab Tips: 结构体递归式打印--Dump matlab struct content recursively
- Matlab在线IDE:MATLAB Online介绍与计算定积分案例