模拟移动机器人控制（四）

clc;close all;clear

hold on
axis([0,200,0,200]);
[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(0.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;

while (D>V)%与目标点距离大于一个速度继续移动
    if (PT(1)>X&&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 = 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));%当前与目标点的角度
    end
    V= 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;
        Rl= abs(Vl/w);
        Rr= abs(Vr/w);
        dsl= 2*Rl*sin(0.5*abs(w));
        dsr= 2*Rr*sin(0.5*abs(w));
        c= atan((Pr(2)-Pl(2))/(Pr(1)-Pl(1))); 
        C= c+pi/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)),});
        Pl= Pl+[-1*n*dsl*cos(pi/2+c+w),-1*n*dsl*sin(pi/2+c+w)];
        Pr= Pr+[n*dsr*cos(pi/2+c+w),n*dsr*sin(pi/2+c+w)];
        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(0.1);    
        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.07*V;
    Vr= V+(2*rand-1)*0.07*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));
    c= atan((Pr(2)-Pl(2))/(Pr(1)-Pl(1))); 
    C= c+pi/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)),});
    Pl= Pl+[dsl*cos(pi/2+c+w),dsl*sin(pi/2+c+w)];
    Pr= Pr+[dsr*cos(pi/2+c+w),dsr*sin(pi/2+c+w)];
    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(0.1);
    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

角度有点问题！待改进！