Navigation Stack
The USV navigates with the Nav2 framework. The Spatio-Temporal Voxel Layer (STVL) plugin adds 3D LiDAR data to the costmap, so Nav2 handles local obstacle and buoy avoidance while the mission layer only sends global waypoints. Nav2's keep-out zones are used to define areas the USV must not enter, such as the static keep-out area in the Advanced tier and the dynamic one in the Disruptive tier.
Acoustic Pinger and Lidar Localization
The USV finds the pinger buoy by combining a 4-hydrophone array with LiDAR. The hydrophones use Time Difference of Arrival (TDOA) to estimate the pinger's direction at long range, and that direction filters the LiDAR point cloud to select the correct buoy, which LiDAR then locates precisely. In simulation, the system has an error of 0.12 m and 5.59°. The same array is also used to track the UUV.
Gate Pairing and Ordering
For Safe Passage, the red and green buoys detected by the UAV are paired into gates and ordered along the direction of travel, since detection order does not match the transit sequence. Pairing uses the Hungarian algorithm, which minimizes total pairing distance and holds up well under positional noise from wave motion and UAV drift. Gates are then ordered with a sequential greedy method, which works for curved and S-shaped corridors. Each waypoint's heading is set so the USV crosses the gate perpendicular to the red–green axis, and the sequence is sent to Nav2's NavigateThroughPoses.