73、用于增强位姿估计的导航技术研究

用于增强位姿估计的导航技术研究

1. 预测定位质量

1.1 质量评分与系统目标

定位质量评分与直接稀疏里程计（DSO）相关，具有诊断形式。它要求视觉里程计实时运行，且只有在实际出现视觉质量潜在损失时，质量评分才会下降。目标是在机器人部署时避免出现此类情况，因此采用训练好的分类器，通过分析前视相机图像提前预测定位质量，使系统具有前瞻性，选择能保证部署时定位性能不降低的路径。

1.2 目标轨迹预测器

目标轨迹预测器基于深度卷积神经网络（DCNN），该网络以Resnet - 18架构为基础，能为前视图像中一组点（以这些点为中心的小图像块）预测质量评分，并根据最高质量点选择前进方向。网络输入是从整幅图像中提取的图像块，输出是质量评分。为防止过拟合，引入了Dropout层，并使用均方误差成本函数进行训练。

1.3 机器人控制

机器人使用经过调优的PID控制器进行控制，该控制器根据时间平均图像评分及其位置来控制机器人的偏航和俯仰。低级步态控制器将这些角度命令转换为六个鳍状肢的运动。

2. 训练过程

2.1 自监督训练方式

预测质量网络采用自监督方式进行训练，让机器人在各种地形上移动，通过机器人的自校准将定位质量与前视相机中的图像块关联起来。

2.2 训练标签生成

对于训练数据集中的每幅图像，通过将下视图像的相对3D位置反向投影到前视图像历史中，为DCNN生成训练标签。反向投影公式为：
$u_{t’,t}, v_{t’,t} = \pi(T_{w}^{c}(t’)P_{b}(t))$
其中，$\pi$ 是将相对于前视相机的3D点投影到图像平面的函数，$T_{w}^{c}$ 是从世界坐标系中的点到前视相机的变换，$P_{b}$ 是下视图像在世界坐标系中的位置，$u_{t’,t}$ 和 $v_{t’,t}$ 是时间 $t’$ 时下视图像块在时间 $t$ 的像素位置。

2.3 控制流程

• 机器人使用PID控制器根据时间平均图像评分及其位置控制偏航和俯仰。
• 低级步态控制器将角度命令转换为六个鳍状肢的运动。

3. 实验设计

3.1 实验总体思路

方法实时处理图像，以维持导航估计，并在多个层面上闭合规划和控制回路。基于下视“后相机”图像进行准确的视觉里程计（VO）估计，同时使用DCNN过滤“前相机”图像来估计机器人前方区域的定位质量，其输出用于更长远的路径规划，引导机器人前往支持精确定位的区域。

3.2 实验组成部分

实验方法由两部分组成：
- 初步概念验证研究：在公海进行，目的是在真实条件下生成质量评分，验证方法的有效性，并为后续实验的DCNN生成初始训练数据；测试集成硬件（NVidia Jetson TX2）在运行DCNN推理和VO算法时的性能。
- 严格的模拟研究：在先进的模拟环境中进行，可对系统进行可重复的定量评估。模拟环境基于Unreal Engine 4，包含Aqua机器人的动力学模型和视觉惯性传感器模型，能获取视觉和惯性数据。通过自定义ROS2和ROS1消息与真实机器人使用相同定义进行通信，可在模拟和现实世界中使用相同的推理和控制代码。还可自定义海洋环境，改变水的浊度、水流和亮度等条件，用于调整推理模型和控制参数。

3.3 实验平台

实验平台是Aqua机器人，其相机由三台高效且强大的计算机通过千兆以太网联网操作：
- PC104计算机（控制栈）：使用Linux操作系统的实时Xenomai扩展，执行低级电机控制、电源管理以及通过内部惯性测量单元（IMU）和深度传感器进行车辆方向和深度估计。
- Intel i3 NUC计算机（视觉栈）：运行机器人操作系统（ROS），以15 Hz的频率捕获全局快门和硬件同步相机的图像，运行VO算法并执行高级导航任务。
- NVidia Jetson TX2 GPU计算模块（AI栈）：也运行ROS，使用TensorFlow进行基于DCNN的视觉推理，推理结果传输到视觉栈用于导航规划。

3.4 实验流程mermaid图

graph LR
    A[开始] --> B[公海实验]
    B --> C[生成质量评分和训练数据]
    B --> D[测试硬件性能]
    C --> E[模拟实验]
    D --> E
    E --> F[验证系统有效性]
    E --> G[调整模型和参数]
    F --> H[结束]
    G --> H

4. 实验结果

4.1 数据收集与验证

在公海近珊瑚区域进行了为期五天的自主游泳实验，收集了约74分钟（分段为2 - 10分钟）的数据，涵盖不同位置、天气条件（和能见度）以及游泳动作。成功在该数据集上运行DSO，并通过检查评分分布、目视检查明显的好和差的VO图像块以及验证VO失败前是否有极低质量评分来验证质量评分的有效性。

4.2 DCNN分类准确性

模拟数据集包括51,892张测试图像、21,567张验证图像和10,104张测试图像。基于Resnet的DCNN将质量评分预测为五个类别之一。模型使用在ImageNet上预训练的权重初始化，参数如下：
- 批量大小：25
- l2正则化权重：$1e - 5$
- 动量：$1e - 4$
- 初始学习率：$1e - 5$
- 学习率衰减：$1e - 3$

不同图像块大小的DCNN分类准确率如下表所示：
| 图像块大小 | 准确率 |
| ---- | ---- |
| 40 px | 90.59% |
| 60 px | 91.46% |
| 80 px | 93.13% |
| 100 px | 92.56% |
| 120 px | 92.37% |

较小的图像块大小使DCNN运行更快，估计更精细；80×80像素的图像块大小达到最高准确率93.13%，推测较大图像块表现稍差是因为捕获了更大的场景区域，可能包含如沙子和珊瑚等模糊纹理。

4.3 轨迹预测效果

通过比较有和没有轨迹预测时自主游泳收集的质量评分分布，证明了轨迹预测器的有效性。有轨迹预测时，高质量评分集中，表明质量评分估计器和轨迹规划器有效。公海初步实验的质量评分分布与无轨迹预测时相似。

4.4 鲁棒性量化实验

进行了多次模拟实验，比较DCNN轨迹预测控制器和简单航点控制器。DCNN轨迹预测控制器以恒定速度向前移动，引导机器人前往质量评分最高的区域。每个实验中，将预测控制器生成的路径与地面真值进行比较，以定量测量相对误差。简单航点控制器以恒定向前速度移动，使用比例控制器迭代地将机器人指向当前航点，为防止VO误差，限制了旋转角速度。

在三种模拟环境（珊瑚丰富区域、沙子丰富区域和海草丰富区域）中各进行了20次实验，总行驶距离为162 km。结果表明，在所有三种环境中，轨迹预测控制器在平均误差和总方差方面均优于简单航点控制器。以下是部分实验结果示例：
| 实验索引 | 配置（距离、地形） | DCNN轨迹预测相对百分比误差 | 简单航点控制器相对百分比误差 |
| ---- | ---- | ---- | ---- |
| 1 | 714 m，珊瑚区域 | 3.6% | 12.8% |
| 2 | 2156 m，珊瑚区域 | 7.6% | 18.1% |
| 3 | 1703 m，沙子区域 | 11.2% | 30.4% |
| 4 | 1442 m，海草区域 | 6.9% | 11.7% |
| 5 | 1756 m，沙子区域 | 7.5% | 17.3% |

4.5 实验结果总结

• 成功在公海和模拟数据上运行DSO和质量评分系统，验证了质量评分的有效性。
• DCNN在不同图像块大小下分类准确率均超90%，80×80像素图像块达到最高准确率93.13%。
• 轨迹预测器有效，能引导机器人前往高质量区域。
• 轨迹预测控制器在三种模拟环境中均优于简单航点控制器，证明了方法的鲁棒性。

5. 结论与未来展望

5.1 研究成果总结

本研究提出了一种实时的水下导航方法，利用中长期规划优化视觉里程计任务的完成。该方法解决了在世界中选择路线以优化估计和可观测性的重要问题。通过在公海使用水下机器人进行实验验证，并在逼真的模拟环境中进行广泛的定量评估，证明了该方法的有效性。

具体成果如下：
- 定位与路径规划 ：使用控制器显著提高了定位和航点到达能力。DCNN能够以93.13%的准确率预测图像区域的质量评分，代表相应位置视觉里程计的性能。
- 实验对比 ：在多种环境中模拟162 km的自主游泳，将轨迹预测控制器与简单航点控制器进行对比，结果显示该方法在所有简单航点控制器的10个最佳结果中，表现优于其达3.5倍之多。

5.2 未来工作方向

未来计划在更具视觉挑战性和复杂的水下环境中进行更多的海试，长期目标是构建水下环境的3D模型，并对珊瑚礁健康进行近距离自主调查。主要挑战之一是在现实条件下为机器人找到可靠的地面真值来源。预计通过使用简单的子轨迹（如恒定向前平移），可以选择性地简化相机之间的对应问题，从而获取可靠的训练数据。

5.3 未来实验流程mermaid图

graph LR
    A[开始未来实验] --> B[选择更复杂环境]
    B --> C[收集数据]
    C --> D[构建3D模型]
    C --> E[评估珊瑚礁健康]
    D --> F[验证模型准确性]
    E --> F
    F --> G[调整方法和参数]
    G --> H[结束实验]

5.4 总结表格

研究方面	成果与结论
方法验证	在公海和模拟环境中验证了方法的有效性，提高了定位和航点到达能力
DCNN性能	以93.13%的准确率预测图像区域质量评分
实验对比	轨迹预测控制器在多种环境中优于简单航点控制器
未来挑战	寻找可靠地面真值来源，简化相机对应问题
未来目标	构建水下3D模型，进行珊瑚礁健康自主调查

综上所述，本研究提出的水下导航方法在当前实验中取得了良好的效果，但仍有进一步提升和拓展的空间。未来的研究将聚焦于更复杂的环境和更具挑战性的任务，有望为水下机器人的应用带来更广阔的前景。