【水下】鱼类激励水下航行器的同步游泳与编队控制附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​% parametersm = 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 gainsk1 = 0.5;k2 = 2;% circular gainsk1c = 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);% circulantL = 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 colorsshift = 2;%cmap = parula(N); %cmap = cmocean('ice',N);cmap = cmocean('deep',N+shift);% shiftcmap = 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 formationLinit = 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);​%% plotsx_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);         endfor 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);endplot(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:Nplot(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 stater = 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 controlu = 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);    endu_p_lap = -b*k1*omega + b*k2/N*couplingTerm1;%% Parallel: All-to-all controlcouplingTerm2 = zeros(N,1);for k = 1:1:N    for j = 1:1:N        couplingTerm2(k) = couplingTerm2(k) - sin( theta(j) - theta(k) );    endendu_p_all = -b*k1*omega + b*k2/N*couplingTerm2;%% Circular: LaplaciancouplingTerm_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. hereu = 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 modelomegadot = -m*l/b*v.*omega + -u/b; % actual model% state ratexdot(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 CMxdot(N+1:2*N,1) = omega;%th-dotxdot(2*N+1:3*N,1) = l*omega.^2 - d*v;%v-dotxdot(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.

🎈 部分理论引用网络文献，若有侵权联系博主删除

🎁  关注我领取海量matlab电子书和数学建模资料

👇  私信完整代码、论文复现、期刊合作、论文辅导及科研仿真定制

1 各类智能优化算法改进及应用

生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化

2 机器学习和深度学习方面

卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

2.图像处理方面

图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

3 路径规划方面

旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化

4 无人机应用方面

无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配、无人机安全通信轨迹在线优化

5 无线传感器定位及布局方面

传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化

6 信号处理方面

信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化

7 电力系统方面

微电网优化、无功优化、配电网重构、储能配置

8 元胞自动机方面

交通流 人群疏散 病毒扩散 晶体生长

9 雷达方面

卡尔曼滤波跟踪、航迹关联、航迹融合