Linear Quadratic Trajectory Optimization Problems

Unconstrained Problems

Let us start by addressing the case where dynamics of our system are linear and the cost functions quadratic $x_{0 : N}, u_{0 : N - 1} min s.t. q_{N}^{⊤} x_{N} + \frac{1}{2} x_{N}^{⊤} Q_{N} x_{N} + k = 0 \sum N - 1 ([q_{k} r_{k}]^{⊤} [x_{k} u_{k}] + \frac{1}{2} [x_{k} u_{k}]^{⊤} [Q_{k} S_{k} S_{k}^{⊤} R_{k}] [x_{k} u_{k}]) x_{k + 1} = A_{k} x_{k} + B_{k} u_{k} + c_{k}, k = 0, \dots, N - 1 x_{0} = \overset{x}{^}_{0} (2a-c)$ This problem is very similar to that of finite-horizon LQR. In fact, it differs only in the inclusion of the cross term $S_{k}$ , which can also be easily incorporated into LQR and linear terms $q_{k}$ and $r_{k}$ which can be accounted for in LQR if one considers the value function in the form $V (x_{k}, k) = p_{k}^{⊤} x_{k} + x_{k}^{⊤} P_{k} x_{k}$ . Also there is an additional term $c_{k}$ who’s utility we will show later, when discussing nonlinear trajectory optimization.

There are a few approach that can be used to solve this problem which differ mostly in their algorithmic complexity based on how the sparse structure of optimization problem is exploited.

Condensing

One way to exploit the block sparse structure is to eliminate states $x_{0 : N}$ from the problem’s decision variables. As the constraint (2b) corresponds to the evolution of a linear time-varying system, states $x_{0 : N}$ are fully determined by inputs $u_{0 : N - 1}$ and the initial state $\overset{x}{^}_{0}$ $x_{0} x_{1} x_{2} x_{N} = \overset{x}{^}_{0} = A_{0} \overset{x}{^}_{0} + B_{0} u_{0} + c_{0} = A_{1} A_{0} \overset{x}{^}_{0} + A_{1} (B_{0} u_{0} + c_{0}) + (B_{1} u_{1} + c_{1}) ⋮ = k = N - 1 \prod 0 A_{k} \overset{x}{^}_{0} + k = N - 1 \prod 1 A_{k} (B_{0} u_{0} + c_{0}) + k = N - 1 \prod 2 A_{k} (B_{1} u_{1} + c_{1}) + \dots + (B_{N - 1} u_{N - 1} + c_{N - 1}) .$ As a consequence of these eliminations, the resulting optimization problem is unconstrained and the matrix (of the quadratic term) in the objective function is dense. The algorithmic complexity of condensing is typically $O (N^{3} n_{u}^{3})$ .

Solving the KKT system

Another way to exploit the sparse structure is to solve the problem’s KKT conditions $- I - I Q_{0} S_{0} A_{0} S_{0}^{⊤} R_{0} B_{0} A_{0}^{⊤} B_{0}^{⊤} ⋱ - I - I Q_{N - 1} S_{N - 1} A_{N - 1} S_{N - 1}^{⊤} R_{N - 1} B_{N - 1} A_{N - 1}^{⊤} B_{N - 1}^{⊤} - I - I Q_{N} λ_{0} x_{0} u_{0} ⋮ λ_{N - 1} x_{N - 1} u_{N - 1} λ_{N} x_{N} = - \overset{x}{^}_{0} q_{0} r_{0} c_{0} ⋮ q_{N - 1} r_{N - 1} c_{N - 1} q_{N}$

either using sparse linear algebra routines or by manual factorizations. In both cases the algorithmic complexity of $O (N (n_{x} + n_{u})^{3})$ can be achieved.

Riccati recursion

Riccati recursion is a process of factorizing the system of KKT conditions. If we consider the KKT conditions $⋱ - I - I Q_{k} S_{k} A_{k} S_{k}^{⊤} R_{k} B_{k} A_{k}^{⊤} B_{k}^{⊤} - I - I P_{k + 1} λ_{k} x_{k} u_{k} λ_{k + 1} x_{k + 1} = - c_{k - 1} q_{k} r_{k} c_{k} p_{k + 1}$ we can step-by-step condense them into the form $[⋱ - I - I P_{k}] [λ_{k} x_{k}] = - [c_{k - 1} p_{k}]$ where $P_{k} p_{k} = Q_{k} + A_{k}^{⊤} P_{k + 1} A_{k} - (S_{k} + B_{k}^{⊤} P_{k + 1} A_{k})^{⊤} (R_{k} + B_{k}^{⊤} P_{k + 1} B_{k})^{- 1} (S_{k} + B_{k}^{⊤} P_{k + 1} A_{k}) = q_{k} + A_{k}^{⊤} (p_{k + 1} + P_{k + 1} c_{k}) - (S_{k} + B_{k}^{⊤} P_{k + 1} A_{k})^{⊤} (R_{k} + B_{k}^{⊤} P_{k + 1} B_{k})^{- 1} (r_{k} + B_{k}^{⊤} (p_{k + 1} + P_{k + 1} c_{k})) . (3a-b)$ To tackle the KKT system as a whole we start by setting $k = N - 1, P_{k + 1} = Q_{N}, p_{k + 1} = q_{N}$ and traversing the horizon backwards to $k = 0$ . The optimal inputs $u_{k}^{*}$ are given as an affine function of $x_{k}$ $u_{k}^{*} = - (R_{k} + B_{k}^{⊤} P_{k + 1} B_{k})^{- 1} (r_{k} + B_{k}^{⊤} (p_{k + 1} + P_{k + 1} c_{k})) - (R_{k} + B_{k}^{⊤} P_{k + 1} B_{k})^{- 1} (S_{k} + B_{k}^{⊤} P_{k + 1} A_{k}) x_{k} . (4)$

The name “Riccati recursion” is not incidental. In fact there is a close relationship between the factorization process we have just described and obtaining the optimal control using discrete-time finite-horizon LQR. First of all, terms $P_{k}$ and $p_{k}$ can be used to form the value function $V (x_{k}, k) = p_{k}^{⊤} x_{k} + x_{k}^{⊤} P_{k} x_{k} .$ Then if we look at (3a) we can see that it differs from the discrete-time dynamics Riccati equation only in the inclusion of the cross term $S_{k}$ . In fact if we were to include terms $q_{k}$ , $r_{k}$ , $S_{k}$ , $c_{k}$ in the problem when deriving DT-FH-LQR, we would also have terms $p_{k}$ and $P_{k}$ given by (3) and the optimal control input would take the form of (4).

The full derivation of the Riccati recursion is located here.

Constrained Problems

In addition to (2b-c) we may also add other affine equality/inequality constraints, such as $x_{N} = \overset{x}{^}_{N} u_{m i n} \leq u_{k} \leq u_{m a x} G_{x_{k}} x_{k} + G_{u_{k}} u_{k} \geq - g_{k},$ at which point the problem is still classified as a quadratic program. In this case, without the knowledge of constrained optimization method, we can either use optimization modeling languages or use sparse solvers for QP. Alternatively, if constrained optimization methods are applied, they generally form a sequence of problems similar to (2) which can be solved using the Riccati recursion.