Kavraki Lab

A kinodynamic motion planner specifically designed for systems with complex dynamics, where physics-based simulation is necessary.

To perform motion planning for complex realistic systems, accurate modeling of these systems is necessary - a model of their behaviour under different inputs. Typically, such models are systems of equations of motion. However, such equations do not account for friction, gravity or contacts with other bodies. To account for these, which is needed for real robots, physics-based simulation for rigid body dynamics can be used instead.

Although simulating rigid body dynamics can replace equations of motion for propagating the system’s behaviour forward in time, there are a couple of issues that arise: simulation of rigid body dynamics is considerably more computationally expensive than integrating motion models and simulating backward in time is not always possible. As will be shown later, these issues can cause performance degradation even in successful existing planning methods. This work proposes KPIECE (Kinodynamic Planning by Interior-Exterior Cell Exploration), a new tree sampling-based motion planning algorithm, specifically designed for use with physics-based simulation. KPIECE is innovative in the sense that while it is able to handle high dimensional systems with complex dynamics, it reduces both runtime and memory requirements by making better use of information collected during the planning process. Intuitively, this information is used to decrease the amount of forward propagation the algorithm needs.

A projection of the state space is discretized into cells. Multiple levels of discretization are defined, as shown in the picture above, depending on the size of the cells. Cells on one level all have the same size and they are created when needed, to cover the tree of motions as it grows. The goal is to estimate the coverage of the state space by looking at the coverage of the different cells. Coarser discretizations (larger cells) are used to grossly identify areas of the space that are less covered. Finer discretizations (smaller cells) can be subsequently used to more accurately identify less covered areas, within the grossly identified ones. In addition, cells are distinguished into interior and exterior:

  • interior cells are ones that have 2n neighbors in a n-dimensional space. In the example picture, we have a 2-dimensional space, so 4 neighbors are needed. Notice this is the maximum number of possible neighbors (we do not consider cells on the diagonals to be neighboring)
  • exterior cells are cells that cover the tree of motions but are not interior

As it is typical for sampling-based planners to spend more than 90% of their computation performing forward propagation, computational improvement over previous methods is observed.