Trajectory Optimization of Chance-Constrained Nonlinear Stochastic Systems for Motion Planning Under Uncertainty

Abstract

We present gPC-SCP: Generalized Polynomial Chaos-based Sequential Convex Programming to compute a sub-optimal solution for a continuous-time chance-constrained stochastic nonlinear optimal control (SNOC) problem. The approach enables motion planning for robotic systems under uncertainty. The gPC-SCP method involves two steps. The first step is to derive a surrogate problem of \emph{deterministic} nonlinear optimal control (DNOC) with convex constraints by using gPC expansion and the distributionally-robust convex subset of the chance constraints. The second step is to solve the DNOC problem using sequential convex programming for trajectory generation and control. We prove that in the unconstrained case, the optimal value of the DNOC converges to that of SNOC asymptotically and that any feasible solution of the constrained DNOC is a feasible solution of the chance-constrained SNOC. We also present the predictor-corrector extension (gPC-SCP$^\mathrm{PC}$) for real-time motion trajectory generation in the presence of stochastic uncertainty. In the gPC-SCP$^\mathrm{PC}$ method, we first predict the uncertainty using the gPC method and then optimize the motion plan to accommodate the uncertainty. We empirically demonstrate the efficacy of the gPC-SCP and the gPC-SCP$^\mathrm{PC}$ methods for the following two test cases: 1) collision checking under uncertainty in actuation and physical parameters and 2) collision checking with stochastic obstacle model for 3DOF and 6DOF robotic systems. We validate the effectiveness of the gPC-SCP method on the 3DOF robotic spacecraft testbed.