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

Kai Mi, Jun Zheng, Yunkuan Wang, Jianhua HuIEEE Access 2019 · OpenReview

Abstract

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.

Introduction

Path planning for high-dimensional robotic manipulators in cluttered environments, particularly those containing narrow passages, remains a significant challenge in robotics. While sampling-based planners like Rapidly-exploring Random Trees (RRT) are computationally efficient, they often produce inconsistent and suboptimal paths. Conversely, search-based planners like A* offer consistency and high path quality but are prone to "heuristic traps."

A heuristic trap occurs when the mathematical guess (the heuristic) leading the robot toward the goal is misleading—for example, pointing the robot toward a wall because the goal is just on the other side, even though the actual path requires moving away from the goal first. Standard algorithms can "stagnate" in these regions, wasting massive amounts of time exploring dead ends.

Stagnation Detection and SD-SMHA*

To address search stagnation, researchers have previously proposed Shared Multi-Heuristic A (SMHA), which utilizes multiple inadmissible (backup) heuristics to escape local minima. However, running multiple heuristics simultaneously increases the search burden even when the primary (anchor) heuristic is performing well.

The proposed SD-SMHA (Stagnation Detection Shared Multi-Heuristic A) introduces a dynamic activation mechanism. The algorithm initially relies on a single consistent anchor heuristic. It only activates additional inadmissible heuristics when it detects that the search has stagnated.

Heuristic value depressions indicating search stagnation regions where the algorithm ceases to make significant progress.

Stagnation is mathematically defined as a "depression" in the heuristic cost curve. As shown in Figure 4, regions where the heuristic value fails to decrease over a window of expanded nodes indicate that the search is stuck in a local minimum. By defining the depth and width of these depressions, the authors created a robust trigger for switching search strategies.

Algorithm 2: The logic for stagnation detection, which monitors heuristic progress over a rolling window of expanded nodes.

The detection logic (Algorithm 2) maintains a rolling window of expanded nodes. If the minimum heuristic value does not decrease by a certain threshold over that window, stagnation is flagged. This allows the algorithm to be "lazy," only using complex backup heuristics when necessary, thereby reducing computational overhead.

Experimental Evaluation

The SD-SMHA* algorithm was evaluated in simulated environments using a 6-DOF Denso robot arm across various tasks, including a cluttered desktop scene and a narrow shelf scene.

Key Results

  • Success Rate: In the challenging narrow shelf task, SD-SMHA achieved a 90.5% success rate, a dramatic improvement over the 20.6% success rate of standard Weighted A.
  • Path Quality: SD-SMHA produced paths that were significantly shorter than those from sampling-based methods like RRT-Connect (approx. 9.36 units vs 16.42 units*).
  • Efficiency: When the primary heuristic was correct, SD-SMHA was nearly as fast as Weighted A and much faster than standard SMHA. In some tests, it reduced the number of expanded states by nearly 20% compared to standard SMHA.

Conclusion

SD-SMHA* effectively balances the high path quality of search-based methods with the robustness needed for cluttered environments. By dynamically activating heuristics only when stagnation is detected, it reduces computational costs without sacrificing the ability to navigate narrow passages. Future work will explore integrating any-angle path planning and task-specific motion primitives to further optimize performance in increasingly complex robotic environments.

Summary updated

Abstract

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.
BibTeX
@inproceedings{mi2019multi,
  author = {Kai Mi and Jun Zheng and Yunkuan Wang and Jianhua Hu},
  title = {{A Multi-Heuristic A* Algorithm Based on Stagnation Detection for Path Planning of Manipulators in Cluttered Environments}},
  booktitle = {IEEE Access},
  year = {2019},
  url = {https://openreview.net/forum?id=S7u-1gepBOH},
}
Figure