机器人避障算法的matlab仿真代码

这是一个学习机器人避障算法的matlab仿真代码，直接下载运行就可以使用，备注非常详细，大家可以自定义避障地图，也可以更改参数实现不同的避障

figure（‘position‘ [300 50 600 600]）;
axis（[0 400 0 400]）;
axis square
grid on%添加栅格
grid minor
hold on
%自定义变量
XSTART = 30;%机器人起始点
YSTART = 30;
TSTART = 0;
XEND = 370;%机器人终点
YEND = 370;
ROW = 400;
COL = 400;
BOXWIDTH = 1;
BOXHEIGHT = 1;
i=1;%记录循环次数
mapgrid = zeros（ROW COL）;

%画①号线

%画②号三角形
for i=50:100%横线
mapgrid（i250）=1;
end
for i=200:250%竖线
mapgrid（50i）=1;
end
for i=50:100%斜线
mapgrid（i150+i）=1;
end
%画③号三角形
for i=100:150%横线
mapgrid（i150）=1;
end
for i=150:170%竖线
mapgrid（150i）=1;
end
%画④号三角形
for i=130:200%横线
mapgrid（i100）=1;
end
for i=70:100%竖线
mapgrid（130i）=1;
end
%画⑤号三角形
for i=200:250%横线
mapgrid（i150）=1;
end
for i=150:200%竖线
mapgrid（200i）=1;
end
for i=200:250%斜线
mapgrid（i400-i）=1;
end
%画⑥号线
for i=160:230
mapgrid（i460-i）=1;
end
%画⑦号矩形
for i=280:360%横线
mapgrid（i150）=1;
end
for i=280:360%横线
mapgrid（i100）=1;
end
for i=100:150%竖线
mapgrid（280i）=1;
end
for i=100:150%竖线
mapgrid（360i）=1;
end
%画⑧号线
for i=270:350
mapgrid（i580-i）=1;
end

%%  绘制所有障碍物
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）;%绘制障碍物
end
end
end
%Plot End Point 绘制结束

plot（XEND YEND ‘rx‘ ‘markersize‘ 10 ‘linewidth‘ 3）;%绘制终点
text（2020‘Start‘）;%起点处标注
text（370360‘Goal‘）;%起点处标注

%%   机器人模拟器参数
%模拟时间步
ts = 0.1;%seconds
%开始位置
xbot = XSTART;%开始坐标
ybot = YSTART;
tbot = TSTART;
% 机器人速度
v = 1;%在编码器中，每一时间刻度
vr = v;
vl = v;
%机器人参数
wheelbase = 15.25;%cm
%机器人大小
wid = 5;
len = 5;
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）;
%预绘图以启用动画
b = plot（rx ry‘linewidth‘ 2 ‘color‘ ‘m‘）;%小车形状
xvect = [0 0];
yvect = [0 0];
c = plot（xvect yvect ‘c‘ ‘linewidth‘ 3）;%小车上面那一条杠
% 导航控制器参数
kt = 1*v; %theta gain
ka = 5; %吸引增益
km = 15; %动量增益
kr = 12000; %排斥增益
kdiff = 0.6; %上卷力矩增益
dzero = 20; %与墙的零效果距离
searchradius = 20; %在所有方向搜索矢量加法的框
lookahead = 5; %何时在终点停车
%%   模拟器
tcom = 0;
tcomold = 0;
tcor = 0;
tend = 0;
while（1）
%%   导航控制器

%End location attraction终点位置
vxend = XEND - xbot;%终点坐标减去起点坐标
vyend = YEND - ybot;
dend = sqrt（vxend^2+vyend^2）;%计算直线距离
xatt = vxend/dend;%sinA
yatt = vyend/dend;%cosA

%统一动量
xmom = cos（tbot）;
ymom = sin（tbot）;

%用于外部搜索的离散机器人位置
xi = floor（xbot/BOXWIDTH）+1;
yi = floor（ybot/BOXHEIGHT）+1;

%搜索并求和所有局部排斥向量
xstart = xi-searchradius;
if xstart<1
xstart = 1;
end
ystart = yi-searchradius;
if ystart<1
ystart = 1;
end
xend = xi+searchradius;
if xend>ROW
xend = ROW;
end
yend = yi+searchradius;
if yend>COL
yend = COL;
end

xrep = 0;
yrep = 0;
for i = xstart:xend
for j = ystart:yend
if mapgrid（i j）>0
x = （i-1）*BOXWIDTH;
y = （j-1）*BOXHEIGHT;
vxobs = （xbot+12*cos（tbot）） - x;
vyobs = （ybot+12*sin（tbot）） - y;
dobs = sqrt（vxobs^2+vyobs^2）;
if dobsxrep = xrep+（（（1/dobs）-（1/dze

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        4793  2020-04-05 17:27  Robot obstacle avoidance\avoidtest.m
     目录           0  2020-04-05 17:28  Robot obstacle avoidance\