★ FEATURED ARTICLE 先讲一段亲身经历。去年夏天我们在某内湖做水面无人艇USV测试全局路径规划模块直接套用了之前AGV项目上的A*实现电子海图解析完画成栅格就喂给算法。结果船在离岸三百米处突然抱死程序判定前方全是障碍物原地打转。排查了半天才发现那片水域的电子海图水深属性是“未知”被我们默认成了障碍区。那次之后我整个人都老实了。基于电子海图做水面无人艇的全局路径规划听起来是个标准的算法题真正动手才发现80%的精力根本不在搜索算法上而在海图数据的理解、可通行区域建模、以及路径与船舶操纵性的匹配上。这篇东西不聊Paper里的花活把我从数据解析到实船测试全链路踩过的坑、验证过有效的做法一次性说清楚。1. 先搞明白水面无人艇的全局路径规划到底在解决什么问题1.1 不是所有路径规划问题都一样很多人上来就写A*但A*只是一个求解器不是问题本身。全局路径规划在USV场景下的完整描述是在已知静态环境信息电子海图的前提下在船舶从起点到终点的整个航行空间内计算一条满足安全性约束、满足船舶运动学约束、并且代价最优的连续路径。这句话里每个词都有分量尤其“船舶运动学约束”这一点是USV与地面机器人的本质差异。地面AGV可以原地转向、可以近似看作在二维平面上自由移动但水面无人艇受舵和推进器限制转弯需要半径航向变化是渐变的速度变化也有很大的滞后。你规划出路径甚至比地图上障碍还精美只要哪个转角超出船舶最小转弯半径实际根本走不出来。1.2 问题建模方式我习惯把问题抽象成标准形式设位姿空间为包含位置和航向角的状态集合自由空间是排除了所有禁航区、浅水区和碍航物的可航行水域。给定起始状态和目标区域寻找一条连续路径使路径完全落在自由空间内且满足曲率约束——即任意位置的曲率半径不小于该船当前航速下的最小转向半径。代价函数则综合三块路径总长度、途经水域的风险度浅滩、航道外、渔网区等赋予不同权重、以及转向代价减少急转弯。实际工程中我大量使用了加权方式实现而不是简单地在禁航区给个无穷大代价。后续会展开讲为什么。1.3 评价路径的三条标准安全路径不得穿越禁航区与碍航物保持足够的安全距离水深全程满足富余水深要求。这是底线没得商量。平滑路径曲率变化平缓没有尖角满足无人艇的转弯能力约束。不平滑的路径要么船走不了要么跟踪控制器疯狂振荡。可执行路径最终要变成一系列航路点下发到自动驾驶模块航路点数量和密度要匹配控制周期与导航精度太密了控制器来不及处理太疏了中间可能撞上小障碍。2. 电子海图“图”只是表象本质是一套空间数据库2.1 S-57数据的组织方式我们平时说的电子海图在国际标准语境下绝大多数是S-57格式的ENCElectronic Navigational Chart。S-57的核心不是给我们画图用的而是一种面向对象的数据传输标准。所有地理要素都以物标Object的形式组织每个物标有特征属性attribution和空间记录spatial record。规划算法真正关心的要素集中在几个物标类。举一些我常用的物标类中文含义规划用途DEPARE水深区域判定可航行水深范围DEPCNT等深线提取安全等深线SOUNDG水深点水深离散采样数据OBSTRN碍航物禁航区生成WRECKS沉船禁航区生成通常更严格COALNE岸线陆地边界天然禁航区NAVARE航行通告区临时禁航或警告区FAIRWY航道优先通行区域你从海图服务器或S-57文件里拿到的往往是一个个要素对象不是直接在图层上的图形。需要按物标类编码去过滤再提取其几何信息和水深属性。我用的是GDAL/OGR库的S-57驱动驱动会自动把S-57转成OGR图层但图层命名是S57类的缩写要先去查驱动文档里对应关系。提示S-57的要素在不同生产机构可能使用不同的空间记录类型节点、边、面尤其是水深区域DEPARE有面状也有点状补充。处理时最好统一做一次几何类型规范化不然栅格化阶段会出各种稀奇古怪的bug。2.2 从矢量要素到规划地图我的标准处理流程分四步过滤出上述关键物标类按重要程度和更新日期去重。同一物标在不同图层出现多次的情况非常常见需要用要素标识符去重。将矢量要素投影到平面坐标系。S-57原始坐标是经纬度WGS84直接算距离和面积会有很大误差。投影方式我后面会专门讲。按设定的分辨率栅格化。把点、线、面统一转换成栅格标注层水深点插值成水深网格、岸线围成陆地掩膜、碍航物膨胀成禁航区。最后合成一张统一代价地图这是路径规划器的直接输入。每一步都可能出大问题。比如把水深点位数据插值成连续水深场插值算法选不好会在深水区出现假浅滩全局规划给出绕远路甚至认为无解。我后来改为“最浅水深优先”的保守插值策略宁可多绕路不能冒险穿过插值出来的浅点。2.3 投影与坐标系最容易踩的坑S-57的坐标是经纬度而A*、RRT这类算法在栅格上工作需要平面坐标。简单地把经纬度当成平面坐标用在低纬度小范围还能勉强凑合一旦到了中高纬度或者规划范围超过几十公里距离变形会很夸张。我选投影有两个条件一是等角性质保证航向角的正确性二是范围匹配避免跨带。推荐的做法是使用UTM投影但要注意规划区域跨越UTM分带时分成两段分别处理或者改用墨卡托投影处理较长航线的全局规划。此外海图水深数据全部基于海图基准面Chart Datum通常是理论最低潮面而实际航行中的水深是海图水深加上实时潮高再扣除船体下沉量这一步建模时必须区分开。3. 可通行性建模告诉A*哪些水域能走、哪些不能走3.1 水深安全判定吃水、富余水深和潮位很多人做可通行性分析直接拿“海图水深 船吃水”作为判定条件这是非常危险的。真实工程要求的是富余水深UKCUnder Keel Clearance满足要求。我采用的标准计算公式是可用水深 海图水深 实时潮高预报 - 船体下沉量squat - 波浪影响的附加吃水判定条件可用水深 ≥ 静态吃水 安全富余水深具体富余水深取值对内河或湖泊测试通常留0.5米以上沿海航行有人会用吃水的10%~15%但不同船型、海况要求差异很大我只说底线思路按最保守的方式留余量而且要把潮汐预报接入做实时修正而不是规划一次管全程。潮位变化达到1~2米时上午能走的路径下午可能就搁浅了。3.2 碍航物与禁航区的提取碍航物物标处理远比想象中麻烦。沉船WRECKS和碍航物OBSTRN在海图里除了位置外往往带一个“深度不确定”或简单的水深值。如果属性里写的是“深度未知”按最坏情况处理——直接标记为禁航区哪怕测绘部门实际可能知道它没露出水面。渔网、养殖区、临时布标这些常见的实际碍航因素标准电子海图里往往不及时更新或压根没有。做内河和近岸项目时我会叠加上卫星影像和现场调研结果把明显的养殖区人工标成禁航区。这个过程很土但非常有效。3.3 从二值栅格到连续代价场新手容易犯的毛病把地图直接二值化0是能走1是不能走。我建议养成做连续代价场的习惯。连续代价场里禁航区是极大值浅水区按危险程度给一个递增的惩罚系数航道和深水区给一个低代价深水开阔区代价更低。这样做的理由有两个第一A*这类算法在二值图上找出来的路径会贴边走贴着禁航区边界蹭。真实水域船有定位误差、海流推挤贴着边界跑等于赌运气。连续代价场会让路径自动往高安全区靠拢。第二当路径不得不穿过缓解区比如宽航道边缘的浅水带时二值图会直接判定无解连续代价场至少能给出最优妥协路径。在全局规划中无解往往比绕路更麻烦因为上游决策系统可能需要直接取消任务。3.4 膨胀必须把船的“体积”算进去如果我们用船的中心点代表整条船就必须把船的外廓尺寸算到障碍物扩展里。膨胀半径的基础是船宽的一半但绝不只这么多。一个工程化的膨胀半径膨胀半径 船宽/2 安全间距 定位误差 海图水平误差定位误差取决于用的是RTK还是普通GPS。RTK能做到厘米级到分米级普通单点GPS有1~3米误差。海图水平误差则要看原图的测量精度部分老旧海图可能存在几十米的偏差。注意如果是高速无人艇转弯时船体有横倾实际扫过的水线面积比静态船宽要大横倾明显时应额外加余量。4. 全局路径规划算法选型A*、RRT、D*Lite谁适合海图场景4.1 为什么全局层选A*而不是RRTRRT快速探索随机树和RRT*在机械臂、无人机等领域很流行因为状态空间维度高、约束复杂。但在二维海图全局规划场景我认为它并不合适。算法选型的本质是对问题结构做匹配而不是追热点。二维平面上的大规模静态路径规划本质是一个低维度离散搜索问题图搜索算法有确定性、有最优性保证这对船舶安全至关重要。RRT类算法的随机采样特性意味着两次运行可能给出不同的路径这对一套需要反复验证和仿真回归的系统来说很难接受。而且RRT要达到稳定收敛并接近最优需要的采样点数量很大在几百平方公里海图上未必比A快。A*在全局规划的定位是给定一条初始参考路径而不是一条可以直接执行的最终轨迹。有了这个定位就有了下面的演化方向。4.2 启发式函数与搜索效率A*的效率取决于启发式函数h(n)的质量。二维海图场景里最常用的是欧氏距离因为它始终满足可采纳性admissible保证找到最优路径同时计算简单。如果觉得搜索太慢可以考虑加权A*把启发式代价乘以一个大于1的权重比如1.5或2。它不再保证最优但能显著加快搜索速度。我常用于快速生成候选航路的初筛阶段得到一条“够好”的路径。真正的保守场景我才用原始A*保证最优解。当然栅格搜索船的航向维度如果也考虑进去状态变成x, y, heading就要用混合A*Hybrid A*它是在连续状态空间中做搜索同时满足曲率约束生成的路径更接近船舶运动学。代价是搜索空间膨胀了一个数量级计算量大增。我的做法是在开阔水域直接用普通A生成粗路径用Dubins曲线做平滑时就满足了曲率约束混合A一般在复杂狭窄水域或需要精确靠泊时才用。4.3 改进方向JPS、分层搜索、双向搜索当规划范围达到几百平方公里、栅格分辨率又比较精细时A*的搜索空间会很大。我有几个工程上的提速经验JPS跳点搜索利用栅格地图的对称性跳过大量无意义节点在开阔水域效果好得惊人但需要地图是规则栅格且八连通实现时注意边界条件。分层规划先在一个低分辨率的粗栅格上快速搜索出一条走廊再在走廊范围内用细分辨率搜索精确路径。这个思路在远海大范围规划中非常实用。双向A*从起点和终点同时搜索在空旷水域通常能减少一半以上的扩展节点数。但注意要保证两个方向的搜索“相遇”判定正确处理不当会错过最优解。5. 路径后处理从折线航路到无人艇能跟随的轨迹5.1 转向能力约束为什么折线路径不能直接用A*输出的路径是网格上的折线段转折点处航向会发生突变。对无人艇来说突变的航向在实际跟踪中会出现明显横漂甚至导致跟踪误差过大在狭窄航道里直接压到禁航区边界。所以全局规划的路径后处理是一个独立且极其重要的模块。我的处理流程从栅格路径提取关键转折点用道格拉斯-普克Douglas-Peucker算法抽稀减少航路点数量再用Dubins曲线连接最后重校验。5.2 Dubins曲线生成可航轨迹Dubins曲线是无反向运动情况下满足最小转弯半径的最短路径由圆弧和直线段组合而成组合方式包括LSL、RSR、LSR、RSL、RLR、LRL。水面无人艇通常无法倒车或倒车代价极大所以Dubins曲线天然适合。应用时需要知道船的最小转弯半径这个参数从实船操纵性试验获取或者用经验公式估算一般取船长的2~3倍。高速时转弯半径比低速要大很多所以规划时按该航段的巡航速度计算最小转弯半径。注意对双体船、浅吃水快艇这类特殊船型经验公式偏差很大有条件的一定要做旋回试验取实际参数。用错误的转弯半径生成Dubins轨迹看起来很美实际跟着跟着就偏了。5.3 重校验与航路点抽稀平滑完成后的路径必须重新放回代价地图验证这一步很多人会跳过但我遇到过不止一次Dubins曲线在连接两个折线段时圆弧部分扫过了原先不经过的栅格如果那个位置靠近禁航区就可能穿进去。重校验要做两件事一是逐点检查平滑后的路径是否都在可通行区域二是计算路径各点曲率半径确认没有小于最小转弯半径的航段。校验不通过时我倾向回到A*层把对应区域临时加大代价重新搜索而不是在原路径上反复尝试修补。航路点抽稀方面Douglas-Peucker的阈值一般取栅格尺寸的1~2倍抽稀出来的航路点数量适合直接下发给航向控制器就行有些系统还要求航路点间隔保持均匀避免控制周期时间差导致的不稳定。6. 实测中的坑和调参经验6.1 海图数据质量和时效性问题电子海图听上去很权威但数据更新是分区域的。有的港口海图更新勤快近海航道水深五年没重测也正常。我的做法是建立“数据置信度”机制根据海图发行机构、测量日期、数据源类型给每个区域一个置信度评分置信度低的区域在代价场里额外增加风险代价。还有一次河试海图上标注的深水航道距离实际深水航道偏移了大约20米。原因可能是原图测量年代久远或者河床变迁。这种情况靠纯算法很难完全规避我最后配合实时单波束测深仪的数据做了局部修正——全局规划器在开航前接收到最新的测深数据如果与海图明显冲突以实测为准。水面无人艇加装测深仪本来就应该是个标配不算额外成本。6.2 膨胀半径怎么定才不过度保守膨胀半径调太大会让路径规划在狭窄水域直接宣布无解调太小又有碰撞风险。这个参数的标定没有捷径我是在仿真环境里做参数扫描再配合湖试验证。具体做法在目标水域的代表性场景下将膨胀半径从1倍船宽逐步加到4倍船宽观察无解率和路径绕航率。选一个无解率接近0、绕航率又可接受的最小值。注意这个参数要分场景配置开阔海域可以放大保守一些狭窄航道必须缩小否则任务根本完不成。6.3 水面动态干扰全局规划与局部避碰的分工有人问为什么全局路径规划不直接考虑移动船我的看法是全局规划获取静态环境信息解决“往哪走”动态船舶、漂浮物这类变化因素交给局部避碰模块基于AIS、雷达和视觉数据实时响应。两者分层分工是水面无人艇导航架构里比较成熟的控制逻辑。如果把动态障碍全部放进全局规划会发现海图上的船每秒钟都在变全局路径每秒钟都在重算最后谁都没服务好。全局规划应该在收到“前方动态障碍持续占道”这类明确信号时才触发走廊局部重规划而不是把所有动态信息都搅进来。6.4 性能优化大范围海域的搜索效率一次沿海规划范围大约几十公里乘几十公里栅格分辨率5米普通A*在单核处理器上可能跑到几十秒。对开航前规划来说还能接受但如果要做多任务动态调整这个时间就有点慢了。我有两个优化建议第一按任务阶段拆分成多段规划。比如从码头到开放水域、从开放水域到目标点分段每段都只搜索当前需要关注的区域。第二将代价地图分块预处理提前把禁航区膨胀好、把好走的区域标注好运行时直接加载内存省掉反复IO和矢量化开销。实时小船载计算平台性能有限代码层面优先保证缓存友好和内存连续访问实测比盲目上GPU划算得多。写在最后的经验基于电子海图的全局路径规划真正考验人的不是搜索算法本身而是对海图数据质量的理解、可通行性建模的严谨程度以及路径与实船操纵性的匹配。A*、Dubins这些算法都成熟到不能再成熟工程难点全在这些“外围”环节。我现在拿到一个无人艇路径规划任务第一件事永远是问海图数据是哪来的、更新到什么时候、船的最小转弯半径实测过没有。这三件事弄清楚了路径规划只是最后一步编码问题。这个项目后续如果继续扩展我建议把海图置信度评估和动态潮汐修正作为下一阶段的核心方向这两块的收益远大于继续调算法。
阅读完成 · 觉得有帮助？
