A Two-Stage Motion Planning Algorithm Combining Geometric Heat Flow and Fine-Tuning

In this work, we address the problem of motion planning for control-affine systems. We first employ the geometric heat flow (GHF) method to generate an admissible trajectory that adheres to kinematic constraints and ensures obstacle avoidance. However, due to inherent limitations of GHF, the resulting trajectory’s final state may deviate from the goal state. To address this issue, we introduce a novel gradient descent algorithm for fine-tuning, which iteratively adjusts the trajectory to minimize the final state error while maintaining compliance with kinematic and obstacle avoidance constraints, and ensuring that the control effort remains reasonably small. By leveraging a novel barrier function based on superellipses during both GHF and fine-tuning stages, our motion planning algorithm reduces conservativeness in collision avoidance between rectangular objects and obstacles. We validate our approach through simulation examples and comparison with other motion planning algorithms for tractor-trailer models, confirming its efficacy and efficiency in generating intricate maneuvers for control-affine systems in complex operational spaces.