Final-state constrained optimal control via a projection operator approach

In this paper we develop a numerical method to solve nonlinear optimal control problems with final-state constraints. Specifically, we extend the PRojection Operator based Netwon's method for Trajectory Optimization (PRONTO), which was proposed by Hauser for unconstrained optimal control problems. While in the standard method final-state constraints can be only approximately handled by means of a terminal penalty, in this work we propose a methodology to meet the constraints exactly. Moreover, our method guarantees recursive feasibility of the final-state constraint. This is an appealing property especially in realtime applications in which one would like to be able to stop the computation even if the desired tolerance has not been reached, but still satisfy the constraints. Following the same conceptual idea of PRONTO, the proposed strategy is based on two main steps which (differently from the standard scheme) preserve the feasibility of the final-state constraints: (i) solve a quadratic approximation of the nonlinear problem to find a descent direction, and (ii) get a (feasible) trajectory by means of a feedback law (which turns out to be a nonlinear projection operator). To find the (feasible) descent direction we take advantage of final-state constrained Linear Quadratic optimal control methods, while the second step is performed by suitably designing a constrained version of the trajectory tracking projection operator. The effectiveness of the proposed strategy is tested on the optimal state transfer of an inverted pendulum.