赞
踩
控制协议为:
u
i
(
t
)
=
K
∑
j
=
1
M
w
i
j
(
(
x
i
(
t
)
−
h
i
(
t
)
)
−
(
x
j
(
t
)
−
h
j
(
t
)
)
)
+
K
∑
k
=
M
+
1
N
w
i
k
(
(
x
i
(
t
)
−
h
i
(
t
)
)
−
x
k
(
t
)
)
+
v
i
(
t
)
=
K
∑
j
=
1
M
w
i
j
(
(
x
i
−
h
i
)
−
(
x
j
−
h
j
)
)
+
K
∑
k
=
M
+
1
N
w
i
k
(
(
x
i
−
h
i
)
−
x
k
)
+
v
i
=
K
∑
j
=
1
M
w
i
j
(
δ
i
−
δ
j
)
+
K
∑
k
=
M
+
1
N
w
i
k
(
δ
i
−
x
k
)
+
v
i
%% Time-varying formation tracking for linear multiagent systems with multiple leaders % Author: Zhao-Jichao % Date: 2021-07-08 clear clc %% System Description global A B L1 L2 K A = [0 1 1 1 2 1 -2 -6 -3]; B = [0 1 -1 0 0 0]; Bbar = [0 -1 0 1 0 0]; Btilde=[0 0 1]; delta = 0.55; Q = eye(3); R = eye(2); P = care(A,B,Q,R); L = [1 -1 0 0 0 0 0 0 0 0 0 0 0 0 3 0 0 0 0 0 0 0 0 -1 -1 -1 -1 0 1 0 0 0 0 0 0 0 0 0 0 0 0 -1 1 0 0 0 0 0 0 0 0 0 0 0 0 -1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 -1 0 0 0 0 0 0 0 0 0 0 0 0 1 -1 0 0 0 0 0 0 0 0 0 0 0 -1 5 0 -1 -1 -1 -1 0 0 0 0 0 0 0 -1 1 0 0 0 0 0 0 0 0 0 0 0 0 -1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]; L1 = L(1:10, 1:10); L2 = L(1:10, 11:13); lambda1 = min(eig(L1)); K = -delta*1/(lambda1)*inv(R)*B'*P; % Initial states of followers x1 = [03 05 07]'; x2 = [04 06 08]'; x3 = [05 07 09]'; x4 = [06 08 10]'; x5 = [07 09 11]'; x6 = [08 10 12]'; x7 = [09 11 13]'; x8 = [10 12 14]'; x9 = [11 13 15]'; x10= [12 14 16]'; % Initial states of leaders x11= [13 15 17]'; x12= [14 16 18]'; x13= [15 17 19]'; XF = [x1' x2' x3' x4' x5' x6' x7' x8' x9' x10']'; XE = [x11' x12' x13']'; in = [XF' XE']'; syms t real h1 = [15*sin(t+(1-1)*pi/5) -15*cos(t+(1-1)*pi/5) 30*cos(t+(1-1)*pi/5)]'; h2 = [15*sin(t+(2-1)*pi/5) -15*cos(t+(2-1)*pi/5) 30*cos(t+(2-1)*pi/5)]'; h3 = [15*sin(t+(3-1)*pi/5) -15*cos(t+(3-1)*pi/5) 30*cos(t+(3-1)*pi/5)]'; h4 = [15*sin(t+(4-1)*pi/5) -15*cos(t+(4-1)*pi/5) 30*cos(t+(4-1)*pi/5)]'; h5 = [15*sin(t+(5-1)*pi/5) -15*cos(t+(5-1)*pi/5) 30*cos(t+(5-1)*pi/5)]'; h6 = [15*sin(t+(6-1)*pi/5) -15*cos(t+(6-1)*pi/5) 30*cos(t+(6-1)*pi/5)]'; h7 = [15*sin(t+(7-1)*pi/5) -15*cos(t+(7-1)*pi/5) 30*cos(t+(7-1)*pi/5)]'; h8 = [15*sin(t+(8-1)*pi/5) -15*cos(t+(8-1)*pi/5) 30*cos(t+(8-1)*pi/5)]'; h9 = [15*sin(t+(9-1)*pi/5) -15*cos(t+(9-1)*pi/5) 30*cos(t+(9-1)*pi/5)]'; h10= [15*sin(t+(10-1)*pi/5) -15*cos(t+(10-1)*pi/5) 30*cos(t+(10-1)*pi/5)]'; H = [h1' h2' h3' h4' h5' h6' h7' h8' h9' h10']'; %% tBegin = 0; tFinal = 100; [t, out] = ode45(@odeFun, [tBegin, tFinal], in); %% Draw Results figure(1) plot3(out(:,1), out(:,2), out(:,3), 'linewidth',1.5); hold on; grid on; plot3(out(:,4), out(:,5), out(:,6), 'linewidth',1.5); hold on; grid on; plot3(out(:,7), out(:,8), out(:,9), 'linewidth',1.5); hold on; grid on; plot3(out(:,10),out(:,11),out(:,12), 'linewidth',1.5); hold on; grid on; plot3(out(:,13),out(:,14),out(:,15), 'linewidth',1.5); hold on; grid on; plot3(out(:,16),out(:,17),out(:,18), 'linewidth',1.5); hold on; grid on; plot3(out(:,19),out(:,20),out(:,21), 'linewidth',1.5); hold on; grid on; plot3(out(:,22),out(:,23),out(:,24), 'linewidth',1.5); hold on; grid on; plot3(out(:,25),out(:,26),out(:,27), 'linewidth',1.5); hold on; grid on; plot3(out(:,28),out(:,29),out(:,30), 'linewidth',1.5); hold on; grid on; plot3(out(:,31),out(:,32),out(:,33), 'linewidth',1.5); hold on; grid on; plot3(out(:,34),out(:,35),out(:,36), 'linewidth',1.5); hold on; grid on; plot3(out(:,37),out(:,38),out(:,39), 'linewidth',1.5); hold on; grid on; legend('f1','f2','f3','f4','f5','f6','f7','f8','f9','f10','l11','l12','l13') figure(2) % Plot snapshots t = 1000; scatter3(out(t,1), out(t,2), out(t,3), '*', 'linewidth',1.5); hold on; grid on; scatter3(out(t,4), out(t,5), out(t,6), '*', 'linewidth',1.5); hold on; grid on; scatter3(out(t,7), out(t,8), out(t,9), '*', 'linewidth',1.5); hold on; grid on; scatter3(out(t,10),out(t,11),out(t,12),'o', 'linewidth',1.5); hold on; grid on; scatter3(out(t,13),out(t,14),out(t,15),'o', 'linewidth',1.5); hold on; grid on; scatter3(out(t,16),out(t,17),out(t,18),'d', 'linewidth',1.5); hold on; grid on; scatter3(out(t,19),out(t,20),out(t,21),'d', 'linewidth',1.5); hold on; grid on; scatter3(out(t,22),out(t,23),out(t,24),'d', 'linewidth',1.5); hold on; grid on; scatter3(out(t,25),out(t,26),out(t,27),'^', 'linewidth',1.5); hold on; grid on; scatter3(out(t,28),out(t,29),out(t,30),'^', 'linewidth',1.5); hold on; grid on; scatter3(out(t,31),out(t,32),out(t,33),'p', 'linewidth',1.5); hold on; grid on; scatter3(out(t,34),out(t,35),out(t,36),'p', 'linewidth',1.5); hold on; grid on; scatter3(out(t,37),out(t,38),out(t,39),'p', 'linewidth',1.5); hold on; grid on; legend('f1','f2','f3','f4','f5','f6','f7','f8','f9','f10','l11','l12','l13') %% function out = odeFun(t, in) global A B L1 L2 K XF = in(1:30,:); XE = in(31:39,:); h1 = [15*sin(t+(1-1)*pi/5) -15*cos(t+(1-1)*pi/5) 30*cos(t+(1-1)*pi/5)]'; h2 = [15*sin(t+(2-1)*pi/5) -15*cos(t+(2-1)*pi/5) 30*cos(t+(2-1)*pi/5)]'; h3 = [15*sin(t+(3-1)*pi/5) -15*cos(t+(3-1)*pi/5) 30*cos(t+(3-1)*pi/5)]'; h4 = [15*sin(t+(4-1)*pi/5) -15*cos(t+(4-1)*pi/5) 30*cos(t+(4-1)*pi/5)]'; h5 = [15*sin(t+(5-1)*pi/5) -15*cos(t+(5-1)*pi/5) 30*cos(t+(5-1)*pi/5)]'; h6 = [15*sin(t+(6-1)*pi/5) -15*cos(t+(6-1)*pi/5) 30*cos(t+(6-1)*pi/5)]'; h7 = [15*sin(t+(7-1)*pi/5) -15*cos(t+(7-1)*pi/5) 30*cos(t+(7-1)*pi/5)]'; h8 = [15*sin(t+(8-1)*pi/5) -15*cos(t+(8-1)*pi/5) 30*cos(t+(8-1)*pi/5)]'; h9 = [15*sin(t+(9-1)*pi/5) -15*cos(t+(9-1)*pi/5) 30*cos(t+(9-1)*pi/5)]'; h10= [15*sin(t+(10-1)*pi/5) -15*cos(t+(10-1)*pi/5) 30*cos(t+(10-1)*pi/5)]'; H = [h1' h2' h3' h4' h5' h6' h7' h8' h9' h10']'; dXE = kron(eye(3),A)*XE; U = kron(L1,eye(2))*kron(eye(10),K)*(XF-H)... + kron(L2,eye(2))*kron(eye(3),K)*XE; dXF = kron(eye(10),A)*XF + kron(eye(10),B)*U; out = [dXF; dXE]; end
%% Time-varying formation tracking for linear multiagent systems with multiple leaders % Author: Zhao-Jichao % Date: 2021-07-08 clear clc %% System Description global A B L1 L2 K A = [0 1 1 1 2 1 -2 -6 -3]; B = [0 1 -1 0 0 0]; Bbar = [0 -1 0 1 0 0]; Btilde=[0 0 1]; delta = 0.55; Q = eye(3); R = eye(2); P = care(A,B,Q,R); L = [1 -1 0 0 0 0 0 0 0 0 0 0 0 0 3 0 0 0 0 0 0 0 0 -1 -1 -1 -1 0 1 0 0 0 0 0 0 0 0 0 0 0 0 -1 1 0 0 0 0 0 0 0 0 0 0 0 0 -1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 -1 0 0 0 0 0 0 0 0 0 0 0 0 1 -1 0 0 0 0 0 0 0 0 0 0 0 -1 5 0 -1 -1 -1 -1 0 0 0 0 0 0 0 -1 1 0 0 0 0 0 0 0 0 0 0 0 0 -1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]; L1 = L(1:10, 1:10); L2 = L(1:10, 11:13); lambda1 = min(eig(L1)); K = -delta*1/(lambda1)*inv(R)*B'*P; % Initial states of followers x1 = [03 05 07]'; x2 = [04 06 08]'; x3 = [05 07 09]'; x4 = [06 08 10]'; x5 = [07 09 11]'; x6 = [08 10 12]'; x7 = [09 11 13]'; x8 = [10 12 14]'; x9 = [11 13 15]'; x10= [12 14 16]'; % Initial states of leaders x11= [13 15 17]'; x12= [14 16 18]'; x13= [15 17 19]'; XF = [x1' x2' x3' x4' x5' x6' x7' x8' x9' x10']'; XE = [x11' x12' x13']'; in = [XF' XE']'; syms t real h1 = [15*sin(t+(1-1)*pi/5) -15*cos(t+(1-1)*pi/5) 30*cos(t+(1-1)*pi/5)]'; h2 = [15*sin(t+(2-1)*pi/5) -15*cos(t+(2-1)*pi/5) 30*cos(t+(2-1)*pi/5)]'; h3 = [15*sin(t+(3-1)*pi/5) -15*cos(t+(3-1)*pi/5) 30*cos(t+(3-1)*pi/5)]'; h4 = [15*sin(t+(4-1)*pi/5) -15*cos(t+(4-1)*pi/5) 30*cos(t+(4-1)*pi/5)]'; h5 = [15*sin(t+(5-1)*pi/5) -15*cos(t+(5-1)*pi/5) 30*cos(t+(5-1)*pi/5)]'; h6 = [15*sin(t+(6-1)*pi/5) -15*cos(t+(6-1)*pi/5) 30*cos(t+(6-1)*pi/5)]'; h7 = [15*sin(t+(7-1)*pi/5) -15*cos(t+(7-1)*pi/5) 30*cos(t+(7-1)*pi/5)]'; h8 = [15*sin(t+(8-1)*pi/5) -15*cos(t+(8-1)*pi/5) 30*cos(t+(8-1)*pi/5)]'; h9 = [15*sin(t+(9-1)*pi/5) -15*cos(t+(9-1)*pi/5) 30*cos(t+(9-1)*pi/5)]'; h10= [15*sin(t+(10-1)*pi/5) -15*cos(t+(10-1)*pi/5) 30*cos(t+(10-1)*pi/5)]'; H = [h1' h2' h3' h4' h5' h6' h7' h8' h9' h10']'; %% tBegin = 0; tFinal = 100; [t, out] = ode45(@odeFun, [tBegin, tFinal], in); %% Draw Results % figure(1) % plot3(out(:,1), out(:,2), out(:,3), 'linewidth',1.5); hold on; grid on; % plot3(out(:,4), out(:,5), out(:,6), 'linewidth',1.5); hold on; grid on; % plot3(out(:,7), out(:,8), out(:,9), 'linewidth',1.5); hold on; grid on; % plot3(out(:,10),out(:,11),out(:,12), 'linewidth',1.5); hold on; grid on; % plot3(out(:,13),out(:,14),out(:,15), 'linewidth',1.5); hold on; grid on; % plot3(out(:,16),out(:,17),out(:,18), 'linewidth',1.5); hold on; grid on; % plot3(out(:,19),out(:,20),out(:,21), 'linewidth',1.5); hold on; grid on; % plot3(out(:,22),out(:,23),out(:,24), 'linewidth',1.5); hold on; grid on; % plot3(out(:,25),out(:,26),out(:,27), 'linewidth',1.5); hold on; grid on; % plot3(out(:,28),out(:,29),out(:,30), 'linewidth',1.5); hold on; grid on; % plot3(out(:,31),out(:,32),out(:,33), 'linewidth',1.5); hold on; grid on; % plot3(out(:,34),out(:,35),out(:,36), 'linewidth',1.5); hold on; grid on; % plot3(out(:,37),out(:,38),out(:,39), 'linewidth',1.5); hold on; grid on; % legend('f1','f2','f3','f4','f5','f6','f7','f8','f9','f10','l11','l12','l13') % figure(2) % Plot snapshots % t = 1000; % scatter3(out(t,1), out(t,2), out(t,3), '*', 'linewidth',1.5); hold on; grid on; % scatter3(out(t,4), out(t,5), out(t,6), '*', 'linewidth',1.5); hold on; grid on; % scatter3(out(t,7), out(t,8), out(t,9), '*', 'linewidth',1.5); hold on; grid on; % scatter3(out(t,10),out(t,11),out(t,12),'o', 'linewidth',1.5); hold on; grid on; % scatter3(out(t,13),out(t,14),out(t,15),'o', 'linewidth',1.5); hold on; grid on; % scatter3(out(t,16),out(t,17),out(t,18),'d', 'linewidth',1.5); hold on; grid on; % scatter3(out(t,19),out(t,20),out(t,21),'d', 'linewidth',1.5); hold on; grid on; % scatter3(out(t,22),out(t,23),out(t,24),'d', 'linewidth',1.5); hold on; grid on; % scatter3(out(t,25),out(t,26),out(t,27),'^', 'linewidth',1.5); hold on; grid on; % scatter3(out(t,28),out(t,29),out(t,30),'^', 'linewidth',1.5); hold on; grid on; % scatter3(out(t,31),out(t,32),out(t,33),'p', 'linewidth',1.5); hold on; grid on; % scatter3(out(t,34),out(t,35),out(t,36),'p', 'linewidth',1.5); hold on; grid on; % scatter3(out(t,37),out(t,38),out(t,39),'p', 'linewidth',1.5); hold on; grid on; % legend('f1','f2','f3','f4','f5','f6','f7','f8','f9','f10','l11','l12','l13') figure(3) for time = 1:1:length(t)-00 out1 = [out(:,1) out(:,4) out(:,7) out(:,10) out(:,13) out(:,16) out(:,19) out(:,22) out(:,25) out(:,28) out(:,31) out(:,34) out(:,37)]; out2 = [out(:,2) out(:,5) out(:,8) out(:,11) out(:,14) out(:,17) out(:,20) out(:,23) out(:,26) out(:,29) out(:,32) out(:,35) out(:,38)]; out3 = [out(:,3) out(:,6) out(:,9) out(:,12) out(:,15) out(:,18) out(:,21) out(:,24) out(:,27) out(:,30) out(:,33) out(:,36) out(:,39)]; scatter3(out3(time,:), out2(time,:), out1(time,:), 'linewidth',1.5); axis([-300, 300, -100,100, -100,100]); tt = kron(ones(1,size(out1,2)),t); pause(0.1) end disp('end') %% function out = odeFun(t, in) global A B L1 L2 K XF = in(1:30,:); XE = in(31:39,:); h1 = [15*sin(t+(1-1)*pi/5) -15*cos(t+(1-1)*pi/5) 30*cos(t+(1-1)*pi/5)]'; h2 = [15*sin(t+(2-1)*pi/5) -15*cos(t+(2-1)*pi/5) 30*cos(t+(2-1)*pi/5)]'; h3 = [15*sin(t+(3-1)*pi/5) -15*cos(t+(3-1)*pi/5) 30*cos(t+(3-1)*pi/5)]'; h4 = [15*sin(t+(4-1)*pi/5) -15*cos(t+(4-1)*pi/5) 30*cos(t+(4-1)*pi/5)]'; h5 = [15*sin(t+(5-1)*pi/5) -15*cos(t+(5-1)*pi/5) 30*cos(t+(5-1)*pi/5)]'; h6 = [15*sin(t+(6-1)*pi/5) -15*cos(t+(6-1)*pi/5) 30*cos(t+(6-1)*pi/5)]'; h7 = [15*sin(t+(7-1)*pi/5) -15*cos(t+(7-1)*pi/5) 30*cos(t+(7-1)*pi/5)]'; h8 = [15*sin(t+(8-1)*pi/5) -15*cos(t+(8-1)*pi/5) 30*cos(t+(8-1)*pi/5)]'; h9 = [15*sin(t+(9-1)*pi/5) -15*cos(t+(9-1)*pi/5) 30*cos(t+(9-1)*pi/5)]'; h10= [15*sin(t+(10-1)*pi/5) -15*cos(t+(10-1)*pi/5) 30*cos(t+(10-1)*pi/5)]'; H = [h1' h2' h3' h4' h5' h6' h7' h8' h9' h10']'; dXE = kron(eye(3),A)*XE; U = kron(L1,eye(2))*kron(eye(10),K)*(XF-H)... + kron(L2,eye(2))*kron(eye(3),K)*XE; dXF = kron(eye(10),A)*XF + kron(eye(10),B)*U; out = [dXF; dXE]; end
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。