路径规划基于matlab AI抗疫服务移动机器人路径规划系统含Matlab源码 2096期

Posted 2022-09-09 海神之光

一、简介

【路径规划】基于matlab AI抗疫服务移动机器人路径规划系统【含Matlab源码 2096期】

二、部分源代码

clear all
close all
%Set up field
figure(‘position’, [300, 50, 600, 600]); %指定窗口的大小和位置
axis([0, 725, 0, 725]);
axis square
grid on %添加栅格
grid minor
hold on
load coordinate.mat
%User defined constants 用户自定义变量
UH=1;
XSTART = coordinate(UH,1);%机器人起始点
YSTART = coordinate(UH,2);
UH=UH+1;
%XSTART2 = 400;%机器人终点
%YSTART2 = 180;
TSTART =-pi/2; %起始朝向
XEND = coordinate(UH,1);%机器人终点
YEND = coordinate(UH,2);

%XEND2 = 100;%机器人终点
%YEND2 = 200;
ROW = 290;
COL = 200;
BOXWIDTH = 5; %障碍物的宽度 每个圆点
BOXHEIGHT = 5;
i=1;%记录循环次数
mapgrid = zeros(ROW, COL);
% %Well
%%横线：
for i=40:145
mapgrid(i,29)=1;
end
for i=44:54
mapgrid(i,29)=0;
end
for i=61:69
mapgrid(i,29)=0;
end
for i=81:89
mapgrid(i,29)=0;
end
for i=101:109
mapgrid(i,29)=0;
end
for i=116:124
mapgrid(i,29)=0;
end
for i=131:139
mapgrid(i,29)=0;
end
for i=40:144
mapgrid(i,41)=1;
end
for i=41:49
mapgrid(i,41)=0;
end
for i=56:64
mapgrid(i,41)=0;
end
for i=81:89
mapgrid(i,41)=0;
end
for i=101:109
mapgrid(i,41)=0;
end
for i=116:124
mapgrid(i,41)=0;
end
for i=131:139
mapgrid(i,41)=0;
end
for i=116:124
mapgrid(i,29)=0;
end
for i=55:145
mapgrid(i,70)=1;
end
%竖线
for i=1:85
mapgrid(40,i)=1;
end
for i=31:39
mapgrid(40,i)=0;
end
for i=41:70
mapgrid(55,i)=1;
end
for i=1:29
mapgrid(60,i)=1;
end
for i=1:29
mapgrid(80,i)=1;
end
for i=41:70
mapgrid(80,i)=1;
end
for i=1:30
mapgrid(100,i)=1;
end
for i=40:71
mapgrid(100,i)=1;
end
for i=1:29
mapgrid(115,i)=1;
end
for i=41:70
mapgrid(115,i)=1;
end
for i=1:29
mapgrid(130,i)=1;
end
for i=41:70
mapgrid(130,i)=1;
end
%外围竖线
for i=1:85
mapgrid(1,i)=1;
end
for i=50:60
mapgrid(1,i)=0;
end
for i=1:100
mapgrid(145,i)=1;
end
for i=84:100
mapgrid(100,i)=1;
end
%横线
for i=1:145
mapgrid(i,1)=1;
end
for i=1:100
mapgrid(i,85)=1;
end
for i=100:145
mapgrid(i,100)=1;
end
for i=129:137
mapgrid(i,100)=0;
end
%% Plot All Obstacles 绘制所有障碍物
for i = 1:ROW
for j = 1:COL
if mapgrid(i, j)>0
x = (i-1)*BOXWIDTH;
y = (j-1)*BOXHEIGHT;
% rectangle(‘position’, [x, y, BOXWIDTH, BOXHEIGHT],‘Curvature’,1);%绘制障碍物 1圆角矩形—曲率最大为圆；0无曲率—矩形
rectangle(‘position’, [x, y, BOXWIDTH, BOXHEIGHT],‘Curvature’,0,‘FaceColor’,‘b’,‘EdgeColor’,‘b’);%绘制障碍物 1圆角矩形—曲率最大为圆；0无曲率—矩形
% rectangle(‘position’, [x, y, BOXWIDTH, BOXHEIGHT],‘Curvature’,0,‘FaceColor’,‘k’,‘EdgeColor’,‘k’);%绘制障碍物 1圆角矩形—曲率最大为圆；0无曲率—矩形
end
end
end
%Plot End Point 绘制结束
text(540,515,‘工作通道’,‘color’,‘k’,‘FontSize’,10)
text(540,425,‘清洁区’,‘color’,‘g’,‘FontSize’,13)
text(215,403,‘半污染区’,‘color’,‘c’,‘FontSize’,13)
text(145,170,‘污染区’,‘color’,‘r’,‘FontSize’,13)
text(98,275,‘等候区’,‘color’,‘k’,‘FontSize’,13)
text(220,240,‘缓冲’,‘color’,‘k’,‘FontSize’,10)
text(225,100,‘登记’,‘color’,‘k’,‘FontSize’,13)
text(545,285,‘疑 似 病 房’,‘color’,‘k’,‘FontSize’,13)
text(300,285,‘问诊区’,‘color’,‘k’,‘FontSize’,13)
text(410,285,‘护士站’,‘color’,‘k’,‘FontSize’,13)
text(305,90,‘生化检验’,‘color’,‘k’,‘FontSize’,10)
text(428,90,‘X-ray’,‘color’,‘k’,‘FontSize’,10)

%plot(XEND, YEND, ‘rx’, ‘markersize’, 10, ‘linewidth’, 3);%绘制 终点
%text(XEND-10, YEND+12,‘End’,‘FontWeight’,‘bold’); %标注文字End终点加粗
plot(XSTART, YSTART, ‘rx’, ‘markersize’, 10, ‘linewidth’, 3);%绘制起点
text(XSTART-12, YSTART-10,‘Start’,‘FontWeight’,‘bold’); %标注文字Start起点加粗

%% Robot Simulator Parameters 机器人模拟器参数
%simulation time step模拟时间步
ts = 0.2;%seconds

%Robot Start Position 开始位置
xbot = XSTART;%开始坐标
ybot = YSTART;

tbot = TSTART;
%Robot Velocity + 机器人速度
v = 1;%in encoder ticks per time tick在编码器中，每一时间刻度
vr = v;
vl = v;
%Robot size parameters机器人参数
wheelbase = 15.25;%cm
%Robot Box Size (for plotting image on map)机器人大小
wid = 20;
len = 20;
r = sqrt((wid/2)^2 + (len/2)^2);
tcorn = [atan2(wid, len), atan2(wid, -len), atan2(-wid, -len), atan2(-wid, len),atan2(wid, len)];

% rx = zeros(1, 5); %方形机器人
% ry = zeros(1, 5)

rx=zeros(1, 5);
ry=zeros(1, 5);
%pre-plot to enable animation预绘图以启用动画
b = plot(rx, ry,‘linewidth’, 2, ‘color’, ‘m’);%小车形状

xvect = [0 0];
yvect = [0 0];
R = plot(xvect, yvect, ‘c’, ‘linewidth’, 3);%小车上面那一条杠
%Navigation controller parameters 导航控制器参数
kt = v; %theta gain
ka =5; %attractive gain吸引增益
km = 16.8; %momentum gain动量增益
kr = 12000; %repulsive gain排斥增益
kdiff = 0.22; %windup torque gain上卷力矩增益
dzero = 29; %zero effect distance from walls与墙的零效果距离
searchradius = 10; %boxes to search in all directions for vector addition在所有方向搜索矢量加法的框
lookahead = 5; %when to stop at end point何时在终点停车
%% Simulator 模拟器
%initialize simulation variables
tcom = 0;
tcomold = 0;
tcor = 0;
tend = 0;

dend=6;

while(1)
%% Navigation Contoller 导航控制器

if dend<5
UH=UH+1;
XEND = coordinate(UH,1);%机器人终点
YEND = coordinate(UH,2);
end

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除