水面无人艇这东西,看着挺科幻,真上手做起来,第一个要解决的头疼事往往不是什么高深控制算法,而是“船到底该往哪儿开”。陆地上的自动驾驶,有车道线、有红绿灯、有高精地图兜底;水面不一样,一望无际的水面上没有路标,连“路”这个概念都不存在。这时候,电子海图就成了无人艇唯一可信的“道路模型”。
我这个项目,名字就叫“基于电子海图的水面无人艇全局路径规划”,核心就四个字:读图、找路。读图,是把电子海图里那些水深点、暗礁、岸线、禁航区的矢量信息,翻译成计算机能直接计算的栅格地图;找路,是在这张地图上,用全局路径规划算法给无人艇算出一条从起点到目标点、既安全又经济的航线。这篇文章主要围绕整个技术链路展开,包括海图数据怎么解析、坐标系怎么对齐、栅格图怎么构建、A*算法怎么设计,以及我在实跑过程中踩过的一些坑。适合正在做无人艇、无人船路径规划,或者想了解电子海图二次开发的朋友参考,对刚入手水上机器人项目的人尤其友好——这些东西在论文里多半是几句话带过的,但真正动手时每一步都是细节。
1. 项目整体思路与设计拆解
1.1 为什么全局路径规划一定离不开电子海图
先聊一个基础问题:水面无人艇为什么不能像扫地机器人那样,随便撞一撞再绕开?因为水面环境对安全性的要求高得多,而且雷达、摄像头能感知的范围极其有限。船载传感器(激光雷达、毫米波雷达、视觉)本质上是“局部感知”,只能在几十到几百米范围内发现障碍物;但全局路径规划要解决的是几公里甚至几十公里的航路问题,这已经远远超出了传感器能覆盖的范围。
所以在整个决策链路里,全局路径规划必须依赖先验环境信息,也就是电子海图(ENC,Electronic Navigational Chart)。电子海图的好处不在于它是一张“好看的图”,而在于它是结构化、矢量化的数据。海洋里的陆地、暗礁、沉船、浅滩、电缆、禁航区,都被编码成了标准的要素(feature),每个要素还带了一堆语义属性,比如水深值、底质类型、灯光特征。这意味着我能直接告诉算法:“这里水深只有1米,船吃水0.6米,不能走。”如果换成卫星影像,我还得先做语义分割,又慢又不可靠。
我最初也考虑过用公开的岸线轮廓数据做简化版规划,后来放弃了。原因很简单:岸线数据只告诉你哪里是陆,哪里是水,但没告诉你“这片水域能不能走”。水面不像路面,看着是水的地方,底下可能有暗礁、有浅滩、有沉船。真正的全局规划,必须把水深和碍航物一起考虑进去。这也就是为什么这个项目的输入必须是电子海图,而不是普通地图。
1.2 技术路线选型:从海图到航线的总链路
整个项目技术链路其实可以想象成一条流水线:电子海图读取 → 要素解析与坐标转换 → 栅格化代价地图 → 全局路径搜索 → 路径平滑与输出。没有一步是可以省掉的。
电子海图的读取,国内项目用得比较多的是S-57(IHO标准)和S-101(新一代标准),文件后缀一般是.000,内部用ISO/IEC 8211封装,需要专门的库去解析,比如GDAL/OGR、OpenS57,或者一些商业组件。得到要素后,并不能直接拿经纬度在平面上做路径规划,因为地图投影和GPS用的坐标基准可能不是一回事。至少要把S-57里的经纬度统一到WGS84,然后在局部作业区域转换到米制平面坐标,比如高斯-克吕格投影或UTM投影,后面拿这些坐标去算距离、算栅格,才有意义。
坐标搞定了,下一个关键决策是怎么把矢量海图变成算法能算的地图。矢量要素本身是点、线、面,不适合直接喂给搜索算法,尤其对新手来说很别扭;更通用、更工程化的做法是栅格化。把作业区域切分成固定大小的网格,每个格子标记为“可通行”“障碍”或者“带代价的危险区”。这东西一铺开,A*、Dijkstra这些经典寻路算法就能直接在网格上跑了。整个项目中,栅格化这一步做得好不好,基本决定了路径规划结果的质量,比后面算法本身还重要。
2. 电子海图数据的工程化读取与预处理
2.1 看懂S-57海图的内容结构
想用好电子海图,必须先搞清楚它里面到底装的是什么。S-57数据模型分三层:要素(Feature)、几何图元(Geometry)、属性(Attribute)。通俗地说,要素是“地物是什么”,几何是“地物在哪里”,属性是“地物有多危险”。举个例子,一个名叫OBSTRN的要素代表孤立碍航物,它的几何可能是一个点,属性里可能包含WTWCD(对航行有影响的明/暗礁标识)、QUASOU(探测性质)等。
我实际解析时最常用到以下几类要素,这里做成了速查表,方便大家对照:
| 要素类别 | S-57首字母缩写 | 含义 | 路径规划中的处理方式 |
|---|---|---|---|
| 水深区 | DEPARE | 一片区域的水深范围 | 最重要,生成可通行代价区域 |
| 等深线 | DEPCNT | 连接相同水深的曲线 | 提取安全等深线,比如2米线、3米线 |
| 陆地 | LNDARE | 陆地边界 | 直接标为不可通行障碍 |
| 暗礁 | UWTROC | 水下岩石 | 点/面障碍物,标为不可通行 |
| 孤立的碍航物 | OBSTRN | 沉船、桩柱、养殖区等 | 点/面障碍物,需要膨胀处理 |
| 岸线 | COALNE | 水陆分界 | 辅助确认边界 |
| 禁航区 | 各种限带要素 | 军事区、锚地边界等 | 根据工程需求决定是否禁用 |
有一个细节新手很容易踩坑:S-57里的水深值是有符号的,正值表示水深,负值表示干出高度(低潮时露出水面的高度)。如果解析代码里没有处理这个符号,直接把负值当成水深参与计算,很可能会把一片干出滩涂当作可以走的水域,后果不堪设想。我当时就是在解析DEPARE时忘了这一步,导致规划路径横穿了一大片浅滩。
2.2 坐标基准与投影转换
电子海图里存的坐标默认是经纬度(WGS84椭球),但路径规划里的距离计算、膨胀半径、栅格偏移量这些,都需要在米制坐标系里操作。你可能想说,直接用经纬度算球面距离不行吗?行,但很麻烦,尤其是要做网格化和邻域扩展时,经纬度在每个纬度上对应的距离不一样,代码写起来又乱又容易出错。
我的做法是在规划前,把作业区域统一转换到UTM投影坐标系。UTM把全球划成60个带,每个带是6度经度范围,在北纬84°到南纬80°之间都有较高的精度。选UTM而不是某个地方坐标系,是为了通用性,换一片水域也能直接用。转换可以依赖Proj库或者GDAL的osr模块,代码几行就搞定。
需要注意,UTM的中央经线上没有长度变形,但远离中央经线时会产生误差,所以作业范围很大(比如横跨两个UTM带)时,建议按带分割或者用兰勃特等角圆锥投影。水面无人艇的作业半径一般不会超过几十公里,UTM完全够用。
2.3 从海图要素到代价栅格图
栅格化是整个项目里我最想详细讲的一块。直接说结论:除非迫不得已,不要用三维数组表达栅格状态,一个二维numpy矩阵就够了。矩阵的每个格子存放一个整数或浮点数,代表当前格子的状态或通行代价值。
我自定义了一个简单的状态体系:
- 0:自由通行水域
- 1:陆地/干出/暗礁等绝对障碍
- 大于1的浮点代价值:受水深影响的危险区域,越界但可能可通过,或者可通行但代价高
构建流程是这样的:先把作业区域按设定分辨率划分成网格,网格边长我通常取海图显示比例尺对应精度的2~3倍。比如一张1:50000的海图,图上0.1毫米对应地面5米,那我就用15米或20米的网格。太细会让A*搜索空间爆炸,太粗又会丢掉细窄水道和暗礁等关键信息。
然后遍历解析出的矢量要素:
- 对于陆地
LNDARE、禁航区、孤立碍航物:用多边形或半近像素填充算法,把覆盖到的格子标为1。 - 对于点状暗礁
UWTROC、OBSTRN:除了点所在的格子,还要按安全半径向外膨胀,把周边一圈格子也标为1,避免规划出的路径离危险物太近。 - 对于水深区
DEPARE:读取最大水深DRVAL2(有些实现用最小水深DRVAL1作为保守值),把水深数据填到对应区域,之后再统一换算成代价值。
这一步做完,你会得到一整张“外圈陆地包围水域、水域内部散布障碍物”的代价图。这张图一旦生成,后续A*的搜索效率会非常高。我实测过一张大约20公里长、5公里宽的河道海图,15米分辨率大概也就一百多万个格点,在普通PC上构建耗时不到两秒。
3. 全局路径规划算法实现与核心参数设计
3.1 为什么我选择A*而不是Dijkstra或RRT
算法选型这块,我一开始就排除了RRT和RRT*。原因很简单:RRT系算法适合高维连续空间或运动约束强的场景,但在这个项目里,环境已经是栅格化的,且维度固定为2D,A*能在离散网格上直接给出确定性最优解,RRT的随机性反而会造成不必要的抖动。至于Dijkstra,它在栅格地图上确实能找到最短路径,但它是均匀向外扩展,没有目标点方向引导,搜索范围比A*大很多,在百万级格点的地图上,性能差异肉眼可见。
所以最终选择的就是经典的A*,配欧几里得距离启发函数。这里有个细节非常重要:水面无人艇可以在任意方向航行,不像四轮小车只能“上下左右”移动。如果用曼哈顿距离作为启发函数,会高估路径代价,导致A*扩展很多无效节点;用欧几里得距离,最快最稳,出来的路径也更符合船的真实运动。
3.2 代价函数设计:不是“能走”就行,是“好走”才走
如果只把网格分成“能走”和“不能走”两类,A*很容易给出贴着暗礁边缘、擦着浅滩边界走的危险航线——它确实最短,但人不敢这么开船。为了解决这个问题,我在代价值里加了三项:
基础距离代价:移动一步增加的距离,对角线移动按√2计算。
水深代价:当格子水深大于安全水深时,代价为0;当水深大于“最低可通行水深”但小于安全水深时,代价按线性增长;当水深小于最低可通行水深时,直接设置为不可通行。这么做等效于让算法“宁绕远、不走浅”。
障碍物距离代价:在已知障碍物周围做距离场,离障碍物越近,代价值越高,相当于给所有障碍物套了一个“风险衰减带”。这个策略很有用,可以明显降低路径与暗礁、岸边贴太近的概率。
代价值的组合公式大致是:
total_cost = dist_cost + depth_penalty + obstacle_penalty其中depth_penalty和obstacle_penalty需要根据实际船型和作业环境调权重。我用的一条经验曲线是:安全水深2米以上的水域完全不惩罚,1.5~2米之间惩罚系数线性加到0.5倍距离代价,1.5米以下直接禁行。这套参数在一个内河复杂水域场景里跑出来的效果很理想,虽然路径比纯最短路径长了约20%,但全程有富余水深,心里踏实很多。
3.3 吃水限制与安全水深判定逻辑
安全水深的计算,可能是这个项目里最“海事”的知识点了。很多人第一次做的时候,以为吃水0.6米,选1米水深的区域就够了,真出事的往往就是这种想法。因为船在航行时会有下沉效应,尤其是速度稍快一点,船体尾部会下沉,实际吃水可能比静态吃水多出0.3~0.5米;再加上波浪造成的摇摆下沉,以及观测误差和海图数据的年代问题,必须留足富余水深。
我用的规则是:
安全水深 = 静态吃水 + 下沉量 + 富余水深 + 误差补偿按我测试的船型(艇长5米,静态吃水0.6米)计算,下沉量取0.2米,富余水深取0.5米,误差补偿取0.2米,最终安全水深约1.5米。也就是说,电子海图里所有标称水深小于1.5米的区域,在栅格地图里都会被打上较高的代价或者直接设为障碍。这个过程最好在解析海图时就算好,而不是等A*运行的时候才临时判断,能省很多内存和CPU时间。
3.4 核心代码实现:A*在栅格地图上的落地
这里给出一个可以直接参考的A*核心结构,基于Python实现。代码做了简化,但主干逻辑完整,适合在此基础上二次开发:
import heapq import math def heuristic(a, b): # 欧几里得距离作为启发函数,允许任意方向航行 return math.hypot(a[0] - b[0], a[1] - b[1]) def a_star_search(grid, start, goal): # grid: 0=自由, 1=障碍, >1=危险代价,此处简化为0/1判断 rows, cols = grid.shape open_set = [] heapq.heappush(open_set, (0.0, start)) came_from = {} g_score = {start: 0.0} f_score = {start: heuristic(start, goal)} directions = [(1,0), (-1,0), (0,1), (0,-1), (1,1), (1,-1), (-1,1), (-1,-1)] while open_set: current = heapq.heappop(open_set)[1] if current == goal: path = [] while current in came_from: path.append(current) current = came_from[current] path.append(start) path.reverse() return path for dx, dy in directions: neighbor = (current[0] + dx, current[1] + dy) if not (0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols): continue if grid[neighbor] == 1: continue step_cost = math.hypot(dx, dy) tentative_g = g_score[current] + step_cost + grid[neighbor] if tentative_g < g_score.get(neighbor, float('inf')): came_from[neighbor] = current g_score[neighbor] = tentative_g f_score[neighbor] = tentative_g + heuristic(neighbor, goal) heapq.heappush(open_set, (f_score[neighbor], neighbor)) return None这是一个很标准的A*实现。需要注意的点有两个:
第一,启发函数要满足可采纳性(admissible),也就是它给出的估值不能大于真实代价。在上面的场景里,我们允许8方向移动,且对角线代价约等于1.414,所以欧几里得距离是安全的。如果把方向扩展成16邻域或者加入转向代价,启发函数也要跟着调整,否则结果可能不是最优。
第二,grid[neighbor]作为额外代价这一步,只有在障碍和危险区代价远小于1时才能直接加。如果代价数值很大,会导致A*过度避让,路径绕得太远,甚至把一条完全安全、只是稍微擦着边缘的航线彻底放弃。更稳妥的做法是:先用0/1标记做一遍纯避障搜索,再做一遍带水深代价的优化搜索,两遍结果取优。
3.5 实跑效果参考
我在某个内河近岸测试水域跑了一组数据。起点在河道东侧码头,目标点是上游约4公里处的一个开阔水域,中间分布着三处浅滩和一条水下电缆。人工经验航线约4.8公里,规划出的航线约5.6公里,看似绕了将近一公里,但全程最小水深都在2米以上,且和浅滩边界保持了至少30米的安全间距。对比纯最短路径算法的结果,A*的多绕行基本都花在绕浅滩和水下障碍上,这个牺牲是值得的。
4. 常见问题排查与工程避坑实录
4.1 高频问题速查表
| 问题现象 | 可能原因 | 解决思路 |
|---|---|---|
| 路径穿过了明显是浅滩的区域 | 海图水深符号解析错误,负数被当作水深 | 检查DRVAL1/DRVAL2符号,干出区域单独设置 |
| 路径贴着岸边或暗礁走 | 没有做障碍物膨胀或距离代价 | 增加膨胀半径,或加obstacle_penalty风险带 |
| 栅格图生成极慢 | 分辨率过高,或者要素裁剪不彻底 | 先按外接矩形裁剪,从20米分辨率起步试跑 |
| 在开阔水域路径绕远过多 | 水深代价权重过高 | 降低depth_penalty权重,或提高安全水深阈值 |
| 路径出现锯齿状来回折 | 16邻域/8邻域搜索后没有做平滑 | 后端加Douglas-Peucker抽稀和贝塞尔平滑 |
| 海图坐标系与GPS信标对不上 | 基准面或投影带不一致 | 统一到WGS84,改用UTM投影 |
4.2 最容易翻车的水深数据陷阱
前面提过水深符号的问题,这里专门展开说。电子海图的DEPARE要素里通常有DRVAL1和DRVAL2两个属性,分别代表区域内的最浅水深和最深水深。在S-57标准里,如果数值是负的,表示该区域在理论最低潮面以上,属于干出滩。我在调试时曾经打印出某一格的水深值,发现是-0.3米,下意识以为这是“水下0.3米”的意思,实际它意味着那一片低潮时是裸露的泥滩。这种错误极其隐蔽,因为数值看着很合理,唯一能够暴露问题的方法就是把栅格地图可视化,用色带标出水深区间,肉眼看一遍。所以我强烈建议,任何海图数据在进入规划算法前,必须做人工目检。别嫌这一步“不自动化”,它救过我至少三次。
另外,真实作业时千万不要忽略潮汐。电子海图上的水深是相对理论最低潮面的,也就是说,实际水深 = 海图水深 + 当时潮高。我测试的那个水域,潮差最大能到2米以上。做全局路径规划时,如果不代入潮汐预报数据,按照海图水深算出的“安全航线”在高潮还行,低潮时可能直接搁浅。项目里我把潮高作为一个全局偏移量,在栅格化的同时统一叠加到水深代价值上,一行代码就能解决,但效果天差地别。
4.3 图幅边界与孤立碍航物的特殊处理
电子海图是按图幅发布的,不同图幅之间会存在接边缝隙。如果作业区域正好跨越两幅图,很容易出现一侧是干净水域、另一侧是“无数据区域”的情况。我的做法是把无数据区域默认标记为未知而不是可通行,同时在边界处做5个格子的渐变缓冲。宁可让A*多绕一点,也不要让它在一无所知的地方贸然穿行。这个思路也沿用到海图上那些只画了边界、内部没有任何水深信息的湖泊或水库区域。
孤立碍航物则是另一个容易漏掉的点。A*搜索的本质是网格上的邻域扩展,如果某个暗礁只占据了一个格子,路径从它旁边斜着穿过时,从几何上看并没有碰障碍,但实际上船身不可能那么细。所以对点状碍航物必须额外做膨胀处理。膨胀半径我一般取前半宽加安全距离,小型无人艇是5~10米,大一点的船看情况调到20米。半径设大了,窄水道会被封死;设小了,安全性又不够。这个参数多花点时间调,值得。
4.4 路径后处理:让航线真正能跟着开
A*直接搜出来的路径是网格折线,存在大量45度锯齿,尤其是8邻域搜索时,看起来就像一串阶梯。这种路径直接发给底层控制器,无人艇会频繁转向,既费电又增加侧倾风险。我一般做两步后处理:
第一步,用Douglas-Peucker算法抽稀路径点,把几乎共线的点合并,减少转折数量。阈值选一个栅格外加一点,比如25米,效果比较稳。
第二步,用三次贝塞尔曲线或者三次样条对转折点做平滑,让航向连续变化。需要注意,平滑后的曲线可能会“切弯”,也就是从障碍物角落里穿过去,所以平滑完必须逐点回查栅格地图,把任何不安全的点拉回到膨胀边界之外。
最终输出的航路点(waypoints)我会保存成带速度建议的JSON或者GPX格式,直接喂给局部避障模块和制导控制模块。到这里,全局路径规划的任务才算真正闭环。
我个人在实际测试里,最大的感悟是——这项目难的不是算法,而是数据。A*的代码放到网上随便一搜就有,三天能写完;但把电子海图读明白、把坐标系对齐、把水深语义转成代价、把各种坑都避开,可能要花三周。很多人一上来就钻进算法调参,结果数据没处理好,路径再漂亮也是纸面功夫。如果你正准备做类似的水面无人艇项目,我的建议很直接:先花大量时间把海图数据处理管线做得扎实、可视化做得直观,等你在屏幕上看到一张干净、可解释的代价栅格图,后面的路径规划反而是水到渠成的事。