Iterative Methods for Efficient Sampling-Based Optimal Motion Planning of Nonlinear Systems

TL;DR

This paper enhances the RRT* algorithm to efficiently handle nonlinear kinodynamic constraints in motion planning by introducing an affine quadratic regulator-based pseudo metric and iterative boundary value problem solvers, maintaining asymptotic optimality.

Contribution

It introduces a novel extension of RRT* that effectively manages nonlinear constraints using new distance measures and optimized segment computation methods.

Findings

Successfully handles nonlinear kinodynamic constraints.

Maintains asymptotic optimality of the original RRT*.

Validated through three numerical case studies.

Abstract

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 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 for computing 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.