Onboard Vision and MPC Navigation for Underwater Robots: An Open BlueROV2 Platform for Multi-Robot Experiments & Docking

I. INTRODUCTION

• Underwater deployment remains challenging because nonlinear vehicle dynamics, limited communication, uncertain localization, optical degradation, and constrained onboard computation coincide in confined environments.Source: §I · p. 1
• The open-source framework combines calibrated stereo relative localization, quaternion-based state estimation, reference planning, and NMPC for BlueROV2 navigation and docking.The platform also includes a low-cost docking station, PX4 SITL/Gazebo environments, and multi-robot simulation tools, with experiments evaluating its components.Source: §I · p. 1

A. Vehicle Model

• The modified BlueROV2 Heavy is modeled in North-East-Down navigation and Forward-Right-Down body frames, with position, quaternion orientation, and body-frame linear and angular velocities.Source: A. Vehicle Model · p. 2
• The dynamics adapt the BlueROV2 model to account for the autonomy tube’s changed mass and buoyancy.Source: A. Vehicle Model · p. 2

• At each time step, the vehicle receives noisy IMU, stereo-camera, and external-pose measurements, plus a desired terminal pose for navigation or docking.Source: B. Problem Formulation · p. 2
• The formulation estimates the current state, generates a reference sequence to the terminal pose, and chooses body wrenches so predicted states track that reference.Source: B. Problem Formulation · p. 2
• The wrench sequence must remain within a nonempty actuator-limited set, and all computations must finish onboard within the sampling period.Source: B. Problem Formulation · p. 2

A. Autonomy Hardware and Software

• A modular pressure housing carries the Jetson Orin NX, RealSense D435i, and Ethernet switch on removable trays, and clamps to the BlueROV2 roof racks.The housing can be installed on an existing BlueROV2 without modifying its components.Source: A. Autonomy Hardware and Software · pp. 2–3
• ROS 2 nodes onboard acquire and align camera data, perform relative localization, run the EKF and planner, and solve the NMPC without an external computer.The external connection is retained for monitoring, data recording, and delivery of motion-capture measurements.Source: A. Autonomy Hardware and Software · p. 3

B. Underwater Stereo Perception

• The open-source project repository provides CAD files, a Bill of Materials, assembly instructions, and software installation procedure; its modular design installs autonomy on existing BlueROV2 without modifying components.The perception module supports ArUco-based relative pose estimation for docking.Source: A. Autonomy Hardware and Software · p. 3, B. Underwater Stereo Perception · p. 3
• Underwater-calibrated stereo depth is aligned with color, while fine-tuned YOLO detections are filtered to accepted depth samples (Fig. 2).The accepted depth samples are associated with each detected vehicle region.Source: B. Underwater Stereo Perception · p. 3, Figure 2 · p. 3
• Detected vehicle positions are backprojected in the camera frame and transformed to the body frame using calibrated camera-to-body extrinsics.The relative position uses onboard measurements and does not require communication or the vehicle’s global position.Source: B. Underwater Stereo Perception · p. 3

C. Simulation and Docking Infrastructure

• The PX4 SITL/Gazebo model reproduces the modified BlueROV2’s eight thrusters, mass, buoyancy, inertia, and hydrodynamics, and shares PX4/ROS 2 interfaces with the physical platform.The same estimation, planning, and control nodes can operate in simulation and in the tank.Source: C. Simulation and Docking Infrastructure · p. 3
• The Marinarium-like simulated tank includes boundaries and a docking station, while a multivehicle launch file creates independent PX4 instances with separate identifiers, namespaces, and topics.The simulated dock reproduces the physical station’s geometry and marker placement.Source: C. Simulation and Docking Infrastructure · p. 3
• The detachable physical dock uses standard aluminium profiles and 3D-printed PLA coated with capillary sealant, without custom-machined components.Its CAD files and setup instructions are provided, and the PX4 software developments and Gazebo models and environments have been merged upstream.Source: C. Simulation and Docking Infrastructure · p. 3

IV. STATE ESTIMATION AND NMPC NAVIGATION

• The quaternion error-state EKF fuses global-pose measurements with onboard accelerometer and gyroscope data despite motion-capture noise and occasional outliers.The IMU provides specific-force and angular-velocity measurements.Source: A. Quaternion-Based State Estimation · pp. 3–4
• Between global-pose updates, IMU measurements propagate the state at the IMU sampling rate, with accelerometer and gyroscope biases modeled as random walks.The process model includes measurement noise and bias terms.Source: A. Quaternion-Based State Estimation · p. 4
• The estimator propagates covariance using discrete error-dynamics Jacobians and IMU and bias random-walk covariances.The state includes a local attitude error.Source: A. Quaternion-Based State Estimation · p. 4
• Pose updates use position and multiplicative attitude residuals; the quaternion correction is multiplicative while the remaining state components are corrected additively.The update uses error-state correction,.Source: A. Quaternion-Based State Estimation · p. 4

B. Reference Generation and Docking

• RRT* generates a geometric path from the current vehicle position to the desired terminal position, then interpolation and constant-speed time parametrization produce the reference.The same planning approach is used for navigation and docking.Source: B. Reference Generation and Docking · p. 4
• For docking, the RRT* goal is the predefined dock position, and the attitude reference is interpolated from the initial attitude to the dock attitude.The perception module also supports ArUco-based relative dock-pose estimation.Source: B. Reference Generation and Docking · p. 4

C. Nonlinear Model Predictive Control

• Quaternion sign correction selects the shortest representation and avoids unwinding in the attitude error.The terminal cost uses the same state-tracking terms as the stage cost, weighted by Pp, Pq, Pv and Pω, but has no input penalty.Source: C. Nonlinear Model Predictive Control · pp. 4–5
• The NMPC optimal-control problem is implemented symbolically in CasADi and solved with acados using SQP-RTI and a Gauss-Newton Hessian approximation.The solver uses four-stage explicit Runge-Kutta integration, partial-condensing HPIPM, and a shifted previous solution to warm-start each optimization.Source: C. Nonlinear Model Predictive Control · p. 5
• At each sampling step, only the first optimized body wrench is sent to PX4; the controller then resolves the problem using the updated state estimate and reference.The controller, reference-generation, and EKF code is released open-source.Source: C. Nonlinear Model Predictive Control · p. 5

A. Experimental Setup

• Experiments use underwater motion capture for global pose and vehicle IMU measurements for the EKF, with estimation, planning, and control executed onboard.The NMPC uses Ts = 0.04 s and horizon N = 25, a 1 s prediction interval; Table II summarizes experimental and algorithmic parameters.Source: A. Experimental Setup · p. 5, Table II · p. 5

B. Underwater Perception and Onboard Performance

• Stereo ranging is evaluated against motion-capture ground truth over reference distances from 1.8 to 2.9 m, with depth RMSE and MAE reported.The comparison includes the ideal line for error-free measurements.Source: B. Underwater Perception and Onboard… · p. 5
• The detector is tested on 1067 labeled underwater BlueROV2 images from separate training, validation, and test sequences, reporting precision, recall, mAP50, and mAP50:95.Onboard inference time and CPU, GPU, and memory utilization are reported for the Jetson Orin NX.Source: B. Underwater Perception and Onboard… · p. 5
• The final selected operating profile uses color at 1280x720 and 15 Hz and depth at 848x480 and 30 Hz; it achieves 0.26 m depth RMSE, 55.38 % valid-depth proportion, and a 15 Hz detector rate.Source: B. Underwater Perception and Onboard… · p. 5

C. State Estimation and NMPC Tracking

• During the final hold, the EKF reduces body-velocity standard deviation by approximately 74%, while motion-capture tracking losses include an interruption of approximately 1.19 s.The evaluation focuses on measurement filtering and robustness to losses rather than absolute localization accuracy.Source: C. State Estimation and NMPC Tracking · p. 5, Figure 5 · p. 6
• The docking tracking evaluation compares the measured trajectory with the commanded docking pose using the filtered state estimate.Figure 5b shows the measured trajectory and commanded pose.Source: C. State Estimation and NMPC Tracking · p. 5, Figure 5 · p. 6

D. Autonomous Docking

• The vehicle follows the time-parametrized RRT* reference at 0.10 m/s for 32.1 s, then holds the docking reference with final position and attitude errors of 0.067 m and 10.9 deg.Figure 5 includes the measured horizontal trajectory, commanded dock pose, final position, and physical docking sequence.Source: D. Autonomous Docking · p. 5, Figure 5 · p. 6

VI. CONCLUSION

• The platform combines underwater stereo perception, quaternion-based state estimation, reference planning, and 6 DoF NMPC for BlueROV2 Heavy navigation and docking.The conclusion reports experimental demonstrations of onboard relative perception, state estimation, NMPC tracking, and autonomous docking.Source: §VI · pp. 5–6
• ROS 2/PX4 software, Gazebo environments, multi-robot simulation tools, and a low-cost docking station support reproducible experimentation.The conclusion identifies sampling-based methods, safety-critical multi-robot coordination, and diver-robot interaction as future work.Source: §VI · p. 6