拿到“船舶自动避碰”这个题目我的第一反应不是急着搜代码而是先想清楚一个问题现有路径规划算法那么多为什么偏偏要选人工势场法来做栅格化的全局搜索、随机采样的概率路线图、基于采样的快速搜索树这些方法在陆上机器人领域都成熟得很搬到船上却总有一种“水土不服”的感觉。后来把仿真跑起来才真正体会到人工势场法那种把环境作用力直接叠加、用合力方向指导运动的思路放到低速大惯性的船舶上反而异常贴合计算量小、响应快、调参直观特别适合做动态水域里的实时避碰决策。这篇文章记录的就是我在MATLAB里从零搭建一套基于人工势场法的船舶自动避碰系统仿真平台的完整过程。内容包括势场建模的数学推导、关键参数的选择逻辑、几个核心函数的写法、三个典型场景的仿真结果以及我在调试过程中踩过的坑。如果你正准备做船舶避碰、水面无人艇路径规划或者相关课程设计这篇文章应该能帮你少走不少弯路。1. 为什么船舶避碰会把人工势场法放在首位先把避碰问题的特殊性说清楚。船舶在水面上运动和轮式机器人有个根本差异船不能急停也不能急转。一个中等吨位的船满舵转一圈可能需要三到五倍船长舵效滞后、惯量大加上风浪流的外力干扰控制指令如果变化太剧烈实际航迹根本跟不住。这意味着避碰算法给出的不应该是“几何上的最优路径”而是一条让操舵系统能够平滑执行的可行路径。1.1 船舶避碰任务的三层约束我把船舶避碰的难点拆成三个层面。第一是环境动态性海上的障碍物不是静止的目标船、渔船、浮标都在相对运动而且距离越近相对运动越快算法必须在一个很短的周期内完成感知、判断、决策通常是秒级甚至亚秒级第二是运动学约束船只能沿着当前航向转向不能横移位置和艏向之间隔着复杂的动力学响应第三是规则约束国际海上避碰规则对会遇局面有限制比如对遇要各自向右转向、交叉局面要让位于右舷来船等。这三层约束叠加在一起对算法提出了一个很现实的要求计算延迟必须足够低输出指令必须平滑同时还要能处理连续变化的动态威胁。全局路径规划算法在这种场景下就很尴尬——环境一变整条路径都要重新算重算一次哪怕只有几百毫秒船可能已经冲出去几十米了。1.2 几种主流规划算法的横向对比我在动手前做过一次快速的算法对比把几种常见思路放在同一个坐标系里对照结论非常清晰算法规划方式实时性动态障碍处理轨迹平滑度实现复杂度A* / Dijkstra全局栅格搜索较低依赖频繁重规划折线、需要平滑中RRT / RRT*随机采样搜索中需动态扩展较差、需要剪枝中高动态窗口法速度空间采样高好较好中模型预测控制滚动时域优化低可显式建模好高人工势场法局部力矢量叠加高天然支持连续变化低人工势场法最大的亮点是它的计算量只和障碍物数量成线性关系十几个障碍物每一帧的势场计算都在毫秒级完全不需要预建栅格地图也不需要昂贵的优化求解器。它天然是一个局部规划器每一帧只根据当前感知到的障碍物位置和目标点位置输出一个控制指令环境发生变化时下一帧自动适应这个过程没有任何“重新规划”的开销。1.3 用一个直觉模型理解势场法人工势场法的思想用一句话就能讲清楚目标点对船产生一个“吸引力”障碍物对船产生一个“排斥力”船沿着两个力的合力方向走。你可以想象一个带正电的小球在一个电场里运动正电荷会顺着电场线跑而如果在路径上放一个同性电荷它就会被推开绕过去。船舶就是那个小球目标点是带负电的电极障碍物则是同性电荷。这个直觉模型虽然简单但它背后对应着一套完整的数学框架。势场是一个标量场力是势场的梯度方向船的运动方向由当前位置的梯度下降方向决定。理解了这一点后面所有代码逻辑就都能串起来了。2. 势场建模的数学核心与关键参数选择人工势场法的数学模型是整个系统的心脏。这一段我必须写得细一些因为参数选得对不对直接决定仿真结果是“平滑绕行”还是“疯狂抖动”。2.1 引力场与经典斥力场公式目标点产生的引力势场我用了最经典的二次型若船舶当前位置为 (q)目标点为 (q_{goal})则引力势场为[ U_{att}(q) \frac{1}{2} k_{att} |q - q_{goal}|^2 ]引力是势场的负梯度[ F_{att}(q) -\nabla U_{att}(q) -k_{att} (q - q_{goal}) ]这个表达式很直观距离目标越远引力越大方向始终指向目标点。斥力势场采用经典的障碍物距离函数形式。设船到某个障碍物的距离为 (\rho)障碍物斥力最大作用距离为 (\rho_0)则[ U_{rep}(q) \begin{cases} \frac{1}{2} k_{rep} \left(\frac{1}{\rho} - \frac{1}{\rho_0}\right)^2, \rho \leq \rho_0 \ 0, \rho \rho_0 \end{cases} ]对应的斥力为[ F_{rep}(q) k_{rep} \left(\frac{1}{\rho} - \frac{1}{\rho_0}\right) \frac{1}{\rho^2} \nabla \rho ]注意 (\nabla \rho) 方向是从障碍物指向船外所以合力斥力是“推着船离开障碍物”的方向。公式里有两个细节容易忽略第一斥力只有在 (\rho \leq \rho_0) 时生效这相当于给每个障碍物画了一个“影响圈”圈外的障碍物根本不用参与计算第二当船非常接近障碍物时斥力会以平方级激增这种非线性特性保证了船的“硬安全”——越近推力越大不会像某些软约束那样被突破。2.2 改进斥力场解决目标点附近的震荡问题只做基础版是不够的。经典势场法有一个著名的缺陷叫做“目标不可达问题”船如果绕完障碍物后要经过障碍物附近才能到达目标点目标吸引力和障碍物斥力会在目标点附近形成一个“拔河”局面船会在目标附近来回震荡永远进不了港。解决办法是给斥力场乘上一个与目标距离相关的调节系数[ U_{rep}(q) \begin{cases} \frac{1}{2} k_{rep} \left(\frac{1}{\rho} - \frac{1}{\rho_0}\right)^2 d_{goal}^n, \rho \leq \rho_0 \ 0, \rho \rho_0 \end{cases} ]其中 (d_{goal} |q - q_{goal}|)指数 (n) 一般取 2 或 3。这个改进的物理意义是当船越接近目标时障碍物的排斥力会自动衰减最终保证目标点仍是全局势场的最低点。我在仿真里用的是 (n2)效果已经很稳定。这个系数变化是所有后续仿真能收敛的基础。2.3 从合力到操舵指令的换算算出合力后真正的控制量不是力本身而是船舶的转向指令。我用一个简单但有效的两层映射期望航向角为[ \psi_{desired} \arctan2(F_y, F_x) ]然后当前航向 (\psi) 向期望航向逼近但转角速率必须限幅[ \omega \min\left(\omega_{max}, K_{\psi} \cdot \text{angle_diff}(\psi_{desired}, \psi)\right) ]这里 (K_{\psi}) 是转向增益(\omega_{max}) 是船舶最大转角速率配合船的旋回性指标来设定。这样做的好处是控制指令不会突变船的实际航迹是一条平滑曲线和操舵系统对接时也不会出现剧烈的舵令变化。2.4 障碍物膨胀与参数初值设定在建模阶段我把每个障碍物从“几何点”变成了“几何点加安全半径”。船舶有自己的物理尺寸船位点到障碍物边缘的净距离才是真正的安全距离所以我在代码里把障碍物的等效半径设为“实际半径 船宽的一半 最小避碰安全距离”。这一步不写进公式但直接影响参数表现属于工程上的常规处理。初始参数我按经验值设了一遍引力增益 (k_{att}3)斥力增益 (k_{rep}8)斥力作用范围 (\rho_02.5)最大转角速率 (\omega_{max}0.35) 弧度/秒转向增益 (K_\psi3)仿真步长 0.1 秒。这些初值在后面调试中都有调整但这个打底组合能保证第一版仿真流程先跑通。3. MATLAB主程序框架与核心函数实现工程实现阶段我没有把代码写成一个大脚本堆在一起而是按照“数据流清晰、函数独立可测、绘图单独隔离”的原则拆成了几个部分。这个习惯在后期调试时救了我很多次。3.1 工程目录与数据结构设计项目目录结构是这样的ship_apf/ ├── main_simulation.m % 主循环 ├── computeForce.m % 势场合力计算 ├── updateShip.m % 船舶运动模型更新 ├── drawScene.m % 可视化绘图 └── params.m % 参数集中定义船舶状态我用一个结构体保存字段包括位置坐标 x、y航向角 psi速度 v以及统计用的累计航程、避碰距离等。障碍物用一个 N×2 的矩阵保存每一行是一个障碍物的 x、y 坐标。目标点单独一个 1×2 数组。这种结构让代码的可读性很高后面扩展动态障碍物时直接改障碍物更新逻辑就行。3.2 核心函数 computeForce 的实现这是整个系统最核心的函数。它的输入是当前位置、障碍物数组、目标点和参数结构体输出是合力向量。我直接在代码里实现了基础斥力场和改进斥力场两种模式用参数开关控制切换function F computeForce(pos, obstacles, goal, params) % 计算人工势场合力 % 输入: % pos 当前船舶位置 [x; y] % obstacles 障碍物坐标 Nx2 % goal 目标点坐标 [gx; gy] % params 参数结构体, 包含 k_att, k_rep, rho0, dynRep % 输出: % F 合力向量 [Fx; Fy] F zeros(2, 1); % 引力项: 指向目标点 d_goal norm(pos - goal); if d_goal 1e-6 Fatt -params.k_att * (pos - goal); F F Fatt; end % 斥力项: 遍历所有障碍物 k_rep params.k_rep; rho0 params.rho0; for i 1:size(obstacles, 1) obs obstacles(i, :); rho norm(pos - obs); if rho rho0 rho 1e-6 % 从障碍物指向船的单位方向 gradRho (pos - obs) / rho; coeff k_rep * (1/rho - 1/rho0) / (rho^2); if params.dynRep % 改进斥力场: 乘目标距离平方, 缓解目标不可达 coeff coeff * (d_goal^2); end F F coeff * gradRho; end end % 斥力限幅, 防止数值爆炸 if norm(F) params.Fmax F params.Fmax * F / norm(F); end end代码里那个Fmax限幅是我后来加上的。早期仿真时如果障碍物特别近斥力模值会冲到几百船的航向被瞬间推开一个很大的角度整个轨迹直接发散。加限幅之后系统从“数学上发散”变成了“工程上可控”。3.3 核心函数 updateShip 的实现有了合力之后下一步是把合力换算成船舶的航行状态更新。我用的是一个简化的运动学模型忽略了横漂和侧滑只保留纵向前进速度和转向角速度function ship updateShip(ship, F, dt, params) % 根据合力更新船舶状态 % ship: 包含 x, y, psi, v 的结构体 % F: 合力向量, 来自 computeForce % dt: 仿真步长 % 期望航向角 desiredPsi atan2(F(2), F(1)); % 航向角误差, 用 wrapToPi 映射到 [-pi, pi] errPsi wrapToPi(desiredPsi - ship.psi); % 转角速率: 用 tanh 平滑逼近, 并限幅 omega params.omegaMax * tanh(params.Kpsi * errPsi); % 更新航向角 ship.psi wrapToPi(ship.psi omega * dt); % 更新位置 ship.x ship.x ship.v * cos(ship.psi) * dt; ship.y ship.y ship.v * sin(ship.psi) * dt; endtanh这个函数非常巧妙。当航向误差小的时候tanh(x)近似等于 x转向速率与误差成正比响应线性当误差大的时候输出被饱和到 ±1再乘以omegaMax就是最大转角速率不会出现超调过冲。这是一种工程上非常优雅的软限幅方式比硬裁剪平滑得多。3.4 主循环与安全距离监测主循环逻辑就是把上面两个函数串起来加一个可视化刷新和最近距离记录。每一帧的结构计算合力、更新状态、计算当前到所有障碍物的最近距离、更新历史轨迹数组、刷新绘图。安全距离监测我单独维护了一个变量minDist每一帧都比较一次并保留最小值。% 主循环 for k 1:round(duration/dt) % 计算合力 F computeForce(ship.pos, obstacles, goal, params); % 更新船舶状态 ship updateShip(ship, F, dt, params); % 记录轨迹 traj(k, :) [ship.x, ship.y]; % 监测最小安全距离 dists vecnorm(obstacles - [ship.x, ship.y], 2, 2); minDist min([minDist, min(dists)]); % 每10帧刷新可视化 if mod(k, 10) 0 drawScene(ship, obstacles, goal, traj); end end这一步做出来之后整个仿真主干已经能跑了。我从最简单的单障碍物场景开始验证船能够从起点绕开障碍物到达目标点轨迹平滑没有诡异的震荡。第一版程序从零到跑通大概花了一个下午剩下的时间几乎全在调参和踩坑上。4. 仿真实验三个典型场景下的行为分析参数和代码框架搭好之后我开始按场景梯度推进验证。场景设计逻辑是从简到繁先验证基本功能再验证边界情况最后验证动态能力。每一个场景我都在最终参数下重复运行了至少二十次观察轨迹的一致性和稳定性。4.1 场景一单障碍物绕行第一个场景设置了直线航线上的单障碍物起点 (0, 0)目标点 (10, 0)障碍物放在 (5, 0)。这个配置的目标很直接——验证基本路径规划和避障能力。为了保证对比的公平性我固定了一组参数跑了几轮观察了船迹的变化。得到的结果是典型的“圆弧绕行”轨迹船在进入斥力圈之前沿直线走接近障碍物后由于斥力分量逐渐增大航向角开始偏转偏转角度随着横向距离增大而增大直到横向偏离量足够压倒引力分量后开始回正最终在越过障碍物后平滑回归到目标方向。整个轨迹没有出现尖角也没有明显抖动。转向的起点大致在离障碍物约 (rho0) 处这说明斥力作用距离直接决定了“开始打舵的时机”和预期一致。4.2 场景二多障碍物与局部极小值问题第二个场景是多障碍物组成的窄通道。我放了三到五个随机分布的障碍物起点和终点分别位于通道两端。这里遇到了经典问题——船停在两个障碍物之间不动或者在一个局部势场的最低点反复震荡。这个问题的本质需要解释一下。人工势场法是一个“贪心梯度下降”方法它只保证船一直沿着势场下降方向走不保证最终能到达势场最低点。当目标引力和多个障碍物斥力在某个位置正好达到平衡时船就陷入了局部极小值无论怎么迭代都无法脱困。我在场景里观察到的现象是船停在某个位置合力的模值几乎为零航向角在 ±5° 范围内微幅摆动但位置几乎不动。针对这个问题我在仿真里加了一个“脱困机制”连续检测到船在 3 秒内位移小于某个阈值时判定陷入局部极小值临时在合力的垂直方向加一个侧向偏置力打破力的平衡把船“推”出极小值区域。这个方案的有效期有限——如果势场本身存在多个连通的极小值区域靠扰动有可能反复触发。更根本的方案是设置分段子目标把全局路径拆成多段每段只做局部避碰这个思路在第六章还会展开。4.3 场景三动态目标船的避让第三个场景加入了运动目标。我模拟了一艘从侧前方横穿的目标船初始位置在 (6, 2)以恒定速度向下负 y 方向运动。本船保持向目标点运动每帧更新目标船的位置并参与斥力计算。这个场景暴露了一个关键问题——目标船的“更新时钟”和主仿真的“积分时钟”必须对齐。如果目标船每 5 秒才更新一次位置而本船每 0.1 秒更新一次那么目标船的位置在两次更新之间是“跳变”的势场跟着跳变航向角也跟着跳变轨迹出现明显的锯齿状。解决方法是把目标船的运动状态保存下来每一帧按线性插值更新它的位置这相当于对目标船做了一个速度模型外推。改用插值之后轨迹立刻变得平滑避让动作也更符合实际的操舵习惯。这个场景同样用来观察安全距离的保持能力。设定目标船初始距离较近的情况下船会在进入危险距离之前提前转向。最小安全距离出现在目标船横穿航线正前方的那一瞬间距障碍物边缘约 0.8 米——和斥力作用范围的设计值基本吻合。5. 参数整定与踩坑排查记录所有项目做到最后最花时间的一定是调参和排查。这一部分把我的踩坑记录整理出来每条都是真实出现过的现象排查链路也写得尽量完整。5.1 参数扫描与经验区间汇总我没有一个一个地试参数而是采用了控制变量法先固定其他参数只扫描某一个观察轨迹的变化规律再换下一个。这种做法的好处是结果可解释、可复现。参数含义调小的影响调大的影响经验区间k_att引力增益入斥力圈更晚、绕行半径小入斥力圈更早、绕行半径大0.5~10k_rep斥力增益安全裕度小、可能剐蹭安全裕度大、轨迹更弯5~30rho0斥力作用距离开始转向晚开始转向早1.0~3.5omegaMax最大转角速率转弯慢、曲线半径大转弯快、轨迹更贴近障碍物0.2~0.5Kpsi转向增益响应迟缓响应快但易震荡2~5最典型的规律是 k_rep 和 k_att 的比值关系。比值太小斥力压不住引力船会直线冲向障碍物比值太大引力几乎被完全抵消船会在远离障碍物处就开始大弧度转弯绕行半径过大甚至出现“不必要的绕行”。5.2 踩坑一斥力作用距离内的锯齿航向最早调试时我发现船在进入斥力圈后航向角会出现一个高频小幅摆动大概每 10 帧左右震动一次。排查链路是这样的先打印合力分量和航向角误差发现合力方向本身是平滑的但航向角误差在高频跳动再检查 updateShip 里的角速度计算发现角速度限幅的写法是硬裁剪if omega omegaMax, omega omegaMax没有对负方向的对称限制导致某个方向响应快、另一个方向响应慢修正为对称裁剪后再看锯齿消失。这个问题的本质是限幅函数的不对称性。改进写法可以直接用omega min(max(omega, -omegaMax), omegaMax)或者像我前面代码那样直接用tanh做软限幅两种都行。5.3 踩坑二坐标轴绘图的视角错觉这个坑其实非常低级却很浪费时间。初期在双目标场景里船明明朝着目标点运动画图却看起来在反向走。排查发现绘图脚本里把ship.x和ship.y的顺序写反了plot(y, x)和plot(x, y)在坐标系里是沿 yx 对称的两个图形。这个问题的教训是所有坐标都统一用 [x, y, psi] 顺序绘图和计算函数共用同一个坐标规范不搞两套。5.4 踩坑三动态障碍物位置跳变导致的锯齿轨迹前面在场景三提到过动态目标船如果不做插值轨迹会呈锯齿状。具体排查时我先是怀疑步长太大、怀疑滤波器参数最后打印目标船的位置才看到每 5 帧跳变一次的距离。这种锯齿本质上不是控制问题而是外部输入更新率过低导致的采样问题。通过保留目标速度并对位置做线性外推即可解决不需要修改任何控制器参数。5.5 关于避碰规则的初步融合尝试人工势场法本身完全不理解避碰规则它只知道“别撞”不知道“该往哪边让”。在会遇局面里两个船很可能都选择同一个方向避让反而造成新的碰撞风险。我在仿真里做了一个非常初级的尝试检测到右侧一定扇区内存在目标船时给合力叠加一个向右的偏置分量检测到左侧目标船时叠加一个向左的偏置分量把船从“可能相向而行”变成“错开”。这个偏置的思路其实相当粗糙更完善的规则融合需要把行为决策层和路径规划层分开让规则模块先决定避让策略再由势场法去执行。6. 从仿真走向实船之前还需要补哪几块仿真能跑通离实船应用还有一段距离。这一段我想说清楚哪些东西在仿真里可以忽略真到了工程现场没办法忽略。6.1 运动模型的维度升级仿真里用的运动学模型是“只要给航向角下一秒就能转到”实际船舶可没有这个能力。螺旋桨推力、舵力、船体绕流、横摇纵摇这些都会让实际船的位置变化滞后于指令变化。往实船走第一步就是把模型换成三自由度的运动模型至少把位置、艏向角、纵向速度、横向速度、转艏角速度这五个状态都纳入计算让力和力矩层面的关系进入系统。势场法的输出仍然可以作为期望航向或者期望转艏率的指令源但底层必须有一个能够跟踪这个指令的动力学控制器。6.2 感知信息的噪声与不确定性仿真里的障碍物坐标是上帝视角给的每一帧精确已知。真船上障碍物位置来自雷达回波、AIS 报文和电子海图这些数据天然带噪声目标船的位置还有更新延迟。势场法这种对位置敏感的算法直接把带噪声的坐标喂给它航向指令会跟着抖动。工程上需要先用卡尔曼滤波或粒子滤波处理目标轨迹再进势场计算模块。这个滤波环节不是可选项是必需品。6.3 全局规划与局部避碰的混合架构人工势场法定位成“全局路径规划器”其实不太合适它更适合做“局部避碰层”。完整的工程架构应该是两层上层用全局规划算法比如在电子海图的航路点之间做 A* 或 Dubins 曲线规划算出一条基本的航路点序列下层再由人工势场法在航路点之间做细化处理当前环境中实时出现的障碍物。上层管“战略方向”下层管“战术规避”这样既保证了全局最优性的弱依赖又把实时防撞能力留在底层。这个混合架构是当前水面无人艇路径规划的主流思路之一。最后再分享一点这次做下来的体会整个项目做完我最深的感觉是人工势场法这套东西入门门槛低到不可思议但要做到好用并不轻松。低门槛来自它的物理直觉两道公式就能讲清楚模型不好用则体现在所有细节里——参数标定要反复试局部极小值要设计退出机制动态目标要处理采样同步规则约束要另想办法接进去。每一个问题单独拿出来都不难但问题叠在一起时如果没有一套清晰的数据流和可视化的调试环境很容易在乱成一团的参数里迷失方向。给你一个很具体的建议如果是从头开始复现类似系统第一版先别追求复杂场景用一个障碍物、一条直线航线跑通闭环观察力矢量的直观效果。看船在接近障碍物时合力箭头是如何从“指向目标”逐渐转到“斜向推开”的这个画面会帮你建立对势场法最深层的直觉。建立了这个直觉后面的任何扩展都只是在给这个基础模型做加法。