避障程序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;
end
%梯形
for i=12:17%下水平
mapgrid(i,2)=1;
end
for i=12:25%上水平
mapgrid(i,10)=1;
end
for i=1:9%斜边
mapgrid(17+i,i+1)=1;
end
for i=1:9%竖直
mapgrid(12,i+1)=1;
end
%矩形
for i=1:10%上边
mapgrid(10+i,60)=1;

end
for i=1:10%下边
mapgrid(10+i,55)=1;

end
for i=1:5
mapgrid(10,55+i)=1;
end
%
x0=30;
y0=30;
r=5;
theta=0:pi/10:2pi;
x=floor(x0+rcos(theta));
disp(x)
x(1)
y=ceil(y0+r*sin(theta));
%plot(x,y,’-’,x0,y0,’.’);
for i=1:20
mapgrid(x(i),y(i))=1;
end
%for i=
% Field Walls 场墙
% for i = 1:ROW-10
% mapgrid(i, COL) = 1;
% end
% for i = 1:ROW
% mapgrid(i, 1) = 1;
% end
% for i = 1:COL
% mapgrid(1, i) = 1;
% end
% for i = 1:COL-10
% mapgrid(ROW, i) = 1;
% 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);%绘制障碍物
end
end
end

%Plot End Point 绘制结束

plot(XEND, YEND, ‘rx’, ‘markersize’, 10, ‘linewidth’, 3);%绘制终点
%% 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 = 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);
%pre-plot to enable animation预绘图以启用动画
b = plot(rx, ry,‘linewidth’, 2, ‘color’, ‘m’);%小车形状
xvect = [0 0];
yvect = [0 0];
c = plot(xvect, yvect, ‘c’, ‘linewidth’, 3);%小车上面那一条杠
%Navigation controller parameters 导航控制器参数
kt = 1*v; %theta gain
ka = 5; %attractive gain吸引增益
km = 15; %momentum gain动量增益
kr = 12000; %repulsive gain排斥增益
kdiff = 0.6; %windup torque gain上卷力矩增益
dzero = 30; %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;
while(1)
%% Navigation Contoller 导航控制器

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

%Unitized Momentum统一动量
xmom = cos(tbot);
ymom = sin(tbot);

%Discrete bot location for outward search用于外部搜索的离散机器人位置
xi = floor(xbot/BOXWIDTH)+1;
yi = floor(ybot/BOXHEIGHT)+1;

%Search for and sum all local repulsive vectors搜索并求和所有局部排斥向量
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+12cos(tbot)) - x;
vyobs = (ybot+12sin(tbot)) - y;
dobs = sqrt(vxobs^2+vyobs2);
if dobs xrep = xrep+(((1/dobs)-(1/dzero))*vxobs/dobs^2);
yrep = yrep+(((1/dobs)-(1/dzero))*vyobs/dobs^2);
end
end
end
end

%Totol comand vectors and command angletotool命令向量和命令角度
xtot = kaxatt+krxrep+kmxmom;
ytot = kayatt+kryrep+kmymom;
dtot = xtot^2+ytot2; %measurement used in real bot to select speed实际机器人选择速度的测量方法
tcom = atan2(ytot, xtot);

%Fix atan2 for rolling theta errors
if tcom-tcomold+tcor > pi
tcor = tcor - 2pi;
elseif tcom-tcomold+tcor < -pi
tcor = tcor + 2pi;
end
tcom = tcom+tcor;

%Display total command vector显示总命令向量
xvect = [xbot, xbot+xtot];
yvect = [ybot, ybot+ytot];
c.XData = xvect;
c.YData = yvect;

% %Reversals might be cool to do, but could make us get stuck
% if tcom-tbot>pi/1.5
% tbot = tbot+pi;
% elseif tbot-tcom>pi/1.5
% tbot = tbot-pi;
% end
%Direction of endpoint终点方向
tend = atan2(vyend, vxend);%定义atan2(y,x)返回点(x,y)与x轴正向的夹角
%Difference between bot angle and endpoint angle底角与端点角之差
tdiff = (tend-tbot);

%Windup torque = soft wall following out of local minima上卷力矩=超出局部最小值后的软壁
if tdiff>3pi/2
tdiff = 3pi/2;
elseif tdiff<-3pi/2
tdiff = -3pi/2;
end

%Motor control requests电机控制请求
dv = kt*(tcom-tbot) + kdiff*(tdiff);
vr = v+dv;
vl = v-dv;

%When to stop the ride什么时候停车
if dend vr = 0;
vl = 0;
break
end

%Necessary Update for rolling theta correction滚动theta校正的必要更新
tcomold = tcom;
%% Robot position update using simplified kinematics equations基于简化运动学方程的机器人位置更新

%Update Variables
%tbotold = tbot;
tbotold = tbot;
%Compute position and angle
tbot = tbotold + (16.384*(vr-vl)ts/wheelbase);
dtbot = tbot-tbotold;
tavg = (tbot+tbotold)/2;
vavg = 16.384(vl+vr)/2;
d = vavgts;
% Exact term (innacurate on the bot at varying speed - )
% if dtbot ~= 0
% R = d/abs(dtbot);
% d = Rsqrt(2*(1-cos(dtbot)));
% end
xbot = xbot + dcos(tavg);
ybot = ybot + dsin(tavg);

%% Robot drawing机器人绘图

curtcorn = tcorn+tbot;
for k = 1:5
rx(k) = xbot+rcos(curtcorn(k));
ry(k) = ybot+rsin(curtcorn(k));
end
b.XData = rx;
b.YData = ry;
%disp(rx(1));
i=i+1;
%if mod(i,2)==0
%plot(b.XData,b.YData);%画路径
plot(rx(1),ry(1),’.’,‘markersize’,10)
%plot(b.XData,b.YData, ‘linewidth’, 2, ‘color’, ‘m’)
%end
%% Pause

pause(ts);
end