Optimization algorithm

The optimization algorithm of GRAMPC is based on an augmented Lagrangian formulation with an inner projected gradient method as minimization step and an outer multiplier and penalty update. This section gives a brief sketch of the algorithm. Note that a more detailed description is given in [1].

Augmented Lagrangian method

GRAMPC implements the augmented Lagrangian approach to handle the equality and inequality constraints of the OCP. The constraints are adjoined to the integral cost function using the time-dependent multipliers \(\mb{\mu}= [\mb{\mu}_{\mb{g}}^\mathsf{T}, \mb{\mu}_{\mb{h}}^\mathsf{T}]^\mathsf{T}\) and penalties \(\mb{c}= [\mb{c}_{\mb{g}}^\mathsf{T},\mb{c}_{\mb{h}}^\mathsf{T}]^\mathsf{T}\). Similarly, multipliers \(\mb{\mu}_T = [\mb{\mu}_{\mb{g}_T}^\mathsf{T}, \mb{\mu}_{\mb{h}_T}^\mathsf{T}]^\mathsf{T}\) and penalties \(\mb{c}_T = [\mb{c}_{\mb{g}_T}^\mathsf{T}, \mb{c}_{\mb{h}_T}^\mathsf{T}]^\mathsf{T}\) are used for the terminal constraints. Where appropriate, the syntax \(\mb{\bar \mu} = (\mb{\mu}_{\mb{g}}, \mb{\mu}_{\mb{h}}, \mb{\mu}_{\mb{g}_T}, \mb{\mu}_{\mb{h}_T})\) and \(\mb{\bar c} = (\mb{c}_{\mb{g}}, \mb{c}_{\mb{h}}, \mb{c}_{\mb{g}_T}, \mb{c}_{\mb{h}_T})\) is used to denote all multipliers and penalties. The algorithm requires a reformulation of the inequality constraints that leads to the transformed functions (see [1] for details)

\[\begin{split}\mb{\bar h}(\mb{x}, \mb{u}, \mb{p}, t, \mb{\mu}_{\mb{h}}, \mb{c}_{\mb{h}}) &= \mb{\max}\left\{ \mb{h}(\mb{x}, \mb{u}, \mb{p}, t), -\mb{C}_{\mb{h}}^{-1} \mb{\mu}_{\mb{h}} \right\} \\ \mb{\bar h}_T(\mb{x}, \mb{p}, T, \mb{\mu}_{\mb{h}_T}, \mb{c}_{\mb{h}_T}) &= \mb{\max}\left\{ \mb{h}_T(\mb{x}, \mb{p}, T), -\mb{C}_{\mb{h}_T}^{-1} \mb{\mu}_{\mb{h}_T} \right\}\end{split}\]

with the component-wise max-function and the diagonal matrix syntax \(\mb{C}= {\rm diag}(\mb{c})\). The augmented Lagrangian function is defined as

(3)\[\bar J(\mb{u}, \mb{p}, T, \mb{\bar \mu}, \mb{\bar c};\mb{x}_0) = \bar V(\mb{x}, \mb{p}, T, \mb{\mu}_T, \mb{c}_T) + \int_0^T \bar l(\mb{x}, \mb{u}, \mb{p}, t, \mb{\mu}, \mb{c}) \, \mathrm dt\]

with the augmented terminal cost term

\[\begin{split}\bar V(\mb{x}, \mb{p}, T, \mb{\mu}_T, \mb{c}_T) =\,& V(\mb{x}, \mb{p}, T) + \mb{\mu}_{\mb{g}_T}^\mathsf{T}\, \mb{g}_T(\mb{x}, \mb{p}, T) + \frac12 \| \mb{g}_T(\mb{x}, \mb{p}, T) \|^2_{\mb{C}_{\mb{g}_T}} \nonumber\\ &+ \mb{\mu}_{\mb{h}_T}^\mathsf{T}\, \mb{\bar h}_T(\mb{x}, \mb{p}, T, \mb{\mu}_{\mb{h}_T},\mb{c}_{\mb{h}_T}) + \frac12 \| \mb{\bar h}_T(\mb{x}, \mb{p}, T, \mb{\mu}_{\mb{h}_T},\mb{c}_{\mb{h}_T}) \|^2_{\mb{C}_{\mb{h}_T}}\end{split}\]

and the augmented integral cost term

\[\begin{split}\bar l(\mb{x}, \mb{u}, \mb{p}, t, \mb{\mu}, \mb{c}) =\,& l(\mb{x}, \mb{u}, \mb{p}, t) + \mb{\mu}_{\mb{g}}^\mathsf{T}\, \mb{g}(\mb{x}, \mb{u}, \mb{p}, t) + \frac12 \| \mb{g}(\mb{x}, \mb{u}, \mb{p}, t) \|^2_{\mb{C}_{\mb{g}}} \nonumber\\ &+ \mb{\mu}_{\mb{h}}^\mathsf{T}\, \mb{\bar h}(\mb{x}, \mb{u}, \mb{p}, t, \mb{\mu_h},\mb{c_h}) + \frac12 \| \mb{\bar h}(\mb{x}, \mb{u}, \mb{p}, t, \mb{\mu_h},\mb{c_h}) \|^2_{\mb{C}_{\mb{h}}} \,.\end{split}\]

Instead of solving the original problem, the algorithm solves the max-min-problem

(4)\[\begin{split}\max_{\mb{\bar\mu}} \, \min_{\mb{u}, \mb{p}, T} \quad& \bar{ J}(\mb{u}, \mb{p}, T, \mb{\bar\mu}, \mb{\bar c}; \mb{x}_0) \\ \textrm{s.t.} \quad& \mb{M} \mb{\dot x}(t) = \mb{f}(\mb{x}, \mb{u}, \mb{p}, t) \,,\quad \mb{x}(0) = \mb{x}_0 \\ & \mb{u}(t) \in [\mb{u}_{\min}, \mb{u}_{\max}] %\,,\quad t \in [0, T] \,,\quad t\in[0,T] \\ & \mb{p} \in [\mb{p}_{\min}, \mb{p}_{\max}] \,,\quad T \in [T_{\min}, T_{\max}] \,,\end{split}\]

whereby the augmented Lagrangian function \(\bar{J}\) is maximized with respect to the multipliers \(\mb{\bar \mu}\) and minimized with respect to the controls \(\mb{u}\), the parameters \(\mb{p}\) and the end time \(\mb{T}\). Note that the full set of optimization variables \((\mb{u},\mb{p},T)\) is considered in what follows for the sake of completeness. The max-min-problem (4) corresponds to the dual problem of (1) in the case of \(\mb{\bar c} = \mb{0}\). The maximization step is performed by steepest ascent using the constraint residual as direction and the penalty parameter as step size. See [1] for a detailed description of the augmented Lagrangian algorithm.

Projected gradient method

GRAMPC uses a projected gradient method to solve the inner minimization problem subject to the dynamics \(\mb{f}(\mb{x}(t), \mb{u}(t), \mb{p}, t)\) as well as the box constraints \(\mb{u}(t) \in \left[\mb{u}_{\min}, \mb{u}_{\max}\right]\) and \(\mb{p} \in \left[\mb{p}_{\min}, \mb{p}_{\max}\right]\). The algorithm is based on the first-order optimality conditions that can be compactly stated using the Hamiltonian

\[H(\mb{x}, \mb{u}, \mb{p}, \mb{\lambda}, t, \mb{\mu}, \mb{c}) = \bar{ l}(\mb{x}, \mb{u}, \mb{p}, t, \mb{\mu}, \mb{c}) + \mb{\lambda}^\mathsf{T}\mb{f}(\mb{x}, \mb{u}, \mb{p}, t)\]

with the adjoint states \(\mb{\lambda}\). The canonical equations are then given by

(5)\[ \begin{align}\begin{aligned}\mb{M}\mb{\dot x} &= \mb{f} (\mb{x}, \mb{u}, \mb{p}, t) \,, &\mb{x}(0) &= \mb{x}_0 \,,\\\mb{M}^\mathsf{T}\mb{\dot \lambda} &= -H_{\mb{x}}(\mb{x}, \mb{u}, \mb{p}, \mb{\lambda}, t, \mb{\mu}, \mb{c}) \,, &\mb{M}^\mathsf{T}\mb{\lambda}(T) &= \bar{ V}_{\mb{x}}(\mb{x}(T), \mb{p}, T, \mb{\mu}_T, \mb{c}_T)\end{aligned}\end{align} \]

consisting of the original dynamics \(\mb{\dot x}\) and the adjoint dynamics \(\mb{\dot \lambda}\). The canonical equations can be iteratively solved in forward and backward time for given initial values of the optimization variables. In each iteration and depending on the optimization variables of the actual problem to be solved, the gradients

\[\begin{split}\mb{d}_{\mb{u}} &= H_{\mb{u}}(\mb{x}, \mb{u}, \mb{p}, \mb{\lambda}, t, \mb{\mu}, \mb{c}) \\ \mb{d}_{\mb{p}} &= \bar V_{\mb{p}}(\mb{x}(T), \mb{p}, T, \mb{\mu}_T, \mb{c}_T) + \int_0^T H_{\mb{p}}(\mb{x}, \mb{u}, \mb{p}, \mb{\lambda}, t, \mb{\mu}, \mb{c}) \, {\rm d}t \\ d_T &= \bar V_T(\mb{x}(T), \mb{p}, T, \mb{\mu}_T, \mb{c}_T) + H(\mb{x}(T), \mb{u}(T), \mb{p}, \mb{\lambda}(T), T, \mb{\mu}(T), \mb{c}(T))\end{split}\]

with respect to the controls \(\mb{u}\), parameters \(\mb{p}\), and end time \(T\) are used to formulate a line search problem

\[\min_{\alpha} \bar{J} \left( \mb{\psi}_{\mb{u}} \left( \mb{u} - \alpha \mb{d}_{\mb{u}} \right), \mb{\psi}_{\mb{p}} \left( \mb{p} - \gamma_{\mb{p}} \alpha \mb{d}_{\mb{p}} \right), \psi_{T} \left( T - \gamma_{T} \alpha d_{T} \right); \mb{\bar \mu}, \mb{\bar c}, \mb{x}_0 \right)\]

with projection functions \(\mb{\psi}_{\mb{u}}\), \(\mb{\psi}_{\mb{p}}\) and \(\psi_{T}\) and, finally, to update the optimization variables according to

\[\mb{u} \leftarrow \mb{\psi}_{\mb{u}} \left( \mb{u} - \alpha \mb{d}_{\mb{u}} \right) \,,\quad \mb{p} \leftarrow \mb{\psi}_{\mb{p}} \left( \mb{p} - \gamma_{\mb{p}} \alpha \mb{d}_{\mb{p}} \right) \,,\quad T \leftarrow \psi_{T} \left( T - \gamma_{T} \alpha d_{T} \right) \,.\]

See [2][3][1] for a detailed description of the projected gradient algorithm. GRAMPC provides two methods for the approximate solution of the line search problem, which are explained in Line search.

Algorithmic structure

Algorithm 1 (Basic algorithmic structure of GRAMPC.)

Optional: Shift trajectories by sampling time \(\Delta t\)

For \(i = 1\) to \(i_{max}\) do

For \(j = 1\) to \(j_{max}\) do

If \(i > 1\) and \(j = 1\) then

Set \(\mb x^{i|j} =\mb x^{i-1}\)

else

Compute \(\mb x^{i|j}\) by forward time integration of system dynamics

Evaluate all constraints

End If

Compute \(\mb \lambda^{i|j}\) by backward time integration of adjoint system

Evaluate gradients \(\mb{d_u}^{i|j}\), \(\mb{d_p}^{i|j}\) and \(d_T^{i|j}\)

Solve line search problem to determine step size \(\alpha^{i|j}\)

Update controls \(\mb u^{i|j+1}\), parameters \(\mb p^{i|j+1}\) and end time \(T^{i|j+1}\)

If minimization is converged then

Break inner loop

End If

End For

Set \(\mb u^i = \mb u^{i|j+1}\), \(\mb p^i = \mb p^{i|j+1}\) and \(\mb T^i = \mb T^{i|j+1}\)

Compute \(\mb x^i\) by forward time integration of system dynamics

Evaluate all constraints

Update multipliers \(\mb{\bar{\mu}}^{i+1}\) and penalties \(\mb{\bar{c}}^{i+1}\)

If minimization is converged & constraint thresholds are satisfied then

Break outer loop

End If

End For

Compute cost \(J\) and norm of constraints

The basic structure of the algorithm that is implemented in the main calling function grampc_run is outlined in Algorithm 1. The projected gradient method is realized in the inner loop and consists of the forward and backward integration of the canonical equations as well as the update of the optimization variables based on the gradient and the approximate solution of the line search problem. The outer loop corresponds to the augmented Lagrangian method consisting of the solution of the inner minimization problem and the update of the multipliers and penalty parameters.

As an alternative to the augmented Lagrangian framework, the user can choose external penalty functions in GRAMPC that handle the equality and inequality constraints as “soft” constraints. In this case, the multipliers \(\mb{\bar \mu}\) are fixed at zero and only the penalty parameters are updated in the outer loop. Note that the user can set the options PenaltyIncreaseFactor and PenaltyDecreaseFactor to \(1.0\) in order to keep the penalty parameters at the initial value PenaltyMin. The single steps of the algorithm and the related options are described in more detail in the following sections.

The following options can be used to adjust the basic algorithm. The corresponding default values are listed in Table 4 in the appendix.

• MaxMultIter: Sets the maximum number of augmented Lagrangian iterations \(i_\text{max} \geq 1\). If the option ConvergenceCheck is activated, the algorithm evaluates the convergence criterion and terminates if the inner minimization converged and all constraints are satisfied within the tolerance defined by ConstraintsAbsTol.

• MaxGradIter: Sets the maximum number of gradient iterations \(j_\text{max} \geq 1\). If the option ConvergenceCheck is activated, the algorithm terminates the inner loop as soon as the convergence criterion is fulfilled.

• EqualityConstraints: Equality constraints \(\mb{g}(\mb{x}(t), \mb{u}(t), \mb{p}, t) = \mb{0}\) can be disabled by the option value off.

• InequalityConstraints: To disable inequality constraints \(\mb{h}(\mb{x}(t), \mb{u}(t), \mb{p}, t) \le \mb{0}\), set this option to off.

• TerminalEqualityConstraints: To disable terminal equality constraints \(\mb{g}_T(\mb{x}(T), \mb{p}, T) = \mb{0}\), set this option to off.

• TerminalInequalityConstraints: To disable terminal inequality constraints \(\mb{h}_T(\mb{x}(T), \mb{p}, T) \le \mb{0}\), set this option to off.

• ConstraintsHandling: State constraints are handled either by means of the augmented Lagrangian approach (option value auglag) or as soft constraints by outer penalty functions (option value extpen).

• OptimControl: Specifies whether the cost functional should be minimized with respect to the control variable \(\mb{u}\).

• OptimParam: Specifies whether the cost functional should be minimized with respect to the optimization parameters \(\mb{p}\).

• OptimTime: Specifies whether the cost functional should be minimized with respect the horizon length \(T\) (free end time problem) or if \(T\) is kept constant.

[1] (1,2,3,4)

T. Englert, A. Völz, F. Mesmer, S. Rhein, and K. Graichen. A software framework for embedded nonlinear model predictive control using a gradient-based augmented Lagrangian approach (GRAMPC). Optimization and Engineering, 20(3):769–809, 2019. doi.org/10.1007/s11081-018-9417-2. doi:10.1007/s11081-018-9417-2.

[2]

K. Graichen and B. Käpernick. A real-time gradient method for nonlinear model predictive control. In T. Zheng, editor, Frontiers of Model Predictive Control, pages 9–28. InTech, 2012.

[3]

B. Käpernick and K. Graichen. The gradient based nonlinear model predictive control software GRAMPC. In Proceedings of the European Control Conference (ECC), 1170–1175. Strasbourg (France), 2014.