An Efficient Navigation Algorithm for Unmanned Surface Vehicles in Dynamic Environments

Abstract

Unmanned surface vehicles (USVs) are increasingly being deployed for tasks in complex maritime environments such as harbors, coastal monitoring, and port patrols. Safe navigation in congested, dynamic waters requires an integrated approach to perception, mapping, path planning, and motion control. In this paper, we present a fully integrated navigation system for a catamaran-style USV operating in a busy harbor scenario. The proposed system closes the loop from environmental sensing to control actuation. It fuses data from GPS, IMU, 3D LiDAR, and camera sensors to perform simultaneous localization and mapping (SLAM) and obstacle detection in real time. A tightlycoupled LiDAR-inertial odometry algorithm (LIO-SAM) provides robust localization within a feature-rich environment, while an Extended Kalman Filter (EKF) combines GPS/IMU data for improved odometry. For path planning, a global planner based on the A* algorithm computes efficient routes on an occupancy map, and a local planner using the Dynamic Window Approach (DWA) refines the trajectory to avoid dynamic obstacles. A twoloop PID controller executes smooth motion control of the USV’s linear and angular velocities. We validate the system in a high fidelity Gazebo simulation of a crowded harbor, demonstrating that the USV can safely navigate to target waypoints while avoiding static and moving obstacles. The results highlight that the integration of SLAM with global A* planning and local DWA obstacle avoidance significantly enhances autonomous navigation capabilities. The proposed framework emphasizes system-level efficiency and is
extensible to incorporate advanced vision-based perception (e.g., water segmentation and object detection) to further improve safety in dynamic environments. This work contributes an efficient navigation architecture for USVs and provides insights for future field deployment in real maritime environments.