1. FAR-AVIO系统概述FAR-AVIO是一种专为水下机器人设计的快速鲁棒声学-视觉-惯性里程计框架。水下环境对传统的视觉-惯性里程计系统提出了严峻挑战包括强光衰减、海洋雪、浑浊水体以及弱激励运动等问题这些问题会降低惯性观测性并导致长期运行中频繁的跟踪失败。FAR-AVIO通过将Schur补公式嵌入扩展卡尔曼滤波器(EKF)实现了精确的联合位姿-地标优化同时通过高效边缘化地标状态保持恒定时间更新。1.1 系统核心创新FAR-AVIO的主要创新点包括三个方面Schur补基础的紧耦合声学-视觉-惯性里程计框架这是首个基于Schur补的紧耦合声学-视觉-惯性里程计框架严格建模来自多普勒频移原理的DVL测量并将其嵌入基于滤波器的后端优化中同时联合执行在线DVL-IMU外参标定。AWARE模块(自适应权重调整和可靠性评估)这是一个在线健康评分机制基于实时可靠性评估动态调整传感器融合的sigma尺度使系统能够在传感器性能下降和故障条件下保持鲁棒运行。高效在线标定方案无需专用标定操作即可联合估计DVL-IMU外参提高了系统的实用性和部署便利性。2. 系统架构与关键技术2.1 整体系统架构FAR-AVIO的系统架构如图2所示输入来自配备立体相机、IMU和DVL的水下机器人。系统主要由以下几个模块组成视觉前端跟踪基于稀疏Shi-Tomasi角点和光流跟踪器并增加了基于IMU的位姿先验。Schur补基础的视觉更新将视觉测量投影到雅可比空间构建等效观测模型。DVL测量更新基于多普勒频移原理严格建模DVL测量。在线传感器标定联合估计DVL-IMU外参。AWARE模块在线评估传感器可靠性并自适应调整融合权重。2.2 Schur补基础的视觉更新视觉更新是FAR-AVIO的核心技术之一。假设第j个3D地标ˆξwj在世界帧w中表示并被关键帧i的相机观测到。投影模型可以表示为ˆzij π(ˆxb, ˆξwj)其中ˆzij ∈ R2是地标在图像平面中的预测像素坐标π(·)表示基于当前状态估计ˆxb将世界帧中的3D地标映射到2D像素坐标的相机投影函数。通过在标称状态附近线性化得到重投影残差rij zij - ˆzij ≃ Hx,ijδX Hf,ijδξwj nij其中rij和zij分别是来自前端跟踪结果的重投影残差和视觉测量nij是测量噪声Hx,ij和Hf,ij分别是投影函数对系统状态和地标位置的雅可比。将所有滑动窗口中多个关键帧的观测堆叠起来得到r HxδX Hfδξ n然后直接将方程(9)的测量模型投影到雅可比空间[Hx Hf]⊤构建等效观测模型[Hx⊤ Hf⊤]r [Hx⊤ Hf⊤][Hx Hf][δX δξ] n′由于系统状态的维度限制滤波器状态不包括地标扰动δξ必须通过边缘化δξ构建标准观测模型。为此采用基于Schur补的消除方法从观测模型中消除地标状态得到仅依赖于误差状态δX的等效观测模型[b1 - C2C3^-1b2] [C1 - C2C3^-1C2⊤]δX n′′1这样得到的等效残差可以插入到标准EKF更新过程中。2.3 DVL测量模型2.3.1 单波束多普勒速度DVL换能器发射已知载波频率ft的窄带声波并接收海底或水中散射体反射的回波。设fr表示接收频率∆f fr - ft表示测量的多普勒频移。在标准窄带、小速度假设|vr| ≪ cs下(cs表示水中的声速)经典的单静态多普勒关系产生沿声束的径向速度vrvr ≈ -cs/(2ft)∆f符号约定选择为vr 0对应于车辆沿波束方向向海底移动。对于四个DVL波束中的每一个(索引为i 1,...,4)设˜vi表示通过方程(14)转换其测量的多普勒频移获得的标量径向速度。实际上它会因噪声和偶尔的异常值而损坏。我们将单波束测量建模为˜vi vr,i ni, ni ∼ N(0,σi^2)其中ni表示方差为σi^2的零均值高斯测量噪声。2.3.2 DVL速度测量模型DVL声学中心的线速度在DVL帧{D}中表示为D˜v [D˜vx, D˜vy, D˜vz]⊤。根据图3中的DVL几何形状每个波束ei相对于帧{D}的方向已通过固定倾斜角α从水平面xDyD和关于zD轴的方位角βi参数化。然后该波束测量的真实径向速度简单地是3D速度在波束方向上的投影vr,i ei⊤D˜v将(18)代入(15)单波束DVL测量模型可以写为˜vi ei⊤D˜v ni这表明每个换能器提供车辆速度沿其自身声轴的观测。将四个标量波束测量堆叠成向量b˜v [˜v1, ˜v2, ˜v3, ˜v4]⊤并将波束方向向量收集到矩阵中单波束模型(19)可以写成紧凑形式b˜v ED˜v nb其中E [e1, e2, e3, e4]⊤ ∈ R4×3是波束方向矩阵nb [n1, n2, n3, n4]⊤是堆叠的噪声向量。只要波束不共面(标准DVL配置就是这种情况)矩阵E就是满列秩(秩3)。因此3D速度D˜v可以通过求解超定线性系统(20)以最小二乘意义从四个波束测量中唯一确定D˜v (E⊤E)^-1E⊤b˜v其中矩阵逆(E⊤E)^-1仅依赖于已知的波束几何形状可以离线预计算。2.3.3 ESKF状态更新的DVL残差给定估计状态预测的DVL帧速度计算为Dˆv (ˆRbD)⊤[(ˆRwb)⊤ˆvwb ⌊bˆω⌋׈pbD]其中bˆω是体帧中偏差校正的角速度ˆpbD是体帧中表示的IMU-DVL杆臂ˆRbD是从DVL到体帧的旋转。根据方程(26)和(21)我们可以得到状态更新的DVL残差rDVL D˜v - Dˆv然后该残差相对于误差状态线性化并在具有测量协方差ΣD的标准ESKF更新中使用。2.4 AWARE模块大多数基于融合的滤波器假设每个传感器具有固定的、时不变的测量噪声这在实际中很少有效视觉质量随纹理、光照和运动而变化DVL测量在底部锁定不良、散射或流动干扰下会下降。如果忽略这种变化即使其他传感器保持可靠来自一种模态的坏测量突发也会破坏整个估计。AWARE通过持续评估视觉和DVL质量、调整其有效协方差并暂时禁用严重降级的传感器来解决这个问题从而防止任何单个故障源主导融合。对于每个传感器s ∈ {VIS, DVL}AWARE维护一个可靠性尺度σs和一个固定长度的最近不健康事件队列Qs。在每次测量时计算传感器特定的质量分数qs ∈ 0,1 。这些分数驱动协方差缩放和传感器门控决策。3. 实验验证3.1 实验设置与数据集我们在公开的Tank数据集上评估了提出的FAR-AVIO该数据集提供了在波浪池中收集的同步立体、IMU、DVL和深度测量。精确的地面真实(GT)相机姿态由使用安装在水下结构上的AprilTag标记的TankGT管道生成能够在真实水下条件下进行定量基准测试。八个序列分为三种轨迹类型(Structure、HalfTank和WholeTank)具有由车辆速度、光照和无纹理区域数量确定的不同难度级别(Easy/Medium/Hard)。图1说明了在HalfTank-Easy序列中遇到的典型视觉挑战和运行结果。3.2 定位性能比较我们将FAR-AVIO与五个代表性基线进行了基准测试AQUA-SLAM、UVA-SLAM、SVIN2、ORB-SLAM3和VINS-Fusion。为了公平性所有方法都使用相同的相机/IMU内参和外参并在立体-惯性(或立体-惯性-DVL当可用时)配置中运行。估计的轨迹使用[26]中描述的方法与地面真实对齐绝对平移误差(ATE)的平均均方根误差(RMSE)和标准差(STD)总结在表I中。标记为NaN的条目表示在序列完成之前重复跟踪失败或发散。总体而言FAR-AVIO在所有完成所有序列的方法中实现了最佳的平均平移精度并且在每个单独序列上始终排名第一或第二。在更具挑战性的Structure-Hard、HalfTank-Hard和WholeTank-Medium序列上FAR-AVIO明显优于基线与AQUA-SLAM相比(例如Structure-Hard上0.13米对0.50米)将平移RMSE降低了约75%与纯视觉-惯性方法相比降低了一个数量级以上。相比之下ORB-SLAM3、VINS-Fusion和SVIN2(它们只能利用立体惯性数据没有声纳没有压力)在具有强浑浊或长时间视觉降级的序列中经常出现大的漂移或直接跟踪失败如米级误差和NaN条目(例如HalfTank-Easy上VINS-Fusion为29.83米HalfTank-Hard上SVIN2为77.6米)所反映的那样。3.3 运行时和计算负载我们评估了具有公共实现的方法(ORB-SLAM3、VINS-Fusion、SVIN2、FAR-AVIO)在桌面CPU(AMD Ryzen 9 7950X32GB RAM)和嵌入式平台(NVIDIA Jetson Orin NX8GB RAM)上的运行时和计算负载。图5显示FAR-AVIO在所有基线中始终表现出最低的CPU利用率和内存占用。在Orin NX上表II中的每模块细分显示FAR-AVIO在28.28毫秒(≈35 Hz)内处理帧比VINS-Fusion(61.65毫秒≈16 Hz)实现了约2.2倍的加速。主要增益来自后端VINS-Fusion在视觉优化中花费33.76毫秒(54%)而FAR-AVIO视觉更新仅需要6.08毫秒(21%)额外的DVL更新仅增加0.78毫秒(2%)。3.4 AWARE模块和外参标定的消融研究我们进行了消融实验以量化提出的AWARE模块和IMU-DVL外参在线标定的贡献。该研究包括真实的Tank序列和具有合成IMU和DVL测量的纯数值模拟其中地面真实外参是已知的。3.4.1 AWARE模块的效果为了评估AWARE是否按预期行为我们沿估计轨迹可视化应用于视觉和DVL测量的瞬时置信度尺度。对于两个代表性序列Structure-Easy(SE)和WholeTank-Hard(WH)我们绘制轨迹并根据相应的视觉和DVL Σscale ∈ [0,1]为每个点着色其中接近1的值表示高置信度(标称加权)接近0的值表示对该传感器的强烈降权。在SE序列上该序列具有相对清洁的水和稳定的光照视觉前端很少经历长期降级。因此几乎整个轨迹上的视觉和DVL尺度都保持接近1只有轻微的波动(图6a)。这表明当所有传感器正常运行时AWARE不会引入不必要的重新加权并且在简单序列上有效地减少到标准的紧耦合VI-DVL融合方案。相比之下WH序列更具挑战性强浑浊、不均匀光照和大部分具有弱或缺失图像特征的轨迹导致视觉跟踪不可靠的延长时期。在这些段中视觉Σscale沿轨迹的相应部分显著降低而DVL尺度保持接近1(图6b)。这表明当前端报告跟踪质量差时AWARE自动降低视觉更新的权重同时更多地依赖DVL约束来稳定状态估计。3.4.2 IMU-DVL外参标定的效果我们首先在数值模拟中验证IMU-DVL外参标定收敛。图7绘制了三种不同初始化下外参误差随时间的演变恒等变换(Identity)以及具有中等(Noise Middle)和小(Noise Small)噪声水平的两个扰动初始外参。在所有情况下估计的IMU-DVL外参都向地面真实收敛最终误差稳定在一个小的残差水平。我们进一步量化对轨迹精度的影响如表IV所示真实Tank序列和模拟序列都用扰动外参初始化。在Tank数据上标定将RMSE降低10-25%在具有较大扰动的模拟中损坏的外参导致RMSE超过3-9米而没有标定而在线标定恢复亚米精度(0.124-0.574米)。平均而言启用标定将RMSE从8.152米降低到0.263米证实提出的模块可靠地从噪声初始猜测中恢复准确的外参。4. 结论与未来工作本文提出了FAR-AVIO一种用于水下机器人的快速鲁棒基于Schur补的声学-视觉-惯性融合里程计框架具有在线传感器标定和AWARE模块。在真实世界序列和合成场景上的广泛评估表明与最先进的水下和陆地基线相比FAR-AVIO实现了竞争性或更优的定位精度同时需要显著更低的CPU和内存资源并在嵌入式硬件上舒适地实时运行。未来工作包括扩展到更广泛的传感器模态(如前视声纳和磁力计)以及探索基于学习的传感器可靠性评估方法。此外计划将系统部署在实际的海洋勘探任务中以进一步验证其在开放水域环境中的性能。