机器人编队(three_car_formation_control.m)

【实例简介】机器人编队运行
【实例截图】
【核心代码】%% 初始化参数
dx0 = 0; dx1 = 0;      dx2 = -0.4; dx3 = 0.4;  %编队队形相对位置
dy0 = 0; dy1 = 0.4928; dy2 = -0.2; dy3 = -0.2;
x0 = 0;  x1 = 0;  x2 = -1.5; x3 = 1.5;  %机器人初始位置
y0 = -1; y1 = 0;  y2 = 0;    y3 = 0; 

inter = 0.125; %时间间隔
t = 0:inter:10000; 
kx = 3; ky = 3; 
v1 = 0.1; v2 = 0.1; v3 = 0.1; %机器人初始速度
theta1 = pi; theta2 = 5*pi/6; theta3 = 0; %机器人初始航向角
%% 绘制机器人初始位置
plot(x0,y0,'.','markersize',50); %查看机器人分布
text(x0 2,y0,'0号','FontSize',20)
hold on
plot(x1,y1,'.','markersize',50);
text(x1 2,y1,'1号','FontSize',20)
hold on
plot(x2,y2,'.','markersize',50);
text(x2 2,y2,'2号','FontSize',20)
hold on
plot(x3,y3,'.','markersize',50);
text(x3 2,y3,'3号','FontSize',20)
grid MINOR
xlabel('x/m')
ylabel('y/m')
title('机器人初始位置分布','Color','m')
set(gca,'FontSize',16)
axis equal
%% 领航者坐标（x,y）、航向角、速度、角速度---全过程
Agv0 = 0.5;  %领航者角速度
Theta0 = Agv0*t;  %领航者角度
V0 = 0.5;  %领航者速度
r0 = 0; %领航者速度导数
uxr = -V0*sin(Theta0)*Agv0;
uyr = V0*cos(Theta0)*Agv0;
%% 从机1-5初始化阶段----从机1-5
error = 0.05; %Toi时间点
Theta1 = []; V1 = []; Agv1 = []; r1 = [];
Theta2 = []; V2 = []; Agv2 = []; r2 = [];
Theta3 = []; V3 = []; Agv3 = []; r3 = [];

ux1 = 0; ux2 = 0; ux3 = 0;
uy1 = 0; uy2 = 0; uy3 = 0; 
zx1 = 0; zx2 = 0; zx3 = 0; 
zy1 = 0; zy2 = 0; zy3 = 0; 

X0 = []; Y0 = []; X0(1) = x0; Y0(1) = y0;
X1 = []; Y1 = []; X1(1) = x1; Y1(1) = y1;
X2 = []; Y2 = []; X2(1) = x2; Y2(1) = y2;
X3 = []; Y3 = []; X3(1) = x3; Y3(1) = y3;

Theta1(1) = theta1; V1(1) = v1; 
Theta2(1) = theta2; V2(1) = v2;
Theta3(1) = theta3; V3(1) = v3;

i = 1;
v0_temp = 0.5; v1_temp = 1; v2_temp = 1; v3_temp = 1; %速度临时变量
a0_temp = 0.5; a1_temp = 1; a2_temp = 1; a3_temp = 1; %角度临时变量
figure
grid MINOR
axis equal
triangle_line1 = []; 
triangle_line2 = [];
triangle_line3 = [];
while(1)
    
   
      if(i <= 120) 
      Agv1(i) = phi_theta(Theta1(i) - Theta0(i)) Agv0;
      Agv2(i) = phi_theta(Theta2(i) - Theta0(i)) Agv0;
      Agv3(i) = phi_theta(Theta3(i) - Theta0(i)) Agv0;
   
      r1(i) = phi_theta(V1(i) - V0) r0;
      r2(i) = phi_theta(V2(i) - V0) r0;
      r3(i) = phi_theta(V3(i) - V0) r0; 
     
  
      Theta1(i 1) = Theta1(i) Agv1(i)*inter;
      Theta2(i 1) = Theta2(i) Agv2(i)*inter;
      Theta3(i 1) = Theta3(i) Agv3(i)*inter;
    
      V1(i 1) = V1(i) r1(i)*inter;
      V2(i 1) = V2(i) r2(i)*inter;
      V3(i 1) = V3(i) r3(i)*inter;
     
      X0(i 1) = X0(i) V0*cos(Theta0(i 1))*inter;
      Y0(i 1) = Y0(i) V0*sin(Theta0(i 1))*inter;
      X1(i 1) = X1(i) V1(i 1)*cos(Theta1(i 1))*inter;
      Y1(i 1) = Y1(i) V1(i 1)*sin(Theta1(i 1))*inter;
      X2(i 1) = X2(i) V2(i 1)*cos(Theta2(i 1))*inter;
      Y2(i 1) = Y2(i) V2(i 1)*sin(Theta2(i 1))*inter;
      X3(i 1) = X3(i) V3(i 1)*cos(Theta3(i 1))*inter;
      Y3(i 1) = Y3(i) V3(i 1)*sin(Theta3(i 1))*inter;
      
   else
      
      zx1 = 0.5*(2*X1(i) - X0(i) -X2(i) - 2*dx1 dx2 dx0);
      zy1 = 0.5*(2*Y1(i) - Y0(i) -Y2(i) - 2*dy1 dy2 dy0);
      zx2 = X2(i) - X3(i) - dx2 dx3;
      zy2 = Y2(i) - Y3(i) - dy2 dy3;
      zx3 = X3(i) - X1(i) - dx3 dx1;
      zy3 = Y3(i) - Y1(i) - dy3 dy1;
      
      ux1 = -kx*(V1(i)*cos(Theta1(i)) - V0*cos(Theta0(i)) - phi_xy(zx1));
      uy1 = -ky*(V1(i)*sin(Theta1(i)) - V0*sin(Theta0(i)) - phi_xy(zy1));
      ux2 = -kx*(V2(i)*cos(Theta2(i)) - V0*cos(Theta0(i)) - phi_xy(zx2));
      uy2 = -ky*(V2(i)*sin(Theta2(i)) - V0*sin(Theta0(i)) - phi_xy(zy2));
      ux3 = -kx*(V3(i)*cos(Theta3(i)) - V0*cos(Theta0(i)) - phi_xy(zx3));
      uy3 = -ky*(V3(i)*sin(Theta3(i)) - V0*sin(Theta0(i)) - phi_xy(zy3));
      ux1 = ux1 uxr(i);
      uy1 = uy1 uyr(i);
      ux2 = ux2 uxr(i);
      uy2 = uy2 uyr(i);
      ux3 = ux3 uxr(i);
      uy3 = uy3 uyr(i);
      
      r1(i) = cos(Theta1(i))*ux1 sin(Theta1(i))*uy1;
      Agv1(i) = ((-sin(Theta1(i))/V1(i))*ux1) ((cos(Theta1(i))/V1(i))*uy1);
      r2(i) = cos(Theta2(i))*ux2 sin(Theta2(i))*uy2 r0;
      Agv2(i) = ((-sin(Theta2(i))/V2(i))*ux2) ((cos(Theta2(i))/V2(i))*uy2);
      r3(i) = cos(Theta3(i))*ux3 sin(Theta3(i))*uy3 r0;
      Agv3(i) = ((-sin(Theta3(i))/V3(i))*ux3) ((cos(Theta3(i))/V3(i))*uy3);
      
      V1(i 1) = V1(i) r1(i)*inter;
      V2(i 1) = V2(i) r2(i)*inter;
      V3(i 1) = V3(i) r3(i)*inter;
      
      Theta1(i 1) = Theta1(i) Agv1(i)*inter;
      Theta2(i 1) = Theta2(i) Agv2(i)*inter;
      Theta3(i 1) = Theta3(i) Agv3(i)*inter;
      
      X0(i 1) = X0(i) V0*cos(Theta0(i 1))*inter;
      Y0(i 1) = Y0(i) V0*sin(Theta0(i 1))*inter;
      X1(i 1) = X1(i) V1(i 1)*cos(Theta1(i 1))*inter;
      Y1(i 1) = Y1(i) V1(i 1)*sin(Theta1(i 1))*inter;
      X2(i 1) = X2(i) V2(i 1)*cos(Theta2(i 1))*inter;
      Y2(i 1) = Y2(i) V2(i 1)*sin(Theta2(i 1))*inter;
      X3(i 1) = X3(i) V3(i 1)*cos(Theta3(i 1))*inter;
      Y3(i 1) = Y3(i) V3(i 1)*sin(Theta3(i 1))*inter;
      
      a0_temp = Theta0(i 1); a1_temp = Theta1(i 1); a2_temp = Theta2(i 1); a3_temp = Theta3(i 1);
      v0_temp = V0; v1_temp = V1(i 1);v2_temp = V2(i 1);v3_temp = V3(i 1);
   end
   hold on
    plot([X1(i),X1(i 1)],[Y1(i),Y1(i 1)],'r-','LineWidth',2)
    hold on
    plot([X2(i),X2(i 1)],[Y2(i),Y2(i 1)],'b-','LineWidth',2)
    hold on
    plot([X3(i),X3(i 1)],[Y3(i),Y3(i 1)],'g-','LineWidth',2)
    hold on
    plot([X0(i),X0(i 1)],[Y0(i),Y0(i 1)],'k-.','LineWidth',2)
    
    delete(triangle_line1);
    delete(triangle_line2);
    delete(triangle_line3);
    
    triangle_line1 = plot([X1(i 1),X2(i 1)],[Y1(i 1),Y2(i 1)],'k--');
    hold on 
    triangle_line2 = plot([X2(i 1),X3(i 1)],[Y2(i 1),Y3(i 1)],'k--');
    hold on
    triangle_line3 = plot([X3(i 1),X1(i 1)],[Y3(i 1),Y1(i 1)],'k--');
    pause(0.05)
      i = i 1;
   
end