解决了大于360°时机器人无限调整角度的问题;
clc;close all;clear
hold on
axis([0,200,0,200]);
t= 0.01;
[X,Y] = ginput(1);
C= pi/2;
P= [X,Y,C];
L= 4;
V= 4;
Pl=[X-L/2,Y];
Pr=[X+L/2,Y];
plot((Pl(1)+Pr(1))/2,(Pl(2)+Pr(2))/2,'*');
plot(Pl(1),Pl(2),'o');
plot(Pr(1),Pr(2),'o');
text(150,180,{strcat('P= ',num2str((Pl(1)+Pr(1))/2),', ',num2str((Pl(2)+Pr(2))/2))});
pause(t);
while(1)
[Xt,Yt] = ginput(1); %在figure上用鼠标抓取数据,1表示抓取一次
PT = [Xt;Yt];%PT = [xt;yt;ct]目标点
plot(PT(1),PT(2),'r*','LineWidth',4);
D= ((P(1)-PT(1)).^2+(P(2)-PT(2)).^2).^0.5;
%((Pr(1)-Pl(1)).^2+(Pr(2)-Pl(2)).^2).^0.5
while (D>V)%与目标点距离大于一个速度继续移动
if (PT(1)>=P(1)&&PT(2)>=P(2))
if (PT(1)>=P(1)&&PT(2)>=P(2))
Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
elseif(PT(1)<=P(1)&&PT(2)>=P(2))
Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
elseif(PT(1)<=P(1)&&PT(2)<=P(2))
Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
elseif(PT(1)>=P(1)&&PT(2)<=P(2))
Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
end
elseif(PT(1)<=P(1)&&PT(2)>=P(2))
if (PT(1)>=P(1)&&PT(2)>=P(2))
Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
elseif(PT(1)<=P(1)&&PT(2)>=P(2))
Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
elseif(PT(1)<=P(1)&&PT(2)<=P(2))
Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
elseif(PT(1)>=P(1)&&PT(2)<=P(2))
Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
end
elseif(PT(1)<=P(1)&&PT(2)<=P(2))
if (PT(1)>=P(1)&&PT(2)>=P(2))
Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
elseif(PT(1)<=P(1)&&PT(2)>=P(2))
Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
elseif(PT(1)<=P(1)&&PT(2)<=P(2))
Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
elseif(PT(1)>=P(1)&&PT(2)<=P(2))
Ct = 2*Pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
end
elseif(PT(1)>=P(1)&&PT(2)<=P(2))
if (PT(1)>=P(1)&&PT(2)>=P(2))
Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
elseif(PT(1)<=P(1)&&PT(2)>=P(2))
Ct = pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
elseif(PT(1)<=P(1)&&PT(2)<=P(2))
Ct = -pi+atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
elseif(PT(1)>=P(1)&&PT(2)<=P(2))
Ct = atan((PT(2)-P(2))*(PT(1)-P(1)).^(-1));%当前与目标点的角度
end
end
V= 0.1;
w= 2*V/L;
while(C>(Ct+0.5*w)||C<=(Ct-0.5*w))%角度调整
if (C>Ct)
n = -1;
else
n = 1;
end
Vl= -1*n*V;
Vr= n*V;
dsl= L*sin(0.5*abs(w));
dsr= L*sin(0.5*abs(w));
if (PT(1)>=P(1)&&PT(2)>=P(2))
if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2))
c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2))
c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
end
elseif (PT(1)<=P(1)&&PT(2)>=P(2))
if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2))
c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2))
c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
end
elseif (PT(1)<=P(1)&&PT(2)<=P(2))
if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2))
c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2))
c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
end
elseif (PT(1)>=P(1)&&PT(2)<=P(2))
if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2))
c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2))
c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
end
end
C= c+pi/2;
Pl= Pl+[-1*n*dsl*cos(C+n*w/2),-1*n*dsl*sin(C+n*w/2)];
Pr= Pr+[n*dsr*cos(C+n*w/2),n*dsr*sin(C+n*w/2)];
% ((Pr(1)-Pl(1)).^2+(Pr(2)-Pl(2)).^2).^0.5
clf;
hold on
axis([0,200,0,200]);
text(150,180,{strcat('P= ',num2str((Pl(1)+Pr(1))/2),', ',num2str((Pl(2)+Pr(2))/2),', ','C= ',num2str(C/pi),'*pi'),
strcat('Vl= ',num2str(Vl),', ','Vr= ',num2str(Vr),', ','w= ',num2str(w)),
strcat('Ct= ',num2str(Ct/pi),'*pi')
});
plot((Pl(1)+Pr(1))/2,(Pl(2)+Pr(2))/2,'*');
plot(Pl(1),Pl(2),'o');
plot(Pr(1),Pr(2),'o');
plot(PT(1),PT(2),'r*','LineWidth',4);
pause(t);
P= [(Pl(1)+Pr(1))/2,(Pl(2)+Pr(2))/2,C];
D= ((P(1)-PT(1)).^2+(P(2)-PT(2)).^2).^0.5;
end
V= 4;
Vl= V+(2*rand-1)*0.05*V;
Vr= V+(2*rand-1)*0.05*V;
w= (Vr-Vl)/L;
Rl= abs(Vl/w);
Rr= abs(Vr/w);
dsl= 2*Rl*sin(0.5*abs(w));
dsr= 2*Rr*sin(0.5*abs(w));
if (PT(1)>=P(1)&&PT(2)>=P(2))
if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2))
c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2))
c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
end
elseif (PT(1)<=P(1)&&PT(2)>=P(2))
if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2))
c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2))
c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
end
elseif (PT(1)<=P(1)&&PT(2)<=P(2))
if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2))
c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2))
c = pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
end
elseif (PT(1)>=P(1)&&PT(2)<=P(2))
if (Pr(1)>=Pl(1)&&Pr(2)>=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)>=Pl(2))
c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)<=Pl(1)&&Pr(2)<=Pl(2))
c = -pi+atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
elseif(Pr(1)>=Pl(1)&&Pr(2)<=Pl(2))
c = atan((Pr(2)-Pl(2))*(Pr(1)-Pl(1)).^(-1));
end
end
C= c+pi/2;
Pl= Pl+[dsl*cos(C+w/2),dsl*sin(C+w/2)];
Pr= Pr+[dsr*cos(C+w/2),dsr*sin(C+w/2)];
clf;
hold on
axis([0,200,0,200]);
text(150,180,{strcat('P= ',num2str((Pl(1)+Pr(1))/2),', ',num2str((Pl(2)+Pr(2))/2),', ','C= ',num2str(C/pi),'*pi'),
strcat('Vl= ',num2str(Vl),', ','Vr= ',num2str(Vr),', ','w= ',num2str(w)),
strcat('Ct= ',num2str(Ct/pi),'*pi')
});
plot((Pl(1)+Pr(1))/2,(Pl(2)+Pr(2))/2,'*');
plot(Pl(1),Pl(2),'o');
plot(Pr(1),Pr(2),'o');
plot(PT(1),PT(2),'r*','LineWidth',4);
pause(t);
P= [(Pl(1)+Pr(1))/2,(Pl(2)+Pr(2))/2,C];
D= ((P(1)-PT(1)).^2+(P(2)-PT(2)).^2).^0.5;
end
end