Trajectory Planning for Unmanned Surface Vehicles in Complex Environments via CasADi-Based Optimal Control

Abstract

Autonomous trajectory planning for unmanned surface vehicles (USVs) in complex environments must ensure collision safety, kinematic feasibility, and smooth motion. This paper presents an optimal-control trajectory optimization method implemented in CasADi and solved with the IPOPT interior-point algorithm. The USV is represented by a simplified planar kinematic model with position, heading, and surge speed as states. Longitudinal acceleration and yaw rate are used as the control variables. Obstacle avoidance is modeled through squared-distance inequality constraints for circular obstacles inflated by safety radii. The planning problem is formulated as a finite-horizon nonlinear program with objectives for terminal accuracy, control effort, control smoothness, and path compactness. By optimizing path generation and kinematic feasibility in a single problem, the formulation avoids the decoupling common in staged planning pipelines. Simulations in five scenarios, including single static, multiple static, narrow-corridor, single dynamic, and multi-dynamic obstacle cases, validate the method. Across all scenarios, the optimizer achieved sub-millimeter terminal errors while maintaining positive obstacle clearances. Parameter sensitivity analysis shows a predictable trade-off between safety margin and path length, and ablation experiments quantify each objective term’s contribution. These results indicate that the proposed formulation can generate safe, feasible, and smooth USV trajectories in representative complex environments.