Provably-Good Approximation Algorithms for Optimal Kinodynamic Robot Motion Plans

Abstract

The kinodynamic planning problem is to synthesize a robot motion obeying simultaneous kinematic and dynamics constraints. To maximize robot performance we can consider optimal kinodynamic planning: for a given robot system, find a minimal-time trajectory that goes from a start state to a goal state, avoids obstacles by a speed-dependent safety margin, and respects the dynamics laws and dynamics bounds governing the system. In general, previous work on algorithmic motion planning does not address dynamics; furthermore, even in simple cases, finding exact globally-optimal solutions is $\cal NP$-hard. In response, we obtain provably-good, polynomial-time approximation algorithms that synthesize optimal kinodynamic trajectories. These algorithms forge new mathematical links between control theory and complexity theory, and our analysis investigates how discrete-control trajectories can approximate optimal solutions We cast optimal kinodynamic planning into the form of an $\epsilon$-approximation problem in which $\epsilon greater than 0$ characterizes closeness to optimality in terms of trajectory time, observance of the safety margin and closeness to the start and goal states If $T_{opt}$ is the time of an optimal trajectory, then an $\epsilon$-optimal trajectory takes at most (1 + $\epsilon$)$T_{opt}$ time. We present (pseudo)-polynomial-time $\epsilon$-approximation algorithms for a family of robot classes, including fully-controllable open kinematic chains. These algorithms run in time polynomial in $\frac{1}{\epsilon}$ and the geometric complexity of the constraints. The basic idea behind the algorithms is to reduce the trajectory planning problem to a shortest-path problem on a polynomial-sized reachability graph embedded in the robot state space. These graphs are generated by control primitives and a timestep that the algorithm chooses to ensure $\epsilon$- optimality. To obtain our complexity and approximation results, we introduce both continuous and combinatorial tools to analyze the robot's dynamical system. These include scaling-tracking proof methods that capture the key insight necessary for provably-good results, tracking lemmas on how closely we can approximate an optimal or time-rescaled optimal trajectory, constructive trajectory proofs, adversary game proofs and Time-Safety planning trade-offs. We also decribe an implementation and experiments in a restricted domain.