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/2;
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,90-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 = 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
% 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 = 1v; %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(vxend2+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(vxobs2+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 = xtot2+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(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