领航跟随型编队（七）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