避障matlab程序,避障程序matlab 一

clear all

close all

%Set up field

figure(‘position’, [300, 50, 600, 600]);

axis([0, 400, 0, 400]);

axis square

grid on%添加栅格

grid minor

hold on

%User defined constants 用户自定义变量

XSTART = 30;%机器人起始点

YSTART = 30;

TSTART = pi;

XEND = 370;%机器人终点

YEND = 370;

ROW = 160;

COL = 160;

BOXWIDTH = 5;

BOXHEIGHT = 5;

i=1;%记录循环次数

mapgrid = zeros(ROW, COL);

%Well

for i = 40:50%右上方边

mapgrid(i,80-i) = 1;

end

for i = 30:40%添加部分1

mapgrid(i,85-i) = 1;

end

for i = 40:50%添加部分2

mapgrid(i,100-i) = 1;

end

for i = 55:65%添加部分3

mapgrid(i,115-i) = 1;

end

for i=1:10%添加部分4

mapgrid(30+i,30)=1;

end

for i = 30:50%左上方边

mapgrid(i-10,i+10) = 0;

end

for i = 30:50 %右下方边

mapgrid(i+10,i-10) = 0;

end

for i = 20:20%左上

mapgrid(i, 60-i) = 0;

end

for i = 25:35%左下

mapgrid(i, 60-i) = 1;

end

%三角形

%三角形1

for i=15:20%对角线

mapgrid(i,i+5)=1;

end

for i=15:20%竖直

mapgrid(15,i+5)=1;

end

for i=15:20

mapgrid(i,25)=1;

end

%三角行2

for i=25:35

mapgrid(i,i+15)=1;

end

for i=35:45

mapgrid(25,i+5)=1;

end

for i=25:35

mapgrid(i,50)=1;