Pose-Smoothing A* Path Planning Method for Three-Dimensional Environments

Abstract

The traditional A* algorithm ensures global optimality under admissible heuristics but ignores pose continuity in 3D, causing abrupt orientation changes. We extend A* with quaternion-based pose modeling, dynamic weighting, and Slerp refinement. The node state combines position and pose, while the heuristic adaptively balances distance and orientation. A coarse-to-fine strategy rapidly generates an approximate path and then refines poses using Slerp. MATLAB and ROS validations demonstrate smoother trajectories and near-zero terminal error at moderate computational cost. Although the algorithm no longer strictly satisfies admissibility, it achieves a practical balance between efficiency and pose smoothness, providing a promising solution for UAV and robot navigation.