%初始化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
