仿生航行器同步游泳与编队控制的Matlab/Simulink实现

做水下机器人这块的同行应该都有类似感受：单台仿生航行器在Matlab里跑通运动学模型并不难，真正头疼的是让多条“鱼”在同一个仿真环境里既保持各自的游动节奏，又能整体形成一个稳定的编队队形。我这次完成的项目，就是围绕“鱼类启发水下航行器的同步游泳与编队控制”搭了一套完整的Matlab实现，从鱼体波动运动学的建模，到多机相位同步，再到基于图论的一致性编队控制，最后落到Simulink仿真验证。整个过程踩了不少坑，也沉淀了一些可以直接复用的代码和参数配置，这里完整记录下来，给准备做类似方向的朋友一个参考。

先说结论：同步游泳的核心不在“速度一致”，而在“摆动相位耦合”；编队控制的核心也不在“跟点”，而在“拓扑连通性下的分布式误差消解”。这两个点理解透了，Matlab实现只是时间问题。

1. 为什么用“鱼的方式”游：仿生推进的工程取舍

1.1 鱼类推进模式怎么选型

鱼类游泳推进方式大体分两类：BCF（身体/尾鳍推进）和MPF（中间鳍/对鳍推进）。BCF靠躯干和尾鳍的摆动产生推力，适合长距离巡游，高速效率高；MPF靠胸鳍、背鳍、臀鳍的波动产生推力，低速机动性强，但高速效率相对一般。

编队任务里航行器绝大多数时间处于巡航状态，运动模式是“长时间直航+间歇性小幅度转向”，所以我选择了BCF模式，具体参考的是鲹科鱼类的游动方式。鲹科模式的特点是身体前2/3部分摆动幅度小、后1/3部分和尾鳍摆动幅度大，整体呈现一个从头部向后传递的驻波形态。这种模式的工程好处很直接：可以把身体主干抽象成刚体，只保留尾鳍摆动自由度，建模难度大幅降低，后续控制器设计也更容易收敛。

很多刚接触这个方向的人会纠结要不要做完整的计算流体力学（CFD）耦合仿真，我建议从控制角度讲，在编队控制这个层面，完全没有必要。CFD适合分析单条鱼的推进效率，但多条鱼的编队控制器需要的是“输入-输出”的简化映射关系，也就是我给定摆动频率和摆动幅度，航行器大概以什么速度前进、以什么角速度转弯，这层映射关系确定了，才能把上层控制器从底层流体细节中解放出来。

1.2 从生物波动到控制输入的抽象路径

鱼体波动可以用经典的旅行波方程描述，也是我在Matlab中实际使用的鱼体波曲线：

[ y_{body}(x,t) = A(x) \cdot \sin(k_w x - \omega t) ]

其中 (x \in [0, L]) 是身体轴线坐标，(A(x)) 是摆动幅度包络，(k_w = 2\pi/\lambda) 是体波波数，(\omega = 2\pi f) 是摆动角频率。

幅度包络 (A(x)) 通常取二次多项式形式：

[ A(x) = a_0 + a_1 x + a_2 x^2 ]

其中 (a_0) 很小，代表头部基本不摆的现实；(a_2) 较大，让尾部摆动幅度放大。这个公式我在Matlab里直接用向量化语句就能生成一条完整的鱼体波曲线，后续控制不需要逐关节去算，只要提取三个关键量就行：摆动频率、摆动幅度包络、摆动偏置角度。

这里有一个工程上很关键的抽象：把连续体波动压缩成控制输入。控制输入只有三路——

• 摆动频率 (f)：决定前进速度
• 摆动偏置角 (\sigma)：决定转向角速度
• 左右摆动幅度不对称系数：产生侧向力，用于辅助航向修正

我在第二种运动学层用的一阶惯性模型如下：

dx/dt = v * cos(psi) dy/dt = v * sin(psi) dpsi/dt = r dv/dt = (v_cmd - v) / tau_v dr/dt = (r_cmd - r) / tau_r

(v_{cmd}) 和 (r_{cmd}) 是上层的速度指令和转向角速度指令，(\tau_v)、(\tau_r) 是执行器惯性时间常数，分别设为0.4s和0.2s，模拟鱼体从摆动指令到实际速度响应的滞后。

为什么不直接在模型里保留鱼体波方程？因为控制器的目标是编队和同步，不是研究鱼体波形态。把连续体动力学压缩成一阶惯性环节之后，控制器看到的是一条“可跟踪速度指令的质点+航向角”，这是经典多智能体控制的标准接口，大量文献的协议代码可以直接套用。这也是我在项目初期最大的领悟：仿生不是把生物细节全部搬进控制器，而是把生物机制抽象成控制约束。

2. 单机运动学与控制：先把一条鱼调试成可跟踪轨迹的智能体

2.1 鱼体波曲线在Matlab里的生成

先给出一段可以直接运行的鱼体波生成函数。这个函数接收身体长度、采样点数、摆动频率和幅度系数，输出某一时刻鱼体轮廓的横向偏移曲线。

function y_body = fish_body_curve(L, N, f, a0, a1, a2, t) x = linspace(0, L, N)'; A = a0 + a1 * x + a2 * x.^2; k_w = 2 * pi / (0.8 * L); omega = 2 * pi * f; y_body = A .* sin(k_w * x - omega * t); end

调用示例，模拟一条体长0.5m的仿生鱼，摆动频率1.2Hz：

L = 0.5; N = 100; f = 1.2; a0 = 0.02; a1 = -0.05; a2 = 0.3; t = 0; y_body = fish_body_curve(L, N, f, a0, a1, a2, t); plot(linspace(0, L, N), y_body);

跑出来是一条从头部到尾部幅度逐渐增大的波形，尾端摆动幅度大概在0.12m左右，和真实鱼类比例接近。注意参数里 (a_1) 我用了负值，这会让前段包络略微内收，更贴近鲹科鱼类的形态特征。这个细节不影响控制稳定性，但会让仿真动画看起来更自然。

2.2 从摆动节律到速度与转向的映射

单鱼运动学模型的关键是建立一张“摆动参数-运动响应”的映射表。我通过控制变量法，在纯脚本模式下把不同摆动频率和偏置角的组合跑了一遍，得到近似线性关系如下（体长0.5m的参数样本）：

这张表的作用是把控制器的输出从“期望速度”转换成“期望摆动频率”，中间隔着一个比例系数。实际控制中我直接用一个增益 (K_f) 把速度指令映射成摆动频率指令：(f_{cmd} = K_f \cdot v_{cmd})，(K_f) 在仿真里取2.86。这个映射虽然粗糙，但足够支撑上层编队控制器。

2.3 单鱼航向保持的调试经验

单鱼航向控制是最容易被忽视但最容易翻车的一环。鱼体摆动时会产生周期性侧向力，即使摆动偏置角为零，航行器也会出现一个低频的蛇形游动轨迹，在仿真图上看就是前进方向左右摇摆。

我第一次调试时直接在航向环上用了高增益PD控制器，结果航向是稳住了，但速度损失非常大，因为控制器不断输出偏置角修正，相当于一直在做小幅转向。后来把航向闭环拆成两层：

• 外层：根据航向误差生成期望转向角速度 (r_{cmd})
• 内层：把 (r_{cmd}) 映射成摆动偏置角，并通过低通滤波消除高频分量

实际参数是航向比例增益 (K_{p_psi} = 1.2)，阻尼增益 (K_{d_psi} = 0.6)，低通滤波时间常数0.15s。这样调完之后，单鱼能在5秒内把初始航向误差从30度收敛到2度以内，且前进速度损失控制在8%以内。

这个经验一定要记下来：仿生航行器的航向控制不要直接追求响应快，要优先保证速度稳定性。编队控制比单鱼控制对速度损失更敏感，单鱼上的一点稳态误差，在编队里会被耦合放大成队形偏移。

3. 让多条鱼“步调一致”：相位同步控制的耦合建模

3.1 为什么同步的是摆动相位，而不是速度

现实中鱼群巡航时相邻个体的尾鳍摆动存在明显的相位锁定现象。从流体力学角度看，同相摆动可以降低相互干扰，避免尾流杂乱叠加；从工程控制角度看，如果多条航行器的摆动周期完全一致，那么它们产生的流场扰动也是周期性的、可预测的，这对编队稳定性和传感器测量都有好处。

同步的物理量是摆动相位而不是前进速度。两条鱼速度一致但相位不一致，体现在运动上就是它们尾迹流场的拍频叠加，形成周期性扰动；而相位一致时即使速度略有差异，扰动也是协调的。所以我把同步问题建模成相位耦合问题，而不是简单的速度一致问题。

3.2 Kuramoto耦合振荡器模型

多智能体相位同步的经典模型是Kuramoto耦合振荡器。对第 (i) 条航行器的摆动相位 (\phi_i)，动态方程为：

[ \frac{d\phi_i}{dt} = \omega_i + \sum_{j \in N_i} K_{ij} \sin(\phi_j - \phi_i) ]

其中 (\omega_i) 是自然频率，由期望前进速度决定，(K_{ij}) 是耦合强度。当耦合强度大于临界值时，所有相位收敛到一个共同值，实现同相游泳。

我在Matlab里用ode45直接求解这个系统：

function dphi = kuramoto_rhs(t, phi, K, omega) N = length(phi); dphi = zeros(N, 1); for i = 1:N dphi(i) = omega(i); for j = 1:N if j ~= i dphi(i) = dphi(i) + K(i, j) * sin(phi(j) - phi(i)); end end end end

初始相位差设为 ([0, \pi/3, 2\pi/3])，耦合强度全连接且统一取 (K = 0.8)，自然频率都取 (1.2Hz) 对应的角频率。仿真结果很理想，大约10秒后最大相位误差降到0.01rad以下，可以视作完全同步。

3.3 同步层与控制层的串联策略

Kuramoto模型求解出来的同步相位不能直接当作控制输入。需要设计一个同步修正环：每个航行器的自身摆动频率跟踪一个参考频率，同时接收邻居的相位信息做修正：

[ \omega_{i_cmd} = \omega_{ref} + K_{\phi} \sum_{j \in N_i} \sin(\phi_j - \phi_i) ]

这里 (K_{\phi}) 为同步增益，我取0.6。实现时每个航行器的控制律分成了两路：

• 运动环：接收编队控制器的速度指令和转向角速度指令
• 相位环：接收邻居相位误差，修正本机摆动频率

两路指令叠加后，通过前面说的摆动参数映射生成最终的底层执行指令。项目中的Simulink实现就是在MATLAB Function块里把这两个环并联，一个输出速度，一个输出频率修正量。

这里有一个很关键的工程细节：相位误差必须用wrapToPi归一化。如果不做处理，相位从 (2\pi) 跳到 (0) 时，相位差会在 (\pm 2\pi) 之间突然跳变，耦合修正项会产生一个巨大的尖峰，导致摆动频率瞬间变化，航行器猛地加速或减速。我实际踩过这个坑，第一次仿真相位同步跑到一半，领航者位置出现一个明显的突跳，排查了半天发现就是相位缠绕问题。在Matlab里用内置函数wrapToPi(err)一行代码解决。

4. 编队控制落地：一致性协议、领航者机制与同步的整合

4.1 图论表达编队拓扑

编队控制的数学基础是图论。把每条航行器看作一个节点，通信链路看作有向边，整个编队就是一个拓扑图。我设计的典型拓扑是领航者-跟随者模式，共4个节点：

• 节点0：领航者，按预设轨迹运动
• 节点1、2、3：跟随者，需要保持相对领航者的期望位置

邻接矩阵 (A) 描述了节点间的信息交互，拉普拉斯矩阵 (L = D - A) 描述系统的整体连通性。这里我不展开图论的数学推导，但有一个结论必须知道：只要通信拓扑是连通的，且至少有一个节点能收到领航者信息，一致性协议就能让整个编队收敛到期望队形。这个条件在Matlab里验证非常简单，用eig(L)看拉普拉斯矩阵第二小特征值是否大于零即可。

4.2 一致性协议：位置误差与速度误差的联合消解

我用的分布式编队控制律是经典的一致性协议加领航者项：

[ u_i = -\sum_{j \in N_i} a_{ij} \left[ (p_i - p_j) - (d_i - d_j) \right] - \gamma \sum_{j \in N_i} a_{ij} (v_i - v_j) + g_i \left[ (p_0 + d_i) - p_i \right] ]

其中：

• (p_i)、(v_i)：第 (i) 条航行器的位置和速度
• (d_i)：第 (i) 条航行器相对领航者的期望位移
• (g_i)：是否收到领航者信息，收到为1，否则为0
• (\gamma)：速度耦合增益，取1.0

这个公式的实际物理含义很好理解：每个航行器根据邻居的位置偏差和速度偏差调整自己的目标速度。期望队形反映在期望位移差 (d_i - d_j) 上，而不是直接反映在绝对位置。所以编队平移时，只要领航者移动，所有跟随者自动跟上，队形保持稳定。

在Simulink里实现时把这个控制律写进一个MATLAB Function块：

function u = formation_law(p_i, p_j, v_i, v_j, p0, d_i, g_i) a_ij = 1.0; gamma = 1.0; err_pos = (p_i - p_j) - d_i + d_j; err_vel = v_i - v_j; err_lead = p0 + d_i - p_i; u = -a_ij * err_pos - gamma * a_ij * err_vel + g_i * err_lead; end

这个函数的输出是二维平面上的期望加速度向量，再通过一个低层控制器把它转换成期望速度和转向角速度，最后进入前面提到的运动学模型。

4.3 同步与编队如何合流

把同步控制和编队控制合流的时候，我遇到的最大的认知问题：这两套控制律作用于不同的时间尺度。编队控制通过位置误差修正速度，响应较慢，时间常数大约3~5秒；相位同步通过频率修正调整节律，响应较快，时间常数约1~2秒。如果两个环的增益设置不当，会出现一个典型现象：编队还在收敛过程中，相位已经同步，但同步后的相位耦合反过来影响速度指令，导致编队误差出现振荡。

我的合流策略是让两个环正交化：

• 编队控制环的输出是速度幅值和航向角，不影响摆动相位
• 同步控制环的输出是摆动频率修正量，不影响航向角

在物理上，速度由摆动频率幅值决定，转向由摆动偏置角决定，两者解耦。这个正交关系保证了即使相位同步收敛快，也不会直接干扰编队收敛。这个结构是项目中最重要的一步设计决策。

仿真中我配置了一个等边三角队形，边长3m，领航者从原点出发沿 (y = 0.5x) 直线轨迹匀速运动，三个跟随者的初始位置故意偏离期望队形2~3米，初始相位差设为 ([0, \pi/3, 2\pi/3])。仿真总时长60秒。

5. Matlab/Simulink 工程实现：实际踩过的坑和对处理办法

5.1 纯脚本还是Simulink

项目前期我在纯脚本里验证了单鱼运动学和Kuramoto同步，这两个部分用脚本跑既快又灵活。但到了编队控制阶段，多节点状态交互、多路反馈叠加，纯脚本的代码结构变得非常混乱，沈断点排查也麻烦。后来我迁到Simulink，用模块化方式搭了三个子系统：运动学子系统、编队控制子系统、同步控制子系统。

我的建议是：单机验证用脚本，多机协同用Simulink。脚本适合快速迭代算法参数，Simulink适合把复杂的多节点交互可视化。不要一开始就在Simulink里搭全套，调试会非常痛苦。

5.2 MATLAB Function块的选型与数据类型坑

Simulink里的MATLAB Function块是实现自定义控制律最方便的方式，比Level-2 S-Function更直观，也更容易调试。但它有一个非常隐蔽的坑：输出信号尺寸推断。

我在编队控制块中返回的u是一个2x1向量，但第一次运行时Simulink把它推断为标量输出，结果下游的积分器维度不匹配，直接报错。解决方法是双击MATLAB Function块，在编辑器的“Edit Data”面板里手动把输出端口u的尺寸从1改成[2 1]。这个设置不会自动推断，必须手动指定。

另一个坑是数据类型不一致。运动学模型内部用double计算，但Simulink的时钟信号、控制指令输出在某些情况下会被强制转成uint类型，导致系统运行变慢甚至发散。统一做法是在每个信号连线处加一个Data Type Conversion块，全部转成double。

5.3 代数环和求解器设置

编队控制中有位置反馈直接进入控制律，再控制速度积分回到位置，这个反馈回路在Simulink中会形成代数环。代数环会导致仿真求解器每步迭代多次，严重拖慢速度，甚至出现不稳定。

我第一次跑仿真时，模型能跑但慢得离谱，步长0.01s，60秒仿真跑了近15分钟。检查后发现是三个航行器的位置反馈连成了一个大代数环。解决方案很粗暴直接：在反馈路径上插入一个Unit Delay块，把反馈信号延迟一个步长。这样就打破了代数环，仿真速度立刻恢复到秒级。

求解器方面，我最终采用的是：

• 求解器：ode4（固定步长四阶龙格库塔）
• 步长：0.01s
• 仿真时间：60s

为什么不选变步长ode45？因为相位同步和位置控制耦合的情况下，变步长求解器为了满足精度容差会频繁地缩小步长，仿真效率显著下降，而且每次缩步引入的数值误差不同，多机同步状态下表现不稳定。固定步长反而保证了所有航行器在同一时刻点的状态一致性，这在分布式控制仿真里很重要。

5.4 增益参数怎么调

编队控制的三个核心增益决定了系统的收敛速度、收敛精度和稳定性。我调参时的经验顺序是：

1. 先锁定领航者跟随增益 (g_i)，它决定收敛到领航者的速度，太小收敛慢，太大容易超调
2. 再调位置耦合项系数，调整编队内部相对位置的收敛性
3. 最后调速度耦合项 (\gamma)，它的作用是阻尼，能有效抑制编队误差的振荡

实际参数取值：

调参过程中最直观的现象是：位置耦合系数过大时，编队初始会快速收敛，但随后会出现明显的队形振荡；速度耦合系数太小，振荡持续期长。最优参数下，编队误差曲线呈平滑指数收敛，没有明显超调。

6. 仿真结果怎么验证：同步误差、编队误差与鲁棒性测试

6.1 同步误差收敛曲线

相位同步的评价指标是最大成对相位差：

[ e_{\phi}(t) = \max_{i,j} \left| \phi_i(t) - \phi_j(t) \right| ]

初始最大相位差为 (2\pi/3)，大约10秒后收敛到0.02 rad以下，之后一直保持稳定。耦合强度 (K_{\phi}=0.6) 时收敛速度和稳定性平衡得比较好。如果K值降到0.3，收敛时间会拉长到25秒；如果K值升到1.5，初期会出现相位过冲，虽然最终也收敛，但同步过程有明显振荡。

6.2 编队误差收敛曲线

编队位置误差定义为每个跟随者相对期望位置的欧氏距离的均方根值：

[ e_{form}(t) = \sqrt{ \frac{1}{3} \sum_{i=1}^{3} \left| p_i(t) - (p_0(t) + d_i) \right|^2 } ]

初始误差约2.5m，经过约25秒收敛到0.15m以下并保持稳定。这个稳态误差主要来自运动学模型的执行器响应滞后和编队控制器有限增益，0.15m在体长0.5m的尺度下大约为0.3倍体长，可以接受。

6.3 通信断连的鲁棒性测试

分布式控制相比集中式控制的优势在于对通信故障有一定容忍度。我在仿真第30秒时切断了节点1到节点2之间的通信链路，同时保持其他链路完整，观察编队是否会发散。

测试结果是：编队不会发散，但暂态误差从0.15m增大到0.28m左右，持续约5秒后重新回到0.15m水平。原因是从节点2的角度看，少了一条邻居路径，一致性协议的修正力度暂时下降，但它仍然能直接收到领航者信息，所以最终还能收敛。

这个测试很有价值，它验证了只要拓扑连通性保持，编队控制就能自愈。实际设计中应该保证每个节点至少有一条路径能到达领航者，这是编队控制可靠性的最低要求。

6.4 进一步能扩展的方向

项目目前实现的是平面编队和固定拓扑场景，后续可以扩展的方向包括：

• 动态避障：在一致性协议里叠加人工势场项，让航行器在保持编队的同时绕开障碍物
• 时变拓扑：模拟实际水声通信环境下的拓扑切换和丢包延迟
• 编队变换：通过改变期望位移 (d_i)，让三角形编队平滑过渡到纵队或横队
• 三维拓展：把二维模型扩展到三维，加入深度控制通道

这些扩展在现有Matlab框架下都是增量式工作，不需要推翻前面的模型。

最后分享一个个人体会：这类仿生编队项目最大的坑不是算法本身，而是“层级设计的混乱”。一开始很容易把鱼体波动、流体耦合、速度控制、相位同步、编队协议全部堆在一个大模型里，结果哪里都在算，哪里都算不准。我实际调试中最有效的做法是先把每一层单独验证到收敛，再逐层叠加。先让单鱼游稳，再让两条鱼相位同步，最后再上编队拓扑。每一步都确认收斂特性没有退化，再进下一步。这个过程虽然慢，但能帮你准确定位每一个异常是从哪一层引入的。如果你正在做类似项目，强烈建议按这个节奏来。