Provably Good Approximation Algorithms for Optimal Kinodynamic Planning for Cartesian Robots and Open Chain Manipulators

We consider the following problem: given a robot system, find a minimal-time trajectory from a start state to a goal state, while avoiding obstacles by a speed-dependent safety-margin and respecting dynamics bounds. In [CDRX] we developed a theoretical, provably good approximation algorithm for the minimum-time trajectory problem for a robot system with decoupled dynamics bounds (e.g. a point robot in). This algorithm differs from previous work in three ways. It is possible (1) to bound the goodness of the approximation by an error term $\epsilon$; (2) to polynomially bound the computational complexity of our algorithm; and (3) to express the complexity as a polynomial function of the error term. Hence, given the geometric obstacles, dynamics bounds, and the error term $\epsilon$, the algorithm returns a solution that is $\epsilon$-close to optimal and requires only a polynomial (in ($\frac{1}{\epsilon}$)) amount of time. We extend the results of [CDRX] in two ways. First, we reanalyze the [CDRX] algorithm for robots with decoupled dynamics bounds. We halve the exponent in the polynomial bounds and prove a better approximation accuracy. These new results indicate that an implementation of the theoretical algorithm could be reasonable. We report on a preliminary implementation of the extended algorithm and experiments. Second, we extend [CDRX] to $d$-link, revolute-joint 3D robots will full rigid body dynamics. Specifically, we first prove a generalized trajectory- tracking lemma for robots with coupled dynamics bounds. Then, using this result we describe polynomial-time approximation algorithms for Cartesian robots obeying $L_{2}$ dynamics bounds and open kinematic chain manipulators with revolute and prismatic joints; the latter class includes most industrial manipulators. We obtain a general $O(n^{2}$(log$n$) ($\frac{1}{\epsilon})^{6d-1})$ algorithm, where $n$ is the geometric complexity