机器人避障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/4;
XEND = 370;%机器人终点
YEND = 370;
ROW = 80;
COL = 80;
BOXWIDTH = 5;
BOXHEIGHT = 5;
mapgrid = zeros(ROW, COL);
%Well
for i = 40:60%右上方边
mapgrid(i,100-i) = 1;
end
for i = 30:50%左上方边
mapgrid(i-10,i+10) = 1;
end
for i = 30:50 %右下方边
mapgrid(i+10,i-10) = 1;
end
for i = 20:25
mapgrid(i, 60-i) = 1;
end
for i = 35:40
mapgrid(i, 60-i) = 1;
end
% 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]);
end
end
end
%Plot End Point
plot(XEND, YEND, ‘rx’, ‘markersize’, 10, ‘linewidth’, 3);
%% Robot Simulator Parameters
%simulation time step
ts = 0.1;%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 = 17;
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);
%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 = 1v; %theta gain
ka = 5; %attractive gain
km = 15; %momentum gain
kr = 12000; %repulsive gain
kdiff = 0.15; %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;
yatt = vyend/dend;

%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-0.5)BOXWIDTH;
y = (j-0.5)BOXHEIGHT;
vxobs = (xbot+10cos(tbot)) - x;
vyobs = (ybot+10sin(tbot)) - y;
dobs = sqrt(vxobs^2+vyobs2);
if dobs<dzero
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 angle
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);
%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<lookahead
vr = 0;
vl = 0;
break
end

%Necessary Update for rolling theta correction
tcomold = tcom;
%% Robot position update using simplified kinematics equations

%Update Variables
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;

%% Pause

pause(ts);
end

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/4;
XEND = 370;%机器人终点
YEND = 370;
ROW = 80;
COL = 80;
BOXWIDTH = 5;
BOXHEIGHT = 5;
mapgrid = zeros(ROW, COL);
%Well
for i = 40:60%右上方边
mapgrid(i,100-i) = 1;
end
for i = 30:50%左上方边
mapgrid(i-10,i+10) = 1;
end
for i = 30:50 %右下方边
mapgrid(i+10,i-10) = 1;
end
for i = 20:20%左上
mapgrid(i, 60-i) = 1;
end
for i = 35:40%左下
mapgrid(i, 60-i) = 1;
end
% 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]);%绘制障碍物
end
end
end
%Plot End Point 绘制结束
plot(XEND, YEND, ‘rx’, ‘markersize’, 10, ‘linewidth’, 3);%绘制终点
%% Robot Simulator Parameters 机器人模拟器参数
%simulation time step模拟时间步
ts = 0.1;%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 = 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);
%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’, 6);%小车上面那一条杠
%Navigation controller parameters 导航控制器参数
kt = 1v; %theta gain
ka = 5; %attractive gain吸引增益
km = 15; %momentum gain动量增益
kr = 12000; %repulsive gain排斥增益
kdiff = 0.15; %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;
yatt = vyend/dend;

%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-0.5)BOXWIDTH;
y = (j-0.5)BOXHEIGHT;
vxobs = (xbot+10cos(tbot)) - x;
vyobs = (ybot+10sin(tbot)) - y;
dobs = sqrt(vxobs^2+vyobs2);
if dobs<dzero
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<lookahead
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(b.XData);
plot(b.XData,b.YData, ‘linewidth’, 2, ‘color’, ‘m’);%画路径
%% Pause

pause(ts);
end