Abstract-Rapidly-exploring Random Trees (RRTs) are widely used to solve large planning problems where the scope prohibits the feasibility of deterministic solvers, but the efficiency of these algorithms can be severely compromised in the presence of certain kinodynamics constraints. Obstacle fields with tunnels, or tubes are notoriously difficult, as are systems with differential constraints, because the tree grows inefficiently at the boundaries. Here we present a new sampling strategy for the RRT algorithm, based on an estimated feasibility set, which affords a dramatic improvement in performance in these severely constrained systems. We demonstrate the algorithm with a detailed look at the expansion of an RRT in a swingup task, and on path planning for a nonholonomic car.
I. INTRODUCTIONThe problem of robotic motion planning, whether for a wheeled robot, an aerial vehicle, or a quadruped, has motivated the development of powerful tools for planning in high dimensional spaces. Of these techniques, randomized sample-based planning strategies have proven particularly effective in quickly solving for a series of actions that drive the system to a desired state. Perhaps the most widely used single-query sample-based planners are those based upon Rapidly-exploring Randomized Trees (RRTs). RRTs have been used to solve a broad range of planning problems that include motion for manipulators and kinematic chains [1], [2].It is well-known that these algorithms can quickly become inefficient when planning on systems with complicated kinodynamic constraints [3], and there has been considerable work in attempting to modify the basic RRT algorithm for these situations [3], [4]. The essential symptom of this inefficiency is that nodes at the boundaries of these constraints tend to be sampled and expanded repeatedly, with little progress towards the ultimate goal.In this paper, we present a modified adaptive sampling strategy for the RRT algorithm which takes into account local reachability, as defined by differential constraints, while building the tree. The algorithm is based on the observation that sampling points at random and checking collisions is relatively cheap, but adding extra nodes to the tree (through the Extend algorithm) is relatively more expensive -both in its instantaneous cost and in the added cost of having a larger tree. Therefore, we attempt to build sparse trees through kinodynamic obstacles, with a simple heuristic that quickly throws away random samples that otherwise would not have the effect of extending the tree into previously unexplored regions of state space. This effectively changes the sampling distribution, so that points are selected uniformly from a