Sensitivity-based Warmstarting for Nonlinear Model Predictive Control with Polyhedral State and Control Constraints

TL;DR

This paper presents a sensitivity-based warmstarting method for nonlinear MPC with polyhedral constraints, significantly reducing computation time by predicting solution changes without strict assumptions, demonstrated on UAV control.

Contribution

It introduces a novel warmstarting strategy leveraging semiderivatives for nonlinear MPC with polyhedral constraints, avoiding constraint qualification assumptions.

Findings

Reduces MPC computation time significantly.

Applicable to systems with nonlinear dynamics and polyhedral constraints.

Validated on unmanned aerial vehicle control scenarios.

Abstract

Model predictive control (MPC) is of increasing interest in applications for constrained control of multivariable systems. However, one of the major obstacles to its broader use is the computation time and effort required to solve a possibly non-convex optimal control problem (OCP) online. This paper introduces a sensitivity-based warmstarting strategy for systems with nonlinear dynamics and polyhedral constraints with the goal of reducing the computational footprint of MPC controllers. It predicts changes in the solution of the parameterized OCP as the parameter varies, by calculating the semiderivative of the solution mapping. The main novelty of the paper is that the polyhedrality of the constraints allows us to avoid imposing any constraint qualification conditions or strict complementarity assumptions. A numerical study featuring MPC applied to unmanned aerial vehicles illustrates the proposed approach.

Equations

$$N_C(v)=\{\begin{array}{ll}\{y|y^T(w-v)\le 0\forall w\in C\}& \text{if}v\in C,\\ \varnothing & \text{otherwise,}\end{array}$$

$$K^{\circ }=\{y|⟨y,x⟩\le 0,\forall x\in K\},$$

$$K_1\pm K_2=\{z\mid z=z_1\pm z_2,z_1\in K_1,z_2\in K_2\}.$$

$$K_C(x,v)=\{y|y\in T_C(x),y^{T}v=0\}.$$

$$C=\{x|\Gamma x\le b\}.$$

$$\mathcal{A}(x)=\{i\in [1,l]|{\Gamma }_{i}x=b_i\},$$

$$\begin{array}[]{ll}K_{C}(x,v)=\{y~{}|&\Gamma_{i}y\leq 0,~{}i\in\mathcal{A}(x),~{}y^{T}v=0\},\end{array}$$

$$C_i=\{y|{\Gamma }_{j}y\le b_j,j\in [1,l]\setminus i\}$$

$$\bar{\mathcal{A}}(x,v)=\{i\in \mathcal{A}(x)|v+N_{C_i}(x)\ni 0\},$$

$$\begin{array}[]{ll}K_{C}(x,v)=\{y~{}|&\Gamma_{i}y\leq 0,~{}i\in\bar{\mathcal{A}}(x,v),~{}y^{T}v=0\\ &\Gamma_{i}y=0,~{}i\in\mathcal{A}(x)\setminus\bar{\mathcal{A}}(x,v)\},\end{array}$$

$$\underset{x,u}{\min }J(x,u)=\varphi (x_N)+\sum _{i=0}^{N-1}\ell (x_i,u_i)$$

$$x_{i+1}=f(x_i,u_i),i=0,\dots ,N-1,x_0=p,$$

$$(x_i,u_i)\in Z_i,i=1,\dots ,N-1,$$

$$u_0\in U_0,x_N\in X_N$$

$$Z_i=\{(x,u)|E_i[x_i^T{u}_i^T{]}^T\le c_i\},$$

$$X_N=\{x|E_{N}x\le c_N\},U_0=\{u|E_{0}u\le c_0\}$$

$$\underset{v}{\min }J(v)$$

$$\text{ subject to }g(p,v)=0,v\in V,$$

$$g(p,v)=\left[\begin{array}{c}{x}_1-f(p,u_0)\\ x_2-f(x_1,u_1)\\ ⋮\\ x_N-f(x_{N-1},u_{N-1})\end{array}\right],$$

$$\mathcal{L}(p,v,q)=J(v)+q^{T}g(p,v),$$

$$\text{the matrix}{\nabla }_{v}g(\bar{p},\bar{v})\in {\mathbb{R}}^{Nn\times N(m+n)}\text{ is surjective}$$

$${\nabla }_v\mathcal{L}(p,v,q)+N_V(v)\ni 0,$$

$$g(p,v)=0.$$

$$F(p,z)+N_E(z)\ni 0.$$

$$x_1-B_0{u}_0={\xi }_0,$$

$$x_{i+1}-A_i{x}_i-B_i{u}_i={\xi }_i,i=1,\dots ,N-1,$$

$$p\mapsto S(p)=\{z|F(p,z)+N_E(z)\ni 0\}.$$

$$\Vert \psi (\bar{p}+\Delta p)-\psi (\bar{p})-D\psi (\bar{p})(\Delta p)\Vert \le \epsilon \Vert \Delta p\Vert .$$

$$K(p,z):=K_E(z,F(p,z))=\{w\in T_E(z)\mid w^{T}F(p,z)=0\},$$

$$K^{+}(p,z)=K(p,z)-K(p,z)$$

$$K^{-}(p,z)=K(p,z)\cap [-K(p,z)]$$

$$\Lambda z+N_K(z)\ni r$$

$$\Lambda z+N_K(z)+{\nabla }_{p}F(\bar{p},\bar{z})\Delta p\ni 0.$$

$$w\in K^{+},\Lambda w\perp K^{-},⟨w,\Lambda w⟩\le 0⟹w=0.$$

Full text

I Introduction

In Model Predictive Control (MPC) [1, 2] control actions are computed by solving constrained optimal control problems (OCPs) in real-time. MPC can systematically handle nonlinearities and constraints, but it requires solving (possibly approximately) a potentially non-convex OCP at each sampling instance. This motivates research into advanced numerical methods to enable MPC implementation.

At each sampling instance model predictive controllers measure or estimate the system state and then solve a discrete-time OCP, using the estimated state as an initial condition in the OCP, to determine the MPC-generated control action. As a result, the OCP is parameterized by the initial state. Often the states of the system at subsequent sampling instances are close so that, if the OCP satisfies appropriate regularity conditions, the solutions of the OCPs will be close as well. If we can determine an estimate of the change of the optimal solution, we can use this information to predict the optimal solution at the next time step and then start an optimization procedure from that prediction resulting in reduced computation time. The practice of exploiting sensitivity estimates to initialize an optimization algorithm is often referred to as sensitivity based warmstarting and is closely related to continuation/homotopy or solution tracking methods.

Many early sensitivity methods, e.g., CGMRES[3], are based on continuation methods for smooth nonlinear equations and cannot directly handle inequality constraints. In [4] Zavala and Biegler proposed an advanced step strategy which exploits the following result established in [5]: if the strong second order sufficient conditions (SSOSC), linear independence constraint qualification (LICQ) and strict complementarity slackness (SCS) condition hold, then the solution mapping is continuously differentiable in a vicinity of the solution. The derivative of the solution mapping, evaluated at the solution obtained at the previous sampling instance, can be used as a predictor for the optimal solution at the next sampling instance. However, the SCS condition is difficult to satisfy at all time instants. E.g., it cannot hold when an inequality constraint changes its activity status; typically, in such cases, the solution mapping is not differentiable with respect to the parameter. A similar method, IPA-SQP [6] computes a derivative based predictor using neighboring extremals and combines it with an sequential quadratic programming (SQP) based corrector. It handles constraint (de)activation using an active set strategy and requires that the SSOSC and LICQ hold.

In [7] Zavala and Anitescu developed a path-following strategy in the framework of parameterized generalized equations (GEs) using an augmented Lagrangian corrector-only approach. They assumed the SOSSC and LICQ but relaxed the SCS assumption, allowing the active set to vary. Similar approaches were proposed in [8] using a sequential convex programming based corrector, in [9], and in [10], where a predictor and corrector are derived using tools from nonsmooth analysis. A more elaborate analysis of a path-following method for tracking solution trajectories of parameterized variational inequalities is presented in [11].

If one replaces the LICQ with the weaker Mangasarian-Fromovitz constraint qualification (MFCQ) [12] but requires a strengthened form of the SSOSC, i.e., that the SSOSC holds for all Lagrange multipliers, then the optimal solution can be shown to be directionally differentiable [13]. This is exploited in a paper by Jäschke, Yang and Biegler [14] to enhance the advanced step warmstart by relaxing the SCS assumption used in [4]. However, this method requires solving additional linear and quadratic programming problems to handle the non-uniqueness of the Lagrange multipliers. A survey on sensitivity and solution tracking methods can be found in [15].

To the best of our knowledge, all existing sensitivity based methods require a constraint qualification of some sort, e.g., [4, 7, 6, 8] require the LICQ and [14] requires the MFCQ. Constraint qualifications are difficult to verify, both a-priori and a-posteriori, because they require a very accurate estimate of the solution. Moreover, the absence of a constraint qualification can lead to numerical difficulties for most optimization algorithms; in extreme cases this can lead to failure of the optimization routine and the associated MPC controller.

In this paper, we present a novel warmstarting strategy based on a sensitivity analysis of the OCP’s parameter to solution mapping; our predictor is based on the Bouligand or B-derivative [16], also known as the semiderivative; it reduces to the standard derivative when the SSOSC, LICQ, and SC all hold. This approach requires no constraint qualification, only a numerically verifiable second order sufficient condition. In exchange, we restrict ourselves to the case where the dynamics are nonlinear but the state and control constraints are represented by convex polyhedra.

I-A Some useful mappings

This paper makes extensive use of several kinds of set valued mappings. A set-valued mapping $\mathcal{F}$ acting between $\mathbb{R}^{k}$ and $\mathbb{R}^{l}$ is denoted as {\cal F}:\mathbb{R}^{k}\;{\lower 1.0pt\hbox{\rightarrow}}\kern-12.0pt\hbox{\raise 2.8pt\hbox{\rightarrow}}\;\mathbb{R}^{l}, to distinguish it from a function $f:\mathbb{R}^{k}\to\mathbb{R}^{l}$, while its inverse is defined as $y\mapsto{\cal F}^{-1}(y)=\{x\mid y\in{\cal F}(x)\}$. Given a closed convex set $C\subseteq\mathbb{R}^{n}$, the tangent cone to $C$ at a point $x\in C$ is the set of all $v$ such that $\frac{1}{\varepsilon_{k}}(x^{k}-x)\to v$ for some x^{k}\to x,x^{k}\in C,\varepsilon_{k}{\raise 1.0pt\hbox{\scriptstyle,\searrow,}}0 the normal cone mapping of $C$ is defined as

[TABLE]

The polar of a closed, convex cone $K$ is

[TABLE]

and then the tangent cone $T_{C}(v)=N_{C}^{\circ}(v)$. The euclidean projection onto the set $C$ is denoted by $\Pi_{C}(\cdot)$ and set addition/subtraction is defined as

[TABLE]

For any $x\in C$ and $v\in N_{C}(x)$ the critical cone to $C$ at $x$ for $v$ is defined as

[TABLE]

Now suppose $C$ is polyhedral; then, by definition, there exists a matrix $\Gamma$ and a vector $b$ of appropriate dimensions such that

[TABLE]

To obtain a computationally tractable expression for inclusions of the type $y\in K_{C}(x,v)$, define the active constraint set,

[TABLE]

where $l$ is the number of rows in $\Gamma$. Then the critical cone can be expressed as

[TABLE]

see e.g., [17, Theorem 2E.3]. This can be further simplified by noting that a constraint can be deactivated if it is locally redundant with respect to the other constraints. Given an active constraint $i\in\mathcal{A}(x)$ define the polyhedral set

[TABLE]

as the set that satisfies all other constraints. Due to the constraint $y^{T}v=0$ in (6) if $v+N_{C_{i}}(x)\ni 0$, the constraint $i$ is redundant, meaning that the critical cone will remain unchanged if one were to ignore the constraint $\Gamma_{i}y\leq 0$. By defining the set of redundant constraints

[TABLE]

the critical cone (6) can be rewritten as

[TABLE]

where all the non-redundant constraints are now treated as equalities. In practice, the set $\bar{\mathcal{A}}(x,v)$ can be obtained by checking if $v=\Pi_{C_{i}}(v)$ for each $i\in\mathcal{A}(x)$ using the relationship between normal cones are projections [17]. This procedure is summarized in Algorithm 1.

II Problem formulation

We consider the following discrete-time OCP:

[TABLE]

where $N$ is a natural number denoting the discrete-time horizon, $x_{i}\in\mathbb{R}^{n},~{}u_{i}\in\mathbb{R}^{m}$, $x=(x_{1},\dots,x_{N})$ and $u=(u_{0},\dots,u_{N-1})$ are the discrete-time state and control sequences, and the initial state is regarded as a parameter $p\in\mathbb{R}^{n}$. The functions $\ell:\mathbb{R}^{n}\times\mathbb{R}^{m}\to\mathbb{R},$ $\phi:\mathbb{R}^{n}\to\mathbb{R}$, and $f:\mathbb{R}^{n}\times\mathbb{R}^{m}\to\mathbb{R}^{n}$ are assumed twice continuously differentiable everywhere for simplicity. The families of closed and convex sets $Z_{i}\subseteq\mathbb{R}^{n+m}$, $U_{0}\subseteq\mathbb{R}^{m}$ and $X_{N}\subseteq\mathbb{R}^{n}$ describe state and control constraints that may vary in time. Throughout the paper we assume that each of these sets is a polyhedral set; that is, it can be described by linear inequality constraints of the form

[TABLE]

for some matrices $E_{i}$ and vectors $c_{i}$ of compatible dimensions. We also assume that for a fixed reference value $\bar{p}$ of the parameter problem (10) has a solution $(\bar{x},\bar{u})$.

Remark 1**.**

In this paper we consider only the initial state as a parameter for brevity. The sensitivity analysis presented can be easily extended to OCPs in which the functions in the cost and state equations also depend on additional parameters, e.g., a target state or a previewed time-varying input, if the problem functions are continously differentiable with respect to the parameters. Since these parameters often enter the OCP nonlinearly, e.g., through a quadratic term in the cost function, our approach is more general than the generalized tangential predictor described in Section 5.3 of [18] which requires that the parameter enter linearly.

III Optimality and sensitivity

Problem (10) is a special case of the following parameterized optimization problem

[TABLE]

where $v=(u_{0},x_{1},u_{1}~{}\ldots,~{}u_{N-1},x_{N})\in\mathbb{R}^{j},$ $j=N(m+n)$, $J:\mathbb{R}^{j}\to\mathbb{R}$ is defined as in (10), the function $g:\mathbb{R}^{n+j}\to\mathbb{R}^{d}$, $d=nN$, is given by

[TABLE]

and where $V=U_{0}\times Z_{1}\times\ldots\times Z_{N-1}\times X_{N}$. The associated Lagrangian has the form

[TABLE]

where $q\in\mathbb{R}^{Nn}$ is the vector of the Lagrange multipliers associated with the equality constraints in (12); in the context of optimal control it is usually called the vector of costates.

Let $\bar{v}=(\bar{u}_{0},\bar{x}_{1},\bar{u}_{1},\ldots,\bar{u}_{N-1},\bar{x}_{N})$ be a local minimizer of (12) for a reference value $\bar{p}$ of the parameter. It is known, see e.g., [19, Theorem 6.14] and [20, Theorem 5.1.1], that under the constraint qualification condition

[TABLE]

the first-order necessary conditions for optimality are

[TABLE]

Noting that $\nabla_{q}{\cal L}=g$, defining $z=(v,q)$, $E=V\times\mathbb{R}^{d}$, and letting $F(p,z)=\nabla_{z}{\cal L}(p,v,q)$, we arrive at the following parameterized variational inequality (VI):

[TABLE]

It is easy to show that (15) holds for problem (10). Indeed, denoting $B_{0}=\nabla_{u_{0}}f(\bar{p},\bar{u}_{0}),$ $A_{i}=\nabla_{x_{i}}f(\bar{x}_{i},\bar{u}_{i})$, and $B_{i}=\nabla_{u_{i}}f(\bar{x}_{i},\bar{u}_{i}),i=1,\dots,N-1,$ the surjectivity of the matrix $\nabla_{v}g(\bar{p},\bar{v})$ becomes the condition that for every $\xi=(\xi_{0},\dots,\xi_{N-1})\in\mathbb{R}^{Nn}$ the system

[TABLE]

has a solution. This condition clearly holds: simply choose an arbitrary sequence $(u_{0},\dots,u_{N-1})$ and determine $(x_{1},\dots,x_{N})$ recursively.

The solution mapping of (18) is

[TABLE]

There is a well-developed theory for the properties of $S(p)$, most of which is collected in [17, Chapter 2]. We will use the following definitions:

Definition 1**.**

(Strong regularity) A set-valued mapping {\cal F}:\mathbb{R}^{k}\;{\lower 1.0pt\hbox{\rightarrow}}\kern-12.0pt\hbox{\raise 2.8pt\hbox{\rightarrow}}\;\mathbb{R}^{l} is said to be strongly regular at $\bar{x}$ for $\bar{y}$ if $\bar{y}\in{\cal F}(\bar{x})$ and the inverse ${\cal F}^{-1}$ has a Lipschitz localization around $\bar{y}$ for $\bar{y}$; that is, there exist neighborhoods $U$ of $\bar{x}$ and $V$ of $\bar{y}$ such that the truncated inverse mapping $V\ni\bar{y}\mapsto{\cal F}^{-1}(y)\cap U$ is a function which is Lipschitz continuous around $\bar{y}$.

Definition 2**.**

(Semidifferentiability) A function $\psi:\mathbb{R}^{k}\to\mathbb{R}^{l}$ is said to be semidifferentiable at $\bar{p}$ if there exists a positively homogeneous function $D\psi(\bar{p}):\mathbb{R}^{k}\to\mathbb{R}^{l}$ with the property that for every $\varepsilon>0$ there exists $\delta>0$ such that for every $\Delta p\in\mathbb{R}^{k}$ with $\|\Delta p\|\leq\delta$ one has

[TABLE]

When $D\psi(\bar{p})$ happens to be linear, then it becomes the usual (Fréchet) derivative of $\psi$ at $\bar{p}$. A semidifferentiable function is, in particular, directionally differentiable with the derivative in the direction of $h$ satisfying $\psi^{\prime}(\bar{p};h)=D\psi(\bar{p})(h)$. The opposite implication holds if the function $\psi$ is Lipschitz continuous. For more about semidifferentiable functions, including examples, see e.g. [17, Section 2D].

The critical cone for (18) at $(p,z)\in\mathop{\rm gph}\nolimits S$ is

[TABLE]

see Section I-A for more information on critical cones. Accordingly, the subspace defined as

[TABLE]

is the smallest subspace that includes the critical cone, while the subspace

[TABLE]

is the largest subspace that is included in $K(p,z)$.

Using these concepts, we state the following theorem, which is a compilation of [17, Theorems 2E.6, 2E.8], and characterizes the strong regularity and the semidifferentiability properties of the solution mapping of (18):

Theorem 1**.**

Let $\Lambda=\nabla_{z}F(\bar{p},\bar{z})$ and let $K=K_{E}(\bar{z},F(\bar{p},\bar{z}))$ be the corresponding critical cone. Then suppose that the mapping $\Lambda+N_{K}$ is strongly regular at [math] for [math], this being equivalent to the condition that the linear variational inequality

[TABLE]

has a unique solution $\bar{s}(r)$ for each $r\in\mathbb{R}^{j+d}$. Then the solution mapping $S$ of (18) with values $S(p)$ for $p=\bar{p}+\Delta p$ has a Lipschitz localization $s$ at $\bar{p}$ for $\bar{z}$ which is semidifferentiable and its semiderivative $Ds(\bar{p})(\Delta p)$ is the solution of the following variational inequality:

[TABLE]

Furthermore, in terms of the critical subspaces $K^{+}=K^{+}(\bar{p},\bar{z})$ and $K^{-}=K^{-}(\bar{p},\bar{z})$, defined in (22) and (23), a sufficient condition for strong regularity of $\Lambda+N_{K}$ or, equivalently, single-valuedness of $(\Lambda+N_{K})^{-1}$, is as follows:

[TABLE]

Thus, in order to determine the change of the solution for a given variation $\Delta p$ of the parameter, one could compute the corresponding semiderivative, which in turn reduced to finding the critical cone $K$ and solving a linear VI.

Let $\Delta p$ be a change of the parameter (the initial state). From Theorem 1 it follows that the semiderivative

[TABLE]

is a solution of the linear VI

[TABLE]

where $R=\nabla_{v}^{2}\mathcal{L}(\bar{z},\bar{p})$, $G=\nabla_{v}g(\bar{v},\bar{p})$, $W=K_{V}\times\mathbb{R}^{n}$, where the critical cone $K_{V}$ is

[TABLE]

and

[TABLE]

In order to apply Theorem 1, we need to adapt the sufficient condition for strong regularity (26) to our case. Denote

[TABLE]

and let $W^{+}=W-W$ and $W^{-}=W\cap[-W]$ be the critical subspaces associated with the cone $W$. Then condition (26) becomes

[TABLE]

One should note that condition (31) is a special case of the more elaborate critical face condition obtained in [21], which characterizes the strong regularity of solution mappings associated with variational inequalities over polyhedral convex sets. Obtaining a sharp, numerically tractable, form of the critical face condition for state and control constrained discrete time optimal control problems is beyond the scope of the present paper and is left for future research. However, it is possible to derive a verifiable sufficient condition, observe that (31) holds provided that the matrix $T$ is positive definite on $W^{+}$. This in turn holds if the matrix $R$ is positive definite on the null-space of $G$; that is

[TABLE]

This condition is a standard second order condition in optimization[12] and is always satisfied, for example, when the cost function is strongly convex and the system is linear.

The following statement summarizes the procedure for computing the semiderivatives of the solution mapping.

Theorem 2**.**

Let $\bar{z}=(\bar{v},\bar{q})$ satisfy $\bar{z}\in S(\bar{p})$, let $\Delta p$ be a variation of the parameter and assume the condition (32) holds at $(\bar{p},\bar{z})$. Then the solution mapping $p\mapsto S(p)$ has a Lipschitz localization $s(p)$ at $\bar{p}$ for $\bar{z}=(\bar{v},\bar{q})$ which is semidifferentiable at $\bar{p}$ and the corresponding semiderivative $Ds(\bar{p})(\Delta p)=(\Delta z,\Delta q)$, or, equivalently, the directional derivative $s^{\prime}(\bar{p};\Delta p)$, is the unique solution of the linear VI (28).

In Theorem 2 we assume that (32), which implies (31), is satisfied at the reference solution. This condition can be enforced by appropriately regularizing the cost function [22]. Moreover, it’s possible to monitor if (32) holds by checking if

[TABLE]

where the columns of $Z$ form a basis for the nullspace of $G$ and $(\cdot)\succ 0$ denotes positive definiteness. This is straightforward to check numerically by, e.g., using a QR decomposition of $G^{T}$ to form $Z$ and attempting to compute a Cholesky factorization of $Z^{T}RZ$, see [12, Section 16.1] for more details.

Remark 2**.**

The Generalized Tangential Predictor (GTP) [9][18, Section 5.3] applied to our OCP (12) is the directional derivative of the solution mapping of the following parameterized nonlinear program

[TABLE]

where $V=\{v~{}|~{}Mv\leq h\}$. The GTP is of the form $(p,\Delta p)\mapsto(\Delta v(p,\Delta p),\Delta q(p,\Delta p),\Delta\mu(p,\Delta p))$, where $v$ are the decision variables and $q$ and $\mu$ are the dual variables associated with the equality and inequality constraints respectively. If the SSOSC and LICQ hold at a KKT point $(\bar{p},\bar{v},\bar{q},\bar{\mu})$ of (34), then the mapping $(\bar{p},\Delta p)\mapsto(\Delta v(\bar{p},\Delta p),\Delta q(\bar{p},\Delta p))$, i.e., the first two components of the GTP, coincides with our proposed predictor. The advantage of our predictor over the GTP is that it is well defined also in the case when the LICQ does not hold.

Remark 3**.**

The main advantage of our predictor is that it does not require any constraint qualification. Other schemes require a constraint qualification, typically the LICQ, to ensure regularity or uniqueness of the Lagrange multipliers associated with inequality constraints. Its easy to construct a situation where the LICQ does not hold by e.g., duplicating a constraint or enforcing a constraint of the form $0\leq u\leq 0$. However, the LICQ cannot be expected to always hold even in the case of linearly independent polyhedral constraints. For example, if the number of simultaneously active constraints is larger than the number of control inputs, the gradients of the of the equality constraints (10b) and active inequality constraints can easily become linearly dependent.

IV A predictor-corrector algorithm

Theorem 2 shows that the semiderivatives of the solution mapping can be computed by solving a linear VI. Note that (28) are the first-order necessary conditions (i.e., the optimality system) for the following quadratic program (QP),

[TABLE]

where $R$, $G$, $K_{V}$, $r_{1}$ and $r_{2}$ are defined in (28), (29) and (30). The critical cone constraint (35c) can be simplified by recalling that $K_{V}$ can be expressed in terms of the index set of the active constraints, see Section I-A. In addition, recall that we can express the set $V$ as $V=\{v~{}|~{}Mv\leq h\}$ (see (11)) where

[TABLE]

The QP can thus be rewritten as

[TABLE]

where $\mathcal{A}(\bar{v})$ and $\bar{\mathcal{A}}=\bar{\mathcal{A}}(\bar{v},F(\bar{p},\bar{z}))$ are defined in Section I-A, $P=\nabla_{pv}L(\bar{p},\bar{z})$, $Q=\nabla_{p}g(\bar{p},\bar{v})$ and $\bar{z}=(\bar{v},\bar{q})\in S(\bar{p})$. The solution of (28) can be obtained by solving (37) and extracting the primal solution and the multipliers associated with (37b). Note that (37) requires identification of the set of active constraints but does not require any notion of strongly/weakly active constraints. Identifying a constraint as strongly or weakly active requires knowledge of a dual variable associated with that inequality. This can be problematic at points where the LICQ does not hold since then identifying the set of strongly active constraints becomes challenging, see e.g., [23] and the references therein.

This QP may be difficult to solve in practice since it is not necessarily convex. This can be addressed by modifying the Hessian as follows:

[TABLE]

where $\rho\in(0,\infty)$. If (32) is satisfied then the modified $R$ will be positive semidefinite provided $\rho$ is chosen large enough [24, Proposition 4.8]. This modification of the Hessian changes the Lagrange multipliers associated with the co-state, but the original multipliers can be recovered as $\Delta q\leftarrow\Delta q^{*}-\rho Q\Delta p$ where $\Delta q^{*}$ are obtained by solving (37) with $R\leftarrow R+\rho G^{T}G$, and extracting the multipliers associated with (37b), see e.g., [24, Section 4.2].

The predictor allows constraints that were previously active to de-activate. After the predictor step a corrector may be needed to remove constraint violations. For the corrector we use the Josephy-Newton (JN) method. Given a fixed parameter value $p$, a VI of the form (18), and an initial guess $z_{0}$, the JN method constructs an iterative sequence by repeatedly solving the following linearized VI

[TABLE]

for $z_{k+1}$. It is well known that strong regularity of the VI implies local quadratic convergence of the JN method [25]. Denoting $\delta v=v^{k+1}-v^{k},$ and $\delta q=q^{k+1}-q^{k},$ (39) becomes

[TABLE]

Note that this is the optimality system of the following QP:

[TABLE]

When the Josephy-Newton iteration (39) is determined by solving the corresponding quadratic problem (41), this method is called Sequential Quadratic Programming (SQP). We can now summarize the predictor-corrector algorithm as Algorithm 2. At each sampling instance the system state is measured, and the sensitivity based predictor is used to estimate the updated iterate corresponding to the parameter change based on the solution of the OCP at the previous sampling instance. This estimate is then passed to an JN based corrector loop which stops when the norm of the residual

[TABLE]

is within the specified tolerance. The control input is then extracted from the solution and applied to the system.

Note that both the predictor and corrector steps are realized by solving QPs. The predictor QP usually has significantly fewer constraints than the corrector QP, see Section I-A, which can lead to reduced computation times. In addition, an initial feasible guess for the predictor QP is available, which can be helpful for both primal active-set and primal-barrier interior point methods.

Remark 4**.**

The predictor-corrector method described in Section IV does not introduce dual variables associated with the polyhedral inequality constraints in (12) and thus does not require any constraint qualifications. However, the methods used to solve (37) and (41) for the predictor and corrector steps may introduce dual variables internally. Thus if the LICQ does not hold it could potentially cause issues for the QP solver. Primal active set methods, see e.g., [12], can encounter issues if the LICQ does not hold as can primal-dual semismooth methods [26]. However, many implementations include heuristics or regularization schemes that handle the issue. Dual active set methods, e.g., [27] and interior point methods, see [12], typically do not require any constraint qualifications beyond Slater’s condition.

V Numerical simulations

To illustrate the theoretical results, we consider the following model of rotational and translational dynamics of an Unmanned Aerial Vehicle (UAV),

[TABLE]

where the state vector is given by the position $p\in\mathbb{R}^{3}$, velocity $v\in\mathbb{R}^{3}$, attitude $\theta\in(-\pi,\pi]^{3}$, and angular velocity $\omega\in\mathbb{R}^{3}$ of the UAV, whereas the input vector is composed of the total thrust $T\in\mathbb{R}^{+}$ and torques $\tau\in\mathbb{R}^{3}$ generated by the propellers. The mass and inertia matrix of the UAV are $m=2~{}kg$ and $J=\text{diag}([0.82~{}0.82~{}1.62])\times 10^{-2}~{}kgm^{2}$, respectively, $e_{3}=[0~{}0~{}1]^{T}$, and

[TABLE]

is the attitude kinematic matrix. The system dynamics are discretized using the forward Euler approximation with $T_{s}=0.075~{}s$. The MPC is designed using the quadratic cost function,

[TABLE]

where $Q=\text{diag}([5~{}5~{}5~{}10~{}10~{}10~{}0.1~{}0.1~{}0.1~{}1~{}1~{}1])$, $R=\text{diag}([0.1~{}0.01~{}0.01~{}0.01])$, and $P$ was computed from these values using the LQR terminal cost for the system linearized about the origin. The system is subject to the input constraints $T\in[18,22]$, $|\tau_{j}|\leq 0.06$, as well as the state constraints $|v_{j}|\leq 2$, where $j\in\{1,2,3\}$ refers to each component of the vector.

Figure 1 displays the closed-loop response obtained using the MPC, which successfully enforces all the constraints. Note that the UAV position and velocity is reported for a longer horizon to show that the system is asymptotically stable, whereas all other signals are focused on the initial transient. Note that the LICQ is violated at $t=0.6s$. Figure 2 compares the computational cost obtained111The simulations were performed on a DELL Latitude 7390 2-in-1, Intel Core i7-8650U, 2.11 GHz, 16 GB laptop running MATLAB 2018b. All QPs were solved using MATLABs built in quadprog command, which implements an interior point algorithm, using default settings. using a standard one-step shift warmstarting strategy, see e.g., [18, Section 5.1], and the proposed predictor, Figure 3 is an enlarged view of the first three seconds. The natural residual was subject to the tolerance value $\pi(p,z)\leq 10^{-5}$.

The comparison in Figure 2 shows that the predictor successfully achieves lower computational costs by reducing the number of SQP iterations performed by the solver. The average computational cost of one predictor step is in the order of $25\mu s$, compared to the computational cost of one SQP iteration which is around $100\mu s$. This reduction in computational time is achieved without incurring significant drawbacks and without having to assume the LICQ, or indeed any CQ, for a system where that assumption is not guaranteed to hold.

VI Conclusions

In this paper we proposed a new sensitivity-based warmstarting strategy for model predictive control of systems with nonlinear dynamics and linear state and control inequality constraints. The strategy involves a predictor which utilizes the semiderivative of the solution of the optimality system. The method exploits the polyhedrality of the constraint set and requires fewer assumptions than comparable methods in the literature. Specifically, it does not require a difficult to verify constraint qualification. Numerical simulations demonstrate the potential of the strategy. Future work includes moving from polyhedral to more general constraints, relaxing the conditions for strong regularity, and developing tailored quadratic programming solvers for the predictor.