# Asymptotically Optimal Sampling-based Kinodynamic Planning

Many state of the art algorithms for motion planning are concerned with asymptotic optimality (variants of PRM* and RRT*). One of the requirements to use these algorithms is the availability of a steering function for the system being planned for. When the problem domain includes systems with dynamic constraints, this steering function corresponds to a boundary value problem (BVP) solver, which may not be available.

One way to plan for systems with dynamics is to use a tree-like structure as in RRT. The algorithm is beneficial for systems with dynamics due to only requiring forward propagation primitives. The drawback is that RRT will always return a suboptimal solution.

To alleviate this problem, Stable Sparse-RRT (SST) uses two modifications to RRT, one which has been used in previous literature and another new modification which has beneficial properties (outlined in the following table). (Software for SST can be found here)

Property RRT-Extend (Random Propagation) RRT* SST/SST*
Completeness Probabilistically Complete Probabilistically Complete Probabilistically ∂-Robustly Complete/Probabilistically Complete
Optimality Provably Suboptimal Asymptotically Optimal Asymptotically Near-Optimal/Asymptotically Optimal
Extension Primitive Forward Propagation Steering Function Forward Propagation
Extensions Per Iteration 1 (O(log(V))) 1
Nearest Neighbor Queries 1 Nearest Neighbor Query 1 Nearest Neighbor Query + 1 K-Nearest Query 1 Nearest Neighbor Query + 1 K-Nearest Query
Data Structure Size All nodes are retained All nodes are retained Sparse Data Structure/Converges to all collision free samples.

SST follows a similar flow to RRT, with a few key differences. First, the selection process for determining which node in the tree to propagate from is different. Instead of sampling a random state and finding its nearest node in the tree, we find a set of nodes within a certain distance, ∂v. Then among those nodes, the one with the best path cost from the root is chosen for propagation.

Next, another criterion must be satisfied to allow a newly propagated state to be added to the tree. We must see if there are any other states in the vicinity with a better path cost. This is accomplished through the use of an auxiliary point set, S. Each point in this set is mapped one-to-one with a node in the tree. When a new state is generated, the closest point in S is found, and the path cost of its mapped node in the tree is compared with the newly generated node. If the old node is better, the new node is disregarded. If the new node is better, the old node is marked for deletion (moving it to the V_inactive set) and removed from the distance metric that performs nearest neighbor queries. In the event that the closest point in the auxiliary set is farther then a certain distance, ∂s, then a new point is added to S which is the same as the newly propagated state.

In the following example videos, a comparison between RRT, RRT*, and SST are shown. In the cases where RRT* is not appropriate, a version of RRT* that employs a shooting method is used for comparison.

##### Kinematic System

In this example, the trees for a simple point system are shown evolving over time. This comparison is used to show that SST can provide improving path quality over time. It is important to notice that in cases where a steering function is available (like in this example), RRT* provides some benefits over SST. However, in most cases, where steering functions are not available, the use of RRT* is impossible, whereas SST and SST* can still provide increasingly higher quality paths over time.

##### Inverted Pendulum

Here the goal is to swing a pendulum from a resting position to an upright balancing position. Here the comparison is shown using the phase space, where the x-axis is the pendulum's angle (-3.14, 3.14), and the y-axis is the pendulums rotational velocity (-1,1). Shown below are runs from RRT and Sparse-RRT, the predecessor to SST.

These phase space (theta and theta_dot) plots are for an inverted pendulum. Path costs for every node in the tree are denoted with shaded regions from light (high cost) to dark (low cost).

Here the goal is to swing the acrobot from a resting position to an upright balancing position while avoiding obstacles.

##### Cart-Pole

The goal for the cart-pole system is to move from the leftmost position to the rightmost position while avoiding obstacles.

The quadrotor example requires movement through two openings in two walls in order to reach the destination. Here is a comparison of RRT with Sparse-RRT.

##### Physically-simulated Car

Because SST does not require a solution to the BVP and only used forward propagation, SST can be used in scenarios such as this where a physics engine provides the evolution of the system. The video below shows the resulting tree after running SST for 3 minutes. RRT was not able to find a solution in 3 minutes in this environment, so SST's sparseness provides benefits in this domain.

##### Planning with Physics-based Simulation

Traditional motion planning computes collision-free, continuous paths for free-floating bodies. Although a challenging geometric problem, modern graph-based search algorithms that employ sampling solve difficult and high-dimensional instances. The problem is more complex, however, for systems with differential constraints. Such constraints arise in non-holonomic vehicles that are under-actuated or in general when the control influence of a system is small compared to momentum. The challenge is that not every collision-free path is acceptable; it must also be feasible given the constraints. Additionally, the dynamic model may not be available, but simulated by a software package, like a physics engine.

Search-based techniques for kinodynamic planning, inspired by a dynamic programming approach, explore the entire state space for trajectories that respect the differential constraints. A major advantage is that they are applicable to a variety of different systems. They construct a “reachability tree” in the state space through a sampling operation. Although effective in eventually solving hard planning instances, they have high computational requirements as search methods, especially in kinodynamic problems that are typically higher-dimensional compared to geometric ones.

Our work paper focuses on reducing the solution time of search-based techniques for kinodynamic planning. For instamce, we have developed a general framework for balancing greedy and methodical search while providing guarantees that eventually every problem can be solved. In simple parts of the state space the exploration is greedily guided by the heuristic. In constrained parts, such as narrow passages, the heuristic may not be beneficial and the algorithm automatically explores alternative routes for a solution. This methodical behavior is a result of an adaptive state-space subdivision scheme that estimates online the algorithm’s performance in exploring the entire state space.

Experimental comparisons on physically simulated systems between the general framework and uninformed planners as well as with informed alternatives show that framework outperforms the alternatives. The approach also reports better quality paths in certain complicated workspaces, as in maze-like environments.

The following videos provide illustrative examples of motion planning with the proposed framework and physics-based simulation:

##### Acknowledgements

This work has been supported by NSF CNS 0932423. Any opinions and findings expressed in this paper are those of the authors and do not necessarily reflect the views of the sponsor.

Faculty: