水下机器人FAR-AVIO：声学-视觉-惯性里程计技术解析
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表示水中的声速)，经典的单静态多普勒关系产生沿声束的径向速度vr：
vr ≈ -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 7950X，32GB RAM)和嵌入式平台(NVIDIA Jetson Orin NX，8GB 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和内存资源，并在嵌入式硬件上舒适地实时运行。
未来工作包括扩展到更广泛的传感器模态(如前视声纳和磁力计)，以及探索基于学习的传感器可靠性评估方法。此外，计划将系统部署在实际的海洋勘探任务中，以进一步验证其在开放水域环境中的性能。