An Improved RRT Algorithm with Cost Function for Efficient Path Planning

The Rapidly-exploring Random Tree (RRT) algorithm offers strong probabilistic completeness and excels in exploring complex environments; however, it lacks effective goal guidance. In contrast, $A^{*}$ and $D^{*}$ algorithms exhibit strong heuristic search capabilities and clear directionality but are less effective in unknown or unstructured spaces. To leverage the strengths of both approaches, this paper proposes an improved RRT algorithm that optimizes the random sampling strategy and incorporates a goal-oriented sampling mechanism. Furthermore, a cost function is introduced to evaluate the accumulated path cost from the starting point to each newly added node, enabling the algorithm to prefer lower-cost paths and effectively avoid redundant or suboptimal branches. This enhancement significantly improves the planning efficiency, convergence speed, and path quality, while maintaining the probabilistic completeness of the original RRT. Simulation experiments demonstrate that the proposed algorithm outperforms the traditional RRT in both planning performance and practical applicability.