做水下航行器集群的人应该都有同感：单条鱼游起来容易，让好几条鱼像被一根线牵着一样同步游动、保持编队，才是真正让人掉头发的地方。我最近用Matlab完整跑通了一套鱼类启发水下航行器的同步游泳与编队控制仿真，从鱼体波动建模、CPG节律信号，到领航-跟随编队保持，最后用动画把整个过程跑了出来。这套方案解决的核心问题是：多台仿生水下航行器如何共享一套“节律大脑”，在无死锁、无漂移的前提下形成稳定编队。
如果你正在做仿生水下机器人，或者准备验证水下多智能体协同算法但还没找到合适的仿真切入点，这篇文章非常合适。哪怕你完全没接触过CPG，从建模到代码、从参数整定到坑点排查，我都按实操顺序捋一遍，尽量让你看完能直接复现。
1. 项目背景：为什么非要用“鱼”来做集群
1.1 项目来源与要解决的问题
这个项目的原始需求来自某实验室的仿生水下集群预研课题。当时要验证的并不是“鱼能不能游”，而是“一群鱼能不能协同”。传统螺旋桨推进的水下机器人做编队会有一个很难受的问题：桨叶尾流大、噪音大，靠得太近时相互之间受流场干扰严重，编队控制精度很难提上去。仿生推进方式恰恰相反，它通过身体波动产生推力，尾流场相对干净，尤其适合近距离、多台协同的场景。
但要真把“鱼”做成实物，前期的控制算法验证必须做足。如果直接在实物上试编队，一次池试就要调半天设备，数据难采集、参数不好改。所以在项目初期，我们决定先在Matlab里搭一套完整的仿真闭环：包含鱼体动力学、CPG节律生成、编队控制器、可视化四个模块。这样算法逻辑能快速迭代，参数也能反复调，等仿真结果稳定后再往实物平台迁移。
1.2 为什么这套方案能落地
选择“同步游泳+编队控制”作为项目主线，是有实际考量的。同步游泳解决的是“节律对齐”问题，多台航行器即便初始相位、频率各不相同，也要通过耦合逐步调整到同一节拍，这是仿生集群的基础能力。编队控制解决的是“空间位置对齐”问题，在节律同步基础上，各台航行器还要按照期望几何队形前进。
Matlab在这里的优势很明显。一是矩阵运算天然适合多智能体状态量表达，N条鱼的状态直接一个矩阵就装下了；二是调试交互性强，跑完直接画图看曲线，哪里发散一眼就能定位。用Simulink也能做，但纯脚本对版本控制、参数批处理更友好。这个项目最终选择纯Matlab脚本框架，原因就是这么实际。
项目落地时使用了完全虚构的模拟项目配置：某实验室以三条鱼体长0.5米的中型仿生航行器为对象，设计等边三角形编队，目标航速0.8m/s。仿真环境加入横向水流干扰，用来验证控制器的抗扰能力。所有代码模块均从零编写，不依赖特定工具箱，只有基础Simulink环境也能运行。
2. 鱼体建模与同步控制的核心思路
2.1 运动学模型：行波方程与推进效率
鱼类推进的经典建模方法是Lighthill细长体理论，但在编队控制层面不需要过分深挖流体细节，更常见的是把鱼体波动抽象成一条中心线的行波。鱼体中心线的横向位移用行波方程描述：
w(x,t) = A(x) · sin(k·x - ω·t)
其中，x是从头部到尾部的归一化坐标，A(x)是振幅包络函数。真实的鱼游动时，头部几乎不动，振幅沿身体逐渐增大，到尾鳍达到最大。因此A(x)通常写成线性组合的形式，比如 A(x)=a1·x + a2·x²，具体系数要看鱼体的刚度分布和游动模态。
这个模型看起来简单，但它决定了下游所有控制量的物理意义。频率ω决定拍尾快慢，直接影响推进速度；振幅A(x)的包络形状决定推力和尾迹形态；波数k决定身体弯曲的波长，太长则推进效率低，太短则像痉挛一样难看且费能。实际选型时一般参考Strouhal数：
St = f · A_tail / U
其中f是摆动频率，A_tail是尾尖双振幅，U是游动速度。自然界高效巡游的鱼类St数基本落在0.25~0.4区间。这个规律我在项目中直接拿来用：已知目标速度U=0.8m/s，取尾尖振幅A_tail=0.05m，选St=0.3，反推频率 f=0.3×0.8/0.1≈2.4Hz，对应角频率约15rad/s。这样一来，仿真参数的每一档都有生物学依据，而不是随手填一个数。
2.2 CPG节律信号的同步机制
单条鱼的摆动是节律运动，但多条鱼要同步，不能靠每台各自定一个同样的摆动频率就能做到。实际系统中，推进电机、负载、水流扰动都会让“名义频率相同”变成“实际频率漂移”。没有耦合机制，相位差会慢慢拉大，编队也就散了。
我采用的方案是基于中枢模式发生器（CPG）的节律耦合网络。CPG可以理解为一个“节律发生器”，它天然输出周期性信号，不依赖外部时钟。每条鱼都有自己的CPG单元，单元之间通过耦合项连接，形成网络。当一个单元的相位比邻居超前时，耦合项会产生纠正作用把它拉回来；滞后时同理。这就像一群人合唱，每个人耳朵听到旁边人的声音，自动对齐拍子。
在Matlab里我用的是带极限环的Hopf振荡器，方程如下：
dx_i/dt = α(μ - r_i²)x_i - ω_i·y_i + K·Σ(x_j - x_i)
dy_i/dt = α(μ - r_i²)y_i + ω_i·x_i + K·Σ(y_j - y_i)
其中r_i² = x_i² + y_i²，表示振荡幅值。μ控制极限环半径，α控制收敛到极限环的速度，ω_i是固有频率，K是耦合强度。这个形式的优势在于：频率、振幅、耦合三项完全解耦，调同步不影响振幅，调振幅不影响频率，工程上非常好操作。
CPG的输出x_i、y_i经过简单变换后，可以作为鱼体行波方程中A(t)和ω(t)的实时调制信号。这样整条鱼的拍尾动作就和CPG网络绑定在一起，同步问题被转化为CPG网络的相位同步问题。
2.3 同步控制器参数怎么定
CPG网络能不能收敛到同步，主要看两个条件：一是通信拓扑是否连通，二是耦合强度和固有频率差之间的匹配。
对于三个节点的全连接网络，只要所有ω_i初始设置在同一频段，K取足够大就能实现相位同步。理论上有一个粗略的判定依据：耦合强度K应大于频率差最大值与网络代数连通度之比。全连接三节点的代数连通度为3（未归一化情况下），如果固有频率差的最大值为0.5rad/s，那么K取大于0.2基本可以保证同步。实际我取K=2，留了相当大的裕量。
但这里有个初学者容易忽略的细节：如果初始相位差在π附近，网络可能出现反相同步或半周期同步，看起来“同步了”但队形全乱。所以在初始化时，我会把各CPG的初始相位差控制在π/2以内，并且让初始频率接近目标频率。这不是作弊，而是模拟真实系统的“慢启动”——先让航行器在静止或低速状态下完成节律对齐，再逐步提速进入编队。
注意：CPG同步的参数调整必须遵循“先幅值、后频率、再耦合”的顺序。先确保每个单元单独工作时极限环稳定，再接入耦合项，否则问题混在一起很难定位。
3. 编队控制设计：从同步到保持队形
3.1 领航-跟随拓扑与控制律推导
同步游泳解决的是“节拍对齐”，编队解决的是“位置对齐”。项目里采用的是最常见的领航-跟随结构：指定一台航行器为领航者，按预设轨迹运动，其余航行器作为跟随者，实时计算自己与领航者的期望位置偏差，通过控制器消除这个偏差。
三条鱼的期望队形设为等边三角形，领航者在某个顶点，两个跟随者分列后方两侧。设期望相对位置矢量为r_1和r_2，在领航者坐标系中表示为：
r_1 = [d·cos(θ_1), d·sin(θ_1)]
r_2 = [d·cos(θ_2), d·sin(θ_2)]
以等边三角形为例，θ_1=60°，θ_2=300°，d为边长。跟随者i期望的全局位置是：
p_des_i = p_L + R(ψ_L)·r_i
其中R(ψ_L)是领航者航向角对应的旋转矩阵。这里特别关键：期望位置必须在领航者坐标系中旋转到全局坐标，否则领航者一旦转弯，编队就会“散架”。这个坑我后面调试时踩过，先在这里埋个伏笔。
跟随者的位置误差定义为e_i = p_i - p_des_i。为了便于设计控制器，我会把误差投影到领航者的纵向和横向轴上，得到纵向误差e_long和横向误差e_lat。控制律分两层：
- 纵向控制：u_i = u_L + k1·e_long，即跟踪领航者速度，并根据前后位置偏差微调速度；
- 横向控制：ω_i = ω_L + k2·atan(e_lat / d)，即通过偏航角速度响应横向偏差。
横向控制里的atan(e_lat/d)本质是视线导引法，角度比例增益k2决定了纠偏强度。这个控制器形式非常简洁，没有复杂的矩阵推导，但稳定性有保障，适合工程实现。
3.2 基于航向旋转的编队保持
刚才提到R(ψ_L)是编队保持的关键。举一个具体场景：领航者向右转向90°，如果不做旋转变换，两个跟随者的期望位置还停留在惯性坐标系里，编队就会变成一字形，甚至穿越到领航者轨迹的另一侧。这在Matlab动画里一眼就能看出来，队形像被拧了一样。
换成带旋转的期望位置后，跟随者会跟着领航者的朝向整体转动，队形像一块刚性板一样平移旋转。这个设计思想叫“虚拟结构”或“虚拟编队框架”。实现时只要把领航者的航向角ψ_L实时广播给跟随者，跟随者每步计算R(ψ_L)即可。
值得注意的是，旋转矩阵对ψ_L的连续性很敏感。如果ψ_L出现跳变（比如从179°跳到-179°），R(ψ_L)也会跳变，导致控制量突变。项目里我对ψ_L做了角度连续性处理：一旦检测到跳变超过π，就对当前角加减2π，保证数据在代码层面连续。这个小细节如果不处理，仿真后期它会以“编队莫名抖动”的方式报复你。
3.3 扰动下的控制器补偿
单纯的比例反馈在无扰动环境下够用，但水下环境必定有流。项目仿真中我加了两种扰动：恒定横向流和正弦波动流。横向流会让航行器向下游漂移，如果不补偿，编队会整体偏航。
补偿策略有两层。第一层是在纵向和横向控制量中添加水流速度前馈项：假设领航者能感知环境流速u_current，用它修正期望速度。第二层是通过积分项消除残余偏差。纵向控制律调整为：
u_i = u_L + k1·e_long + k_i·∫e_long dt
积分项的作用是消除静态误差。但积分项也是震荡的温床，一旦增益设得过大或者控制器更新频率过低，系统容易进入限幅循环。项目中积分增益k_i设为k1的1/10，在抗漂移和稳定性之间取了一个相对保守的平衡。
4. Matlab实现：从仿真框架到跑通结果
4.1 仿真框架与文件组织
整个仿真工程按模块拆成了5个文件，尽量避免一个脚本从头写到尾。良好的模块边界对我后续调参和移植代码帮助极大。
- main_formation_sim.m：主脚本，负责参数初始化、仿真循环、结果绘图；
- fish_dynamics.m：鱼体平面运动动力学函数，输入控制量，输出状态更新；
- cpg_network.m：CPG网络更新函数，输入当前CPG状态和耦合矩阵，输出下一时刻的节律信号；
- formation_controller.m：编队控制器函数，输入状态误差，输出控制量；
- plot_results.m：结果可视化脚本，绘制轨迹、相位误差、位置误差曲线。
主循环采用固定步长更新，步长h=0.01s，仿真时长60s。初始状态全部为零，领航者从原点出发，沿预设直线路径前进；跟随者初始位置在原点附近随机散布，初始CPG相位随机，模拟真实系统上电启动时的杂乱状态。
4.2 关键代码片段解析
CPG网络是整个项目中最核心的部分，代码量不大但逻辑必须写对。核心更新函数如下：
function [x_new, y_new] = cpg_network(x, y, omega, mu, alpha, K, dt, adj) n = length(x); x_new = zeros(n,1); y_new = zeros(n,1); for i = 1:n r2 = x(i)^2 + y(i)^2; dx = alpha * (mu - r2) * x(i) - omega(i) * y(i); dy = alpha * (mu - r2) * y(i) + omega(i) * x(i); for j = 1:n if adj(i,j) ~= 0 && i ~= j dx = dx + K * (x(j) - x(i)); dy = dy + K * (y(j) - y(i)); end end x_new(i) = x(i) + dt * dx; y_new(i) = y(i) + dt * dy; end end这里adj是邻接矩阵，全连接三节点就是对角线为0、其余为1的3×3矩阵。变量r2表示当前振荡幅值的平方，用它构建极限环。如果α设为100，μ设为1，系统会迅速收敛到半径为1的极限环上，输出幅值稳定。
编队控制器的实现如下：
function [u_cmd, w_cmd] = formation_controller(p_i, psi_i, p_L, psi_L, u_L, w_L, r_des_i, kp, kw) R = [cos(psi_L), -sin(psi_L); sin(psi_L), cos(psi_L)]; p_des = p_L + R * r_des_i; e = p_i - p_des; e_long = e' * [cos(psi_L); sin(psi_L)]; e_lat = e' * [-sin(psi_L); cos(psi_L)]; d = norm(r_des_i); u_cmd = u_L + kp * e_long; w_cmd = w_L + kw * atan(e_lat / d); end这个函数的输出直接输入鱼体动力学模型。纵向控制修正速度，横向控制修正偏航角速度。kp和kw初始值分别取0.5和1.5，后续根据仿真曲线做微调。
4.3 仿真参数表与结果判定
我将关键参数整理成表格，方便对照调整：
| 参数 | 数值 | 说明 |
|---|---|---|
| 航行器数量N | 3 | 1条领航者+2条跟随者 |
| 体长L | 0.5m | 参考实际仿生鱼样机尺寸 |
| 目标航速U | 0.8m/s | 巡游速度 |
| 摆动频率f | 2.4Hz | 由St=0.3反推 |
| 尾尖振幅A_tail | 0.05m | 体长的10% |
| 编队间距d | 1.5m | 3倍体长，避免尾流重叠 |
| 仿真步长h | 0.01s | 固定步长RK4更新 |
| 仿真时长T | 60s | 覆盖启动、同步、编队收敛全阶段 |
| CPG耦合强度K | 2.0 | 裕量充足，保证同步 |
| 位置比例增益kp | 0.5 | 纵向速度修正增益 |
| 航向比例增益kw | 1.5 | 横向纠偏增益 |
结果是否合格，我通常看三条曲线：一是CPG相位误差曲线，应在前3秒内收敛到0附近；二是编队位置误差曲线，稳态误差应小于0.1m；三是轨迹图，三条鱼形成稳定等边三角形并沿目标方向前进。三者同时满足，基本可以判定算法跑通。
4.4 典型结果怎么看
以领航者走直线路径为例，初始时刻跟随者的位置误差较大，CPG相位杂乱，轨迹图上三条鱼几乎扭成一团。约3秒后，CPG相位对齐，跟随者开始向期望位置收拢。10秒左右，等边三角形逐渐成形，此后整个队形像一块刚性板一样稳定前移。
如果领航者走S形路径，编队转弯时会看到内外侧跟随者速度差异，外侧鱼速度略快，内侧鱼速度略慢，但整体队形保持良好。这正是旋转变换带来的效果：跟随者跟随的是“领航者坐标系里的相对位置”，而不是盲目追着领航者的全局坐标跑。
5. 调试实录：那些让我折腾很久的问题
5.1 同步不收敛：相位差恒定在某个值
第一次接入CPG网络时，我发现相位差始终维持在1.2rad左右，怎么都缩不到0。排查后确认是耦合强度不够。当时三台航行器的固有频率分别设为1.8、2.0、2.2rad/s，最大频率差0.4rad/s，K只给了0.5，理论上好像够了，但实际因为极限环收敛速度α设置偏小，网络动态响应太慢，导致同步需要极长时间。
解决方法是把α从20提高到100，同时K加大到2.0。这里有个经验：α和K要协同调整，α决定单个振荡器回到极限环的快慢，K决定网络间“拉齐”的力度。两者中任何一个过小，同步都会失败。
5.2 反相同步：表面上同步实则队形错乱
调好参数后，相位误差曲线显示同步了，但我仔细看动画发现，一条鱼居然在反向游动。原因是最初某条鱼的CPG初始相位与其他鱼差了接近π，收敛到了反相状态。虽然CPG网络数学模型上这确实是稳定点，但物理上完全不可用。
解决方法是在初始化阶段做相位对齐：把所有CPG初始相位设到同一象限，或者差值限制在π/2以内。同时在CPG方程里加了一个弱单向引导项，让网络倾向于选择“全同相”而非“反相”的同步模式。这是一个非常容易被忽略的问题，强烈建议你在初始化阶段就考虑。
5.3 编队震荡：转弯时位置误差来回甩
第三个坑出现在S形路径测试中。领航者一开始转弯，跟随者的位置误差曲线就出现明显震荡，幅度越来越大，最后直接发散。问题根源有两个：一是控制器增益过高，kw=3时偏差过大反而激起偏航角超调，产生二次误差；二是期望位置旋转变换引入了偏航角微分噪声，ψ_L跳变导致位置期望振荡。
最终把kw降到1.5，并且对ψ_L做角度连续性处理，震荡明显消失。另一个改进是在控制输出端加了速率限制，即单步控制量变化不超过设定上限，模拟真实电机的响应约束。这个小改动对抑制高频抖动非常有效。
5.4 仿真速度慢：ode45在复杂场景下效率反而不高
早期版本我用ode45做动力学积分，好处是自适应步长精度高，但在CPG网络和控制器频繁切换的情况下，ode45会频繁缩小步长，仿真60秒场景实际跑了十几分钟，非常痛苦。后来全部换成固定步长RK4，步长0.01s，精度完全够用，速度提升了一个数量级。
注意：用固定步长之前，一定要做一次收敛性验证。把步长减半再跑一遍，如果结果曲线一致，说明步长选择合理；如果变化明显，说明步长太大或系统刚度过高。
5.5 编队整体漂移：跟随者速度与领航者不完全一致
最后一次测试出现了一个奇怪现象：队形看起来稳定，但整体位置与领航者期望路径发生了偏移，跟随者虽然保持相对位置，却始终落后一大截。定位后发现是纵向控制律缺少前馈项，只有比例反馈让跟随者速度依赖误差，误差缩小则速度趋近于零，永远追不上匀速前进的领航者。加入u_L前馈项，问题立刻解决。
这类问题特别容易出现在“看起来队形没散但位置不对”的场景，一定要检查控制器是否有速度前馈，不能只靠比例项纠偏。
6. 实操心得与后续可扩展方向
如果只总结一条经验，我会说：这个项目的核心不是CPG也不是编队公式，而是“模块边界清晰才能快速定位问题”。CPG网络、运动学模型、编队控制器三者之间的接口设计得好，几乎每个调试问题都能在几分钟内定位到具体模块。接口设计得差，出了问题就要从头排查，时间全浪费在看曲线上了。
另一个体会是，仿真里遇到的很多问题都来自“初始状态不合理”。真实系统上电时初始相位、位置、航向都不确定，但也不能完全随机。合理的做法是让系统先低速同步、再进入编队，分阶段启动。这在项目里不是偷懒，而是更接近真实工程中的安全策略。
后续如果要继续扩展，我会优先做三件事：一是把编队从三节点扩展到N节点，队形矩阵化定义，控制器基本不用变，只是耦合矩阵和领航者广播的内容变多；二是引入通信丢包模型，观察CPG同步和编队控制在缺失通信下的鲁棒性；三是把纯脚本迁移到Simulink，与底层动力学模型甚至硬件在环平台对接。这个项目目前的状态作为算法验证已经完全够用，下一步的挑战是把仿真中证明可行的控制律写进真实的仿生航行器里，让它真正游起来。
