FBstab: A Stabilized Semismooth Quadratic Programming Algorithm with Applications in Model Predictive Control

TL;DR

FBstab is a new robust algorithm for convex quadratic programming that combines proximal point and semismooth Newton methods, showing strong performance in model predictive control applications.

Contribution

The paper introduces FBstab, a novel algorithm that improves robustness, warmstarting, and handling of degenerate solutions in quadratic programming.

Findings

FBstab is numerically robust and easy to warmstart.

It handles degenerate primal-dual solutions and detects infeasibility.

FBstab often outperforms existing methods in benchmarks.

Abstract

This paper introduces the proximally stabilized Fischer-Burmeister method (FBstab); a new algorithm for convex quadratic programming that synergistically combines the proximal point algorithm with a primal-dual semismooth Newton-type method. FBstab is numerically robust, easy to warmstart, handles degenerate primal-dual solutions, detects infeasibility/unboundedness and requires only that the Hessian matrix be positive semidefinite. We outline the algorithm, provide convergence and convergence rate proofs, report some numerical results from model predictive control benchmarks, and also include experimental results. We show that FBstab is competitive with and often superior to, state of the art methods, has attractive scaling properties, and is especially promising for model predictive control applications.

Tables

Table 1. Table 1: Problem data for the QPs.

	Servo	HCW	Copoly
Number of States	4	6	18
Number of Controls	1	3	5
Number of timesteps	$40$	$100$	$200$
Horizon Length	30	40	80
Sparse Problem
Variables	$155$	$369$	$1863$
Equality constraints	$124$	$246$	$1458$
Inequality constraints	$124$	$492$	$810$
Condensed Problem
Variables	$31$	$123$	$405$
Inequality constraints	$124$	$492$	$810$
Hessian condition number	$189$	$3.19\times {10}^8$	$1.4\times {10}^3$

Table 2. Table 2: Summary of normalized Speedgoat benchmarking reporting the maximum and average QP solutions times for each sequence. Warm and cold starting are indicated by W and C respectively.

	FBstab	FBstab	FBRS	PDIP	NNLS	DAS
	MPC	Dense
Servo Motor, Normalization $=4.5ms$
Normalized Cost of Condensing $=1.1$
MAXW	1.00	2.9	2.4	3.2	3.3	1.7
AVEW	0.2	1.4	1.4	2.1	1.8	1.3
MAXC	1.5	5.0	5.0	3.0	2.9	2.1
AVEC	0.2	1.6	1.6	2.1	1.8	1.3
Spacecraft, Normalization $=63.9ms$
Normalized Cost of Condensing $=1.5$
MAXW	1.00	14.8	F	11.7	15.6	F
AVEW	0.1	3.3	F	8.3	6.5	F
MAXC	3.4	73.2	F	29.3	7.6	F
AVEC	2.2	62.8	F	25.5	3.7	F
Copolymerization, Normalization $=97.1ms$
Normalized Cost of Condensing $=76.3$
MAXW	1.00	96.6	102.9	238.7	94.4	149.2
AVEW	0.4	82.5	82.6	204.2	85.8	96.2
MAXC	1.5	113.5	112.9	238.2	88.4	293.3
AVEC	0.3	83.0	82.9	205.3	85.9	101.7

Table 3. Table 3: Inverted Pendulum Parameters

Parameter	Value	Parameter	Value
$a$	1.3031	$b$	0.0759
$c$	5.400	$d$	0.0330
$e$	0.7450	$f$	0.0024
$k_1$	7.7443	$k_2$	1.7265

Equations

$$\underset{z}{min.}\frac{1}{2}$$

$$s.t$$

$$Az\le b,$$

$$L(z,\lambda ,v)=\frac{1}{2}{z}^{T}Hz+f^{T}z+{\lambda }^T(Gz-h)+v^T(Az-b),$$

$$\underset{u,\lambda ,v}{min.}\frac{1}{2}{u}^{T}Hu+b^{T}v+h^T\lambda ,$$

$$s.tHu+f+G^T\lambda +A^{T}v=0,$$

$$v\ge 0.$$

$${\nabla }_{z}L(z,\lambda ,v)=0,$$

$$Gz=h,$$

$$Az-b\le 0,v\ge 0,v^T(Az-b)=0.$$

$$\Gamma =\{z|Az\le b,Gz=h\},$$

$$\begin{gathered} \partial G(x)=\mathrm{co}\{J\in {\mathbb{R}}^{M\times N}| \\ \exists \{x^k\}\subset D_G:\{x^k\}\to x,\{\nabla G(x_k)\}\to J\}, \end{gathered}$$

$$\bar{\partial }G=\partial G_1\times \partial G_2\times ...\times \partial G_M,$$

$$\underset{J\in \partial G(x+\xi )}{\sup }||G(x+\xi )-G(x)-J\xi ||=o(||\xi ||),$$

$$x_{k+1}=x_k-V^{-1}G(x_k),V\in \partial G(x_k),$$

$$⟨x-y,u-v⟩\ge 0,\forall u\in T(x),v\in T(y),x,y\in D,$$

$$\mathrm{gph}T=\{(x,u)\in D\times D|u\in T(x)\},$$

$$N_C(x)=\{\begin{array}{ll}\{w|⟨x-u,w⟩\ge 0,\forall u\in C\},& x\in C,\\ \varnothing ,& x\notin C.\end{array}$$

$$x_{k+1}=P_k(x_k),P_k=(I+{\sigma }_k^{-1}T{)}^{-1},$$

$$||x_{k+1}-P_k(x_k)||\le {\delta }_k{\sigma }_k,\sum _{k=0}^{\infty }{\delta }_k<\infty .$$

$${\nabla }_{z}L(z,\lambda ,v)=0,$$

$$h-Gz=0,$$

$$b-Az+N_{+}(v)\ni 0$$

$$T(x)=F(x)+N(x)\ni 0,$$

$$F(x)=\left[\begin{array}{ccc}H& G^T& A^T\\ -G& 0& 0\\ -A& 0& 0\end{array}\right]\left[\begin{array}{c}z\\ \lambda \\ v\end{array}\right]+\left[\begin{array}{c}f\\ h\\ b\end{array}\right]=Kx+w.$$

$$T_{\sigma }(x)=F(x)+{\sigma }_k(x-x_k)+N(x)\ni 0,$$

$$\pi (x)=x-{\Pi }_C(x-F(x)),$$

$$\varphi (a,b)=0\iff a\ge 0,b\ge 0,ab=0.$$

$$\varphi (a,b)=\alpha (a+b-\sqrt{a^2+b^2})+(1-\alpha )a_{+}{b}_{+},$$

$$R_k(x)=R(x,x_k,{\sigma }_k)=\left[\begin{array}{c}{\nabla }_{z}L(x)+{\sigma }_k(z-z_k)\\ h-Gz+{\sigma }_k(\lambda -{\lambda }_k)\\ \varphi (y,v)\end{array}\right],$$

$$y=b-Az+{\sigma }_k(v-v_k)$$

$${\theta }_k(x)=\frac{1}{2}||R_k(x)|{|}_2^2.$$

$$x_{i+1|k}=x_{i|k}-t{V}^{-1}{R}_k(x_{i|k}),V\in \bar{\partial }{R}_k(x_{i|k}),$$

$${\pi }_k(x)=x-{\Pi }_{\Gamma }(x-F(x)+{\sigma }_k(x-x_k)),$$

$$V=\left[\begin{array}{ccc}{H}_{\sigma }& G^T& A^T\\ -G& \sigma I& 0\\ -CA& 0& D\end{array}\right],$$

Full text

\savesymbol

AND

††thanks: This research is supported by the National Science Foundation Award Number CMMI 1562209.

,

1 Introduction

Model Predictive Control (MPC)[7, 26] is an optimization based control methodology that is gaining in popularity for controlling constrained and/or nonlinear systems. In the case of a linear prediction model, polyhedral constraints and quadratic costs the MPC control law is defined by the solution of a quadratic program (QP). Moreover, QPs are commonly used in methods for nonlinear MPC e.g., in sequential quadratic programming (SQP) [34] based methods, such as the real-time iteration scheme [14], for nonlinear MPC, or branch and bound methods for mixed-integer optimization [21]. These QPs need to be solved in real-time on embedded systems with limited computing power; warmstarting, where the solver is initialized using a solution from the previous sampling instance, and structure exploitation are often necessary to meet real-time requirements. Moreover, reliability, exception safety, robustness to early termination, and infeasibility detection are important concerns due to the safety critical nature of many MPC controllers.

A number of useful algorithms and packages for solving convex QPs have been developed including: Active Set (AS) methods [19, 23, 4, 11], Interior Point (IP) methods [52, 44, 15, 32], first order (FO) methods [36, 37, 49, 24], and Dual Newton (DN) methods [38, 22]. AS methods are typically very fast for small to medium problem sizes and can be easily warmstarted using an estimate of the active constraint set. However, they do not scale well, since they have difficulties exploiting sparsity, and are not robust to early termination, i.e., intermediate iterates do not approximate the solution in a meaningful way. IP methods are fast, efficient, robust to early termination and can solve large, structured problems efficiently using advanced linear algebra techniques. However, they are notoriously difficult to warmstart. FO methods can be warmstarted easily, and are attractive from a certification standpoint due to their simplicity and the availability of tight complexity bounds. However, they have slow convergence rates relative to AS, IP, and DN methods; they tend to be most effective on small, strongly convex, or simply constrained problems. Dual Newton methods can exploit structure and be warmstarted but require restrictive assumptions, e.g., that the QP be strongly convex and the linear independence constraint qualification (LICQ) holds. These assumptions reduce their applicability and may cause robustness issues. Finally, primal-dual Newton-type methods, e.g., FBRS[29], retain the same warmstarting and structure exploitation as DN methods but relax the strong convexity requirement to the weaker strong second order sufficient condition (SSOSC) but still require the LICQ.

Recently, a hybrid method has been proposed that combines a first order and active set method. QPNNLS[5] uses the proximal point algorithm [47] to construct a sequence of regularized QP subproblems whose solutions converge to the solution of the original problem. Each regularized QP is strictly convex and is efficiently solved using a non-negative least squares based active set method [4]. Since proximal-point subproblems are expensive, QPNNLS heavily exploits warmstarting to reduce the cost of solving subsequent subproblems.

In this paper, we propose the proximally stabilized Fischer-Burmeister method (FBstab). FBstab is a hybrid method in the same vein as QPNNLS[5] but we employ a primal-dual version of the proximal point algorithm (i.e., the proximal method of multipliers [46]) rather than a primal version, and solve the proximal subproblems with a primal-dual Newton-type method. Using a Newton-type method, rather than an active set method, allows us to solve the proximal subproblems inexactly in addition to warmstarting them, leading to considerable computational savings. In turn, the proximal regularization yields subproblems which automatically satisfy the regularity conditions needed to guarantee robustness and rapid convergence of the Newton-type method.

The contributions of this paper are as follows: (i) We describe FBstab, a method which is numerically robust, and can handle problems with degenerate solutions. Moreover, it is easy to warmstart making it compatible with SQP or suboptimal [allan2017inherent] methods and thus ideal for MPC applications. (ii) We provide detailed convergence and convergence rate proofs under only the assumption that the Hessian is positive semidefinite and a solution exists. (iii) When a solution does not exist we prove that FBstab can detect and certify infeasibility and unboundedness. (iv) We illustrate the performance of FBstab through numerical examples and an experimental case study. We also demonstrate that, since the linear systems at the core of FBstab are structured similarly to some IP methods, we can exploit existing specialized linear algebra routines for MPC problems to achieve linear execution time scaling in the receding horizon length. (v) We have prepared an open source implementation of FBstab which is available at:

[TABLE]

FBstab significantly extends FBRS [29] by removing the SSOSC and LICQ assumptions, improving its numerical robustness, and enabling infeasibility detection, while still being easy to warmstart and retaining the ability to exploit structure. FBstab requires almost no assumptions aside from (non-strict) convexity making it robust and capable of solving any convex QP; only IP methods based on self-dual embedding, e.g., ECOS[15], QPNNLS [5] and the alternating direction method of multipliers (ADMM) [24, 49] are as widely applicable. Finally the use of a primal-dual proximal point method allows FBstab to detect unboundedness, i.e., dual infeasibility, as well as primal infeasibility, unlike QPNNLS which can only detect primal infeasibility. This paper significantly extends the conference version[30]. In particular, it contains detailed convergence, rate-of-convergence, and infeasibility detection proofs, an additional spacecraft relative motion example, an experimental demonstration, and more numerical experiments, all which are not included in [30].

2 Problem Setting and Mathematical Preliminaries

In this paper, we consider convex QPs of the following form,

[TABLE]

where $H=H^{T}\succeq 0\in\mathbb{R}^{n\times n}$ is the Hessian matrix, $f\in\mathbb{R}^{n}$, $z\in\mathbb{R}^{n}$, $G\in\mathbb{R}^{m\times n}$, $h\in\mathbb{R}^{m}$, $A\in\mathbb{R}^{q\times n}$, and $b\in\mathbb{R}^{q}$. We make no assumptions about the problem data aside from the positive semidefiniteness of the Hessian. The Lagrangian for this problem is

[TABLE]

where $\lambda\in\mathbb{R}^{m}$ and $v\in\mathbb{R}^{q}$ are dual variables, and its dual is

[TABLE]

We will use $x=(z,\lambda,v)\in\mathbb{R}^{l}$ to denote the primal-dual triple. The Karush-Kuhn-Tucker (KKT) conditions for the problem are

[TABLE]

Any vector satisfying (3) is called a critical point. If the feasible set

[TABLE]

is nonempty, then the KKT conditions are necessary and sufficient for global optimality [8]. The objective of the FBstab algorithm is to either find a vector $x^{*}$ which satisfies (3) or to prove that no such vector exists. It does this by applying Newton’s method to a semismooth reformulation of (3). The proximal point algorithm is then wrapped around the semismooth Newton’s method; it stabilizes the Newton iterations and detects primal-dual infeasibility. This improves the robustness of the algorithm and allows it to solve problems with degenerate primal-dual solutions, a class of problems which are typically challenging for Newton-type methods.

2.1 Nonsmooth Analysis

In this section we review some concepts from non-smooth analysis which are used to construct the core Newton-type method. Suppose a function $G:\mathbb{R}^{N}\to\mathbb{R}^{M}$ is locally Lipschitz on a set $U\subseteq\mathbb{R}^{N}$, so that $G$ is differentiable almost everywhere by Rademacher’s theorem [43]. Clarke’s generalized Jacobian [12] is defined as

[TABLE]

where $D_{G}$ is the dense set of points where $G$ is differentiable, and co $(\cdot)$ denotes the convex hull.

We also make use of the C-subdifferential [40] which is defined as

[TABLE]

where $\partial G_{i}$ are the generalized Jacobians of the components mappings of $G$. Note that each element of $\partial G_{i}$ is a row vector. This form of the C-subdifferential is used in [10, 9] and possesses many of the useful properties of the generalized Jacobian but is easier to compute and characterize.

A function $G:\mathbb{R}^{N}\mapsto\mathbb{R}^{M}$ is said to be semismooth [42] at $x\in\mathbb{R}^{N}$ if $G$ is locally Lipschitz at $x$, directionally differentiable in every direction and the estimate

[TABLE]

holds111See [27, A.2] or [34, A.2] for more details on O notation.. If $o(||\xi||)$ is replaced with $O(||\xi||^{2})$ in (7) then $G$ is said to be strongly semismooth at $x$. The generalized Jacobian and the C-subdifferential can be used to construct Newton-type methods for semismooth systems of nonlinear equations [42, 40]. The following Newton-type method,

[TABLE]

is locally superlinearly convergent to roots of $G$ which satisfy some regularity properties [42, 41]. Similar results are available using the C-subdifferential[40, 9].

2.2 Monotone Operators

In this section we review the proximal point algorithm which is used to stabilize FBstab’s core Newton-type method. Recall that a set-valued mapping222Equivalently a multifunction, operator, relation, point to set mapping, or correspondence. $T:D\rightrightarrows\mathbb{R}^{N}$ is said to be monotone if

[TABLE]

where $D=\mathrm{dom}~{}{T}\subseteq\mathbb{R}^{N}$. In addition, if

[TABLE]

is not properly contained in the graph of any other monotone operator then $T$ is said to be maximal [47]. A useful example of a maximal monotone operator is the normal cone. The normal cone mapping of a nonempty closed convex set $C\subseteq\mathbb{R}^{N}$ is defined by

[TABLE]

The proximal point algorithm [47] can be used to find zeros of maximal monotone operators. This algorithm generates a sequence $\{x_{k}\}$ by the rule

[TABLE]

where $\sigma_{k}$ is a sequence of positive numbers. The parameter $\sigma_{k}$ acts as a regularization term; an advantage of the proximal point method over standard regularization techniques is that $\sigma_{k}$ need not to be driven to zero. Since $T$ is monotone, the proximal operator $P_{k}$ is single valued and well defined for all $x\in\mathrm{dom}~{}{T}$. For an arbitrary maximal monotone operator the proximal point algorithm converges to an element of the set $T^{-1}(0)$ if it is nonempty. The proximal point algorithm also allows for approximate evaluation of $P_{k}$, a key consideration when designing a practical algorithm. It was shown in [47] that the proximal point algorithm can tolerate errors which satisfy the following

[TABLE]

In general, if $T^{-1}(0)=\emptyset$ the algorithm diverges. However, we show in Section 4.1 that for the special case considered here the proximal point algorithm can be used to detect infeasibility.

3 The Stabilized Semismooth Algorithm

In this section we describe the FBstab algorithm. The main idea is to regularize the original problem, solve it using a semismooth Newton-type method, then use the proximal point algorithm to iteratively refine the solution. The regularization ensures that each proximal subproblem has a unique primal-dual solution and satisfies the regularity conditions needed to ensure fast convergence of the inner Newton-type solver. In addition, semismooth Newton-type methods can be warmstarted and terminated early. As a result, each proximal subproblem can be solved approximately and warmstarted with the solution of the previous one. This makes FBstab very efficient, often requiring only one to two Newton iterations to solve each proximal subproblem.

The FBstab algorithm is summarized in Algorithms 1- 3. Algorithm 1 implements the proximal point algorithm and is discussed in Section 3.1. Algorithm 2 evaluates the proximal operator, $P_{k}$, using a semismooth Newton’s method as discussed in Section 3.2 and Algorithm 3 checks for infeasibility as discussed in Section 4.1.

Remark 1**.**

Both semismooth Newton methods [20] and proximal point algorithm [47] have been extensively studied in the literature. Our main contribution is the novel synergistic combination of the two methods with infeasibility detection techniques originally derived for ADMM [1] to produce a method that is very general, theoretically justified, and effective in practice.

Remark 2**.**

This method could theoretically be extended to more general smooth convex programs. However, then the curvature of the constraint Hessians would enter into the subproblems and we would need to maintain positivity of the dual variables during the Newton iterations to ensure non-singularity of the generalized Jacobians. This would make warmstarting the algorithm difficult; as a result we have elected to focus on QPs. Moreover, due to the polyhedrality of the solution set, we are able to establish a stronger convergence rate results for QPs than for more general convex programs (see Theorem 3).

3.1 Outer Proximal Point Iterations

The KKT conditions of (1) can be rewritten as the following variational inequality (VI)

[TABLE]

where $N_{+}$ is the normal cone of the nonnegative orthant; (13) can be compactly expressed as,

[TABLE]

where $N$ is the normal cone of $\Gamma=\mathbb{R}^{n}\times\mathbb{R}^{m}\times\mathbb{R}^{q}_{\geq 0}$ and

[TABLE]

Solving (1) is equivalent to finding an element of $T^{-1}(0)$.

Proposition 1**.**

The variational inequality (13) has the following properties: (i) It is maximal monotone. (ii) If nonempty, its solution set, $T^{-1}(0)$, is closed and convex.

{pf*}

Proof. (i): The base mapping (15) is maximal monotone since it is single valued, affine and $K^{T}+K\succeq 0$, where $K$ is defined in (15). Variational inequalities of the form (14) are maximal monotone if the single valued portion $F$ is monotone [48]. (ii): See [33].

Since (13) is maximally monotone we can apply the proximal point algorithm to (14), this is the proximal methods of multipliers [46]. The proximal operator $P_{k}(x)$ can be evaluated by finding $x$ satisfying

[TABLE]

which is itself a variational inequality. However, due to the regularization term, (16) is guaranteed to have a unique solution and to satisfy certain useful regularity properties (Proposition 2). As a result, we can construct a semismooth Newton solver for the subproblems with a quadratic rate of convergence (Theorem 2). In addition, the proximal point algorithm allows for approximate evaluation of $P_{k}$ and we warmstart the semismooth Newton solver at each iteration. Taken together, these measures allow FBstab to evaluate the proximal operator efficiently. The norm of the natural residual function,

[TABLE]

where $\Pi_{C}$ denotes euclidean projection onto the closed, convex set $C$, is used as a stopping criterion for the algorithm. It is a local error bound [35, Theorem 18], i.e333For a closed set $C$, $\mathrm{dist}({x},{C})=\inf_{\bar{x}}\{x-\bar{x}~{}|~{}\bar{x}\in C\}$.., $\mathrm{dist}({x},{T^{-1}(0)})=O(||\pi(x)||)$.

3.2 Inner Semismooth Newton Solver

The core of FBstab is the inner solver which efficiently evaluates the proximal operator by applying a Newton-type method to a semismooth reformulation of (16). We construct an appropriate reformulation using a so-called nonlinear complementarity problem (NCP) function [50]. A NCP function $\phi:\mathbb{R}^{2}\to\mathbb{R}$ has the property that

[TABLE]

In this paper we use the penalized Fischer-Burmeister (PFB) function [9],

[TABLE]

where $\alpha\in(0,1)$ is fixed and $x_{+}$ denotes projection onto the nonnegative orthant. The penalized FB (PFB) function is similar to the Fischer-Burmeister[20] function but has better theoretical and numerical properties [9]. Using this NCP function we can construct the following mapping,

[TABLE]

where the NCP function is applied elementwise. We will also make use of the following merit function

[TABLE]

The properties of $R_{k}$ and $\theta_{k}$ are summarized below.

Proposition 2**.**

The function, $R_{k}$ in (20), and its merit function, $\theta_{k}(x)$, have the following properties:

$R_{k}$* is strongly semismooth on $\mathbb{R}^{l}$.* 2. 2)

$R(x_{k}^{*},x_{k},\sigma_{k})=0$* if and only if $x_{k}^{*}=P_{k}(x_{k})$. Further, $x_{k}^{*}$ is unique and exists irrespective of the problem data.* 3. 3)

$||R_{k}(x)||$* is a global error bound, i.e., there exists $\tau>0$ such that $||x-x_{k}^{*}||\leq\tau||R_{k}(x)||$* 4. 4)

$\theta_{k}$* is continuously differentiable and, for any $V\in\bar{\partial}R_{k}(x)$, its gradient is $\nabla\theta_{k}(x)=V^{T}R_{k}(x)$.*

{pf*}

Proof. 1) The PFB function is strongly semismooth [9, Proposition 2.1] and is composed with affine functions to form $R$. Strong semismoothness of $R$ then follows from the composition rules for semismooth functions, see, e.g., [27, Propositions 1.73 and 1.74].

2) The VI (16) is defined by the sum of a monotone and strongly monotone operator and is thus strongly monotone. Strongly monotone operators always have a unique zero [33]. The zeros of $R$ exactly coincide with those of (16) by the properties of NCP functions.

3) Let $\mathcal{K}=\{x~{}|~{}(K+\sigma I)x+N_{\infty}(x)\ni 0\}$ denote the “kernel” of (16), where $K$ is defined in (15), and $N_{\infty}(x)$ is the normal cone of $\Gamma_{\infty}$, the recession cone of $\Gamma$. Since $\Gamma$ is a convex cone, $\Gamma_{\infty}=\Gamma$. Theorem 20 of [35] states that the norm of the natural residual function $\pi_{k}(x)$, defined in (23), is a global error bound if $\mathcal{K}=\{0\}$. Its clear that $x=0$ satisfies $(K+\sigma I)x+N_{\infty}(x)\ni 0$ and since $K+\sigma I$ is strongly monotone, the solution must be unique. Thus, applying [35, Theorem 20] there exists $\tau_{1}>0$ such that $||x-x_{k}^{*}||\leq\tau_{1}||\pi_{k}(x)||$. The equivalence of $||\pi_{k}(x)||$ and $||R_{k}(x)||$, when used as an error bound, can then be established using the same arguments as [9, Theorem 3.11]; we omit the details for brevity.

4) See [9, Theorem 3.2] or [29, Proposition 2].

The inner solver evaluates $P_{k}$ by solving the rootfinding problem $R_{k}(x)=0$ using a damped semismooth Newton’s method. The inner iterative scheme444$i$ and $k$ are used for inner and outer iterations respectively., is

[TABLE]

where $t\in(0,1]$ is a step length, chosen using a backtracking linesearch, which enforces global convergence. We will show in the next section that the matrix $V$ is always invertible and thus the iteration (3.2) is well defined. In particular, all elements of $\bar{\partial}R_{k}(x^{*})$, where $x^{*}$ is the root, are non-singular which leads to quadratic convergence of (22). The rootfinding problem, $R_{k}(x)=0$, has a unique solution even if the original QP is degenerate or infeasible due to the outer proximal point layer.

Remark 3**.**

The natural residual mapping

[TABLE]

is similar to (20) with $\phi(y,v)$ replaced by $\mathrm{min}(y,v)$ where the min operation is applied component wise.

3.3 The Newton step system

In this section, we analyze the properties of the C-subdifferential and the associated Newton step system. We begin with the following proposition which establishes some properties of the C-subdifferential.

Proposition 3**.**

For any $x,\bar{x}\in\mathbb{R}^{l}$, $\sigma>0$, any $V\in\bar{\partial}R(x,\bar{x},\sigma)$ is of the form

[TABLE]

where $H_{\sigma}=H+\sigma I$, and $C=diag(\gamma_{j})$, $D=diag(\mu_{j}+\sigma\gamma_{j})$, are diagonal matrices with entries $(\gamma_{j},\mu_{j})\in\partial\phi(y_{j},v_{j})$.

{pf*}

Proof. The proof follows [9, Proposition 2.3]. By definition

[TABLE]

thus we need only to characterize the generalized gradients. The first two blocks are continuously differentiable so $\partial R_{i}(x,\bar{x},\sigma)=\{\nabla R_{i}(x,\bar{x},\sigma)^{T}\}$, $i=1,\ldots,n+m$. The last block satisfies

[TABLE]

where $e_{i}$ are rows of identity, by [12, Proposition 2.1 and Theorem 2.3.9].

Explicit expressions for the generalized gradient of $\phi(a,b)$ are given by [9, Proposition 2.1]

[TABLE]

where $r=\sqrt{a^{2}+b^{2}}$, $\eta$ and $\zeta$ are arbitrary numbers satisfying $\eta^{2}+\zeta^{2}=1$, and

[TABLE]

A procedure for computing an element of $\partial R(x,\bar{x},\sigma)$ is given in Algorithm 4.

Next we consider the regularity properties of $V$ which are critical to the behaviour of the semismooth Newton solver. Due to the stabilizing effect of the outer proximal point algorithm all elements of $\bar{\partial}R(x,\bar{x},\sigma)$ are nonsingular, a property we will refer to as C-regularity.

Definition 1**.**

A mapping $G:\mathbb{R}^{N}\to\mathbb{R}^{N}$ is C-regular at a point $x\in\mathbb{R}^{N}$ if it is semismooth at $x$ and all $V\in\bar{\partial}G(x)$ are non-singular.

Theorem 1** (Regularity of the C-subdifferential).**

$R_{k}(x)=R(x,x_{k},\sigma)$* is C-regular for any $x,x_{k}\in\mathbb{R}^{l}$ and $\sigma>0$.*

{pf*}

Proof. For any $V\in\bar{\partial}R_{k}(x)$, the Newton step system (22) has the form

[TABLE]

where $R_{k}(x)=-[r_{1}^{T}~{}~{}r_{2}^{T}~{}~{}r_{3}^{T}]^{T}$ as in (20). For all $j\in\{1,...q\}$ we have that $\mu_{j}\geq 0,~{}\gamma_{j}\geq 0$ and $(\mu_{j},\gamma_{j})\neq 0$. Thus $D_{jj}=\mu_{j}+\sigma\gamma_{j}>0$ implying $D\succ 0$. Since $D\succ 0$ we can eliminate the third row of (28) algebraically and negate the second leading to the following pair of linear systems of equations

[TABLE]

The matrices $E=H_{\sigma}+A^{T}CD^{-1}A$ and $\sigma I$ are positive definite so the block $2\times 2$ matrix in (29) is symmetric quasidefinite and thus invertible [51]. As a result, (28) has a unique solution, implying that $V$ is nonsingular.

The C-regularity property of $R_{k}$ guarantees that the inner iterations are globally well defined and that the iteration (22) will converge at a quadratic rate to the unique solution of (16) (Theorem 2).

4 Convergence Analysis

In this section we discuss the convergence of the FBstab algorithm. First, we prove convergence of the inner Newton-type solver. Our analysis follows the established convergence theory for damped semismooth Newton’s methods, see e.g., [40, 9, 18] and is included for completeness.

Theorem 2** (Inner solver convergence).**

Consider an arbitrary but fixed iteration $k$ of the outer proximal point algorithm. Suppose $x_{0}=x_{0|k}=x_{k}\in\mathbb{R}^{l}$ and let the sequence $\{x_{i}\}$ be generated by the EvalProx procedure in Algorithm 1. Then:

1. i.

The sequence $\{x_{i}\}$ is well defined and converges to the unique point $x^{*}$ satisfying $x^{*}=P_{k}(x_{0})$. 2. ii.

The asymptotic rate of convergence is quadratic i.e.,

[TABLE]

{pf*}

Proof. Suppose that EvalProx generates an infinite sequence $\{x_{i}\}$ satisfying $||R_{k}(x_{i})||>0$; if $R_{k}(x_{i})=0$ the algorithm will terminate. Let $x^{*}$ denote the unique solution of $\eqref{eq:prox_subproblem}$. The sequence $\{x_{i}\}$ is uniquely defined because $R_{k}$ is globally C-regular by Theorem 1.

We begin with a local analysis. Suppose for now that $t=1$, define $e_{i}=x_{i}-x^{*}$, and consider the update equation

[TABLE]

Next we consider global convergence. Since $\theta_{k}$ is continuously differentiable and $\nabla\theta_{k}(x_{i})=V^{T}R_{k}(x_{i})$, see Proposition 2, we have that

[TABLE]

Performing a Taylor expansion yields,

[TABLE]

Since $\nabla\theta(x_{i})^{T}\Delta x_{i}<0$ by (30b) this implies that the linesearch condition (Line  8, Algorithm 2)

[TABLE]

must hold as $t\to 0$ ensuring that the linesearch terminates with $t>0$. Note that

[TABLE]

where we have used (30b). Combining (30e) with (30f) and noting that $t\in(0,1]$, we have that

[TABLE]

since $\eta\in(0,0.5)$. As the sequence $\{\theta_{k}(x_{i})\}$ is strictly decreasing and bounded below by zero it converges to some $\theta^{*}\geq 0$ and $\lim_{i\to\infty}\theta_{k}(x_{i+1})-\theta_{k}(x_{i})=0$. Since

[TABLE]

we have, rearranging (30h) and using (30b), that

[TABLE]

as $i\to\infty$. Since $\theta_{k}(x)=0$ if and only if $x=x^{*}$ and $\theta_{k}$ is continuous this establishes global convergence.

Finally, it remains to show that the algorithm will recover the local quadratic converge rate established earlier. Since $\{x_{i}\}\to x^{*}$ as $i\to\infty$ eventually $x_{i}$ will be sufficiently close to $x^{*}$ for (30a) to hold if a unit step is admitted by the linesearch. Algorithm 2 always begins by checking $t=1$. Since,

[TABLE]

where the second line holds by Lipschitz continuity of $R_{k}$, the third by (30a) and the fourth by the error bound $||x_{i}-x^{*}||=o(||R_{k}(x_{i})||)$ in Proposition 2, the linesearch will eventually accept unit steps implying that (30a) holds as $i\to\infty$.

The following theorem summarizes the convergence properties of FBstab when $T^{-1}(0)\neq\emptyset$, i.e., when (1) has a primal-dual solution. We analyze the behaviour of FBstab when $T^{-1}(0)=\emptyset$ in Section 4.1.

Theorem 3** (Convergence of FBstab).**

Let $x_{0}\in\mathbb{R}^{l}$ be arbitrary, suppose $T^{-1}(0)=(F+N)^{-1}(0)$ is nonempty and let $\{x_{k}\}$ be generated by FBstab. Then $\{x_{k}\}\to x^{*}\in T^{-1}(0)$ as $k\to\infty$. Further, the convergence rate is at least linear and if, in addition, $\sigma_{k}\to 0$ as $k\to\infty$ then the convergence rate is superlinear.

{pf*}

Proof. FBstab is an instance of the proximal point algorithm so we can employ[31, Theorem 2.1] to establish both convergence and the convergence rate. The error bound condition needed by [31, Theorem 2.1] ($(A_{r}^{\prime})$ in [31]) is

[TABLE]

where $T_{\sigma}$ is defined in (16), and $\sum_{k=0}^{\infty}\delta_{k}<\infty$. The inequality (31) is enforced by construction (Line 12 of Algorithm 2) since $||R_{k}(x)||$ is an error bound for the subproblems by Prop 2. Moreover, ${\delta_{k}}$ satisfies, $\delta_{k}\leq{\kappa}^{k}$ (Line 9 of Algorithm 1) thus $\kappa\in(0,1)$, implies $\sum_{0}^{\infty}\delta_{k}=\frac{1}{1-\kappa}<\infty$. It remains to show that there exists $a,\varepsilon>0$ such that for all $w\in\varepsilon\mathbb{B}$

[TABLE]

or equivalently (see e.g., [16, Section 3D])

[TABLE]

where $\mathbb{B}=\{x~{}|~{}||x||\leq 1\}$. This property actually holds globally because $T(x)=F(x)+N(x)$ is a polyhedral variational inequality, see [16, Section 3D]. Thus we can invoke [31, Theorem 2.1] to conclude that FBstab converges, the rate of convergence is globally linear, and that the convergence rate is superlinear if $\sigma_{k}\to 0$ as $k\to\infty$.

Finally, we state the following theorem which provides rigorous justification for the subproblem warmstarting strategy employed in FBstab.

Theorem 4** (Lipschitz continuity of subproblems).**

Let the sequence $\{x_{k}\}$ be generated by FBstab with $x_{0}$ arbitrary. Then the proximal operator is Lipschitz continuous, i.e., at any iteration $k$ the proximal operator $P_{k}$ satisfies

[TABLE]

where $\lambda_{min}(\cdot)$ designates the smallest eigenvalue of a symmetric matrix and $K$ is defined in (15).

{pf*}

Proof. The variational inequality (16) can be written as $F(x)/\sigma+x+N(x)\ni x_{k}$ which is a parameterized variational inequality with $x_{k}$ as the parameter. Its strong monotonicity constant is $\eta>0$. The result then follows from [16, Theorem 2F.6].

Each proximal subproblem computes $P_{k}(x_{k})$ by solving $R_{k}(x)=0$ starting from $x_{k}$ as an initial guess. Theorem 4 implies that eventually $||P_{k}(x_{k})-x_{k}||$ will become sufficiently small so that the quadratic convergence rate of Theorem 2 holds immediately and the semismooth method converges rapidly. This happens because,

[TABLE]

and, since the algorithm is converging, $||x_{k-1}-x_{k-2}||\to 0$ and $||x_{k}-P_{k-1}(x_{k-1})||\to 0$ as $k\to\infty$. We observe this behaviour in practice, typically after the first or second proximal iteration each subsequent proximal subproblem takes only one or two Newton iterations to converge.

4.1 Infeasibility Detection

In this section we apply the techniques developed in [1] to characterize the behaviour of FBstab in the case where either the QP (1) or its dual (2) are infeasible. These results are not specific to FBstab and hold whenever the proximal point algorithm is used to solve (13). We begin by recalling infeasibility conditions for (1) and (2).

Proposition 4**.**

*(Infeasibility conditions)

Dual infeasibility: Suppose there exists a vector $z\in\mathbb{R}^{n}$ satisfying $Hz=0$, $Az\leq 0$, $Gz=0$, and $f^{T}z<0$. Then (2) is infeasible.

Primal infeasibility: Suppose there exits a vector $(\lambda,v)$ such that $G^{T}\lambda+A^{T}v=0$ and $\lambda^{T}h+v_{+}^{T}b<0$, where $v_{+}$ is the projection of $v$ onto the nonnegative orthant. Then the feasible set of (1) is empty.*

{pf*}

Proof. See e.g., [1, Proposition 1].

Any vector satisfying the conditions of Proposition 4 is a certificate of primal or dual infeasibility. We will show that the proximal point algorithm generates these certificates when appropriate. When (1) is feasible, dual infeasibility is the same as the primal problem being unbounded below. First we review the limiting behaviour of the proximal point algorithm. The following lemma summarizes some results for averaged nonexpansive operators, a class which includes the proximal operator.

Lemma 1**.**

Let $T:\mathcal{D}\mapsto\mathcal{D}$ be an averaged nonexpansive operator. In addition, suppose $x_{k}$ is generated by $x_{k}=T^{k}(x_{0})$, $x_{0}\in\mathcal{D}$, define $\delta x_{k}=x_{k+1}-x_{k}$, and let $\delta x$ be the projection of [math] onto $\mathrm{cl}~{}\mathrm{range}~{}(T-I_{d})$ where $\mathrm{cl}$ denotes the closure of a set and $I_{d}$ denotes the identity operator. Then as $k\to\infty$ we have that:

1. i.

$\frac{1}{k}x_{k}\to\delta x$** 2. ii.

$\delta x_{k}\to\delta x$** 3. iii.

If $\mathrm{Fix}~{}T\neq\emptyset$ then $x_{k}\to x^{*}\in\mathrm{Fix}~{}T$, where $\mathrm{Fix}~{}T$ denotes the fixed points of $T$.

{pf*}

Proof. (i): [39, Corrolary 2]. (ii), (iii): [3, Fact 3.2] .

An immediate corollary of this is that $\delta x$ in Lemma 1 satisfies $\delta x=0$ if $\mathrm{Fix}~{}T\neq\emptyset$. The following proposition applies Lemma 1 to our specific situation.

Proposition 5**.**

Let the sequence $\{x_{k}\}=\{(z_{k},\lambda_{k},v_{k})\}$ be generated by the proximal point algorithm and define $\delta x_{k}=x_{k}-x_{k-1}$. Then there exists $\delta x=(\delta z,\delta\lambda,\delta v)\in\mathbb{R}^{l}$ such that $(\delta z_{k},\delta\lambda_{k},\delta v_{k})\to(\delta z,\delta\lambda,\delta v)$ as $k\to\infty$ and also satisfies the following properties:

1. (i)

$H\delta z=0$, 2. (ii)

$A\delta z\leq 0$, 3. (iii)

$G\delta z=0$, 4. (iv)

$\delta v\geq 0$, 5. (v)

$f^{T}\delta z=-\sigma||\delta z||\leq 0$, 6. (vi)

$\delta\lambda^{T}h+\delta v^{T}b\leq 0$, 7. (vii)

$G^{T}\delta\lambda+A^{T}\delta v=0$.

{pf*}

Proof. The proximal operator is firmly non-expansive [47] and thus averaged, see e.g., [2, rmk 4.34]. The convergence of $\delta x_{k}$ to $\delta x$ as $k\to\infty$ then follows from Lemma 1. Note that $\lim_{k\to\infty}~{}\frac{1}{k}\delta x_{k}=0$ and $\lim_{k\to\infty}~{}\frac{1}{k}x_{k}=\delta x$ which we will use often in the sequel. We begin by rewriting (16) in the following form:

[TABLE]

Further, (36) is satisfied exactly in the limit since the condition $\varepsilon_{k}\to 0$ as $k\to\infty$ is enforced by construction in Algorithm 1. We now proceed point by point.

(i): Taking inner products, multiplying (36c) and (36b) by $1/k$, taking the limit, and applying Lemma 1 yields

[TABLE]

The same procedure applied to (36a) yields

[TABLE]

combining this with (37) we obtain that, since $H\succeq 0$,

[TABLE]

(ii): Multiplying the second inequality in (36d) by $1/k$ and taking the limit yields

[TABLE]

(iii): Multiplying (36b) by $1/k$ and taking the limit yields

[TABLE]

(iv): Multiplying (36d) by $1/k$ and taking the limit yields

[TABLE]

(v): Taking the inner product of (36a) with $\delta z_{k}$ then taking the limit and applying (37) and (i) yields:

[TABLE]

(vi): Taking the inner product of (36c) and $\delta v_{k}$, and taking the limit yields

[TABLE]

since $\delta v^{T}A\delta z=0$ we have that

[TABLE]

Applying the same procedure to (36b) yields,

[TABLE]

since $\delta\lambda^{T}G\delta z=0$ this implies $h^{T}\delta\lambda=-\sigma||\delta\lambda||^{2}_{2}$, combining this with (45) yields

[TABLE]

(vii): Dividing (36a) by $k$ and taking the limit yields

[TABLE]

applying (i) we have that $H\delta z=0$ so we obtain

[TABLE]

which completes the proof.

Armed with Proposition 5 we can prove the following theorem summarizing the behaviour of FBstab when (1) or (2) is infeasible.

Theorem 5** (Infeasibility Detection).**

Suppose that (1) is primal-dual infeasible, i.e., the solution set of (14) is empty. Suppose $x_{0}\in\mathbb{R}^{l}$ is arbitrary, let the sequence of iterates $\{x_{k}\}=\{z_{k},\lambda_{k},v_{k}\}$ be generated by FBstab, and define $\delta x_{k}=x_{k+1}-x_{k}$. Then $\delta x_{k}\to\delta x$ as $k\to\infty$ where $\delta x=(\delta z,\delta\lambda,\delta v)$ satisfies the following properties:

1. (i)

If $\delta z\neq 0$ then the dual QP (2) is infeasible and $\delta z$ satisfies the dual infeasibility conditions in Proposition 4. 2. (ii)

If $(\delta\lambda,\delta v)\neq 0$ then the primal QP $\eqref{eq:QP}$ is infeasible and $(\delta\lambda,\delta v)$ satisfies the primal infeasibility conditions in Proposition 4. 3. (iii)

If $\delta x\neq 0$ and $(\delta\lambda,\delta v)\neq 0$ then (1) and (2) are infeasible.

{pf*}

Proof. (i): Follows from points (i), (ii), (iii), and (v), of Proposition 5. Note that if $\delta z\neq 0$ then $f^{T}\delta z=-\sigma||\delta z||<0$.

(ii): Follows from points (vi), (vii), and (iv) of Proposition 5. Note that since $\delta v>0$ due to point (iv) of Proposition 5, the condition $b^{T}\delta v_{+}+h^{T}\delta\lambda<0$ simplifies to $b^{T}\delta v+h^{T}\delta\lambda$.

(iii): Follows from points (i) and (ii) above.

Theorem 5 justifies Algorithm 3. This feature allows FBstab to exit gracefully if there is no primal-dual solution. Infeasibility detection is also important in many applications e.g., in branch and bound algorithms for mixed integer QPs [21].

5 Numerical Experiments

In this section we illustrate the performance of FBstab with some numerical experiments. We solve instances of the following optimal control problem (OCP),

[TABLE]

where $A_{i},Q_{i}\in\mathbb{R}^{n_{x}\times n_{x}}$, $B_{i},S_{i}\in\mathbb{R}^{n_{x}\times n_{u}}$, $R_{i}\in\mathbb{R}^{n_{u}\times n_{u}}$, $q_{i},c_{i}\in\mathbb{R}^{n_{x}}$, $r_{i}\in\mathbb{R}^{n_{u}}$, $E_{i}\in\mathbb{R}^{n_{c}\times n_{x}}$, $L_{i}\in\mathbb{R}^{n_{c}\times n_{u}}$, $d_{i}\in\mathbb{R}^{n_{c}}$, $x_{i},\xi\in\mathbb{R}^{n_{x}}$, $u_{i}\in\mathbb{R}^{n_{u}}$, $x=(x_{0},...,x_{N})$, and $u=(u_{0},\dots,u_{N})$. We require that

[TABLE]

so the problem is convex. This QP is large but sparse and is often called the simultaneous or multiple shooting form of the MPC problem [45]. The QP is also often solved in the so-called condensed form,

[TABLE]

which is in the control variables only and can be derived by eliminating the state variables $x$ in (50) using the dynamic equations see e.g., [28, Section 2.3] or [7]. We consider three linear MPC benchmark problems; their properties are summarized in Table 1.

Control of a Servo Motor[6]: The objective is to drive the motor position $y_{1}$ to a desired angular position $r=30^{\circ}$ while respecting the constraint $|y_{2,k}|\leq 78.5~{}Nm$ on the shaft torque and the constraint $|u|\leq 220~{}V$ on the motor input voltage. The continuous time model is

[TABLE]

which is discretized at $0.05$ s using a zero-order hold. The tuning matrices and initial condition are $Q_{i}=diag([10^{3},0,0,0])$, $R_{i}=10^{-4}$, and $x_{0}=0$. The traces of this model in closed-loop with an MPC controller are shown in Figure 1, the shaft angular position is driven to the reference while respecting the constraints on the shaft torque and input voltage.

Control of Spacecraft Relative Motion[53]: These equations describe the radial, along track, and across track positions $\zeta=[x_{1}~{}x_{2}~{}x_{3}]^{T}$ and velocities $\dot{\zeta}$ of a spacecraft relative to a nominal circular orbit. The control objective is to drive the spacecraft to the origin from $\zeta_{0}=-[2.8~{}0.01~{}1]~{}km$ and $\dot{\zeta}_{0}=0$. The system dynamics are given by the Hill-Clohessy-Wiltshire (HCW) equations,

[TABLE]

where $\omega=0.0011~{}s^{-1}$ is the mean motion of the reference orbit. The dynamics of $x=(\zeta,\dot{\zeta})$ can be compactly written as $\dot{x}=A_{c}x$. The control inputs are modelled as impulsive thrusts which instantaneously change the velocity of the spacecraft, see [53], so that the discrete time model is

[TABLE]

where $u=\Delta v$ is the instantaneous change in velocity due to the impulsive thrusters, $A=e^{A_{c}\tau}$, and $\tau=30~{}s$. The control inputs must satisfy $||u_{k}||_{\infty}\leq 1~{}m/s$. The spacecraft velocity is constrained to satisfy $||\dot{\zeta}_{k}||_{\infty}\leq 1~{}m/s$ and the tuning matrices are $Q=diag([1~{}1~{}1~{}0.001~{}0.001~{}0.001])$ and $R=I_{3\times 3}$. The closed-loop response of the system is shown in Figure 2.

Control of a Copolymerization Reactor[13]: The following normalized transfer function models the copolymerization of methyl methacrylate (MMA) and vinyl acetate (VA) in a continuous stirred tank reactor:

[TABLE]

The normalized inputs are flows of monomer MMA ($u_{1}$), monomer VA ($u_{2}$), initiator ($u_{3}$), transfer agent ($u_{4}$), and the reactor jacket temperature ($u_{5}$). The normalized outputs are the polymer production rate ($y_{1}$), the mole fraction of MMA in the polymer ($y_{2}$), the molecular weight of the polymer ($y_{3}$), and the reactor temperature ($y_{4}$). All inputs and outputs are relative to nominal operating conditions [13]. The model was realized in modal form using the ss command in MATLAB and discretized using a zero-order hold with a normalized sampling period of $0.5$ (corresponding to three hours in physical time). The resulting model has 18 states, 5 inputs and 4 outputs. The states are initially disturbed as $\xi_{0,i}=\sin(i)$ for $i=1,~{}...,~{}18$; the control objective is to drive the outputs to the origin. The inputs are constrained as $||u_{k}||_{\infty}\leq 0.05$, i.e., 5% deviation from nominal. The horizon length is $N=70$, and the weighting matrices are chosen as $Q=C^{T}C$, where $C$ is the output matrix from the realization process, and $R=0.1I_{5\times 5}$. Closed-loop traces are shown in Figure 3.

5.1 Implementation details

The default parameters used throughout are $\sigma_{k}=\sqrt{\epsilon_{m}}$, $\zeta=10^{-14}$, $\tau_{a}=10^{-4}$, $\tau_{r}=0$, $\tau_{inf}=10^{-8}$, $\alpha=0.95$, $\beta=0.7$, $\eta=10^{-8}$, where $\epsilon_{m}$ is machine precision; $\epsilon_{m}\approx 10^{-16}$ for our double precision implementation.

We have implemented two versions of FBstab 555Both are available online at: https://github.com/dliaomcp/fbstab-matlab.git in MATLAB. The first solves problems of the form (1) and is featured in in Sections 5.4 and 5.3. It is implemented using MATLABs built-in dense linear algebra routines and solves the Newton-step systems (29) using a Cholesky factorization. The second solves problems of the form (50) and is featured in Sections 5.2 and 5.3. It exploits the structure present in (50) and is equipped with two different linear solvers for the Newton step systems:

FBstab Ricatti: Uses a Ricatti-like recursion similar to the one in [44] to solve (29). 2. 2.

FBstab MA57: Calls MA57[17] to directly solve (28) using sparse linear algebra.

Both implementations use the non-monotone linesearch technique of Grippo et al. [25] to improve performance without jeopardizing the convergence properties of the algorithm.

Remark 4**.**

In [30] we used a linear algebra framework for FBstab which solves the condensed without forming (52) explicitly. This approach uses the conjugate gradient method to solve linear systems which could be slow when (52) is ill-conditioned, e.g., in the HCW example. We found the direct methods used in paper to be faster and more robust.

5.2 Solver scaling

FBstab can efficiently solve structured problems. The linear systems (28) that are solved in FBstab are highly structured similar to those in IP methods. To demonstrate this, we compared FBstab, implemented using the two different linear solvers, as described in Section 5.1, with the external solvers: (1) quadprog (MATLAB 2017b) which uses sparse linear algebra and the interior-point-convex algorithm, (2) ECOS[15] (self-dual interior point) and (3) qpOASES[19] (active set). We also implemented the following in MATLAB: (4) the dual active set (DAS) method [23], including factorization updating, (5) QPNNLS [5], (6) GPAD [36] and (7) accelerated ADMM[24]. The DAS method, qpNNLS, and qpOASES solve (52), with the cost of condensing included in the analysis, while all other methods solve (50) directly. The MATLAB routines were converted into C code using the mex command. We found that GPAD and ADMM were not competitive; both have been omitted from Figures 4 and 5 for clarity.

We solved the first QP, i.e., at $t=0$, in the servo motor and copolymerization examples666The spacecraft example was omitted because the dynamics are unstable, the resulting condensed problem becomes ill-conditioned enough for large horizons to make most of the methods fail. and measured wall clock times as the horizon was varied from $N=10$ to $N=1000$. All methods were cold started at the origin, the experiments were performed on a 2015 Macbook Pro with a 2.8 GHz i7 processor and 16 GB of RAM running MATLAB 2017b. Recorded execution times were averaged as necessary to obtain consistent timings. Figures 4 and 5 display the results. ECOS, quadprog and FBstab Ricatti/MA57 scale like $O(N)$ with FBstab Ricatti being the fastest method. The active set methods qpOASES, qpNNLS, and DAS are efficient for small problems but are quickly overtaken as $N$ becomes large, they scale like $O(N^{3})$. The DAS method is the quickest method for the servo example for short horizons before being overtaken by FBstab Ricatti. Similarly, for very short horizons, qpNNLS is the quickest method for the copolymerization example before being overtaken by FBstab. As expected, active set methods are very effective for small problems but are quickly overtaken by both interior point methods and FBstab. FBstab is shown to be faster than several interior point methods at all horizon lengths. Overall, FBstab scales well as is competitive with and often superior to several established solvers.

5.3 Benchmarking on real-time hardware

To investigate the performance of FBstab on embedded hardware we performed some benchmarking on a Speedgoat Baseline Real-time Target Machine (SGRTM). The SGRTM (2.0 GHz Celeron CPU, 4 GB RAM) is a rapid prototyping platform which runs a real-time operating system (RTOS) and is representative of an embedded computing environment. Using the RTOS allows us to obtain deterministic execution time measurements. Moreover, the SGRTM does not have any linear algebra libraries available, e.g., BLAS or LAPACK, so solving ill-conditioned problems is more difficult.

We implemented FBstab using structure exploiting linear algebra (FBstab MPC) and using dense linear algebra (FBstab Dense) as described in Section 5.1. For comparison we implemented the following in MATLAB: (1) FBRS (Fischer-Burmeister Regularized and Smoothed)[29], (2) DAS (Dual Active Set) [23], (3) QPNNLS (robust nonnegative least squares[5]), (4) PDIP (primal-dual IP using Mehrotra’s predictor-corrector [34, Algorithm 14.3]), (5) GPAD (accelerated dual gradient projection)[36], and (6) accelerated ADMM [24]. Since quadprog cannot codegenerate, ECOS does not have a Simulink interface, and qpOASES was outperformed by DAS during the scaling trials all three were omitted from testing; GPAD and ADMM were not competitive and were omitted as well. The methods were converted into C code using Simulink Real-time (2017b). A method is deemed to have failed if it is stalls and is unable to solve any of the QPs in the sequence to the desired precision ($10^{-4})$. Note that we have implemented $LDL^{T}$ and $QR$ factorization updating for qpNNLS and DAS to ensure a competitive comparison.

The results are shown in Table 2. When warmstarting is enabled, FBstab MPC is the fastest method in the worst case for all three examples and FBstab Dense is competitive with the IP and AS methods, especially on the larger copolymerization example. In terms of average execution times777Average execution times are an indicator of power draw. This is an important metric in Aerospace applications where reduced power consumptions leads to e.g., extended range for drones. FBstab MPC and FBstab Dense are dominant. When warmstarting is disabled, the PDIP and active set methods become more competitive, however FBstab MPC is still more efficient.

Overall, when the cost of condensing is considered, see Remark 5, FBstab MPC is shown to outperform the other methods tested in terms of both maximum and average execution time. FBstab derives significant benefit from warmstarting, this is especially noticeable for the HCW example, and is significantly faster than FBstab Dense, showcasing the importance of specialized linear algebra routines. Further, both DAS and FBRS fail on the ill-conditioned HCW example while their regularized versions, qpNNLS and FBstab respectively, succeed, demonstrating the expected improved robustness due to proximal regularization. FBstab is often faster than FBRS, demonstrating that the addition of proximal regularization makes the methods more robust without a significant reduction in speed.

Remark 5**.**

The cost of condensing, i.e., of converting (50) to (52) is included in the results reported in Table 2. This simulates solving e.g., trajectory tracking problems or real-time iteration [14] subproblems. These computations can sometimes be moved offline, in this situation the normalized cost of condensing listed in Table 2 should be subtracted from each row of the last five columns. In this scenario, the DAS method is the best method for the servo motor example.

5.4 Degenerate and Infeasible Problems

Consider the following parameterized QP:

[TABLE]

by varying $a_{1},a_{2},b$ and $c$ we can create degenerate or infeasible test problems.

First, we consider degeneracy. Setting $a_{1}=a_{2}=c=0$ and $b=3$ we obtain a degenerate QP with the primal solution set $\Gamma_{p}=\{1\}\times[1,3]$. FBstab signals optimality after 2 proximal iterations and 5 Newton iterations and returns $x^{*}=(1.00,1.00),v^{*}=(0.00,0.00,0.00,2.00,0.00)$ with the norm of the residual $\epsilon=8.97\times 10^{-12}$. Second, we consider a primal infeasible QP by setting $a_{1}=a_{2}=0,c=-1$ and $b=3$. FBstab signals primal infeasibility after 1 proximal iteration and 7 Newton iterations and returns $\delta x^{*}=(-0.18,0.36)\cdot 10^{-5},\delta v^{*}=(4.47,0,0,4.47,4.47)\cdot 10^{7}$. Finally, we consider a dual infeasible QP by setting $a_{1}=a_{2}=0,c=-1$ and $b=\infty$. This leads to a QP for which $x=(0,1)$ is a direction of unbounded descent. FBstab signals dual infeasibility after 3 proximal iterations and 8 Newton iterations and returns $\delta x=(0,671)\cdot 10^{5}$ and $\delta v=0$. FBstab was initialized at the origin for all results reported in this section.

6 Experimental Results

In this section we showcase FBstab’s utility with an experimental demonstration. Consider a pendulum mounted on a cart as shown in Figure 6. The control objective is to drive the cart position, $x_{c}$, to a target value while balancing the pendulum, i.e., keeping $\alpha\approx 0$. The nonlinear equations of motion of the cart-pendulum system are given in (59), a motor is used to accelerate the cart. The corresponding parameters are given in Table 3; these values correspond to a QUANSER Linear Servo Base Unit with Inverted Pendulum device. The states and control inputs are

[TABLE]

i.e., the cart position, pendulum angle, their velocities, and the motor input voltage.

[TABLE]

We implemented a linear MPC controller of the form (50) with $N=10$. The prediction model is obtained by linearizing (59) about the origin then discretizing the resulting linear continuous time model using a zero order told at a sampling time of $0.01s$. The weighting matrices are $Q_{i}=Q=diag(35,0.01,0.1,0.1),~{}i\in\mathbb{Z}_{[0,N-1]}$, $R_{i}=R=0.02,~{}i\in\mathbb{Z}_{[0,N]}$, $S_{i}=0,~{}i\in\mathbb{Z}_{[0,N]}$ and $Q_{N}=P$, where $P$ is the solution of the discrete time algebraic Ricatti equation corresponding to the prediction model. We also impose the following constraints on the input voltage and the pendulum angle $|\alpha|\leq 4^{\circ},~{}|u|\leq 10~{}V$.

We implemented the controller in MATLAB/SIMULINK using the QUANSER QUARC real-time software to interface with the experimental setup; the system sampling rate is $100$ Hz. We solved (52) using the FBstab Dense, the pendulum angle constraint was softened using $L_{2}$ penalties to ensure feasibility despite model mismatch. The resulting QP has $22$ decision variables and $55$ inequality constraints, each QP was warmstarted with the solution from the previous timestep.

The experimental results are shown in Figure 7. The MPC controller is able to track the desired cart position trajectory while balancing the pendulum and respecting input constraints. The pendulum angle constraint is enforced with some minor violations caused by unmodelled dynamics888Specifically, vibration of the table the device is mounted on and flexibility of the track the cart rides along.. FBstab successfully maintains the QP residual below the prescribed precision of $10^{-4}$. The maximum execution time (measured using the QUARC software) was $4.3~{}ms$, safely below the sampling period of $10~{}ms$.

7 Conclusions

This paper presents FBstab, a proximally stabilized Fischer-Burmeister method for convex quadratic programming. FBstab is attractive for real-time optimization because it is easy to code, numerically robust, easy to warmstart, can exploit problem structure, and converges or detects infeasibility under only the assumption that the Hessian of the quadratic program is convex. An open source MATLAB implementation of FBstab is available online. Future work includes exploring the application of stabilized semismooth Newton-type methods to nonlinear problems and preparation of an open source C++ implementation of FBstab.

Acknowledgments

We would like to thank Dr. Chris Petersen of the U.S. Air Force Research Laboratories for encouraging us to develop methods for optimization problems with degenerate solutions and Brian Ha for helping to set up the inverted pendulum system.