Accelerating Kinodynamic RRT* Through Dimensionality Reduction

Dongliang Zheng, Panagiotis Tsiotras

This paper, presented at the 2021 IEEE International Conference on Robotics and Automation (ICRA), introduces a method to significantly speed up motion planning for robots with complex physics. By combining optimal control theory with randomized search, the authors demonstrate how to navigate high-dimensional spaces much faster than traditional methods.

TL;DR

The authors propose Kino-RRT*, an algorithm that accelerates kinodynamic motion planning by reducing the dimensionality of the sampling space. By using a "partial-final-state-free" (PFF) optimal controller, the system only needs to sample a subset of the robot's state (like position), while the controller automatically determines the mathematically optimal values for the remaining dimensions (like velocity).

What Problem Does This Paper Address?

Motion planning for robots isn't just about finding a path; it's about finding a path the robot can actually follow. This is called kinodynamic planning. Standard algorithms like RRT* (Rapidly-exploring Random Trees) work well for simple geometry, but they struggle when the robot has complex dynamics (e.g., a drone that can't stop instantly).

The "curse of dimensionality" is the main culprit. In kinodynamic RRT, you must sample the full state space*—not just where the robot is (position), but how fast it’s going (velocity) and its orientation. For a 10-dimensional system like a quadrotor, the odds of randomly sampling the "perfect" velocity to reach a specific goal are incredibly low. This leads to slow convergence and inefficient trajectories. Furthermore, connecting two points in state space requires solving a Two-Point Boundary Value Problem (TPBVP), which is computationally expensive.

Methods

The authors introduce two primary innovations to solve these bottlenecks:

  1. Partial-Final-State-Free (PFF) Optimal Controller: Instead of sampling a full state (e.g., $x, y, \dot{x}, \dot{y}$), the algorithm only samples a "partial" state (e.g., just position $x, y$). It then uses a PFF controller to find the trajectory that connects the current node to that position while automatically choosing the final velocity that minimizes the total cost. This effectively reduces the dimension of the search space.

  2. Delayed and Intermittent Updates: Solving for the "optimal arrival time" between two states usually requires finding the roots of a high-order polynomial—a slow process. The authors propose growing the tree using a simple heuristic for time (like average speed) and only intermittently updating the edges with the true optimal arrival time. This "lazy" approach saves massive amounts of computation during the initial tree growth.

The paper provides analytical solutions for these controllers specifically for linear and linearized systems, ensuring the "steering" part of the algorithm remains very fast.

Datasets

The researchers evaluated their algorithm on two simulated benchmarks:

  • 2D Double Integrator: A 4-D state space problem where a point mass must navigate a 2D plane with circular obstacles.
  • Linearized Quadrotor: A 10-D state space problem involving 3D position, velocity, orientation (roll/pitch), and angular velocity.

These scenarios are standard in the motion planning community for testing how algorithms handle both obstacles and differential constraints.

Key Insights and Ideas

  • Smart Sampling vs. Random Sampling: Randomly sampling velocity is often a waste of time. The PFF controller acts as a "smart" sampler; it says, "If you want to go to this position, here is the mathematically best velocity to have when you get there."
  • Dimensionality Reduction: By letting the controller "fill in the blanks" for certain state dimensions, the algorithm searches a lower-dimensional manifold, which is exponentially easier to cover with random samples.
  • Optimization can be "Lazy": You don't need a perfect, time-optimal trajectory to build a useful tree. By delaying the expensive root-finding math, the algorithm finds an initial (sub-optimal) solution much faster, which can then be refined later.

Key Results

  • 50x Speedup: In the 2D double integrator tests, Kino-RRT was more than 50 times faster than standard kinodynamic RRT at reaching a specific trajectory cost.
  • Rapid Convergence: For the 2D case, Kino-RRT found a high-quality solution in just 0.14 seconds, whereas the standard version took 8 seconds* to reach a comparable cost.
  • High-Dimensional Success: In the 10-D quadrotor environment, the PFF approach significantly outperformed the baseline, proving that the dimensionality reduction technique scales well to more complex robots.

Limitations and Future Work

  • Linearity Assumption: The analytical solutions provided rely on the system being linear or linearized. While the authors mention that numerical solvers could extend this to nonlinear systems, the computational cost would likely increase.
  • Heuristic Sensitivity: The "delayed update" method depends on having a reasonable initial heuristic for arrival time. If the heuristic is very poor, the initial tree structure might be inefficient.
  • Obstacle Interaction: Because the PFF controller chooses the "free" states to minimize cost, it might occasionally choose a state that leads to a collision in very cramped environments, though the authors' results suggest the sampling-based nature of the algorithm generally overcomes this.

Future work could involve applying this PFF framework to fully nonlinear systems or integrating it with machine learning-based steering functions.

Summary updated

Abstract

Sampling-based motion planning algorithms such as RRT* are well-known for their ability to quickly find an initial solution and then converge to the optimal solution asymptotically. However, the convergence rate can be slow for highdimensional planning problems, particularly for dynamical systems where the sampling space is not just the configuration space but the full state space. In this paper, we introduce the idea of using a partial-final-state-free (PFF) optimal controller in kinodynamic RRT* [1] to reduce the dimensionality of the sampling space. Instead of sampling the full state space, the proposed accelerated kinodynamic RRT*, called Kino-RRT*, only samples part of the state space, while the rest of the states are selected by the PFF optimal controller. We also propose a delayed and intermittent update of the optimal arrival time of all the edges in the RRT* tree to decrease the computation complexity of the algorithm. We tested the proposed algorithm using 4-D and 10-D state-space linear systems and showed that Kino-RRT* converges much faster than the kinodynamic RRT* algorithm.
BibTeX
@inproceedings{zheng2021accelerating,
  author = {Dongliang Zheng and Panagiotis Tsiotras},
  title = {{Accelerating Kinodynamic RRT* Through Dimensionality Reduction}},
  booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
  year = {2021},
  doi = {10.1109/iros51168.2021.9636754},
}
Figure