Improved Soft Duplicate Detection in Search-Based Motion Planning

Nader Maray, Anirudh Vemula, Maxim LikhachevICRA 2021 · DOI · OpenReview

Abstract

Search-based techniques have shown great success in motion planning problems such as robotic navigation by discretizing the state space and precomputing motion primitives. However in domains with complex dynamic constraints, constructing motion primitives in a discretized state space is non-trivial. This requires operating in continuous space which can be challenging for search-based planners as they can get stuck in local minima regions. Previous work on planning in continuous spaces introduced soft duplicate detection which requires search to compute the duplicity of a state with respect to previously seen states to avoid exploring states that are likely to be duplicates, especially in local minima regions. They propose a simple metric utilizing the euclidean distance between states, and proximity to obstacles to compute the duplicity. In this paper, we improve upon this metric by introducing a kinodynamically informed metric, subtree overlap, between two states as the similarity between their successors that can be reached within a fixed time horizon using kinodynamic motion primitives. This captures the intuition that, due to robot dynamics, duplicate states can be far in euclidean distance and result in very similar successor states, while non-duplicate states can be close and result in widely different successors.

Introduction

In robotic motion planning, search-based methods like A typically rely on discretizing the environment. However, for robots with complex kinodynamic constraints*—such as a car with a specific turning radius or a drone with significant momentum—moving precisely between grid centers is mathematically complex.

When these planners operate in continuous space, they often encounter "local minima." Because standard heuristics (like Euclidean distance) do not account for the robot's physical constraints, the search algorithm may expand thousands of nearly identical states in a small area. Traditional "hard" duplicate detection, which prunes any previously visited state, is ineffective in continuous space. While previous "soft" detection methods used Euclidean distance to penalize nearby states, this often fails because two states can be physically close yet lead to entirely different reachable futures due to the robot's orientation or velocity.

The Subtree Overlap Metric

The core contribution of this work is a novel similarity metric called Subtree Overlap ($\eta_H$). Instead of evaluating similarity based on a robot's current position, the metric evaluates where it can go within a fixed time horizon ($H$).

Illustration of subtree overlap between two states s and s'.

If two states $s$ and $s'$ have subtrees of future reachable states that significantly overlap, they are treated as "soft duplicates." This overlap value is used to calculate a "duplicity" score. Within a Weighted A* framework, states with high duplicity have their heuristics inflated, lowering their priority and encouraging the search to explore more unique, promising regions.

To make this computationally feasible, the authors introduced HashSubtree. Since subtree overlap depends only on the relative configuration of two states and the robot's fixed dynamics, these values are precomputed for various relative positions and stored in a hash table. This allows for near-instant lookup during the planning phase, bypassing the need for expensive real-time geometric calculations.

Experimental Evaluation

The approach was evaluated on two simulated domains: a 3D planning task for a car-like robot using a unicycle model, and a 5D planning task for a UAV (drone) with constraints on linear acceleration and angular speed.

Key Results

  • Efficiency and Speed: The HashSubtree method outperformed the previous state-of-the-art (PENALTY) by a factor of 1.5x to 2x in planning time. In the 3D domain, the proposed method required significantly fewer state expansions (approx. 8,200 vs. 57,000 for the PENALTY method).
  • Reliability: Standard Weighted A* failed to solve 5D drone tasks within a 120-second limit in 97% of cases. In contrast, HashSubtree solved them in under 10 seconds on average.
  • Solution Quality: Despite the increased speed, solution costs remained comparable to or better than existing search-based methods and significantly outperformed sampling-based methods like RRT.
Planning time comparison for the 3D experiment, showing HashSubtree's performance relative to WA*, Penalty, and RRT.

Conclusion and Future Work

By prioritizing dynamics over simple distance, Subtree Overlap provides a more accurate measure of state similarity in kinodynamic planning. While highly effective, the current approach faces the "curse of dimensionality"; as the state space grows (e.g., for a 7-DOF arm), the hash table required for precomputation becomes prohibitively large. Future research will explore using function approximators, such as neural networks, to predict subtree overlap offline, potentially allowing the technique to scale to much higher-dimensional robotic systems.

Summary updated

Abstract

Search-based techniques have shown great success in motion planning problems such as robotic navigation by discretizing the state space and precomputing motion primitives. However in domains with complex dynamic constraints, constructing motion primitives in a discretized state space is non-trivial. This requires operating in continuous space which can be challenging for search-based planners as they can get stuck in local minima regions. Previous work on planning in continuous spaces introduced soft duplicate detection which requires search to compute the duplicity of a state with respect to previously seen states to avoid exploring states that are likely to be duplicates, especially in local minima regions. They propose a simple metric utilizing the euclidean distance between states, and proximity to obstacles to compute the duplicity. In this paper, we improve upon this metric by introducing a kinodynamically informed metric, subtree overlap, between two states as the similarity between their successors that can be reached within a fixed time horizon using kinodynamic motion primitives. This captures the intuition that, due to robot dynamics, duplicate states can be far in euclidean distance and result in very similar successor states, while non-duplicate states can be close and result in widely different successors.
BibTeX
@inproceedings{maray2021improved,
  author = {Nader Maray and Anirudh Vemula and Maxim Likhachev},
  title = {{Improved Soft Duplicate Detection in Search-Based Motion Planning}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2021},
  doi = {10.1109/icra46639.2022.9812206},
}
Figure