Real-Time Kinodynamic Planning

We have developed an algorithmic framework that allows for real-time motion planning/obstacle avoidance for high-speed robotic systems, such as quadrotors. This framework, which addresses a critical need in many cutting edge robotic technologies, is enabled by a novel application of machine learning algorithms to estimate cost-limited reachable sets for dynamical systems. As a validating test, we've implemented the framework on an autonomous quadrotor and tasked it with avoiding high speed obstacles (such as a fencing blade) while navigating to objectives in an indoor environment. This stands  as, arguably, the first demonstration of truly online motion planning for a quadrotor system.
 

Conceptual examples of kinodynamic planning problems


What is Kinodynamic planning?

Essentially any time a robot moves through the world to accomplish a task while avoiding obstacles, it is solving a path planning problem. If that robot's motion is described by kinematic or differential equations, then it is solving a kinodynamic planning problem.
 
These types of problems lie at the heart of many cutting edge technologies. From autonomous cars steering through traffic, to aerial drones that will deliver packages in dense urban environments, to robotic spacecraft delicately docking with a space station, the need for effective, real-time kinodynamic planners is ever-present.


The Real-Time Kinodynamic Planning Algorithm

Unconstrained planning problems, often referred to as “geometric problems” where the robot can translate and/or rotate arbitrarily (think of the Roomba robot driving around on the floor), have been studied for decades. Real-time solutions to such problems are achievable by using sampling-based motion planning algorithms.
 
Kinodynamic planning problems, on the other hand, are still considered an 'open problem' when it comes to real-time solutions. The difference between the geometric problem and the kinodynamic problem is in how the robot travels from point to point. For a geometric robot, this path is usually a straight line. When a kinodynamic robot traverses between two points, it must solve a boundary value problem. Solving such a boundary value problem can be quite computationally expensive, on the order of a few tenths of a second. For sampling-based motion planning we may require the solution to hundreds-of-thousands or millions of such boundary value problems. With each BVP requiring a few tenths of a second to compute, the question becomes: how can we perform all of these computations fast enough for a robot to respond to a encroaching obstacle that may collide in less than a second?
 
To address this question we've developed a real-time framework for kinodynamic planning that reduces the amount of necessary online computations by several orders of magnitude. The framework utilizes an offline-online computation paradigm, neighborhood classification through machine learning, sampling-based motion planning with an optimal cost distance metric, and trajectory smoothing to achieve real-time planning for aerial vehicles. This framework accounts for dynamic obstacles with an event-based replanning structure and a locally reactive control layer that minimizes replanning events.

A Real-Time Framework for Kino Dynamic Motion Planning


Timelapse of AgileQuad navigating maze of static obstacles

Tech Demo: Fencing a Quadrotor

We've implemented the kinodynamic planning framework on the AgileQuad test platform at Stanford's Autonomous Systems Laboratory. The framework has been successfully demonstrated real-time planning for static obstacles, low-speed dynamic obstacles, and even high-speed, adversarial obstacles.