A Multi-Heuristic A* Algorithm Based on Stagnation Detection for Path Planning of Manipulators in Cluttered Environments

We consider the problem of planning an obstacle avoidance path for manipulators in cluttered environments especially with narrow passages. Compared to sampling-based planners, heuristic search-based planners are more suitable for such environments due to the consistent heuristic guidance. In order to solve the problem of search stagnation caused by inappropriate heuristic guidance, we use the Shared Multi-Heuristic A* (SMHA*) algorithm and predefine multiple inadmissible heuristics. Meanwhile, when the consistent heuristic guidance is correct and appropriate, in order to avoid the unnecessary inadmissible heuristics to increase the search burden, we improve it by adding heuristic-based stagnation detection for each extended node and the improved algorithm is called SD-SMHA*. Only when the algorithm detects that it ceases to make significant progress towards the goal, the predefined inadmissible heuristics will be introduced. Finally, multiple simulation experiments are carried out and the results show that the improved algorithm effectively improves the planning efficiency and planning success rate in different scenarios.