基本信息
源码名称：机器人编队(three_car_formation_control.m)
源码大小：5.86KB
文件格式：.m
开发语言：MATLAB
更新时间：2020-10-21
友情提示：（无需注册或充值，赞助后即可获取资源下载链接）
嘿，亲！知识可是无价之宝呢，但咱这精心整理的资料也耗费了不少心血呀。小小地破费一下，绝对物超所值哦！如有下载和支付问题，请联系我们QQ(微信同号)：813200300
本次赞助数额为： 2 元×
微信扫码支付：2 元
×
请留下您的邮箱，我们将在2小时内将文件发到您的邮箱
源码介绍
机器人编队运行
%% 初始化参数
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
机器人编队运行
%% 初始化参数
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