✅作者简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,
代码获取、论文复现及科研仿真合作可私信。
🍎个人主页:Matlab科研工作室
🍊个人信条:格物致知。
更多Matlab完整代码及仿真定制内容点击👇
🔥 内容介绍
本文提出了一种非线性控制设计,用于稳定机器鱼模型学校中的平行和圆周运动。鱼机器人的闭环游泳动力学由典型的 Chaplygin 雪橇代表,这是一种由内转子驱动的非完整机械系统。鱼机器人根据连接的无向通信图交换相对状态信息,并形成耦合的非线性二阶振荡器系统。先前关于恒速自驱动粒子集体运动的工作是我们方法的基础。然而,与自驱动粒子不同的是,鱼机器人遵循极限循环动力学,以维持周期性拍动,以不同的速度向前运动。平行运动和圆周运动是在平均意义上实现的。所提出的控制律不包括代理动力学的反馈线性化。数值模拟说明了该方法。
📣 部分代码
clear; close all; clc;
addpath('./brewer');
addpath('./cmocean_v2.0/cmocean');
addpath('./export_fig/');
lastk = 200; % this many points are used to plot final limit cycle
% parameters
m = 1.4;
l = 0.31;
d = 0.5;
b = 0.1395;
a = (m*l^2)/(b*d);
N = 8;
T = 20*120; % total simulation time
% parallel gains
k1 = 0.5;
k2 = 2;
% circular gains
k1c = k1;
k2c = k2;
k3c = -.3;
vref = 0*b*k1/(m*l);
omegaref = 0.2;
% create communication topology
%% graph used by Laplacian controllers
% %all-to-all
% D = eye(N,N)*(N-1);
% A = ones(N,N);
% for i =1:1:N
% A(i,i) = 0;
% end
% L = D-A;
% rng(3);
% circulant
L = zeros(N,N);
for i = 1:1:N
L(i,i) = 2;
% aft neighbor
if ( i == 1)
L(i,end) = -1;
else
L(i,i-1) = -1;
end
% fore neighbor
if ( i == N)
L(i,1) = -1;
else
L(i,i+1) = -1;
end
end
%% ploting
% plot colors
shift = 2;
%cmap = parula(N);
%cmap = cmocean('ice',N);
cmap = cmocean('deep',N+shift);
% shift
cmap = cmap(shift:end,:);
%% initial conditions
% %Random heading and random angular rate
% theta0 = rand([N 1])*2*pi;
% omega0 = rand([N 1])*1;
% %Random heading and zero angular rate
% r0 = rand(N,1)*10 + rand(N,1)*10*1i;
% v0 = 0*rand(N,1);
%
% % theta0 = [0 45 90]'*pi/180;
% % omega0 = 0.1*[-1 0.5 1]';
% theta0 = rand([N 1])*pi;
% omega0 = rand(N,1)*0;
% Equal heading and equal angular rate
% theta0 = [ones(N,1)*0];
% omega0 = [ones(N,1)*1]
% Equal heading and random angular rate
% theta0 = [ones(N,1)*0];
% omega0 = rand([N 1])*1;
% % Np fish anti-parallel and random angular rate
% Np = 1;
% theta0 = [ones([Np 1])*0; ones([N-Np 1])*pi ];
% omega0 = rand([N 1])*1;
% % circular initial formation
% th_circ = [0:2*pi/(N):2*pi];
% th_circ = th_circ(1:end-1)';
% R = 5;
% r0 = R*cos(th_circ) + R*sin(th_circ)*1i;
% v0 = 0*rand(N,1);
% theta0 = rand([N 1])*2*pi;
% omega0 = 0*rand(N,1);
% line initial formation
Linit = 5;
dely = 0;
r0 = linspace(-Linit/2,Linit/2,N)' - 1i*dely;
v0 = 0*rand(N,1);
theta0 = rand([N 1])*2*pi;
omega0 = 0*rand(N,1);
% % from paul's code
% zfix =[ 0 0 1.3110 0 8.7639 9.5789;
% 0 0 1.7552 0 8.9461 5.3317;
% 0 0 0.4410 0 0.8504 6.9188;
% 0 0 0.6224 0 0.3905 3.1552;
% 0 0 2.5156 0 1.6983 6.8650;
% 0 0 3.0419 0 8.7814 8.3463;
% 0 0 0.9847 0 0.9835 0.1829;
% 0 0 2.1750 0 4.2111 7.5014];
% x0 = zfix(:,5);
% y0 = zfix(:,6);
% r0 = x0 + 1i * y0;
% theta0 = zfix(:,3);
% v0 = zeros(N,1);
% omega0 = zeros(N,1);
z0 = [r0; theta0; v0; omega0];
%% simulation
tspan = [0:0.1:T];
%options = odeset('reltol',1e-12,'abstol',1e-12);
options = odeset('reltol',1e-3,'abstol',1e-3);
[t,zhist] = ode45(@parallelSys,tspan,z0,[],m,l,b,d,k1,k2,N,L,a,vref,omegaref,k1c,k2c,k3c);
%% plots
x_hist = real(zhist(:,1:N));
y_hist = imag(zhist(:,1:N));
th_hist = zhist(:,N+1:2*N);
v_hist = zhist(:,2*N+1:3*N);
omega_hist = zhist(:,3*N+1:4*N);
grayRGB = [1 1 1]*0.5;
figure;
hold on;
Lfish = 2;
for i = 1:1:N
plot(x_hist(:,i),y_hist(:,i),'Color',cmap(i,:),'linewidth',1);
end
for i = 1:1:N
[xFish,yFish] = fishCoords(x_hist(end,i), y_hist(end,i), Lfish, th_hist(end,i));
fill(xFish,yFish,cmap(i,:), 'linewidth',1); hold on;
plot(xFish,yFish, 'k', 'linewidth',1); hold on;
plot(x_hist(end-lastk:end,i),y_hist(end-lastk:end,i),'m','Linewidth',2);
end
plot(x_hist(1,:),y_hist(1,:),'ko','MarkerFaceColor','k','MarkerSize',3)
xlabel('X (m)');
ylabel('Y (m)');
box on;
axis square;
axis equal;
axis tight;
set(gcf,'Color','w')
set(gca,'FontSize',16,'FontName','Arial')
%export_fig('parallel_fish_tracks.pdf')
% figure;
% for i = 1:1:N
% plot(t,mod(th_hist(:,i),2*pi)*180/pi,'linewidth',2,'Color',cmap(i,:),'linewidth',2);
% hold on;
% end
% ylabel('Heading (deg.)');
% xlabel('Time (sec.)');
% set(gca,'FontSize',16,'FontName','Arial')
% ylim([0 270]);
% xlim([0 90]);
% yticks([0:90:360]);
% set(gcf,'Color','w')
% %export_fig('parallel_t_vs_theta.pdf')
figure;
for i = 1:1:N
plot(v_hist(:,i), omega_hist(:,i)*180/pi,'Color',cmap(i,:));
hold on;
plot(v_hist(end-lastk:end,i),omega_hist(end-lastk:end,i)*180/pi,'m','Linewidth',2);
plot(v_hist(1,i),omega_hist(1,i)*180/pi,'ko','MarkerFaceColor','k','MarkerSize',3)
hold on;
end
set(gca,'FontSize',16,'FontName','Arial')
set(gcf,'Color','w');
xlabel('Speed (m/s)');
axis square;
ylabel('Angular Rate (deg/s)');
%export_fig('parallel_v_vs_omega.pdf')
% figure;
% for i = 1:1:N
% plot(mod(th_hist(:,i),2*pi)*180/pi, omega_hist(:,i)*180/pi,'Color',cmap(i,:));
% hold on;
% plot(mod(th_hist(end-lastk:end,1),2*pi)*180/pi,omega_hist(end-lastk:end,1)*180/pi,'k','Linewidth',2);
% plot(mod(th_hist(1,i),2*pi)*180/pi,omega_hist(1,i)*180/pi,'ko','MarkerFaceColor','k','MarkerSize',3)
% end
% set(gca,'FontSize',16,'FontName','Arial')
% set(gcf,'Color','w');
% xlabel('Heading (deg)');
% xlim([0 360]);
% axis square;
% ylabel('Angular Rate (deg/s)');
% xlim([0 360]);
% %export_fig('parallel_th_vs_omega.pdf')
function xdot = parallelSys(t,x,m,l,b,d,k1,k2,N,L,a,vref,omegaref,k1c,k2c,k3c)
% unpack state
r = x(1:N);
theta = x(N+1:2*N);
v = x(2*N+1:3*N);
omega = x(3*N+1:4*N);
%% Parallel: Laplacian control
u = zeros(N,1);
couplingTerm1 = zeros(N,1);
for k = 1:1:N
Lk = L(k,:);
val1 = 1i * exp(1i * theta(k) );
val2 = Lk * exp(1i * theta );
couplingTerm1(k) = complexIP(val1,val2);
end
u_p_lap = -b*k1*omega + b*k2/N*couplingTerm1;
%% Parallel: All-to-all control
couplingTerm2 = zeros(N,1);
for k = 1:1:N
for j = 1:1:N
couplingTerm2(k) = couplingTerm2(k) - sin( theta(j) - theta(k) );
end
end
u_p_all = -b*k1*omega + b*k2/N*couplingTerm2;
%% Circular: Laplacian
couplingTerm_c_lap = zeros(N,1);
coefficient = ( (l/d)*omega - vref/omegaref);
c = r + vref/omegaref*1i*exp(1i*theta);
for k = 1:1:N
Lk = L(k,:);
val1 = exp(1i * theta(k) );
val2 = Lk * c; %exp(1i * c);
couplingTerm_c_lap(k) = complexIP(val1,val2);
end
u_c_lap = -b*k1c*omega - b*k2c*sin(theta - omegaref*t) ...
- b*k3c*coefficient.*couplingTerm_c_lap;
% figure(10);
% plot(real(r),imag(r),'bo'); hold on;
% plot(real(c),imag(c),'r+');
% hold on;
%% switch control from parallel/circular etc. here
u = u_c_lap; % circular, laplacian
%u = u_p_lap; % parallel laplacian
%u = u_p_all; % prallel all-to-all
%% dynamics
%omegadot = -a*omega.^3 + -u/b; % simplified model
omegadot = -m*l/b*v.*omega + -u/b; % actual model
% state rate
xdot(1:N,1) = v.*exp(1i*theta);% r-dot trailing edge
%xdot(1:N,1) = v.*exp(1i*theta) + l*omega.*1i.*exp(1i*theta);% r-dot for CM
xdot(N+1:2*N,1) = omega;%th-dot
xdot(2*N+1:3*N,1) = l*omega.^2 - d*v;%v-dot
xdot(3*N+1:4*N,1) = omegadot;
end
⛳️ 运行结果
🔗 参考文献
P. Ghanem, A. Wolek and D. A. Paleyv, "Planar Formation Control of a School of Robotic Fish," 2020 American Control Conference (ACC), Denver, CO, USA, 2020, pp. 1653-1658, doi: 10.23919/ACC45564.2020.9147969.