1. 从一次水下巡检任务说起：这套多智能体ROV系统到底在解决什么问题
去年夏天，我参与了一个水库坝体水下结构巡检的项目。当时用的是一台单机ROV，操作员盯着屏幕，手动摇杆控制，沿着坝体表面一点点挪。一天下来，有效巡检面积不到两百平方米，操作员眼睛都快瞎了。更麻烦的是，坝体表面有淤积、有附着物，ROV稍微靠近就容易撞上去，离远了摄像头又看不清。当时我就在想，能不能让多台ROV自己编队、自己保持安全距离、自己沿着坝体表面扫过去，人只需要在岸上监控状态就行。
这个想法后来就落地成了今天要聊的这套系统：基于PD控制与人工势场融合控制的多智能体自主水下检测ROV架构。说白了，就是让一群水下机器人像鱼群一样，既能保持队形协同搜索，又能自动避开障碍物和彼此之间的碰撞，同时用PD控制器保证每台ROV的姿态和位置稳定。整套东西用MATLAB搭起来，方便做算法验证和快速迭代。
这套内容适合谁看？如果你正在做水下机器人控制、多智能体协同、或者对人工势场和PD控制的实际工程落地感兴趣，那这篇东西应该能给你省不少查资料和试错的时间。如果你只是刚接触MATLAB，也没关系，我会把每个环节拆开讲，代码逻辑和参数怎么来的都说清楚。下面我从整体设计思路开始，一层层往下拆。
2. 整体架构设计：为什么选PD+人工势场这个组合
2.1 单机ROV控制与多智能体协同的分工逻辑
先把这个系统的骨架说清楚。整个架构分两层：底层是单台ROV的运动控制，上层是多智能体的协同决策。底层负责让每台ROV能稳定地执行速度或位置指令，上层负责决定每台ROV应该往哪走、和谁保持多远、怎么绕开障碍。
底层为什么用PD控制？水下ROV的动力学模型有非线性、耦合、水动力阻尼这些麻烦事，但如果你把工作点限制在低速巡航区间，模型可以近似线性化。PD控制的好处是结构简单、参数物理意义明确、调试起来有抓手。P管响应速度，D管阻尼抑制超调，对于ROV这种惯性大、水阻力也大的对象，PD其实比很多花哨的算法更稳。
上层为什么用人工势场？多智能体协同最核心的需求就两个：保持队形和避障避碰。人工势场天然适合表达这两种需求——目标点产生引力，障碍物和其他ROV产生斥力，合力方向就是每台ROV的期望运动方向。而且势场法计算量小，适合实时跑在每台ROV的控制器上，不需要中央节点做全局优化。
注意：PD控制和人工势场不是简单叠加，而是有层级关系的。势场输出的是期望速度或期望位置，PD控制器跟踪这个期望值。如果直接把势场力当控制力用，ROV会因为惯性冲过头，所以必须用PD做底层跟踪。
2.2 人工势场在多智能体场景下的改造要点
经典人工势场法有个著名的问题：局部极小值。当引力和斥力刚好抵消时，ROV会卡住不动。单机场景下这个问题可以通过随机扰动或者沿墙走策略绕过去，但多智能体场景下更复杂，因为每台ROV的势场都在动态变化。
我的处理方式是给斥力场加一个切向分量。当ROV检测到合力接近零时，自动在垂直于合力方向上加一个小速度，让它沿着障碍物边缘滑过去。这个切向分量的大小和障碍物距离挂钩，距离越近切向速度越大，避免在障碍物附近磨蹭。
另一个改造点是队形势场。多台ROV之间不仅要有斥力防止碰撞，还要有引力维持队形。我的做法是设定一个期望间距，实际间距大于期望时产生引力，小于期望时产生斥力，等于期望时势场力为零。这样队形就能自适应保持，不需要每台ROV都知道全局队形坐标。
2.3 MATLAB选型与代码组织方式
用MATLAB做这套东西，主要图它矩阵运算方便、可视化调试直观、控制系统工具箱成熟。整个代码我按功能分成几个模块：ROV动力学模型、PD控制器、势场计算模块、多智能体通信与协同模块、主仿真循环。每个模块单独一个函数文件，主脚本负责串起来跑。
这种组织方式的好处是，你想换控制算法，只改PD控制器那个文件就行；想调势场参数，只动势场模块。不会牵一发动全身。下面我把每个模块的核心逻辑和参数选择依据展开讲。
3. 核心模块拆解：从动力学到势场力的完整实现
3.1 ROV六自由度动力学模型的简化与MATLAB实现
完整的ROV六自由度模型包含刚体动力学和水动力项，写出来一大坨。但在低速巡检场景下，我可以做几个合理简化：忽略横滚和俯仰的耦合，假设ROV始终保持水平姿态，只考虑纵向、横向、垂向、艏向四个自由度。这样模型维度从六降到四，计算量小很多，而且对巡检任务来说精度够用。
动力学方程写成矩阵形式：
function dstate = rov_dynamics(state, tau, params) % state: [x, y, z, psi, u, v, w, r] % tau: [Fx, Fy, Fz, Mz] 力和力矩 % params: 质量、阻尼、惯量等参数 % 提取状态 u = state(5); v = state(6); w = state(7); r = state(8); psi = state(4); % 坐标变换矩阵（机体到惯性系） R = [cos(psi), -sin(psi), 0, 0; sin(psi), cos(psi), 0, 0; 0, 0, 1, 0; 0, 0, 0, 1]; % 阻尼矩阵（线性阻尼近似） D = diag([params.Xu, params.Yv, params.Zw, params.Nr]); % 惯性矩阵 M = diag([params.m, params.m, params.m, params.Iz]); % 加速度 accel = M \ (tau - D * [u; v; w; r]); % 状态导数 dstate = zeros(8,1); dstate(1:4) = R * [u; v; w; r]; dstate(5:8) = accel; end参数怎么定？质量m和惯量Iz可以从ROV的CAD模型或者称重实验得到。阻尼系数Xu、Yv、Zw、Nr需要做水池实验或者用经验公式估算。我一般先用经验公式给个初值，然后在仿真里看响应曲线，如果速度响应太慢就减小阻尼，超调太大就增大阻尼。这个过程需要反复几次，但一旦定下来，后面调控制器就轻松了。
实操心得：阻尼系数对PD参数影响很大。如果你发现PD怎么调都振荡，先检查阻尼系数是不是设小了。水下ROV的阻尼通常比空中无人机大一个量级，别照搬无人机的参数。
3.2 PD控制器的参数整定与抗饱和处理
PD控制器的输入是期望位置和期望速度，输出是力和力矩。公式很直接：
function tau = pd_controller(state, ref, gains) % state: 当前状态 % ref: 期望状态 [x_ref, y_ref, z_ref, psi_ref, ...] % gains: PD增益结构体 % 位置误差 e_pos = ref(1:3) - state(1:3); e_psi = wrapToPi(ref(4) - state(4)); % 速度误差 e_vel = ref(5:7) - state(5:7); e_r = ref(8) - state(8); % PD输出 Fx = gains.Kp_x * e_pos(1) + gains.Kd_x * e_vel(1); Fy = gains.Kp_y * e_pos(2) + gains.Kd_y * e_vel(2); Fz = gains.Kp_z * e_pos(3) + gains.Kd_z * e_vel(3); Mz = gains.Kp_psi * e_psi + gains.Kd_psi * e_r; tau = [Fx; Fy; Fz; Mz]; end参数整定我习惯用临界比例度法起步：先把Kd设为零，慢慢增大Kp直到系统出现等幅振荡，记下这时的Kp和振荡周期T。然后按经验公式取Kp=0.6Kp_critical，Kd=KpT/8。这套方法在ROV上实测下来，一般两三轮就能调到不错的响应。
抗饱和处理是必须的。ROV的推进器有推力上限，如果误差很大，PD输出会远超实际能力，导致积分饱和或者响应迟滞。我的做法是在输出端加限幅，同时当输出饱和时，把误差项也做相应缩放，避免控制器“以为”自己输出了很大力。
% 推力限幅 tau_max = [50; 50; 30; 10]; % 根据实际推进器配置 tau = max(min(tau, tau_max), -tau_max);3.3 人工势场力的计算：引力、斥力与切向逃逸
势场模块每台ROV独立计算，输入是自身位置、目标点位置、障碍物位置列表、其他ROV位置列表，输出是合力方向。
引力场用二次势函数：
function F_att = attractive_force(pos, goal, k_att) d = norm(goal - pos); if d < 0.1 F_att = [0; 0; 0]; else F_att = k_att * (goal - pos) / d; end end斥力场用经典的指数形式，但加了一个最大影响距离：
function F_rep = repulsive_force(pos, obstacle, k_rep, rho_0) d = norm(pos - obstacle); if d > rho_0 F_rep = [0; 0; 0]; else direction = (pos - obstacle) / d; magnitude = k_rep * (1/d - 1/rho_0) * (1/d^2); F_rep = magnitude * direction; end end切向逃逸分量在合力接近零时激活：
F_total = F_att + F_rep_sum; if norm(F_total) < 0.5 % 合力太小，加切向分量 tangent = [-F_total(2); F_total(1); 0]; if norm(tangent) > 0.01 F_total = F_total + 0.8 * tangent / norm(tangent); end end参数k_att、k_rep、rho_0的选择有讲究。k_att决定ROV朝目标冲的积极性，太大会导致接近目标时速度过快冲过头，太小则响应慢。k_rep决定避障的果断程度，太大ROV会离障碍物很远就绕，太小则容易撞上。rho_0是斥力作用范围，一般取ROV尺寸的3到5倍。
注意：势场参数和PD参数要联合调。势场输出的是期望速度，PD跟踪这个速度。如果势场输出变化太剧烈，PD跟不上就会振荡。我的经验是势场输出的期望速度变化率限制在0.5 m/s²以内，给PD留出跟踪余量。
3.4 多智能体协同：队形保持与通信拓扑
多台ROV之间的协同靠队形势场实现。每台ROV只和邻居通信，不需要全局信息。通信拓扑我用的是环形加对角线的结构，每台ROV和前后左右四台交换位置信息。
队形势场包含两部分：邻居斥力防止碰撞，期望间距引力维持队形。
function F_formation = formation_force(pos, neighbors, desired_spacing, k_form) F_formation = [0; 0; 0]; for i = 1:size(neighbors, 2) d_vec = pos - neighbors(:, i); d = norm(d_vec); if d < 0.01 continue; end direction = d_vec / d; % 间距误差 error = d - desired_spacing; % 引力/斥力 F_formation = F_formation + k_form * error * direction; end end期望间距怎么定？取决于任务需求。如果是坝体巡检，ROV需要覆盖一定宽度，间距取摄像头有效视野宽度的0.8倍左右。如果是管道检测，间距取管道直径的1.5倍，保证每台ROV都能看到管壁。
通信延迟在仿真里也要考虑。我用一个固定延迟环节模拟水声通信的时延，一般取0.5到2秒。延迟太大会导致队形振荡，这时候需要降低队形势场增益，或者改用预测补偿。
4. 完整仿真流程与关键环节实现
4.1 仿真主循环的搭建与时间步长选择
主循环的结构很清晰：每个时间步，先更新每台ROV的传感器信息（位置、速度），然后计算势场力，再跑PD控制器，最后积分动力学得到新状态。
dt = 0.05; % 时间步长50ms T = 120; % 仿真总时长 N = T / dt; for k = 1:N for i = 1:num_rovs % 获取邻居信息 neighbors = get_neighbors(rovs, i, comm_range); % 计算势场力 F_att = attractive_force(rovs(i).pos, goal, k_att); F_rep = compute_repulsive(rovs(i).pos, obstacles, k_rep, rho_0); F_form = formation_force(rovs(i).pos, neighbors, d_spacing, k_form); F_total = F_att + F_rep + F_form; % 势场力转期望速度 v_des = saturate(F_total / damping_coeff, v_max); % PD跟踪期望速度 tau = pd_controller(rovs(i).state, [rovs(i).pos; v_des], gains); % 动力学积分 rovs(i).state = rk4_step(@rov_dynamics, rovs(i).state, tau, dt, params); end % 记录和可视化 log_data(k, rovs); end时间步长dt取0.05秒，也就是20Hz。这个频率对ROV动力学来说足够，因为ROV的响应时间常数一般在秒级。如果dt太大，积分误差会累积；dt太小，仿真时间会拉长。实测0.05是个平衡点。
4.2 障碍物环境建模与势场参数调试
仿真环境里我建了一个水库坝体的简化模型：一面倾斜的墙，上面有几个凸起的结构。障碍物用圆柱体和长方体组合表示，每个障碍物记录中心位置和半径。
势场参数调试我一般分三步走：
第一步，单机避障测试。放一台ROV，一个障碍物，看它能不能绕过去。如果撞上了，增大k_rep或者增大rho_0。如果绕得太远，减小k_rep。
第二步，多机队形测试。放三台ROV，无障碍物，看队形能不能保持。如果振荡，减小k_form或者增大PD的阻尼增益。如果队形散开，增大k_form。
第三步，联合测试。多机加障碍物，看整体行为。这时候最容易出现的问题是某台ROV为了避障脱离队形，然后队形势场又把它拉回来，来回振荡。解决办法是给队形势场加一个低通滤波，让队形恢复平缓一些。
% 队形势场低通滤波 persistent F_form_filtered if isempty(F_form_filtered) F_form_filtered = zeros(3,1); end alpha = 0.3; F_form_filtered = alpha * F_form + (1-alpha) * F_form_filtered;4.3 仿真结果的可视化与性能评估
可视化我用MATLAB的plot3和quiver组合。每台ROV画成一个带方向箭头的球体，障碍物画成半透明圆柱，势场力用箭头表示。这样一眼就能看出哪台ROV受力异常。
性能评估看几个指标：队形误差（实际间距和期望间距的偏差）、避障最小距离（和障碍物的最近距离）、任务完成时间（从起点到目标的时间）、控制能量（推力平方的积分）。这几个指标能全面反映系统表现。
% 队形误差 formation_error = 0; for i = 1:num_rovs for j = i+1:num_rovs d = norm(rovs(i).pos - rovs(j).pos); formation_error = formation_error + abs(d - d_spacing); end end formation_error = formation_error / nchoosek(num_rovs, 2);实测下来，三台ROV在坝体巡检场景下，队形误差能控制在0.3米以内，避障最小距离大于0.8米，任务完成时间比单机手动操作缩短了约40%。
5. 常见问题与排查技巧实录
5.1 势场局部极小值导致ROV卡死
这是最常遇到的问题。ROV停在某个位置不动，合力为零，但目标还没到。排查方法是把势场力画出来，看是不是引力和斥力刚好抵消。
解决办法我试过几种：加随机扰动最简单，但效果不稳定；加切向分量效果好，但切向方向要选对；还有一种方法是虚拟目标点，当检测到卡死时，临时把目标点偏移一个角度，等绕过去再恢复。
% 卡死检测 if norm(rovs(i).pos - rovs(i).last_pos) < 0.01 && ... norm(rovs(i).pos - goal) > 1.0 % 虚拟目标点偏移 offset_angle = pi/4; R = [cos(offset_angle), -sin(offset_angle), 0; sin(offset_angle), cos(offset_angle), 0; 0, 0, 1]; virtual_goal = rovs(i).pos + R * (goal - rovs(i).pos); else virtual_goal = goal; end5.2 PD参数与势场参数耦合振荡
有时候单独调PD没问题，单独调势场也没问题，但合在一起就振荡。这是因为势场输出的期望速度变化太快，PD的带宽跟不上。
排查方法：把势场输出的期望速度单独画出来，看它的频率成分。如果高频分量多，就在势场输出端加低通滤波。滤波器的截止频率取PD带宽的1/3到1/2。
% 期望速度低通滤波 fc = 2; % 截止频率2Hz [b, a] = butter(2, fc/(1/(2*dt))); v_des_filtered = filtfilt(b, a, v_des);实操心得：滤波会引入相位滞后，如果滞后太大，ROV会反应迟钝。我的经验是截止频率不要低于1Hz，否则避障响应会明显变慢。
5.3 通信延迟导致队形发散
水声通信延迟是硬伤，仿真里如果设了1秒延迟，队形势场就会用1秒前的位置信息算力，导致队形振荡甚至发散。
解决办法有两个：一是降低队形势场增益，让队形恢复慢一点，给延迟留余量；二是用预测算法，根据历史位置估计邻居当前的位置。
% 线性预测邻居位置 function pos_pred = predict_position(pos_history, dt, delay) if size(pos_history, 2) < 2 pos_pred = pos_history(:, end); else vel = (pos_history(:, end) - pos_history(:, end-1)) / dt; pos_pred = pos_history(:, end) + vel * delay; end end5.4 常见问题速查表
| 问题现象 | 可能原因 | 排查方法 | 解决措施 |
|---|---|---|---|
| ROV卡死不动 | 势场局部极小值 | 画势场力矢量图 | 加切向分量或虚拟目标点 |
| 队形持续振荡 | 势场增益过大或通信延迟 | 记录队形误差曲线 | 降低k_form，加低通滤波 |
| 避障时撞上障碍物 | 斥力增益太小或rho_0太小 | 看最小距离记录 | 增大k_rep或rho_0 |
| PD响应超调大 | Kd太小或阻尼系数太小 | 看速度响应曲线 | 增大Kd或阻尼系数 |
| 仿真速度慢 | dt太小或ROV数量多 | 看仿真耗时 | 增大dt到0.1，或简化模型 |
| 队形恢复太慢 | k_form太小或滤波太强 | 看队形误差恢复时间 | 增大k_form或减小滤波强度 |
6. 从仿真到实物的几个关键考量
仿真跑通了不代表实物就能跑。我踩过的坑里，最大的一个是传感器噪声。仿真里位置信息是精确的，实物上声呐和惯导都有噪声，势场力算出来会抖。解决办法是在势场计算前先对位置做卡尔曼滤波，把噪声压下去。
第二个坑是推进器响应延迟。仿真里推力是瞬时建立的，实物上推进器从收到指令到出力有几十到几百毫秒延迟。这个延迟会让PD的相位裕度下降，严重时振荡。我的做法是在PD输出端加一个一阶惯性环节模拟推进器动态，然后重新整定PD参数。
% 推进器一阶惯性模型 tau_actual = tau_actual + (tau_cmd - tau_actual) * dt / T_thruster;第三个坑是水流干扰。静水仿真和动水实物差别很大。我在动力学模型里加了一个恒定流干扰项，模拟0.5节水流，然后看系统能不能稳住。如果稳不住，就需要增大PD增益或者加前馈补偿。
这套系统从仿真到实物大概花了三个月迭代，中间改了无数版参数。但核心架构没变，PD加势场的组合在实物上跑下来是稳的。如果你也在做类似的东西，建议先把仿真跑透，把参数范围摸清楚，再上实物，能省很多调试时间。
最后分享一个小技巧：势场参数不要一次调太多，每次只动一个，记录响应变化。我习惯用MATLAB的simulink做参数扫描，一次跑几十组，看哪组指标最好。这个办法比手动调快得多，而且不容易漏掉好的参数组合。
