Iterative methods for efficient sampling-based optimal motion planning of nonlinear systems

This paper extends the RRT* algorithm, a recently developed but widely used sampling based optimal motion planner, in order to effectively handle nonlinear kinodynamic constraints. Nonlinearity in kinodynamic differential constraints often leads to difficulties in choosing an appropriate distance metric and in computing optimized trajectory segments in tree construction. To tackle these two difficulties, this work adopts the affine quadratic regulator-based pseudo-metric as the distance measure and utilizes iterative two-point boundary value problem solvers to compute the optimized segments. The proposed extension then preserves the inherent asymptotic optimality of the RRT* framework, while efficiently handling a variety of kinodynamic constraints. Three numerical case studies validate the applicability of the proposed method.