A successive approximation-based approach for optimal kinodynamic motion planning with nonlinear differential constraints

This paper presents an extension to RRT* [1], a sampling-based motion planning with asymptotic optimality guarantee, in order to incorporate nonlinear differential equations in motion dynamics. The main challenge due to nonlinear differential constraints is the computational complexity of solving a two-point boundary-value problem that arises in the tree expansion process to optimally connect two given states. This work adapts the successive approximation method that transforms a nonlinear optimal control problem into a sequence of linear-quadratic-like problems to solve these TPBVPs. The resulting algorithm, termed SA-RRT*, is demonstrated to create more realistic plans compared to existing kinodynamic extensions of RRT*, while preserving asymptotic optimality.