领航跟随型编队（七）leader-follower实现角度不变的直线编队

  %初始化leader 状态；
    leader.w=0.6;
    leader.v=0.75;
    leader.theta=0;%leader 的航向角；
   % leader.beta=pi/6;%两个车的中心线与其中之一的连线，相当于方位角  
    leader.x=4.0;leader.y=9.0;

  %初始化follower1的速度位姿
follower1.x=3;follower1.y=8;follower1.theta=0;follower1_ideal.beta=pi/6;
follower1.v=1.0;follower1.beta(1)=pi/6;
follower1.w=0.6
                                             %初始化follower2 的位姿
follower2.x=3;follower2.y=10;follower2.theta=0;follower2_ideal.beta=pi/6;
follower2.beta(1)=pi/6;
follower2.v=1.0;

%初始化队形参数即增益系数和理想距离和角度
l21=1.4;c21=1.4;c31=1.4;gama21=pi/3;gama31=pi/3;%编队队形

b=0.1315;d=0.15;r=0.095;%车身参数
K11=1;K12=0.6;K13=0.5;%增益系数

df=0.85;%队形的最大宽度
J=20;

for i=1:ts:J
        Step=Step+1;                            
    leader_ideal.w=0.0;
    leader_ideal.v=1;
    leader_ideal.theta=0;%leader 的航向角；
   % leader.beta=pi/6;%两个车的中心线与其中之一的连线，相当于方位角
   leader_ideal.x=leader.x+ts*leader.v*cos( leader.theta);%采样周期内一步一步的计算，
   leader_ideal.y=leader.y+ts*leader.v*sin( leader.theta);
   
                                                  
 follower1_ideal.theta =leader.theta;
 follower1_ideal.y = leader.y-c21*sin(gama21+follower1_ideal.theta);
 follower1_ideal.x =leader.x-c21*cos(gama21+follower1_ideal.theta);
 follower1_ideal.v=leader.v;
 follower1_ideal.w= leader.w;
                                                   
 follower2_ideal.y = leader.y+c31*sin(gama31+follower2_ideal.theta);
 follower2_ideal.x =leader.x-c31*cos(gama31+follower2_ideal.theta);
 follower2_ideal.v=leader.v;
 follower2_ideal.w= leader.w;  
                                                        %计算follower1d的误差
 e1(1) =cos(follower1.theta(step))*(follower1_ideal.x(step)-follower1.x(step))+sin(follower1.theta(step))*( follower1_ideal.y(step)-follower1.y(step));
  e1(2)=-sin(follower1.theta(step))*(follower1_ideal.x(step)-follower1.x(step))+cos(follower1.theta(step))*(follower1_ideal.y(step)-follower1.y(step));
  e1(3)= follower1_ideal.theta(step) -follower1.theta(step);
  follower1_xe=e1(1);
follower1_ye=e1(2);
follower1_thetae=e1(3);

follower1.v=follower1_ideal.v*cos(follower1_thetae)+K11*follower1_xe;
follower1.w=follower1_ideal.w+K12*follower1_ideal.v*follower1_ye+K13*follower1_ideal.v*sin(follower1_thetae);                          follower1.theta=follower1.theta+ts*follower1.w;
 follower1.x=follower1.x+ts*follower1.v*cos( follower1.theta);
 follower1.y=follower1.y+ts*follower1.v*sin( follower1.theta);
 plot( follower1.x, follower1.y, 'y.');hold on;

 e2(1) =cos(follower2.theta(step))*(follower2_ideal.x(step)-follower2.x(step))+sin(follower2.theta(step))*( follower2_ideal.y(step)-follower2.y(step));
 e2(2)=-sin(follower2.theta(step))*(follower2_ideal.x(step)-follower2.x(step))+cos(follower2.theta(step))*(follower2_ideal.y(step)-follower2.y(step));
 e2(3)= follower2_ideal.theta(step) -follower2.theta(step);
  
follower2_xe=e2(1);
follower2_ye=e2(2);
follower2_thetae=e2(3);

    follower2.v=follower2_ideal.v*cos(follower2_thetae)+K11*follower2_xe;
follower2.w=follower2_ideal.w+K12*follower2_ideal.v*follower1_ye+K13*follower2_ideal.v*sin(follower2_thetae);
   
                                       %计算follower2当前的位姿 
   follower2.theta=follower2.theta+ts*follower2.w;
   follower2.x=follower2.x+ts*follower2.v*cos( follower2.theta);
   follower2.y=follower2.y+ts*follower2.v*sin( follower2.theta);
   plot( follower2.x, follower2.y, 'c.');hold on;
   

    end
%%
 figure(1);
 %plot(follower1_xe,follower1_ye,':r');
% hold on;
  plot( leader.x, leader.y, 'r.',follower1.x, follower1.y, 'y.',follower2.x, follower2.y,'c.');
  hold on
  GraphicHandle= plot( leader.x, leader.y, 'b.',follower1.x, follower1.y, 'r.', follower2.x,follower2.y,'c.');
 set( GraphicHandle,'MarkerSize',4);
  xlabel('x[m]') 
  ylabel('y[m]')
  title ('trajectory of the robots ')
 legend('leader','follower1','follower2')
 
 figure(2);
   GraphicHandle= plot( leader.v,'ko');
   hold on;
  ylabel(' leader.v[m/s]') 
  xlabel('t[s]')
  h=title ('velocity of the leader ')
  set(h,'Fontsize',15);
  
  figure(3);
   GraphicHandle= plot( leader_xe,'ko');
   hold on;
  ylabel('leader_xe[m]') 
  xlabel('ts[s]')
  h=title ('error of the leader ')
  set(h,'Fontsize',15);

end