Trajectory Optimization of Chance-Constrained Nonlinear Stochastic Systems for Motion Planning and Control

We present gPC-SCP: Generalized Polynomial Chaos-based Sequential Convex Programming method to compute a sub-optimal solution for a continuous-time chance-constrained stochastic nonlinear optimal control problem (SNOC) problem. The approach enables motion planning and control of robotic systems under uncertainty. The proposed method involves two steps. The first step is to derive a deterministic nonlinear optimal control problem (DNOC) with convex constraints that are surrogate to the SNOC 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 (SCP) 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 derive a stable stochastic model predictive controller using the gPC-SCP for tracking a trajectory in the presence of uncertainty. We empirically demonstrate the efficacy of the gPC-SCP method for the following three test cases: 1) collision checking under uncertainty in actuation, 2) collision checking with stochastic obstacle model, and 3) safe trajectory tracking under uncertainty in the dynamics and obstacle location by using a receding horizon control approach. We validate the effectiveness of the gPC-SCP method on the robotic spacecraft testbed.