前阵子跟着组里跑实测试验AUV自主水下机器人在布放点附近对着一个悬停目标来回绕了三圈才敢穿过去。看回放数据时发现多波束前视声呐明明给了清晰的回波但传统占用栅格图把目标边缘的阴影区全部标成了“可通行”规划器自然就钻进去了。那次之后我彻底明白了声呐感知的不确定性如果不在规划里显式建模路径规划做得再花哨也是白搭。这篇要聊的正是当时我们最终采用的解决思路——基于**时空图神经网络Spatial-Temporal Graph Neural Network, STGNN与条件正规化流Conditional Normalizing Flow, CNF**做声呐概率重构再把这个概率分布接入自适应路径规划框架。整个过程涉及声呐数据的时空建模、概率分布重构、概率代价场构造、分层重规划几个环节。如果你正在做AUV的避障规划或者在做声呐感知的占用建图这篇文章的内容可以直接当作一套技术方案来参考哪怕只是拿去对比自己的路线也应该会有收获。1. 为什么AUV的声呐感知会让路径规划“底气不足”1.1 一张看似干净的声呐图隐藏了大量“错误的确定性”多波束前视声呐FLS的原理本身不复杂声呐发射一束扇形声波按角度分成若干波束每个波束接收回波的时间延迟和强度从而得到一组以极坐标排列的距离-强度数据。工程上我们习惯把它渲染成扇形图像再通过阈值分割、波束成形转成占用栅格地图有强回波就是“占用”没回波就是“空闲”。问题就出在这个“没有回波就是空闲”上。水下环境里目标背后的阴影区是声波物理上到达不了的地方这里没有回波但绝不代表可以通行。还有多径效应反射面、水面、水底、温跃层都可能让声波绕路第一个到达的回波往往不是最短路径在小尺度范围内就会形成“幽灵障碍物”——地图上这里明明有个点实际那里什么都没有。再叠加声线弯曲、波束旁瓣干扰真实声呐图像里充斥着大量“看起来确定实际是错的”信息。传统占用栅格把这些模拟量强制二值化等于把不确定性信息全部丢弃。规划器拿到一张确定性的地图就会以确凿的态度撞向一个不确定的结论。1.2 规划器想要的不只是“有没有障碍”而是“有多不确定”AUV在水下的处境和地面机器人有本质差异。地面机器人有激光雷达、视觉传感器模型相对稳定不确定性主要在里程计漂移。而AUV的感知前端是声呐声呐的可靠性与环境强相关浑水、温跃层、复杂底质、近水面都会让同样一个目标呈现完全不同的回波特征。规划器真正想要的其实是这样的能力知道前方某个区域“大概率是障碍”也知道另一个区域“可能是障碍但证据不足”。两种情况下做出的决策应该完全不同——前者绕行后者减速观察而不是一刀切地“遇到栅格值0.5就绕否则就冲”。所以技术路线的第一步是把感知输出从“单点估计”改成“概率分布”。这个概率分布不能是拍脑袋给一个方差而是要能表达声呐观测特有的长尾不确定性比如阴影区那种“没数据但心里没底”的状态。这就引出了后面要说的方法。2. STGNN: 把多波束声呐序列当成一张“会动的图”2.1 为什么普通CNN和MLP处理声呐帧不够很多人上来就想着把声呐图像当普通图像直接上CNN卷积神经网络。实际操作过就会发现这条路不好走。声呐成像不是光学成像它没有规整的像素网格波束角度分布不均匀距离方向的分辨率也随量程变化远距离处一个栅格覆盖的实际面积是近距离处的几十倍。再加上数据缺失非常常见某几个波束被旁瓣污染直接就成了噪点。CNN的标准卷积核在处理这种不规则、不完整的数据时等于用一个规则网格去适配不规则采样点效果自然打折。换个角度看声呐的每一条波束采样点本来就带有明确的物理身份它在哪个波束角度上、距离多远、和哪些采样点在空间上相邻、和上一帧的哪个采样点对应同一个目标的回波。这些关系天然是一张图。用图神经网络来处理等于顺着传感器的物理结构建模而不是强行像素化。2.2 图怎么建、特征怎么定我们最终选择了“以回波采样点作为节点”的方案而不是把声呐图像重采样成规则网格。原因是重采样过程本身就会引入插值误差丢失原始回波里的细节信息。建图分三层来看节点取当前时刻声呐有效量程内的回波采样点每个采样点即一个图节点。空间边按极坐标网格的物理邻接关系连接节点。可以用半径阈值法两个采样点之间的欧氏距离小于声呐有效量程的10%~15%就建边也可以用K近邻法取K4~8。我们实测下来半径阈值法更符合声呐物理特性K近邻容易把远处孤立的噪点强行连起来。时间边把最近N帧的节点全部加入图中同一波束索引、相邻帧的节点之间建立时间边。这样障碍物从远到近的运动特征就能通过时间边传进当前帧的节点特征里。节点特征我们取了6维当前距离、回波强度、波束角度、帧间时间戳差值、回波强度的局部方差、上一轮概率重构得到的占用估计值。第六个特征很关键它相当于把历史的概率信息反馈给当前图让网络在时间序列上保持记忆而不是每一帧从零开始。这里有一个容易忽略的坑声呐每一帧的节点数是不一样的图结构每帧都在变所以网络必须能处理动态图。这要求图卷积在实现时不能依赖固定的邻接矩阵形状而要用基于边表或者稀疏矩阵的方式。2.3 网络内部空间卷积加时间卷积的组合STGNN的主体是一个双分支结构。空间维度上用图卷积做消息传递——对每个节点聚合邻居特征后通过ReLU激活更新公式上可以写成h_v^(l) ReLU( W * aggregate({ h_u^(l-1) | u in N(v) }) b )其中N(v)是节点v的邻居集合。aggregate可以是mean、max或者attention加权。实测中mean聚合最稳attention在小数据集上容易过拟合。时间维度用一维因果卷积causal conv沿时间边传递信息只在时间轴上向前卷积保证当前帧不会用到未来信息。因果卷积的优点是延迟固定、训练稳定比GRU更容易在嵌入式平台上部署。把N帧的节点特征序列作为输入输出每个节点当前时刻的embedding向量用这个embedding去指导后面的概率重构。这部分训练的细节后面第五节再展开。简单说我们先用声呐模拟器生成的合成数据做自监督预训练——预测下一帧的节点特征让网络学会声呐回波的基本时空演化规律再用少量人工标注的真实数据做微调让网络适应真实的噪声和混响特性。3. CNF概率重构: 给每一条声呐回波补上“置信区间”3.1 输出一个数不是概率输出一个分布才是常规占用估计是给每个栅格一个sigmoid输出比如0.7就认为“可能有障碍”。但0.7这个数字根本无法区分两种情况一种是“我看到了障碍边缘证据充分所以概率高”另一种是“回波很弱但我猜这里可能有障碍”。更麻烦的是声呐阴影区的本质是“观测缺失”任何点估计都不可能正确表达“未知”。我们需要的其实是面对同一个观测输入模型输出一个完整的条件分布。比如对于某个栅格输出的不是p0.6而是“它处于占用状态的概率分布均值0.6、方差0.25”。规划器拿到这个分布就可以回答更有价值的问题这条路线的期望碰撞风险是多少最坏情况下的风险又是多少3.2 条件正规化流的管道设计与训练细节正常化流Normalizing Flow的核心思想是把一个简单分布比如标准高斯分布通过一系列可逆映射变成任意复杂的分布。条件正规化流CNF额外引入一个条件变量也就是把前面STGNN得到的节点embedding作为条件让映射过程随输入变化。先定义一个初始向量z服从标准高斯分布。然后通过一个由神经网络参数化的连续动态系统把z从t0时刻映射到t1时刻dz/dt f(z, t, condition_embedding)在t1时刻得到的值经过变换后就是我们要的占用概率分布采样。这个动态系统可以通过数值积分来求解训练时用最大似然目标——直接对观测数据计算负对数似然损失不需要对抗训练。这也是选择CNF而非生成对抗网络的重要原因训练稳定且似然值精确可算。具体到我们的场景管道流程是这样的STGNN输出每个节点的embedding作为CNF的条件输入CNF在每个节点位置输出两个参数一个是占用概率的均值另一个是表示不确定性的方差参数采样后得到一个带完整分布信息的占用概率场。关于这两个参数的解读需要压一个重点均值表示“最可能是障碍的程度”方差表示“模型对这个判断有多不确定”。方差大对应阴影区、多径污染区方差小对应干净、特征鲜明的回波区。ODE求解器的步数我们最终选在16~24步之间。训练时使用伴随方法adjoint method求梯度可以大幅节省显存不然每次反向传播都要存储所有中间状态嵌入平台上的显卡根本吃不消。3.3 和贝叶斯占用图的对比固定传感器模型是死穴很多人会问这个和传统的贝叶斯占用栅格更新Bayesian occupancy grid有什么区别区别恰恰在传感器模型的灵活性上。贝叶斯更新需要预先给出传感器的逆传感器模型——即“给定一次观测栅格是占用的概率是多少”。问题在于声呐的观测模型不是固定的同样的距离、同样的强度在浑水和清水中在沙质底和泥质底中在温跃层上下可靠程度完全不同。用一个固定的似然函数去套所有环境结果就是模型参数永远调不准。CNF的做法相当于从数据里把“观测-后验分布”的映射关系学出来而且条件输入里包含了回波强度、局部方差、历史估计等特征模型可以自己判断当前观测质量如何。观测质量差时输出的方差会自动变大观测质量好时方差自动收紧。这个自适应能力是固定传感器模型替代不了的。4. 自适应路径规划: 概率场里搜出来的路才是真稳4.1 从概率分布到可优化的代价场规划器没法直接吃一个分布函数所以工程上需要把概率分布转换成代价场。我们用的目标函数是J_path sum_{每个路径点} ( c_motion beta * E[collision] gamma * Var[occupancy] ) - eta * I_gain这里逐项解释c_motion是运动代价包括路径长度、转弯角、下潜深度变化用来保证路径平滑且符合AUV运动学约束。E[collision]是期望碰撞代价用栅格占用概率均值乘以碰撞惩罚系数。均值高期望代价就高路径自然绕开。Var[occupancy]是不确定性惩罚项用占用概率的方差来表达。这一项的加入意味着AUV宁愿走一条已知安全的远路也不愿意赌一条近但“心里没底”的路。I_gain是信息增益项表示沿这条路径能获得多少新的环境信息。这一项是“自适应”的关键后面专门讲。直接套公式容易翻车的地方是beta和gamma的平衡。beta设太大AUV会过度保守永远不敢靠近障碍区域路径绕得离谱gamma设太小又回到了忽略不确定性的老路。我们经验上先固定beta把gamma从一个比较小的值开始调观察仿真里的碰撞次数和平均路径长度变化找到一个事故率和效率的平衡点。4.2 分层规划全局参考加局部滚动重规划实机上我们用了一套分层结构因为全局规划和高频避障在AUV这种算力受限的平台上不可能同时执行得足够快。全局层在概率占用图上跑改进后的A*代价函数用上面的J_path但把I_gain暂时去掉先得出一条粗略的参考路径。这个路径更新频率低大约每隔几秒甚至更久才重算一次负责把控整体方向。局部层是一个滚动时域的规划器以参考路径为引导每0.5到1秒重规划一次接下来几秒的路径片段。局部层的代价函数完整包含J_path的全部四项。当CNF重构输出新增的高概率障碍时局部层能立即响应绕行当某个区域方差很大时它会主动降低穿过该区域的偏好。这套结构的自适应体现在哪里举个例子。AUV朝一个水下目标靠近初始距离远声呐回波弱CNF给出的占用概率均值和方差都很大。局部规划器判断“这里不可贸然穿越”于是减速并偏移。随着距离接近声呐回波变强CNF方差缩小均值清晰规划器才逐渐恢复到目标航向。整个过程没有传感器级的阈值切换也没有手动设置的“安全距离”全靠概率场的演化自然驱动。4.3 信息增益项让AUV主动“多看几眼”这是整套路给里我最喜欢的一部分。路径规划如果只看避障AUV永远只会被动应付当前看到的东西。加上信息增益项之后规划器有了“好奇心”它会在确保安全的前提下主动选择那些能够显著降低环境不确定性的路径。I_gain的计算用信息论的熵差来定义对候选路径覆盖区域的每个栅格用CNF输出的当前分布计算熵再估计观测后分布可能变成的熵做差求和。理论完整但直接实时算会贵得离谱。工程上我们做了简化只对候选路径附近一定范围内、且当前方差超过阈值的栅格计算信息增益方差低于阈值的栅格直接忽略。有了信息增益项之后AUV的行为会发生非常有意思的变化。在仿真里它不再直愣愣地穿过一片阴影区而是会先绕到侧面用声呐“扫一眼”阴影区后面的情况再决定下一步走向。这个行为在之前任何路线规划方法里都看不到很直观地体现了“自适应”。5. 从仿真到水池实验: 训练数据、算力与实施验证中的坑5.1 训练数据从哪来模拟器加域随机化声呐感知数据的一个现实困境是真实水下环境的标注成本极高你很难知道某个栅格的真实占用状态到底是什么。所以我们选择了“模拟器生成大量合成数据、真实数据微调”的组合路线。模拟器方面我们用了商业多波束前视声呐模拟器按仿真环境中的水面、水底、目标几何和声学参数渲染出回波。这还不够如果不做域随机化网络在仿真里学到的全是理想参数到了真实水里必然水土不服。我们在生成时加入了随机扰动随机改变水体混响系数、声速剖面的分层结构、目标的散射强度、噪声底数甚至随机模拟温跃层导致的声线弯曲。每批训练数据的“环境气候”都不一样网络被迫学到不变特征而不是背诵某一种声学条件。真值标注时有一个特别需要注意的细节阴影区的真值不能标成“空闲”而要单独分出一类“未知”。这也是我们把问题定为“概率重构”而非普通二分类的原因之一。训练时对“未知”区域的处理是让模型输出大方差分类损失反而起不到这个效果。5.2 图规模、嵌入式算力和延迟的取舍AUV上有严格的功耗限制实际部署时我们用的是某款嵌入式GPU功率预算有限推理延迟是硬指标。整套模型刚跑起来时单帧推理要四十多毫秒看起来还行但规划周期是0.5秒感知延迟加上控制延迟已经逼近边界不敢再加前端滤波。我们做了三个优化。第一把时间窗口N从8帧压到5帧节点数从四千多降到两千多直观降低图卷积的计算量。第二embedding部分量化到FP16CNF部分因为是逐点积分、对精度相对敏感保持FP32。第三把图卷积替换成采样更少的版本在精度基本不掉的条件下总算把单帧推理压到12毫秒左右。这里要特别提示图神经网络在GPU上的主要开销来自图聚合操作的数据搬运而不是算力本身。优化时要重点关注边表的存储格式和访存模式很多新手都忽略这一点盲目堆算力结果延迟纹丝不动。场景单帧推理延迟毫秒规划周期秒碰撞成功率初始版本42.30.5但余量紧张91%FP16量化后21.60.5余量充裕93%图结构优化后12.20.5余量充裕96%5.3 仿真和真实环境的落差水池实验给我们的教训仿真里跑得很顺一到水池实验问题就一个接一个往外冒。最大的落差来自三方面。第一是真实声呐的旁瓣污染比仿真严重得多。模拟器里旁瓣是干净的数学模型真实设备里旁瓣在近水面会激起整片回波CNF虽然能识别出方差变大但整体观测质量下降还是让规划器出现过短暂的犹豫。第二是AUV的定位漂移直接污染概率场对齐——声呐数据本身可能是准的但坐标累了以后概率场和真实环境对不上规划器会在一个平移过的地图上做决策。第三是时间同步问题。声呐帧和姿态传感器AHRS的时间戳如果对不准差个几十毫秒在转弯场景里就会产生明显的角度错位。水池实验里我们专门为这三类问题加了针对性处理旁瓣污染通过滑动窗口的中值滤波来抑制极值回波定位漂移在短时段内用声呐图与先验地图的匹配来修正时间同步则是在采集端统一加硬件时间戳软件层不再做二次对齐。处理完后做了一轮模拟障碍区的通过测试AUV成功避开了所有障碍物全程没有人工干预。要注意这是个案数据只能说明方法在自己测试条件下的表现不能当成通用基准。5.4 几组有参考价值的对比数据在仿真环境中我们对同一组随机生成任务分别用传统占用栅格加A*、纯CNN占用估计加A*、以及本文的STGNN加CNF加概率规划做了对比。每个场景跑50次统计三个指标方法任务成功率平均路径长度相对最优平均碰撞次数传统占用栅格 A*82%1.087CNN点估计 A*88%1.054STGNNCNF 概率代价规划96%1.040这个结果印证了核心判断方法提升最大的不是平均路径长度而是把碰撞次数压到了零。在水下作业里一次碰撞可能意味着几万块的设备维修费所以为了安全性多绕一点路、多耗一点电完全是划算的。6. 复盘与可复用经验6.1 最容易翻车的三个细节整个项目做下来有几个细节是特别容易被后来者忽视的我单独拎出来说。第一声呐时间窗口N别取太大。我们试过10帧、12帧结果反而比5帧差。水下环境变化太快AUV本身在运动过老的帧带来的是已经过时的空间信息。回声呐帧率的匹配关系一般5到8帧是比较合理的区间具体看AUV的移动速度和声呐量程。第二阴影区的真值标注要单独分类“未知”不要混进“空闲”。如果训练数据把阴影区标成空闲CNF会学到一个“这里没有东西”的确定结论回到了老路。必须让模型在这个区域学到“大方差”才能让后面的规划器把它当成不确定区域来处理。第三CNF的ODE步数不是越多越好。步数从8加到16精度提升明显从16加到32几乎没变化推理时间却线性增长。训练时用24步保证精度部署时选16步够用。这个需要实测不能只看论文里的设置。6.2 这套方法之后还能往哪走从我们手头的经验看这套“时空图模型加概率重构加概率规划”的研究线后续还有三个方向值得尝试。多AUV协同感知。把多个AUV各自的声呐感知场合并成一张全局时空图节点上增加“来自哪个平台”的属性让平台间互为补充覆盖彼此的声呐盲区。这个在密集障碍水域作业里很有价值。和水声通信约束结合。AUV编队之间的水声通信有严重的带宽限制和时延如果规划器能感知通信窗口和丢包概率把通信代价纳入目标函数协同路径规划会更实用。让CNF输出直接驱动采样式规划。现在是把概率分布先转成代价场再搜索路径中间有信息损失。如果直接让CNF为候选路径打分——即所谓“概率重构驱动的路径采样评估”理论上能保留更多分布细节值得一试。最后再分享一点实际体会。那次水池实验结束看着AUV贴着障碍边缘稳稳绕过去我最大的感慨是水下机器人感知和规划之间的鸿沟比大多数人想象得更大。声呐不会告诉你真相它只会告诉你有波回来了。你要做的不是相信它也不是怀疑它而是给每一条回波一个合理的置信区间让规划器在不确定的世界里做确定的决策。这套STGNN加CNF的方案就是向着这个方向走了一步。如果你也在折腾类似的系统希望这篇内容能帮你少走一些我们走过的弯路。