Sequential operator splitting for constrained nonlinear optimal control

We develop TROSS, a solver for constrained trajectory optimization based on a sequential operator splitting framework. TROSS iteratively improves trajectories by solving, using the Alternating Direction Method of Multipliers (ADMM), a sequence of subproblems setup within evolving trust regions around current iterates. TROSS achieves consensus among competing objectives, such as finding low-cost dynamically feasible trajectories respecting control limits and safety constraints. A library of building blocks in the form of inexpensive and parallelizable proximal operators associated with trajectory costs and constraints can be used to configure the solver for a variety of tasks. The method shows faster cost reduction compared to iterative Linear Quadratic Regulator (iLQR) and Sequential Quadratic Programming (SQP) on a control-limited vehicle maneuvering task. We demonstrate TROSS on shortest-path navigation of a variant of Dubin's car in the presence of obstacles, while exploiting passive dynamics of the system. When applied to a constrained robust state estimation problem involving nondifferentiable nonconvex penalties, TROSS shows less susceptibility to non-Gaussian dynamics disturbances and measurement outliers in comparison to an Extended Kalman smoother. Unlike generic SQP methods, our approach produces time-varying linear feedback control policies even for constrained control tasks. The solver is potentially suitable for nonlinear model predictive control and moving horizon state estimation in embedded systems.