SE(3) trajectory planning for autonomous underwater vehicles using motion primitives and hierarchical heuristic optimization

Autonomous underwater vehicles operating in confined environments such as shipwreck interiors, underwater archaeological sites, and aquaculture structures must coordinate both position and orientation to safely navigate narrow passages. Existing path planning methods model the vehicle as a point mass or a sphere, neglecting the rotational degrees of freedom critical to such tasks. This paper proposes an SE(3) trajectory planning method that extends the planning space from three-dimensional position to the pose space of position and orientation, with the attitude determined algebraically from the flat outputs. Using differential flatness to establish an algebraic mapping between control inputs and orientation, motion primitives that carry complete pose information are generated for three control input orders (velocity-level VEL, acceleration-level ACC, and jerk-level JRK), and an A* search determines the optimal primitive sequence. An ellipsoidal vehicle model enables collision detection that accounts for the vehicle attitude. Simulation results show that the planning times for VEL, ACC, and JRK control are 3.36s, 26.49s, and 270.39s, with 26, 1448, and 18,344 search nodes, respectively. These results show a clear computational bottleneck in high-dimensional planning. To address this bottleneck, a hierarchical heuristic optimization (HHO) strategy is proposed, in which a low-dimensional trajectory guides the high-dimensional search, reducing the planning time to 7.29s and the number of search nodes to 476 without noticeable degradation in trajectory quality. Trajectory tracking experiments confirm the physical feasibility of the planned trajectories, with attitude errors maintained within ± 5 ° . These results show that higher-order SE(3) trajectory planning is computationally practical for AUV missions requiring tight coordination between position and orientation.