STD-Trees: Spatio-temporal Deformable Trees for Multirotors Kinodynamic Planning

Hongkai Ye, Chao Xu, Fei Gao

Abstract

In constrained solution spaces with a huge number of homotopy classes, stand-alone sampling-based kinodynamic planners suffer from low efficiency in convergence. Local optimization is often integrated to alleviate this problem. In this paper, we propose to enhance trajectory tree growth by optimizing the tree in the form of "deformation units," where each unit contains one tree node and all its connecting edges. This deformation proceeds both spatially and temporally by optimizing node states and edge time durations efficiently. While each unit only changes the tree locally, it improves the overall quality of the corresponding sub-tree. We study and compare several variants for deforming different tree parts, considering computational burden and optimization levels. Numerical experiments demonstrate that integrating spatio-temporal deformation significantly accelerates convergence and outperforms spatial-only deformation methods.

Introduction

Planning a path for a multirotor is a complex task that goes beyond simple geometric pathfinding. It requires kinodynamic planning, ensuring the resulting trajectory respects physical limits such as velocity, acceleration, and jerk while avoiding obstacles.

Standard sampling-based planners, such as RRT, are effective at exploring large, high-dimensional spaces but are notoriously slow to converge to an optimal solution. Because they rely on random sampling, the probability of generating a perfectly smooth, low-cost path is low, especially in environments with many "homotopy classes" (distinct ways to navigate around obstacles). While previous research has attempted to use local optimization to move tree nodes (spatial deformation), these methods often ignore the time* dimension. In kinodynamic planning, the duration between states is just as important as the states themselves for determining feasibility and efficiency.

Spatio-temporal Deformation Units

The core innovation of this work is the Spatio-temporal Deformation Unit. Instead of attempting to optimize the entire trajectory tree at once—which would be computationally prohibitive for real-time applications—the algorithm focuses on small, manageable units. A deformation unit consists of a central node, its parent edge, and all its children edges.

A deformation unit consists of a node and its connected edges. Optimizing the node state and edge durations locally improves the trajectory quality.

Key technical features of this approach include:

  • Spatio-temporal Optimization: The algorithm simultaneously optimizes the node's state (position, velocity, etc.) and the time duration ($\tau$) of the connected edges.
  • LQMT Formulation: It utilizes a Linear Quadratic Minimum Time formulation to balance travel time against energy consumption.
  • Penalty Functions: Obstacle avoidance and dynamical constraints are handled via soft penalty functions, allowing the use of efficient gradient-based solvers like the Limited Memory Bundle Method (LMBM).

Deformation Strategies

We investigated four distinct strategies for selecting which units to deform as the tree grows, each offering a different trade-off between computational cost and optimization depth:

The four deformation variants: NODE, TRUNK, BRANCH, and TREE. Each offers a different trade-off between computational cost and optimization depth.
  1. NODE: Only the newly added node is optimized.
  2. TRUNK: Optimizes the path from the new node back to the start.
  3. BRANCH: Optimizes the entire sub-tree rooted at the new node.
  4. TREE: Optimizes all nodes in the current tree.

Our experiments indicate that the BRANCH strategy provides the most effective balance. It focuses optimization effort on the parts of the tree most likely to benefit from new information without the massive overhead of a full tree re-optimization.

Key Insights and Results

The integration of temporal optimization is a key differentiator. By allowing edge durations to vary, the planner can find smoother, more efficient paths that spatial-only methods might miss. Numerical simulations in dense "random forest" and city-like environments demonstrate that STD-Trees:

  • Converge Faster: Initial solutions are found in milliseconds, and the cost reduces much more rapidly than in baseline kRRT or search-based methods like weighted A.
  • Produce Better Organized Trees: The local deformations "thrive" the tree, leading to a more structured exploration of the environment and higher-quality final trajectories.
  • Outperform Spatial-Only Baselines: Spatio-temporal optimization consistently yields lower-cost paths than methods that only adjust node positions.

Conclusion

STD-Trees offer a robust way to bridge the gap between the global exploration of sampling-based planners and the local efficiency of optimization-based methods. By treating time as a first-class citizen in the optimization process, we achieve significantly faster convergence for multirotor kinodynamic planning in complex, constrained spaces.

Summary updated

Abstract

In constrained solution spaces with a huge number of homotopy classes, stand-alone sampling-based kinodynamic planners suffer low efficiency in convergence. Local optimization is integrated to alleviate this problem. In this paper, we propose to thrive the trajectory tree growing by optimizing the tree in the forms of deformation units, and each unit contains one tree node and all the edges connecting it. The deformation proceeds both spatially and temporally by optimizing the node state and edge time durations efficiently. The unit only changes the tree locally yet improves the overall quality of a corresponding sub-tree. Further, variants to deform different tree parts considering the computation burden and optimizing level are studied and compared, all showing much faster convergence. The proposed deformation is compatible with different RRT-based kinodynamic planning methods, and numerical experiments show that integrating the spatio-temporal deformation greatly accelerates the convergence and outperforms the spatial-only deformation.
BibTeX
@inproceedings{ye2023std,
  author = {Hongkai Ye and Chao Xu and Fei Gao},
  title = {{STD-Trees: Spatio-temporal Deformable Trees for Multirotors Kinodynamic Planning}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2023},
  url = {https://openreview.net/forum?id=WnOegrNHeX},
}
Figure