基于增强现实标记仿真技术的水下无人航行器位姿估计

摘要

本研究提出了一种基于ROS–Gazebo的单目视觉框架，利用ArUco标记实现自主水下航行器的位姿估计。将估计的位姿与仿真里程计进行比较，结果表明在受控条件下能够实现稳定的跟踪，并与真实位姿高度一致。

摘要

本研究提出了一种基于仿真的单目视觉系统框架，用于在ROS-Gazebo环境中对无人水下航行器（UUV）进行位姿估计。该框架集成了RexRov2 UUV模型与ArUco_ROS，用于在模拟的水下环境中检测虚拟标记并估计其位置与姿态。采用透视-n-点（Perspective-n-Point, PnP）方法进行位姿估计，并基于标记提取的特征，通过比例-积分-微分（PID）控制器调节航行器的运动。系统通过将估计的位姿与仿真器提供的真实里程计数据进行对比来评估性能。在正常条件下，结果表明位姿估计稳定，估计值与真实位置和姿态高度一致。系统能够实现平滑的轨迹跟踪，波动极小，显示出在受控环境中的可靠性能。然而，在水动力干扰增强的情况下，系统在位置和姿态上出现偏差，导致跟踪性能不稳定。这些结果凸显了经典PID控制在非线性水下环境中的局限性，表明需要采用更鲁棒的控制策略。总体而言，所提出的框架为测试水下导航算法以及评估模拟环境中感知与控制的集成提供了一个安全、灵活且成本效益高的平台。

引言

无人水下航行器（UUV）通常用于人类活动受限的场景，例如海洋监测、勘探以及石油工业。它们配备先进的传感器，用于从复杂的水下环境中收集数据，因此需要具备高鲁棒性且成本效益良好的测试平台。仿真技术，包括使用 ArUco 标记的基于视觉的方法，为降低 UUV 在真实环境中部署所面临的风险提供了有效途径。

从最早的特种用途水下研究运载器到现代的自主水下航行器，无人水下航行器（UUV）的发展历史凸显了其在科研、国防和工业领域中的重要性¹。自主水下航行器的演进进一步强调了在模拟环境中实现精确定位、导航与测试的必要性^2,3。

图1展示了无人水下航行器（UUV）仿真系统的传统架构。机器人操作系统（ROS）与Gazebo仿真器的结合为建模、控制和实时通信提供了一个多功能平台。ROS能够实现传感器、执行器与控制模块之间的高效通信，而Gazebo则提供了一个逼真的三维仿真环境，用于测试导航与控制算法^4,5。在此背景下，基于ArUco标记的方法通过精确检测和跟踪参考点，提升了位姿估计的精度⁶。

图1：UUV仿真系统的传统架构。该示意图展示了在ROS–Gazebo框架内仿真、感知与控制组件之间的相互作用。请点击此处查看此图的放大版本。

多项研究已推动了无人水下航行器（UUV）的仿真、导航与感知技术的发展⁷。ROS–Gazebo框架在机器人建模方面的适用性已得到验证⁴，并通过UUV Simulator将该方法扩展至多机器人与干预任务的水下应用⁵。基于视觉的定位技术也通过结合ArUco标记与跟踪算法得到了改进⁶。类似地，先进的仿真平台实现了高保真度的水下任务仿真与监控⁸，同时在受限环境中利用基准标记实现鲁棒定位的研究也已取得成果⁹。

这些技术发展得到了调查研究的补充。水下导航与定位挑战方面的进展已得到全面综述。^10,11，并且已对商业和开源的模拟平台进行了系统评估¹². 其他综述已探讨了自主水下航行器领域的最新进展^13,14，以及多车辆系统的编队控制与通信策略¹⁴此外，鲁棒跟踪控制技术的进步¹⁵ 以及使用深度学习方法（如门控循环单元）进行状态估计¹⁶，进一步提高了自主水下任务的可靠性。这些研究共同展示了在无人水下航行器（UUV）仿真逼真度、导航能力及智能控制方面的显著进展。

尽管取得了这些进展，但仍存在若干研究挑战。目前的仿真平台缺乏精确的传感器模型，特别是前视和侧扫声呐系统。大多数平台仅支持单个自主水下航行器（AUV）操作，在多AUV协同与编队方面能力有限。传感器和动力学模型的不准确性可能导致性能欠佳，而水流和噪声等环境干扰在不同仿真平台中的建模也缺乏一致性。为解决这些局限性，本研究提出了一种基于ROS–Gazebo的仿真框架，引入基于ArUco标记的姿态估计方法，以实现更精确的状态估计，并对无人水下航行器（UUV）导航算法进行严格评估。

本研究提出了三项主要贡献。首先，基于 RexRov2 模型，在水下 ROS–Gazebo 仿真环境中提出了一种基于 ArUco 的单目位姿估计方法。其次，分析了在不同扰动条件和控制场景下的位姿稳定性。第三，指出了经典 PID 控制在动态扰动下的非线性水下环境中存在的局限性。此外，本研究将感知、控制与仿真集成到一个统一的框架中，弥补了以往研究中将这些组件独立处理所存在的不足。

方案

1. 软件环境设置

使用运行 Ubuntu 20.04 和机器人操作系统（Robot Operating System, ROS）Noetic 的基于 Linux 的工作站作为实现水下自主定位系统的主要开发平台¹⁷。系统初始化过程包括创建 catkin 工作空间，命令为 mkdir -p ~/catkin_ws/src，然后通过 catkin_init_workspace 命令进行初始化。将 uuv_simulator 和 aruco_ros 软件包克隆至源目录，并使用 catkin_make 命令进行编译，以支持水下干预建模。

通过启动默认的水下世界并使用 rostopic list 命令确认活跃通信，验证了 ROS 中间件与 Gazebo 9 仿真器之间的连接性。图像处理和位姿估计节点采用 Python 3.8 实现，并集成了 ArUco 库，利用 OpenCV 计算相对位姿。ROS 环境被配置为管理依赖关系并实现实时数据通信，使机器人能够相对于多个人工标记物进行自我定位¹⁸。该设置还支持在不同条件下评估标记物检测性能。图 1 展示了 ROS–Gazebo 环境中仿真、感知和可视化节点之间的通信关系。

2. 无人水下航行器模型与水动力参数化

RexRov2 载具模型取自 uuv_descriptions 软件包，并通过其统一机器人描述格式（URDF）文件在 Gazebo 中启动⁵。该载具模型由连杆和关节组成，干质量为 1,863 kg，排水体积为 1.838 m³，在密度为 1,000 kg/m³ 的水中接近中性浮力。

使用 uuv_underwater_object_plugin 对流体动力效应进行建模，包括线性和二次阻尼力。附加质量纵荡系数设置为 779.79，阻尼参数根据报道的 RexRov2 特性进行校准，以模拟真实的航行器运动¹⁹。图 2展示了整体仿真架构。

图2：仿真环境的整体架构。 该图展示了UUV模型、传感器输入、感知流程以及用于位姿估计和导航的控制模块之间的集成关系。请点击此处查看此图的放大版本。

3. 摄像机、标记物与环境设置

将一个单目相机安装在 rexrov2/camera_link 坐标系上。相机参数设置为 1280 × 720 像素，帧率为 30 Hz。相机的内参在相机配置文件中定义，焦距为 554.25 像素，主点位于 (640.5, 360.5)。

选择6×6_250字典中的一个固定垂直ArUco标记（ID：582），其边长为0.5 m，放置在坐标(20.0, 0.0, -10.0)处。通过/uuv_simulation_app/set_current_velocity ROS服务，将模拟环境设置为水深10 m，并在X方向上具有0.1 m/s的恒定水流速度。使用0.8的光照强度以确保标记的可见性⁹。图3展示了模拟的水下环境。

图 3：Gazebo 9 中模拟的水下环境。 该环境包含 RexRov2 无人水下航行器（UUV）、虚拟 ArUco 标记，以及可控的环境条件，如水深、光照和水流。请点击此处查看此图的放大版本。

4. 位姿估计与导航控制

使用 ArUco ROS 节点启动标记检测流程，该节点订阅了 /rexrov2/forward_camera/image_raw 主题。通过透视-n-点（PnP）方法估计载体的位姿，以计算相对于已知三维标记几何结构的旋转矩阵（R）和平移向量（t）。

通过静态变换发布器，将估计的位姿从相机坐标系转换到全局参考坐标系。使用 PID 控制器向 /rexrov2/thruster_manager/input 话题发送速度指令，以调节航行器的运动。为确保稳定的跟踪性能，纵荡（surge）和升沉（heave）轴向的比例增益（Kp）分别设置为 1500 和 2000²⁰。

5. 评估与真实值比较

基于视觉的位姿估计结果与仿真轨迹之间的一致性通过 uuv_gazebo_ros_p3d 插件提供的 /rexrov2/pose_gt 话题进行评估。基于 ArUco 的流程所估计的位姿与真实位姿的里程计数据进行了比较，以评估系统的稳定性和一致性。

通过轨迹比较和实时可视化进行评估，使用 Rviz 和 rqt_plot 监测位置（X、Y、Z）和姿态（横滚、俯仰、偏航），从而能够在名义条件和受扰动条件下观察偏差。

本研究侧重于定性与比较性评估。使用均方根误差（RMSE）进行定量评估被列为未来工作，届时将通过多次仿真运行来评估位置和姿态误差。

6. 结果生成与可视化

使用 Rviz 作为三维可视化工具，以监测 UUV 的轨迹和姿态。通过可视化 /rexrov2/camera_link 和 /aruco_marker_frame 坐标系，评估载体与标记之间的空间关系²。

使用 rqt_plot 工具，基于 /rexrov2/pose_gt 和 /aruco_pose 话题生成了车辆姿态（横滚、俯仰、偏航）的实时图表。利用 rosbag record 命令记录了仿真数据，包括视频流和控制输入，用于定性分析。

模拟在理想的视觉条件下进行，假设光照良好且水体浑浊度较低。假设 ArUco 标记物始终无遮挡。水动力阻尼效应采用简化系数进行建模，未包含传感器噪声。

结果

采用模拟水下环境，利用RexRov2无人水下航行器（UUV）对基于ROS–Gazebo的系统进行评估。波浪模拟环境（图3）为基于标记的位姿估计系统的评估提供了动态但可控的测试条件。

图4展示了检测到的ArUco标记与无人水下航行器（UUV）轨迹之间的关系。基于ArUco标记检测的感知流程能够实时成功检测并跟踪基准标记。该感知流程与RexRov2模型及PID控制器（图5）集成，实现了对航行器相对于标记位姿的实时估计。检测结果通过透视n点（Perspective-n-Point, PnP）方法用于估计航行器的相对位姿。

讨论

数值观测结果表明，所提出的框架在受控条件下具有有效性。位置（约10⁻² m量级）和姿态（约10⁻² rad量级）的微小变化表明位姿跟踪稳定。这些结果证明，ArUco标记可为短距离定位提供可靠的参考⁶。此外，仿真平台能够生成用于鲁棒SLAM的专用数据集，支持在无法使用人工标记的环境中实现定位²¹。

研究结果还凸显了经典PID控制在非线性水下环境中的局限性。缺乏适应性导致在动态干扰下出现超调和不稳定现象¹⁵。与基于自适应和学习的方法相比，这一局限性更加明显，例如基于粒子滤波的估计方法¹⁶，或利用视觉-惯性传感器融合的自主对接系统²²。

表1展示了RexRov2智能体在扰动条件下的位置估计结果。结果表明，该系统对环境扰动...

材料

本文使用的材料清单

姓名	公司	目录编号	评论
ArUco 标记检测库	OpenCV	N/A (开源)	基于视觉的标记检测，用于位姿估计
Gazebo 仿真器	Open Robotics	N/A (开源)	用于无人水下航行器（UUV）测试的三维仿真环境
REXROV2 UUV 模型	UUV 仿真项目	N/A (开源)	用于位姿估计实验的预定义 UUV 模型
机器人操作系统（ROS）	Open Robotics	N/A (开源)	用于通信与控制的中间件
RViz	Open Robotics	N/A	用于 UUV 模型与传感器数据的可视化工具
Ubuntu Linux (20.04 LTS)	Canonical Ltd.	N/A	支持 ROS 与 Gazebo 的操作系统
UUV 仿真软件包	挪威科学技术大学（NTNU）	N/A (开源)	ROS–Gazebo 水下仿真框架