领航跟随型编队控制（八）编队队形变换程序

c截取部分队形变换的程序片段如下：

for i=1:100
c21=2;c31=2;%编队长度
    leader_ideal.theta(i)=pi/6;
    leader_ideal.x(i+1)=leader_ideal.x(i)+ts*vr*cos(pi/6);
    leader_ideal.y(i+1)= leader_ideal.y(i)+ts*vr*sin(pi/6);
    leader_ideal.theta(i+1)=pi/6;
    
    follower1_ideal.y (1)= leader_ideal.y(1)-c21*sin(follower1_ideal.beta(1)+follower1_ideal.theta(1));
    follower1_ideal.x(1) =leader_ideal.x(1)-c21*cos(follower1_ideal.beta(1)+follower1_ideal.theta(1));
    follower1_ideal.v(1)=vr;
    follower1_ideal.w(1)=wr;
    follower1_ideal.beta(1)=1*pi/6;
    follower1_ideal.theta(1) =pi/6;
    
    follower1_ideal.y(i+1)= leader_ideal.y(i)-c21*sin(follower1_ideal.beta(i)+follower1_ideal.theta(i));
    follower1_ideal.x(i+1) =leader_ideal.x(i)-c21*cos(follower1_ideal.beta(i)+follower1_ideal.theta(i));
    follower1_ideal.v(i+1)=vr;
    follower1_ideal.w(i+1)=wr;
    follower1_ideal.beta(i+1)=1*pi/6;
    follower1_ideal.theta(i+1) =leader_ideal.theta(i);
    
    follower2_ideal.y (1)= leader_ideal.y(1)-c31*sin(follower2_ideal.beta(1)+follower2_ideal.theta(1));
    follower2_ideal.x(1) =leader_ideal.x(1)-c31*cos(follower2_ideal.beta(1)+follower2_ideal.theta(1));
    follower2_ideal.v(1)=vr;
    follower2_ideal.w(1)=wr;
    follower2_ideal.beta(1)=pi/6;
    follower2_ideal.theta(1) =leader_ideal.theta(i)
    
    follower2_ideal.y (i+1)= leader_ideal.y(i)-c31*sin(follower2_ideal.beta(i)+follower2_ideal.theta(i));
    follower2_ideal.x(i+1) =leader_ideal.x(i)-c31*cos(follower2_ideal.beta(i)+follower2_ideal.theta(i));
    follower2_ideal.v(i+1)=vr;
    follower2_ideal.w(i+1)=wr;
    follower2_ideal.beta(i+1)=-pi/6;
    follower2_ideal.theta(i+1) =leader_ideal.theta(i);
end
for i=101:200
c21=5;c31=5;%编队长度
   leader_ideal.theta(i)=pi/6;
    leader_ideal.x(i+1)=leader_ideal.x(i)+ts*vr*cos(pi/6);
    leader_ideal.y(i+1)= leader_ideal.y(i)+ts*vr*sin(pi/6);
    leader_ideal.theta(i+1)=pi/6;
    
    follower1_ideal.y (1)= leader_ideal.y(1)-c21*sin(follower1_ideal.beta(1)+follower1_ideal.theta(1));
    follower1_ideal.x(1) =leader_ideal.x(1)-c21*cos(follower1_ideal.beta(1)+follower1_ideal.theta(1));
    follower1_ideal.v(1)=vr;
    follower1_ideal.w(1)=wr;
    follower1_ideal.beta(1)=1*pi/6;
    follower1_ideal.theta(1) =pi/6;
    
    follower1_ideal.y(i+1)= leader_ideal.y(i)-c21*sin(follower1_ideal.beta(i)+follower1_ideal.theta(i));
    follower1_ideal.x(i+1) =leader_ideal.x(i)-c21*cos(follower1_ideal.beta(i)+follower1_ideal.theta(i));
    follower1_ideal.v(i+1)=vr;
    follower1_ideal.w(i+1)=wr;
    follower1_ideal.beta(i+1)=1*pi/6;
    follower1_ideal.theta(i+1) =leader_ideal.theta(i);

for i=201:300
c21=2;c31=2;%编队长度
   leader_ideal.theta(i)=pi/6;
    leader_ideal.x(i+1)=leader_ideal.x(i)+ts*vr*cos(pi/6);
    leader_ideal.y(i+1)= leader_ideal.y(i)+ts*vr*sin(pi/6);
    leader_ideal.theta(i+1)=pi/6;
    
    follower1_ideal.y (1)= leader_ideal.y(1)-c21*sin(follower1_ideal.beta(1)+follower1_ideal.theta(1));
    follower1_ideal.x(1) =leader_ideal.x(1)-c21*cos(follower1_ideal.beta(1)+follower1_ideal.theta(1));
    follower1_ideal.v(1)=vr;
    follower1_ideal.w(1)=wr;
    follower1_ideal.beta(1)=1*pi/6;
    follower1_ideal.theta(1) =pi/6;
    
    follower1_ideal.y(i+1)= leader_ideal.y(i)-c21*sin(follower1_ideal.beta(i)+follower1_ideal.theta(i));
    follower1_ideal.x(i+1) =leader_ideal.x(i)-c21*cos(follower1_ideal.beta(i)+follower1_ideal.theta(i));
    follower1_ideal.v(i+1)=vr;
    follower1_ideal.w(i+1)=wr;
    follower1_ideal.beta(i+1)=1*pi/6;
    follower1_ideal.theta(i+1) =leader_ideal.theta(i);
    
    follower2_ideal.y (1)= leader_ideal.y(1)-c31*sin(follower2_ideal.beta(1)+follower2_ideal.theta(1));
    follower2_ideal.x(1) =leader_ideal.x(1)-c31*cos(follower2_ideal.beta(1)+follower2_ideal.theta(1));
    follower2_ideal.v(1)=vr;
    follower2_ideal.w(1)=wr;
    follower2_ideal.beta(1)=pi/6;
    follower2_ideal.theta(1) =leader_ideal.theta(i)
figure(1);

plot(leader_ideal.x(1:n),leader_ideal.y(1:n),'r','LineWidth',2);

hold on;plot(follower1_ideal.x(1:n),follower1_ideal.y(1:n),'c','LineWidth',2  );
hold on;plot(follower2_ideal.x(1:n),follower2_ideal.y(1:n),'y','LineWidth',2  );
hold on;plot(follower1.x(1:n),follower1.y(1:n),'k','LineWidth',2  );
hold on;plot(follower2.x(1:n),follower2.y(1:n),'g','LineWidth',2  );
h_xlabel=xlabel('x/m');h_ylabel=ylabel('y/m');
h_legend=legend('leader','follower1_ideal','follower2_ideal','follower1','follower2');legend boxoff;
%  plot(leader_ideal.x(1:n),leader_ideal.y(1:n),'ro');h_xlabel=xlabel('x/m');h_ylabel=ylabel('y/m');h_legend=legend('实际轨迹','期望轨迹');legend boxoff;
set(h_xlabel,'fontsize',16);set(h_ylabel,'fontsize',16);set(gca,'fontsize',14); set(h_legend,'fontsize',14);
title([plot_string , '编队切换固定队形']);

axis([0 100,0 40])

正确的运行结果如下：