1. 人工势场法到底在解决什么问题1.1 从机器人到船舶同一套算法为什么换了玩法做机器人避障时我第一次接触人工势场法觉得这算法的思路简单得有点“可爱”目标点在前面拉障碍物在旁边推路径自然就出来了。等搬到船舶路径规划里以后我发现自己天真了——小船不会原地打转、不会急刹车海流还会帮你“作弊”。这篇文章就记录我从机器人场景转到船舶场景后在人工势场法上踩过的坑、调过的参数和最终的实现思路。机器人尤其是轮式或全向移动机器人运动模型接近于“点质量”可以急停、可以原地转横向位移也不会被环境刻意干扰。而船舶是典型的欠驱动系统横向没有独立的驱动力转向靠舵或侧推低速时舵效几乎为零还要背着很大的惯性。把陆地避障那套势场力直接换算成速度指令船会“横着滑”或者“甩尾”结果姿态和预期完全两回事。我在这个阶段做的最关键改动是把“合力方向”从位置指令改成了“期望艏向”再经过一个航向控制器去执行。这只是第一步更麻烦的是水面环境还有流、有浪、有浅滩障碍物也不再是固定圆斑而是动态船、浮标和作业平台。这些差异加在一起势场法从“拿来就能跑”变成了“得重新设计后才能跑”。1.2 为什么在船舶场景还要选势场法可能有人会问既然这么麻烦为什么不用更“聪明”的算法A*、RRT、DWA都能做路径规划在栅格地图上A*给出的路径全局最优RRT适合高维空间采样DWA直接考虑运动学约束。这些方法我都试过但最后局部避障层还是选用了势场法原因有三个。第一势场法是反应式的每个控制周期只要做一次距离计算和矢量合成延迟极低。在开阔水域面对动态目标时算法必须在几百毫秒内给出新的避让方向重规划式的方法往往来不及。第二势场法生成的路径天然连续平滑没有阶梯状折线符合船舶“大惯性慢转向”的属性。第三它的实现成本低不依赖昂贵算力嵌入式设备也能轻松跑起来。当然势场法有它的毛病局部最小值、目标点不可达、动态避让规则缺失这些都是真实存在的。正因为这些毛病我才觉得把它用在船舶上是一件需要大量工程调优的事而不是一个简单的公式堆叠。接下来的内容就是围绕模型建法、参数调优和问题排查来展开。2. 势场模型怎么建引力、斥力与合力2.1 引力场让船朝着目标走势场法的第一步是构造引力场。经典的思路是把目标点变成一个“势阱”离目标越远势能越高船在势能梯度作用下被“吸”向目标。常用的引力势场写作 U_att 0.5 * k_att * |p - p_goal|^2对应的引力 F_att -k_att * (p - p_goal)。这里的比例系数 k_att 决定了“拉力”的强度。这里要注意一个问题引力大小随距离线性增长如果在开阔水域目标距离有十几公里算出来的引力会非常大整个路径规划就退化成“直奔目标”任何障碍物都拦不住。所以工程上我几乎从不直接用这个原始版本而是改成带阈值距离的分段形式距离小于阈值时用线性引力距离大于阈值时把引力幅值钳到一个上限。这样既保证了远距离航向稳定又不会让障碍物的斥力全部失效。实际操作中引力系数 k_att 的标定和船舶期望巡航速度相关。比方说一艘船长30米、巡航航速8节的船我一开始把 k_att 设为5.0然后在仿真里观察期望艏向的变化幅度。如果艏向抖得太厉害就降下来如果转向反应太迟钝就适当升上去。引力系数和斥力系数从来都不是单独调的后面我会专门讲调参顺序。2.2 斥力场让船绕开障碍物斥力场是势场法的核心也是各家实现差异最大的地方。经典斥力势场定义成 U_rep 0.5 * k_rep * (1/ρ - 1/ρ0)^2其中 ρ 是船到障碍物的最近距离ρ0 是影响半径。对应的斥力绝对值是 k_rep * (1/ρ - 1/ρ0) / ρ^2方向由障碍物指向船。从公式可以看到船越靠近障碍物斥力增长得越快而且是距离平方级别的增长这保证了近距离时安全优先。但真正用到船舶场景时我对这里做了一点改动考虑到船的惯性大斥力作用半径不能太小否则船还没开始转向已经进了危险距离。我曾经用 ρ0 等于2倍船长的参数做过一组实验路径在通过狭窄水道时离障碍物边缘几乎贴平安全裕度很小后来改为5倍船长才留出足够的转向空间。还要提一个容易犯的错误如果所有静态障碍物都用同样半径的圆来处理杂乱的地图会把这些圆连成一片产生一个巨大的“压力墙”。船本来可以从两块浅滩之间穿过去结果被虚拟的墙挡住只能大绕。解决办法是使用“按障碍物实际外接多边形采样生成的局部势场”而不是直接把整个障碍物等价成一个圆。这个细节直接影响路径是否可用。2.3 合力合成与执行约束当船同时受到引力、多个障碍物的斥力后合力 F F_att Σ F_rep。下一步不是直接把速度方向设为合力方向因为我前面说过船有运动学约束。我采用的方案是把合力方向映射为期望航迹向再经过艏向控制环生成舵令。这里有个执行约束要点合力方向变化率不能超过船舶的最大转艏速率。假设最大转艏速率为每秒3度控制周期0.1秒那么一个周期内期望艏向最多改变0.3度。直接在艏向控制环外再加一个限幅器就能有效消除高频抖动。这个限幅器的意义在于算法输出再激进的转向指令船的物理系统也能通过机电限制去消化它避免舵机饱和。如果忽略执行约束就会出现一种典型现象船在障碍物前方快速左右摆动看起来像在“蛇形前进”但实际舵一直打到极限速度还掉得厉害。这种路径在仿真里也许“绕过去了”在真实船上根本做不到。3. 船舶场景下的落地流程与参数调优3.1 运动模型与航向控制器的配合要把势场法接进一条船必须先有个能用的运动模型。船舶三自由度运动学方程很常见x和y方向速度由纵向速度和艏向角决定艏向角变化率由转艏角速率决定。在对地坐标系下还需要考虑流场这部分我在后面扩展里再展开。我用的是一个简化模型船以恒定速度前进只有艏向角可调。这个模型丢掉了很多细节但对于验证势场法够用。航向控制器用的是最朴素的比例控制输出舵角后再过一个一阶惯性环节模拟舵机响应。把期望艏向输入进去船会在转弯时有一定的滞后这个滞后恰恰暴露了势场参数是否过度激进。一个常见的问题是仿真里直接在位置更新时加合力方向看上去路径很顺但接上真实运动模型后却出现大偏差。原因是纯运动学更新里转向是即时生效的而真实船有回转半径舵效靠水动力产生存在迟滞和横倾。所以我在测试时都会强制要求“算法输出控制环运动模型”一起跑用最终的控制精度来评判势场法的好坏而不是只看算法层路径是否漂亮。3.2 仿真场景与初始参数接下来是完整的落地过程。我搭的是一个栅格化的电子海图环境图上标出起点和终点中间布置三块形状不规则的静态障碍物另外还有两个动态目标沿固定航线移动。初始参数先按经验值给再逐步调优。参数初始值说明船长 L_wpp30 m用于换算斥力影响半径巡航速度8 kn约4.1 m/s恒定引力系数 k_att5.0引力上限钳位在12斥力系数 k_rep15.0初始偏大之后降到12斥力影响半径 ρ05 * L_wpp150 m控制周期 dt0.1 s与传感器融合周期一致最大转艏速率3 deg/s舵机与船体回转性能综合这是我最开始的一组参数。第一版跑下来路径非常绕几乎每个障碍物都画了一个大半圆避开原因是斥力系数偏大、影响半径偏大引力在阈值外又被限制住了。后来把 k_rep 降到12ρ0 调整为4倍船长路径才变得合理。所以说参数表里每一项都存在耦合不能只看单项指标。下面放一个势场力计算的关键示意函数方便理解位置姿态怎么转成合力def compute_force(position, goal, obstacles, k_att, k_rep, rho0, att_limit): # 引力项 att k_att * (goal - position) if norm(att) att_limit: att att / norm(att) * att_limit # 斥力项 rep np.zeros(2) for obs in obstacles: d norm(position - obs.pos) if d rho0 and d 1e-3: vec (position - obs.pos) / d rep k_rep * (1.0 / d - 1.0 / rho0) / (d * d) * vec return att, rep注意这里返回的是平面上的力矢量实际落地时还要经过航向控制环转换成舵角指令不能直接把矢量当速度用。3.3 调参重点与现场记录记录一次典型调参过程第一次运行船在距离第一个障碍物280米时就缓缓转向路径绕行半径很大浪费了约300米的航程。第二次运行把 ρ0 从150米减到120米路径的绕行距离缩短约20%但最小距离也变小仍然安全。第三次运行把 k_rep 从15降到12路径开始出现一点震荡于是把最大转艏速率限制从3度每秒降到2.5度每秒震荡被抑制住。最终一组参数稳定运行100秒无碰撞通过动态窗口时保持安全距离不低于50米。这里我总结出两个经验调参顺序要按照“距离尺度优先、力的比例次之、运动约束最后”来。先把影响半径 ρ0 定好让路径的整体形状合理再调 k_att 和 k_rep 的比例让路径的曲率合适最后加适合船体能力的转艏约束消除高频抖动。如果一开始就一个参数一个参数暴力试很容易陷入一个看似安全却极度绕路甚至不收敛的局部参数组合。提示不要一上来就追求“最优参数”。先把地图、障碍物影响半径、合力分布可视化出来在几何上一眼看清楚再去微调力系数效率会高很多。4. 四大头疼问题的排查与解法实录4.1 局部最小值问题船卡在U形障碍区里所有用势场法的人都会遇到这个问题船进入一个凹形障碍结构引力把船往目标方向推但障碍物三面合围形成的斥力把船往外顶合力趋近于零船就在谷底来回徘徊像是被困在一个无形的笼子里。排查的方法是监测合力大小。我的做法是设定一个最小速度阈值当合力模长连续3秒低于阈值且船位几乎不动就判定进入局部最小值。处理手段通常有两种一种是临时生成“虚拟子目标”把子目标放在障碍物缺口外侧先让船朝子目标移动绕出凹槽后再切换回原目标另一种是给合力叠加一个垂直于局部极小值方向的小幅扰动。前者更可控也更符合船舶的操纵习惯。实际工程里我几乎不用随机扰动。因为船有惯性扰动消失后仍然有可能重新陷入同一个势阱如果扰动不合适还会让船大幅偏航。虚拟子目标法虽然要额外维护一个状态机但至少行为是确定的也好在仿真里验收。我当时在船进入U形区域后自动生成了一个绕行子目标船先稳定转向到左侧绕过障碍物尖端后再重新切换回原目标整段路径没有明显停顿。4.2 目标点不可达总是在目的地附近转圈还有一个特别烦人的问题目标点本身离障碍物很近时引力在目标点处被斥力抵消船距离目标点还有十几米就开始原地绕圈永远无法真正“靠泊”。这个现象在学术上称为目标不可达问题GNRON。我当时排查这个现象首先做的不是改代码而是把合力的分解图画出来。果然发现目标点处的斥力方向和引力方向几乎相反净合力为零。解决方法是给斥力场加上距离目标的因子当船接近目标点时把斥力衰减掉。具体实现是把斥力场乘以一个目标距离相关的权重越靠近目标障碍物对船的影响越小引力自然占上风。还有一个更实用的工程做法在目标点周围划定一个“靠泊圆”半径比如1.5倍船长。当船进入靠泊圆时直接切断势场法输出切换到专门的靠泊控制逻辑。实际船舶靠泊本来就要考虑低速和精准定位不是一条单纯势场路径能解决的事所以这个切换逻辑在很多实际系统里都存在而不仅仅是学术上的权宜之计。我在这个项目里就是采用的这种方式最终才彻底告别了目标点附近的震荡圈。4.3 动态会遇只靠距离算斥力远远不够前面说的都是静态障碍物真实水域里更大的威胁是其他正在移动的船。如果用静态斥力公式去处理船只会根据距离远近回避完全没有“相对速度”的概念。结果就是对面来船时我的船会偏向来船行进方向的反向走这在直觉上不对也不符合海上避让习惯。解决这个问题的思路是在斥力项中加入相对速度因子。具体做法是计算我船与目标船的相对速度在相对位置方向上的投影如果相对速度让两船距离变近斥力就放大如果两船已经在相互远离斥力就可以适当减小甚至忽略。这东西说起来简单实现时要注意坐标系统一把相对速度从随船坐标系转换到对地坐标系。另外还要提到纯势场法对动态目标非常容易产生“锯齿形”避让轨迹。为了抑制这种锯齿我在势场输出后再加了一阶滤波滤波时间常数大概0.5秒。这样船在连续两个周期之间收到的指令不会突然反向路径整体平滑很多。缺点是被动变向有些滞后但对我这个场景来说安全性并没有降低。4.4 数值抖动与栅格分辨率的选择最后一个常见问题是数值抖动。栅格地图分辨率太粗时势场梯度在栅格边缘会产生锯齿太细时算力开销增大而且传感器噪声会被直接放大。我在船舶场景里试过不同分辨率最后用100米格比较合适对于90米以内的障碍物边沿再做单独加密。栅格分辨率也直接影响障碍物膨胀半径的设定。船舶路径规划中静态障碍物一般要先膨胀半径膨胀量至少是半个船宽加安全余量。对于机动性较差的大船安全余量不能只看船宽要看制动距离和回转半径。所以我常用的设定是膨胀量从20米起步根据仿真碰撞距离反馈来调整而不是一次性把网格画得很密。如果栅格太细但膨胀量不足路径可能会从狭窄缝隙里穿过去而真实船根本转不过去。5. 把势场法放进更大的路径规划体系里5.1 全局航迹局部避障的组合几乎不会有系统只用势场法来完成全航程规划因为势场法没有全局视野不知道前方是海峡还是开阔水域。我采用的架构是两层规划全局层用A*在电子海图离线栅格上计算一条安全参考航线给出若干航路点局部层在船实时位置周围一定范围内计算势场对参考航线做动态修正。这样做的关键收益是即使局部遇到突发障碍船的偏离也是有界的因为势场法生成的只是局部修正量不是完全脱离航线的随机方向。一旦障碍物越过船又能平滑地回到参考航线上而不会出现“绕过一座岛后完全忘了原来要去哪”的情况。很多从论文转向工程的人常常忽略这一层只盯着势场参数优化结果系统鲁棒性很差。从工程实施角度我给参考航线增加了一个“走廊”约束只要船当前位置距离参考航线的垂线偏差超过某个阈值势场法就增加一个指向航线的恢复力如果偏差在阈值内恢复力权重很小保持正常避障优先。这种带软约束的架构比单纯朝目标点拉引力稳健得多。5.2 水流与海图的耦合船舶场景中还有一个不可忽略的因素水流。在不规则岸线和开阔水域潮汐和洋流会对船产生横向漂移。如果势场法不懂流场船实际走的是对地轨迹但路径规划用的是对水模型两者不一致就会在长距离航行后产生巨大偏差。我采用的做法是在势场输出上叠加一个流场补偿项期望对地速度等于期望对水速度加流速矢量。换句话说看到目标方向虽然是船艏指向但要真正朝那个方向漂移船艏还得向水流上游偏一个角度。把这个补偿量放到合力计算里本质上是把流场当成一个恒定的“背景势”而不是障碍物。这部分在水流较弱的内河航道可能影响不大但在沿海或潮流明显的水域非常关键。我也建议所有做船舶路径规划的人在设计验证环境时就把流场参数加进去否则标定出来的参数拿到真船上很可能会“水土不服”。5.3 势场法还能用在哪些地方既然做了这么多探索势场法的应用范围还可以再延伸。例如把港口泊位、锚地、固定海上平台的边界都抽象成障碍物并用不同影响半径来区分“硬障碍”和“软约束”可以实现更贴合真实航行习惯的路径。还可以把动态船的预测轨迹直接作为时变障碍物输入在时间维度上提前绕行这比单纯在当前位置计算斥力所能获得的路径质量更高。在船舶集群或编队场景中同样可以把“保持编队队形”的期望位置视为目标点引力“避让其他编队成员”视为场内斥力从而把路径规划和编队控制放在同一个势场框架里。这些扩展我并没有全部落地但方向上是可行的。只要记住一点势场法的核心在于“用场来表达约束”而怎么定义好这个场比单纯调整系数更重要。我在实际项目里还有一个体会每次拿到一个新的路径规划场景不要急着调 k_att 和 k_rep。先把地图画出来把障碍物边缘和影响半径可视化再看合力分布图很多问题一眼就能看出来。等我对势场的行为有了直觉再动手改参数效率反而最高。另外所有仿真验证都要至少跑三种典型工况静态障碍物穿插、U形障碍、动态迎面目标。如果这三个场景都能稳定通过再考虑接真实船舶运动模型。我在这个项目里最大的收获并不是用势场法生成了多完美的路径而是学会了一件事算法越简单越要仔细对待它背后的物理假设。船不是粒子势场法也不是万能药把这两点想清楚它就能在船舶路径规划里发光。
1. 人工势场法到底在解决什么问题1.1 从机器人到船舶同一套算法为什么换了玩法做机器人避障时我第一次接触人工势场法觉得这算法的思路简单得有点“可爱”目标点在前面拉障碍物在旁边推路径自然就出来了。等搬到船舶路径规划里以后我发现自己天真了——小船不会原地打转、不会急刹车海流还会帮你“作弊”。这篇文章就记录我从机器人场景转到船舶场景后在人工势场法上踩过的坑、调过的参数和最终的实现思路。机器人尤其是轮式或全向移动机器人运动模型接近于“点质量”可以急停、可以原地转横向位移也不会被环境刻意干扰。而船舶是典型的欠驱动系统横向没有独立的驱动力转向靠舵或侧推低速时舵效几乎为零还要背着很大的惯性。把陆地避障那套势场力直接换算成速度指令船会“横着滑”或者“甩尾”结果姿态和预期完全两回事。我在这个阶段做的最关键改动是把“合力方向”从位置指令改成了“期望艏向”再经过一个航向控制器去执行。这只是第一步更麻烦的是水面环境还有流、有浪、有浅滩障碍物也不再是固定圆斑而是动态船、浮标和作业平台。这些差异加在一起势场法从“拿来就能跑”变成了“得重新设计后才能跑”。1.2 为什么在船舶场景还要选势场法可能有人会问既然这么麻烦为什么不用更“聪明”的算法A*、RRT、DWA都能做路径规划在栅格地图上A*给出的路径全局最优RRT适合高维空间采样DWA直接考虑运动学约束。这些方法我都试过但最后局部避障层还是选用了势场法原因有三个。第一势场法是反应式的每个控制周期只要做一次距离计算和矢量合成延迟极低。在开阔水域面对动态目标时算法必须在几百毫秒内给出新的避让方向重规划式的方法往往来不及。第二势场法生成的路径天然连续平滑没有阶梯状折线符合船舶“大惯性慢转向”的属性。第三它的实现成本低不依赖昂贵算力嵌入式设备也能轻松跑起来。当然势场法有它的毛病局部最小值、目标点不可达、动态避让规则缺失这些都是真实存在的。正因为这些毛病我才觉得把它用在船舶上是一件需要大量工程调优的事而不是一个简单的公式堆叠。接下来的内容就是围绕模型建法、参数调优和问题排查来展开。2. 势场模型怎么建引力、斥力与合力2.1 引力场让船朝着目标走势场法的第一步是构造引力场。经典的思路是把目标点变成一个“势阱”离目标越远势能越高船在势能梯度作用下被“吸”向目标。常用的引力势场写作 U_att 0.5 * k_att * |p - p_goal|^2对应的引力 F_att -k_att * (p - p_goal)。这里的比例系数 k_att 决定了“拉力”的强度。这里要注意一个问题引力大小随距离线性增长如果在开阔水域目标距离有十几公里算出来的引力会非常大整个路径规划就退化成“直奔目标”任何障碍物都拦不住。所以工程上我几乎从不直接用这个原始版本而是改成带阈值距离的分段形式距离小于阈值时用线性引力距离大于阈值时把引力幅值钳到一个上限。这样既保证了远距离航向稳定又不会让障碍物的斥力全部失效。实际操作中引力系数 k_att 的标定和船舶期望巡航速度相关。比方说一艘船长30米、巡航航速8节的船我一开始把 k_att 设为5.0然后在仿真里观察期望艏向的变化幅度。如果艏向抖得太厉害就降下来如果转向反应太迟钝就适当升上去。引力系数和斥力系数从来都不是单独调的后面我会专门讲调参顺序。2.2 斥力场让船绕开障碍物斥力场是势场法的核心也是各家实现差异最大的地方。经典斥力势场定义成 U_rep 0.5 * k_rep * (1/ρ - 1/ρ0)^2其中 ρ 是船到障碍物的最近距离ρ0 是影响半径。对应的斥力绝对值是 k_rep * (1/ρ - 1/ρ0) / ρ^2方向由障碍物指向船。从公式可以看到船越靠近障碍物斥力增长得越快而且是距离平方级别的增长这保证了近距离时安全优先。但真正用到船舶场景时我对这里做了一点改动考虑到船的惯性大斥力作用半径不能太小否则船还没开始转向已经进了危险距离。我曾经用 ρ0 等于2倍船长的参数做过一组实验路径在通过狭窄水道时离障碍物边缘几乎贴平安全裕度很小后来改为5倍船长才留出足够的转向空间。还要提一个容易犯的错误如果所有静态障碍物都用同样半径的圆来处理杂乱的地图会把这些圆连成一片产生一个巨大的“压力墙”。船本来可以从两块浅滩之间穿过去结果被虚拟的墙挡住只能大绕。解决办法是使用“按障碍物实际外接多边形采样生成的局部势场”而不是直接把整个障碍物等价成一个圆。这个细节直接影响路径是否可用。2.3 合力合成与执行约束当船同时受到引力、多个障碍物的斥力后合力 F F_att Σ F_rep。下一步不是直接把速度方向设为合力方向因为我前面说过船有运动学约束。我采用的方案是把合力方向映射为期望航迹向再经过艏向控制环生成舵令。这里有个执行约束要点合力方向变化率不能超过船舶的最大转艏速率。假设最大转艏速率为每秒3度控制周期0.1秒那么一个周期内期望艏向最多改变0.3度。直接在艏向控制环外再加一个限幅器就能有效消除高频抖动。这个限幅器的意义在于算法输出再激进的转向指令船的物理系统也能通过机电限制去消化它避免舵机饱和。如果忽略执行约束就会出现一种典型现象船在障碍物前方快速左右摆动看起来像在“蛇形前进”但实际舵一直打到极限速度还掉得厉害。这种路径在仿真里也许“绕过去了”在真实船上根本做不到。3. 船舶场景下的落地流程与参数调优3.1 运动模型与航向控制器的配合要把势场法接进一条船必须先有个能用的运动模型。船舶三自由度运动学方程很常见x和y方向速度由纵向速度和艏向角决定艏向角变化率由转艏角速率决定。在对地坐标系下还需要考虑流场这部分我在后面扩展里再展开。我用的是一个简化模型船以恒定速度前进只有艏向角可调。这个模型丢掉了很多细节但对于验证势场法够用。航向控制器用的是最朴素的比例控制输出舵角后再过一个一阶惯性环节模拟舵机响应。把期望艏向输入进去船会在转弯时有一定的滞后这个滞后恰恰暴露了势场参数是否过度激进。一个常见的问题是仿真里直接在位置更新时加合力方向看上去路径很顺但接上真实运动模型后却出现大偏差。原因是纯运动学更新里转向是即时生效的而真实船有回转半径舵效靠水动力产生存在迟滞和横倾。所以我在测试时都会强制要求“算法输出控制环运动模型”一起跑用最终的控制精度来评判势场法的好坏而不是只看算法层路径是否漂亮。3.2 仿真场景与初始参数接下来是完整的落地过程。我搭的是一个栅格化的电子海图环境图上标出起点和终点中间布置三块形状不规则的静态障碍物另外还有两个动态目标沿固定航线移动。初始参数先按经验值给再逐步调优。参数初始值说明船长 L_wpp30 m用于换算斥力影响半径巡航速度8 kn约4.1 m/s恒定引力系数 k_att5.0引力上限钳位在12斥力系数 k_rep15.0初始偏大之后降到12斥力影响半径 ρ05 * L_wpp150 m控制周期 dt0.1 s与传感器融合周期一致最大转艏速率3 deg/s舵机与船体回转性能综合这是我最开始的一组参数。第一版跑下来路径非常绕几乎每个障碍物都画了一个大半圆避开原因是斥力系数偏大、影响半径偏大引力在阈值外又被限制住了。后来把 k_rep 降到12ρ0 调整为4倍船长路径才变得合理。所以说参数表里每一项都存在耦合不能只看单项指标。下面放一个势场力计算的关键示意函数方便理解位置姿态怎么转成合力def compute_force(position, goal, obstacles, k_att, k_rep, rho0, att_limit): # 引力项 att k_att * (goal - position) if norm(att) att_limit: att att / norm(att) * att_limit # 斥力项 rep np.zeros(2) for obs in obstacles: d norm(position - obs.pos) if d rho0 and d 1e-3: vec (position - obs.pos) / d rep k_rep * (1.0 / d - 1.0 / rho0) / (d * d) * vec return att, rep注意这里返回的是平面上的力矢量实际落地时还要经过航向控制环转换成舵角指令不能直接把矢量当速度用。3.3 调参重点与现场记录记录一次典型调参过程第一次运行船在距离第一个障碍物280米时就缓缓转向路径绕行半径很大浪费了约300米的航程。第二次运行把 ρ0 从150米减到120米路径的绕行距离缩短约20%但最小距离也变小仍然安全。第三次运行把 k_rep 从15降到12路径开始出现一点震荡于是把最大转艏速率限制从3度每秒降到2.5度每秒震荡被抑制住。最终一组参数稳定运行100秒无碰撞通过动态窗口时保持安全距离不低于50米。这里我总结出两个经验调参顺序要按照“距离尺度优先、力的比例次之、运动约束最后”来。先把影响半径 ρ0 定好让路径的整体形状合理再调 k_att 和 k_rep 的比例让路径的曲率合适最后加适合船体能力的转艏约束消除高频抖动。如果一开始就一个参数一个参数暴力试很容易陷入一个看似安全却极度绕路甚至不收敛的局部参数组合。提示不要一上来就追求“最优参数”。先把地图、障碍物影响半径、合力分布可视化出来在几何上一眼看清楚再去微调力系数效率会高很多。4. 四大头疼问题的排查与解法实录4.1 局部最小值问题船卡在U形障碍区里所有用势场法的人都会遇到这个问题船进入一个凹形障碍结构引力把船往目标方向推但障碍物三面合围形成的斥力把船往外顶合力趋近于零船就在谷底来回徘徊像是被困在一个无形的笼子里。排查的方法是监测合力大小。我的做法是设定一个最小速度阈值当合力模长连续3秒低于阈值且船位几乎不动就判定进入局部最小值。处理手段通常有两种一种是临时生成“虚拟子目标”把子目标放在障碍物缺口外侧先让船朝子目标移动绕出凹槽后再切换回原目标另一种是给合力叠加一个垂直于局部极小值方向的小幅扰动。前者更可控也更符合船舶的操纵习惯。实际工程里我几乎不用随机扰动。因为船有惯性扰动消失后仍然有可能重新陷入同一个势阱如果扰动不合适还会让船大幅偏航。虚拟子目标法虽然要额外维护一个状态机但至少行为是确定的也好在仿真里验收。我当时在船进入U形区域后自动生成了一个绕行子目标船先稳定转向到左侧绕过障碍物尖端后再重新切换回原目标整段路径没有明显停顿。4.2 目标点不可达总是在目的地附近转圈还有一个特别烦人的问题目标点本身离障碍物很近时引力在目标点处被斥力抵消船距离目标点还有十几米就开始原地绕圈永远无法真正“靠泊”。这个现象在学术上称为目标不可达问题GNRON。我当时排查这个现象首先做的不是改代码而是把合力的分解图画出来。果然发现目标点处的斥力方向和引力方向几乎相反净合力为零。解决方法是给斥力场加上距离目标的因子当船接近目标点时把斥力衰减掉。具体实现是把斥力场乘以一个目标距离相关的权重越靠近目标障碍物对船的影响越小引力自然占上风。还有一个更实用的工程做法在目标点周围划定一个“靠泊圆”半径比如1.5倍船长。当船进入靠泊圆时直接切断势场法输出切换到专门的靠泊控制逻辑。实际船舶靠泊本来就要考虑低速和精准定位不是一条单纯势场路径能解决的事所以这个切换逻辑在很多实际系统里都存在而不仅仅是学术上的权宜之计。我在这个项目里就是采用的这种方式最终才彻底告别了目标点附近的震荡圈。4.3 动态会遇只靠距离算斥力远远不够前面说的都是静态障碍物真实水域里更大的威胁是其他正在移动的船。如果用静态斥力公式去处理船只会根据距离远近回避完全没有“相对速度”的概念。结果就是对面来船时我的船会偏向来船行进方向的反向走这在直觉上不对也不符合海上避让习惯。解决这个问题的思路是在斥力项中加入相对速度因子。具体做法是计算我船与目标船的相对速度在相对位置方向上的投影如果相对速度让两船距离变近斥力就放大如果两船已经在相互远离斥力就可以适当减小甚至忽略。这东西说起来简单实现时要注意坐标系统一把相对速度从随船坐标系转换到对地坐标系。另外还要提到纯势场法对动态目标非常容易产生“锯齿形”避让轨迹。为了抑制这种锯齿我在势场输出后再加了一阶滤波滤波时间常数大概0.5秒。这样船在连续两个周期之间收到的指令不会突然反向路径整体平滑很多。缺点是被动变向有些滞后但对我这个场景来说安全性并没有降低。4.4 数值抖动与栅格分辨率的选择最后一个常见问题是数值抖动。栅格地图分辨率太粗时势场梯度在栅格边缘会产生锯齿太细时算力开销增大而且传感器噪声会被直接放大。我在船舶场景里试过不同分辨率最后用100米格比较合适对于90米以内的障碍物边沿再做单独加密。栅格分辨率也直接影响障碍物膨胀半径的设定。船舶路径规划中静态障碍物一般要先膨胀半径膨胀量至少是半个船宽加安全余量。对于机动性较差的大船安全余量不能只看船宽要看制动距离和回转半径。所以我常用的设定是膨胀量从20米起步根据仿真碰撞距离反馈来调整而不是一次性把网格画得很密。如果栅格太细但膨胀量不足路径可能会从狭窄缝隙里穿过去而真实船根本转不过去。5. 把势场法放进更大的路径规划体系里5.1 全局航迹局部避障的组合几乎不会有系统只用势场法来完成全航程规划因为势场法没有全局视野不知道前方是海峡还是开阔水域。我采用的架构是两层规划全局层用A*在电子海图离线栅格上计算一条安全参考航线给出若干航路点局部层在船实时位置周围一定范围内计算势场对参考航线做动态修正。这样做的关键收益是即使局部遇到突发障碍船的偏离也是有界的因为势场法生成的只是局部修正量不是完全脱离航线的随机方向。一旦障碍物越过船又能平滑地回到参考航线上而不会出现“绕过一座岛后完全忘了原来要去哪”的情况。很多从论文转向工程的人常常忽略这一层只盯着势场参数优化结果系统鲁棒性很差。从工程实施角度我给参考航线增加了一个“走廊”约束只要船当前位置距离参考航线的垂线偏差超过某个阈值势场法就增加一个指向航线的恢复力如果偏差在阈值内恢复力权重很小保持正常避障优先。这种带软约束的架构比单纯朝目标点拉引力稳健得多。5.2 水流与海图的耦合船舶场景中还有一个不可忽略的因素水流。在不规则岸线和开阔水域潮汐和洋流会对船产生横向漂移。如果势场法不懂流场船实际走的是对地轨迹但路径规划用的是对水模型两者不一致就会在长距离航行后产生巨大偏差。我采用的做法是在势场输出上叠加一个流场补偿项期望对地速度等于期望对水速度加流速矢量。换句话说看到目标方向虽然是船艏指向但要真正朝那个方向漂移船艏还得向水流上游偏一个角度。把这个补偿量放到合力计算里本质上是把流场当成一个恒定的“背景势”而不是障碍物。这部分在水流较弱的内河航道可能影响不大但在沿海或潮流明显的水域非常关键。我也建议所有做船舶路径规划的人在设计验证环境时就把流场参数加进去否则标定出来的参数拿到真船上很可能会“水土不服”。5.3 势场法还能用在哪些地方既然做了这么多探索势场法的应用范围还可以再延伸。例如把港口泊位、锚地、固定海上平台的边界都抽象成障碍物并用不同影响半径来区分“硬障碍”和“软约束”可以实现更贴合真实航行习惯的路径。还可以把动态船的预测轨迹直接作为时变障碍物输入在时间维度上提前绕行这比单纯在当前位置计算斥力所能获得的路径质量更高。在船舶集群或编队场景中同样可以把“保持编队队形”的期望位置视为目标点引力“避让其他编队成员”视为场内斥力从而把路径规划和编队控制放在同一个势场框架里。这些扩展我并没有全部落地但方向上是可行的。只要记住一点势场法的核心在于“用场来表达约束”而怎么定义好这个场比单纯调整系数更重要。我在实际项目里还有一个体会每次拿到一个新的路径规划场景不要急着调 k_att 和 k_rep。先把地图画出来把障碍物边缘和影响半径可视化再看合力分布图很多问题一眼就能看出来。等我对势场的行为有了直觉再动手改参数效率反而最高。另外所有仿真验证都要至少跑三种典型工况静态障碍物穿插、U形障碍、动态迎面目标。如果这三个场景都能稳定通过再考虑接真实船舶运动模型。我在这个项目里最大的收获并不是用势场法生成了多完美的路径而是学会了一件事算法越简单越要仔细对待它背后的物理假设。船不是粒子势场法也不是万能药把这两点想清楚它就能在船舶路径规划里发光。
平方复杂度无法被常规优化消除｜TaoToken 统一 Key 通道实测)
