A Homotopy-based Nonlinear Interior-Point Method for NMPC

Proceedings of the the 20th World World Congress Proceedings of 20th The International Federation of Congress Automatic Control Proceedings of the 20th World Congress The International Federation of Automatic The International Federation of Congress Automatic Control Control Toulouse, France, July 9-14, 2017 Proceedings of the 20th World The International Federation of Automatic Control Toulouse, France, France, July July 9-14, 9-14, 2017 2017 Available online at www.sciencedirect.com Toulouse, The International of Automatic Control Toulouse, France,Federation July 9-14, 2017 Toulouse, France, July 9-14, 2017

ScienceDirect

A Homotopy-based Nonlinear A Homotopy-based Nonlinear A Homotopy-based Nonlinear Interior-Point Method for NMPC A Homotopy-based Nonlinear Interior-Point Method for NMPC Interior-Point Method for NMPC Method for NMPC ∗ ∗∗ ∗ AndreaInterior-Point Zanelli ∗∗ Rien Quirynen Juan Jerez ∗ ∗∗ Moritz Diehl ∗ IFAC PapersOnLine 50-1 (2017) 13188–13193

Andrea Zanelli Zanelli ∗ Rien Quirynen Quirynen ∗ Juan Jerez Jerez ∗∗ Moritz Moritz Diehl Diehl ∗ Andrea Andrea Zanelli ∗∗ Rien Rien Quirynen ∗∗ Juan Juan Jerez ∗∗ Moritz Diehl ∗∗ ∗∗ ∗ Andrea Zanelli Rien Quirynen Juan Jerez Moritz Diehl ∗ University of Freiburg, Freiburg, Germany (e-mail: ∗ of Freiburg, Freiburg, Germany (e-mail: University of Freiburg, ∗ University {andrea.zanelli, rien.quirynen, moritz.diehl}@imtek.uni-freiburg.de) University of Freiburg, Freiburg, Freiburg, Germany Germany (e-mail: (e-mail: ∗ {andrea.zanelli, rien.quirynen, moritz.diehl}@imtek.uni-freiburg.de) {andrea.zanelli, rien.quirynen, moritz.diehl}@imtek.uni-freiburg.de) University of Freiburg, Freiburg, Germany (e-mail: ∗∗ {andrea.zanelli, rien.quirynen, moritz.diehl}@imtek.uni-freiburg.de) ETH Zurich, Switzerland (e-mail: [email protected]) ∗∗ ∗∗ ETH Zurich, Zurich, Switzerlandmoritz.diehl}@imtek.uni-freiburg.de) (e-mail: [email protected]) [email protected]) {andrea.zanelli, rien.quirynen, Switzerland (e-mail: ∗∗ ETH ∗∗ ETH Zurich, Switzerland (e-mail: [email protected]) ETH Zurich, Switzerland (e-mail: [email protected]) Abstract: This paper introduces a homotopy-based nonlinear interior-point method that can Abstract: This paper aa homotopy-based nonlinear interior-point method that can Abstract: This introduces nonlinear method that exploit warm-starts forintroduces an efficient real-time implementation of nonlinear model Abstract: This paper paper introduces a homotopy-based homotopy-based nonlinear interior-point interior-point methodpredictive that can can exploit warm-starts for an efficient real-time implementation of nonlinear model predictive exploit warm-starts for an efficient real-time implementation of nonlinear model predictive Abstract: This paper introduces a homotopy-based nonlinear interior-point method that can control (NMPC). The algorithm performs a homotopy on a tightened problem with a fixed exploit warm-starts for an efficient real-time implementation of nonlinear model predictive control The performs aa homotopy aa tightened problem with aa fixed control (NMPC). The algorithm performs homotopy on tightened problem with fixed exploit warm-starts foralgorithm an efficient real-time implementation nonlinear model predictive value of(NMPC). the barrier parameter during which the initial on state isofchanged gradually. Once an control (NMPC). The algorithm performs a homotopy on a tightened problem with a fixed value of the barrier parameter during which the initial state is changed gradually. an value of the barrier parameter during which the initial state is changed gradually. Once an control (NMPC). The performs a homotopy on a tightened problem withOnce a fixed approximate solution toalgorithm the tightened problem is obtained, a second homotopy is performed that value of the barrier parameter during which the initial state is changed gradually. Once an approximate solution to the tightened problem is obtained, a second homotopy is performed that approximate solution to the tightened problem is obtained, a second homotopy is performed that value of the barrier parameter during which the initial state is changed gradually. Once an shrinks the barrier parameter in order to compute a solution to the original problem. Theoretical approximate solution to the tightened problem is obtained, a second homotopy is performed that shrinks the barrier parameter in order to compute aa solution to the original problem. Theoretical shrinks the barrier parameter in order to compute solution to the original problem. Theoretical approximate solution to the tightened problem is obtained, a second homotopy is performed that results are presented on the local convergence, which provide a second order contraction estimate shrinksare thepresented barrier parameter in order to compute a solution to the original problem. Theoretical results on the local convergence, provide aa second contraction estimate results are presented on the convergence, which provide order contraction estimate shrinks the barrier in order compute a solution to the original for both phases of parameter the In to order towhich assess the potential oforder theproblem. proposedTheoretical scheme, it results are presented on algorithm. the local local convergence, which provide a second second order contraction estimate for both phases of the algorithm. In order to assess the potential of the proposed scheme, it for both phases of the algorithm. In order to assess the potential of the proposed scheme, it results are presented on the local convergence, which provide a second order contraction estimate has been implemented in the software package FORCES NLP. Its performance on a non-trivial for both phases of the algorithm. In order to assess the potential of the proposed scheme, it has been implemented in the software package FORCES NLP. Its performance on a non-trivial has been implemented in the software package FORCES NLP. Its performance on a non-trivial for both phases of the algorithm. In order to assess the potential of the proposed scheme, it NMPC case study is shown, where a speedup of up to one order of magnitude is obtained. has been implemented in the software package FORCES NLP. Its performance on a non-trivial NMPC case study is shown, where aa speedup of up to one order of magnitude is obtained. NMPC case study is shown, where speedup of up to one order of magnitude is obtained. has been implemented in the software package FORCES NLP. Its performance on a non-trivial NMPC case study is shown, where aofspeedup up to one orderbyofElsevier magnitude is rights obtained. © 2017, IFAC (International Federation Automaticof Control) Hosting Ltd. All reserved. NMPC casenonlinear study is model shown, where a speedup up to one order of embedded magnitude is obtained. Keywords: predictive control, of numerical methods, optimization, Keywords: nonlinear model predictive control, numerical methods, embedded optimization, Keywords: nonlinear predictive nonlinear methods. Keywords:interior-point nonlinear model model predictive control, control, numerical numerical methods, methods, embedded embedded optimization, optimization, nonlinear interior-point methods. nonlinear interior-point methods. Keywords: nonlinear model predictive control, numerical methods, embedded optimization, nonlinear interior-point methods. nonlinear interior-point methods. 1. INTRODUCTION 2007) and the Continuation GMRES (C/GMRES) algo1. INTRODUCTION 2007) and the Continuation GMRES (C/GMRES) algo1. 2007) and GMRES algorithm (Ohtsuka, 2004) use warm-starting and continuation 1. INTRODUCTION INTRODUCTION 2007) and the the Continuation Continuation GMRES (C/GMRES) (C/GMRES) algorithm (Ohtsuka, 2004) use warm-starting and continuation INTRODUCTION (Ohtsuka, 2004) use warm-starting and continuation 2007) and the Continuation GMRES (C/GMRES) algoto track the optimal solution manifold. Nonlinear model 1. predictive control (NMPC) has drawn rithm rithm (Ohtsuka, 2004) use warm-starting and continuation to track track the optimal optimal solution manifold. and continuation Nonlinear model predictive predictive control (NMPC) (NMPC) has drawn drawn to the solution manifold. rithm (Ohtsuka, 2004) use warm-starting Nonlinear model control has increasing attention in both academia and industry in the to track the optimal solution manifold. Nonlinear attention model predictive control (NMPC) has drawn sequential programming (SQP) methincreasing in both academia and industry industry in the the Although track the optimal quadratic solution manifold. increasing attention in academia and in Nonlinear model predictive control capability (NMPC) has drawn past decades. Due to both its inherent to handle Although sequential quadratic programming (SQP) methincreasing attention in both academia and industry in the to Although sequential quadratic programming (SQP) ods are well known to be able to exploit warm-starts, this past decades. Due to its inherent capability to handle sequential quadratic programming (SQP) methmethpast decades. Due to its inherent capability to handle increasing attention in both academia and industry in thea Although multivariable constrained nonlinear systems directly, it is ods are well known to be able to exploit warm-starts, this past decades. Due to its inherent capability to handle ods are well known to be able to exploit warm-starts, this Although sequential quadratic programming (SQP) methis less straightforward for interior-point methods (Gondzio multivariable constrained nonlinear systems directly, it is a ods are well known to be able to exploit warm-starts, this multivariable constrained directly, it is aa is past decades. Due to itsnonlinear capability to handle promising control strategy ininherent severalsystems fields (Qin and Badgless straightforward for interior-point methods (Gondzio multivariable constrained nonlinear systems directly, it is is less straightforward for interior-point methods (Gondzio ods are well known to be able to exploit warm-starts, this and Grothey, 2006). Several issues associated with warmpromising control strategy in several fields (Qin and Badgis less straightforward for interior-point methods (Gondzio promising control strategy in several fields (Qin and Badgmultivariable constrained nonlinear systems directly, it is a and well, 2000). In addition, theoretical results that provide Grothey, 2006). Several issues associated with warmpromising control strategy in several fields (Qin and Badgand Grothey, 2006). Several issues associated with warmis less straightforward for interior-point methods (Gondzio starts are discussed and potential solutions are proposed in well, 2000). In addition, addition, theoretical results that provide Grothey, 2006).and Several issues associated with warmwell, In theoretical results that promising control strategy in several fields (Qin andprovide Badg- and stability guarantees for NMPC schemes under reasonable starts are discussed potential solutions are proposed in well, 2000). 2000). In addition, theoretical results that provide starts are discussed and potential solutions are proposed in and Grothey, 2006). Several issues associated with warmthe literature. In (Yildirim and Wright, 2002), for linear stability guarantees for NMPC schemes under reasonable are discussed and potential solutions2002), are proposed in stability guarantees for NMPC schemes under reasonable well, 2000). In addition, theoretical results assumptions have been developed (Mayne etthat al.,provide 2000). starts the literature. In (Yildirim and Wright, for linear stability guarantees for NMPC schemes under reasonable the literature. In (Yildirim and Wright, 2002), for linear starts are discussed and potential solutions are proposed in programs, two strategies are proposed based on adjustassumptions have been developed (Mayne et al., 2000). the literature. In (Yildirim and Wright, 2002), for linear assumptions have been developed (Mayne et al., 2000). stability guarantees for NMPC schemes under reasonable However, due to the high computational burden associated programs, two strategies are proposed based on adjustassumptions have been developed (Mayne et al., 2000). programs, two strategies are proposed based on adjustthe literature. In (Yildirim and Wright, 2002), for linear ments of iterates available from previously solved neighHowever, due have to the the high developed computational burden associated two strategies are proposed based on adjustHowever, due high burden associated assumptions been (Mayne et al., 2000). programs, with the online solution of the nonlinear and in general ments of iterates available from previously solved neighHowever, due to to the high computational computational burden associated ments of iterates available from previously solved neighprograms, two strategies are proposed on adjustboring problems. It is shown that, for abased sufficiently large with the online solution of the nonlinear and in general ments of iterates available from previously solved neighwith the online solution of the nonlinear and in general However, due to the high computational burden associated nonconvex optimal control problems (OCP), NMPC has boring problems. It is shown that, for a sufficiently large with the online solution of the nonlinear and in general boring problems. It is shown that, for a sufficiently large ments of iterates available from previously solved neighvalue of the barrier parameter, it is possible to obtain a nonconvex optimal control problems (OCP), NMPC has boring problems. It is shown that, forpossible a sufficiently large nonconvex optimal control problems has with the online solution of mainly the nonlinear andNMPC in general been historically employed in (OCP), the chemical indusvalue of the barrier parameter, it is to obtain a nonconvex optimal control problems (OCP), NMPC has value of the barrier parameter, it is possible to obtain a boring problems. It is shown that, for a sufficiently large feasible point for the perturbed problem applying such corbeen historically employed mainly in the chemical indusvalue of the barrier parameter, it is possible to obtain a been historically employed mainly the chemical indusnonconvex optimal controltimes problems (OCP), NMPC has feasible try, where the sampling arein generally sufficiently point for the perturbed problem applying such corbeen historically employed mainly in the chemical indusfeasible point for the perturbed problem applying such corvalue of the barrier parameter, it is possible to obtain a rections to the stored iterates. Similarly, in (Gondzio and try, where the sampling times are generally sufficiently feasible point for the perturbed problem applying such cortry, where the sampling times are generally sufficiently been historically employed mainly ingenerally the chemical indus- rections long (Garc´ ıathe et al., 1989; Qin and Badgwell, 2003). to the stored iterates. Similarly, in (Gondzio and try, where sampling times are sufficiently rections to the stored iterates. Similarly, in (Gondzio and feasible point for the perturbed problem applying such corGrothey, 2006), unblocking heuristics are presented that long (Garc´ıa ıathe et al., al., 1989; Qin Qin and Badgwell, 2003). to2006), the stored iterates. Similarly, inpresented (Gondzio that and long (Garc´ 1989; and try, sampling times areBadgwell, generally2003). sufficiently rections Grothey, unblocking heuristics arelinear long where (Garc´ ıa et et as al., 1989;efficient Qin and Badgwell, 2003). unblocking heuristics that rections to2006), theperformance stored iterates. Similarly, inpresented (Gondzio and can improve when solvingare programs. More recently, more algorithms are being de- Grothey, Grothey, 2006), unblocking heuristics are presented that long (Garc´ ıa et al., 1989; Qin and Badgwell, 2003). can improve performance when solving linear programs. More recently, as more more efficient algorithms are being beingunits de- can improve performance when solving linear programs. Grothey, 2006), unblocking heuristics are presented that More recently, as efficient algorithms are deIn (Shahzad et al., 2010) and (Shahzad and Goulart, veloped and more powerful embedded computing can improve performance when solving linear programs. More recently, as more efficient algorithms are beingunits de- In (Shahzad et al., 2010) and (Shahzad and Goulart, veloped and more more powerful embedded computing In (Shahzad et al., and (Shahzad and Goulart, can improve performance when solving linear programs. veloped and powerful embedded computing units More recently, as more efficient algorithms are being de- 2011), a warmhot-start for interior-point are becoming available, computation times in the milliIn (Shahzad etand al.,aa 2010) 2010) andstrategy (Shahzad and Goulart, veloped and more powerful embedded computing units 2011), aa warmand hot-start strategy for interior-point are becoming available, computation times in the milli2011), warmand a hot-start strategy for interior-point In (Shahzad et al., 2010) and (Shahzad and Goulart, are becoming available, computation times in the milliveloped and more powerful embedded computing units methods are presented, respectively, with applications to and microsecond time-scale have been achieved for optimal 2011), a warmand a hot-start strategy for interior-point are becoming available, computation times in the millimethods are presented, presented, respectively, withforapplications applications to and microsecond time-scale have been been achieved achieved for optimal methods are respectively, with to 2011), a warmand a hot-start strategy interior-point and microsecond time-scale have for optimal are becoming available, computation times in the millilinear model predictive control. For nonlinear nonconvex control problems arising in applications in the fields of aumethods are presented, respectively, with applications to and microsecond time-scale have been achieved for optimal linear model predictive control. For nonlinear nonconvex control problems arising in applications in the fields of aulinear model predictive control. For nonlinear nonconvex methods are presented, respectively, with applications to control problems arising in applications in the fields of auand microsecond haverenewable been achieved for(Ferreau optimal in (Benson and Shanno, 2008), a warm-starting tomotive (Fraschtime-scale et al., 2013), linear model predictive control. For nonlinear nonconvex control problems arising in applications in energy the fields of au- problems, problems, in (Benson and Shanno, 2008), a warm-starting tomotive (Frasch et al., 2013), renewable energy (Ferreau problems, in (Benson and Shanno, 2008), a warm-starting linear model predictive control. For nonlinear nonconvex tomotive (Frasch et al., 2013), renewable energy (Ferreau control problems arising in applications in the fields of autechnique is proposed based on a penalty approach. et al., 2011) and robotics (Diehlrenewable et al., 2006). in proposed (Benson and Shanno, 2008), aapproach. warm-starting tomotive (Frasch et al., 2013), energy (Ferreau problems, is based on aa penalty et al., 2011) 2011) and robotics robotics (Diehlrenewable et al., al., 2006). 2006). technique is based on penalty problems, in proposed (Benson and Shanno, 2008), aapproach. warm-starting et al., and (Diehl et tomotive (Frasch et al., 2013), energy (Ferreau technique technique is proposed based on a penalty approach. et al., 2011) and robotics (Diehl et al., 2006). When using and NMPC to control a sequence of technique is proposed based on a penalty approach. et al., 2011) robotics (Diehl a et system, al., 2006). When using NMPC to control a system, a sequence of 1.1 Contributions and Outline When NMPC to aa system, aa sequence of closely related, parametric optimization has to Contributions and Outline When using using NMPC to control control system,problems sequence of 1.1 closely related, parametric optimization problems has to 1.1 Contributions Contributions and and Outline Outline closely related, parametric optimization problems has to When using NMPC to control a system, a sequence of 1.1 be solved online, each for a different initial state value. closely related, parametric optimization problems has to 1.1 Contributions be solved online, each for aaoptimization different initial state value. This paper proposesand an Outline interior-point method for nonlinear be solved online, each for different initial state value. closely related, parametric problems has to This fact can be exploited to warm-start the algorithm be solved online, each for atodifferent initial state value. This paper proposes an interior-point method for nonlinear This paper proposes an interior-point method for This fact can be exploited warm-start the algorithm nonconvex problems arising from NMPC formulations that This fact can be exploited to warm-start the algorithm be solved online, each for a different initial state value. paper problems proposes an interior-point method for nonlinear nonlinear when solving instances of the nonlinear optimal This This fact cansuccessive be exploited to warm-start the algorithm nonconvex arising from NMPC formulations that nonconvex problems arising from NMPC formulations that This paper proposes an interior-point method for nonlinear when solving successive instances of the nonlinear optimal can exploit warm-starts. Similarly to what is proposed when solving successive instances of the nonlinear optimal This fact can be exploited to warm-start the algorithm nonconvex problems arising from NMPC formulations that control problem. Indeed, efficient online methods such can exploit warm-starts. Similarly to what is proposed when solving successive instances of the nonlinear optimal can exploit warm-starts. Similarly to what is proposed nonconvex problems arising from NMPC formulations that control problem. Indeed, efficient online methods such in (Gondzio and Grothey, 2006) and (Yildirim and Wright, control problem. Indeed, efficient online methods such when solving successive instances of the nonlinear optimal can exploit warm-starts. Similarly to what is proposed as the Real-Time Iteration (RTI) scheme (Diehl et al., control problem. Indeed, efficient online methods such in (Gondzio and Grothey, 2006) and (Yildirim and Wright, in (Gondzio and Grothey, 2006) and (Yildirim and Wright, can exploit warm-starts. Similarly to what is proposed as the Real-Time Iteration (RTI) scheme (Diehl et al., 2002), the algorithm uses an intermediate iterate on a as Iteration (RTI) (Diehl al., control problem. Indeed, online methods (Gondzio and Grothey, and (Yildirimiterate and Wright, as the the Real-Time Real-Time Iterationefficient (RTI) scheme scheme (Diehl et etsuch al., in 2002), the solved algorithm uses2006) an intermediate intermediate on a a 2002), the algorithm uses an iterate on in (Gondzio and Grothey, 2006) and (Yildirim and Wright,  previously neighboring problem. The algorithm as the Real-Time Iteration (RTI) scheme (Diehl et al., This research was supported by the EU via ERC-HIGHWIND (259 2002), the solved algorithm uses an intermediate iterate on a  previously neighboring problem. The algorithm This research was supported by the EU via ERC-HIGHWIND (259  previously solved neighboring problem. The algorithm 2002), the algorithm uses an intermediate iterate on a consists of two separate phases.problem. During theThe initial phase, research was supported by the EU via ERC-HIGHWIND (259 166), (607 957), H2020-ITN-AWESCO (642 682)  This previously solved neighboring algorithm ThisFP7-ITN-TEMPO research was supported by the EU via ERC-HIGHWIND (259 consists of two separate phases. During the initial phase, 166), FP7-ITN-TEMPO (607 957), H2020-ITN-AWESCO (642 682) 682)  This consists of two separate phases. During the initial phase, previously solved neighboring problem. The algorithm 166), FP7-ITN-TEMPO (607 957), H2020-ITN-AWESCO (642 a homotopy is performed on a tightened problem with a and by the DFG in context of the Research Unit FOR 2401. research was supported by the EU via ERC-HIGHWIND (259 consists of two separate phases. During the initial with phase, 166), FP7-ITN-TEMPO (607 957), H2020-ITN-AWESCO (642 682) aaconsists homotopy is performed on a tightened problem a and by the of the Unit is on problem a and by the DFG DFG in in context context of957), the Research Research Unit FOR FOR 2401. 2401. of two separate phases. During the initial with phase, 166), FP7-ITN-TEMPO (607 H2020-ITN-AWESCO (642 682) a homotopy homotopy is performed performed on aa tightened tightened problem with a and by the DFG in context of the Research Unit FOR 2401. a homotopy is performed on a tightened problem with a and by the DFG in context of the Research Unit FOR 2401. Copyright © 2017 IFAC 13730 Copyright © 2017 13730 2405-8963 © IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright © 2017, 2017 IFAC IFAC 13730 Copyright © 2017 IFAC 13730 Peer review under responsibility of International Federation of Automatic Control. Copyright © 2017 IFAC 13730 10.1016/j.ifacol.2017.08.2175

Proceedings of the 20th IFAC World Congress Andrea Zanelli et al. / IFAC PapersOnLine 50-1 (2017) 13188–13193 Toulouse, France, July 9-14, 2017

fixed value of the barrier parameter, which can exploit a warm-start from previous problem instances. Once an approximate solution to the initial barrier problem is obtained, a second homotopy is performed in which the barrier parameter is decreased in order to compute an optimal solution to the original optimization problem. By storing the primal-dual solution to the initial barrier problem, a warm-start for the next OCP instance is made available. The local convergence of this continuation technique is analyzed and a contraction estimate is derived for the proposed algorithm. The presented scheme is implemented in the software package FORCES NLP (Zanelli et al., 2016) that uses a structure-exploiting interior-point method for multi-stage nonlinear nonconvex optimization. The potential of this approach for NMPC applications is assessed on a non-trivial example, where a speedup of up to one order of magnitude can be achieved. The paper is organized as follows: Section 2 presents the preliminaries on NMPC and the problem formulation. In Section 3, the algorithm is described and the theoretical results are derived and discussed in Section 4. Finally, Section 5 presents the numerical case study.

2. PRELIMINARIES 2.1 Nonlinear Model Predictive Control Throughout the paper, the following discrete-time optimal control problem will be considered: min x ,...,x 0

N

u0 ,...,uN −1

s.t.

N −1 

=0 =0 =0 =0 = τ1 .. .

(2)

=0 =0 =0 = τ 1,

where Si is the diagonal matrix having the elements of the slack variable si on its diagonal and 1 denotes a vector of ones. The compact notation wi := [ xTi uTi ]T has been introduced. The barrier parameter τ has been introduced in order to circumvent the nonsmoothness of the complementarity conditions. The equations in (2) for τ = 0, together with the positivity conditions s ≥ 0 and ν ≥ 0, constitute the so-called first-order necessary optimality conditions. The Newton method can then be directly applied to (2) for decreasing values of τ and, as τ → 0, a point that satisfies first-order optimality conditions is recovered. Moreover, under mild assumptions (Nocedal and Wright, 2006), the iterates converge to a local minimum. Practical implementations of interior-point methods generally include additional algorithmic ingredients, to ensure global convergence and improve performance and reliability (W¨ achter and Biegler, 2006), (Vanderbei, 1999). 2.3 Predictor-corrector Methods

li (xi , ui ) + lN (xN )

i=0

ˆ0 = 0 x0 − x xi+1 = f (xi , ui ), g(xi , ui ) ≤ 0, gN (xN ) ≤ 0,

∇x l0 (w0 ) + ∇x f (w0 )λ1 − λ0 − ∇x g(w0 )ν0 ∇u l0 (w0 ) + ∇u f (w0 )λ1 − ∇u g(w0 )ν0 x0 − x ˆ0 g(w0 ) + s0 S0 ν0 .. . ∇x lN (xN ) − λN xN − f (wN −1 ) g(xN ) + sN SN νN

13189

(1) i = 0, . . . , N − 1 i = 0, . . . , N − 1

where the functions li , lN , f , g and gN are twice continuously differentiable. States and inputs of the dynamical system are represented by x and u, respectively. When using NMPC to control a system, problem (1) has to be solved, at every sampling instant, for a new x ˆ0 describing the current state of the system. The first optimal input u∗0 is applied to the system and, at the next sampling instant, a new optimization problem is solved. In this way, the control algorithm can compensate for model uncertainty and disturbances.

2.2 Newton-type Optimization Problem (1) can be solved in several ways. In particular, two main classes of methods can be identified (Nocedal and Wright, 2006): the so-called sequential quadratic programming (SQP) methods and interior-point methods. The first relies on the solution of a series of convex quadratic programs (QP) that locally approximate the original problem. The second directly solves the relaxed Karush-KuhnTucker (KKT) system associated with problem (1):

In the following, problem (2) is referred to in compact form F (z) + Cξ = 0,

(3)

where z is the vector of stacked primal, dual and slack variables and ξ := (ˆ x0 , τ ). An approximate solution to (3) for a given (ˆ x0 , τ ) will be denoted by z˜(ˆ x0 , τ ) ≈ z¯(ˆ x0 , τ ), where z¯(ˆ x0 , τ ) denotes the exact solution. Note that an exact Newton step (Deuflhard, 2011) reads as  ∂F k −1  z k+1 = z k − (z ) F (z k ) + Cξ , (4) ∂z for a given z k and ξ. As discussed in (Tran-Dinh et al., 2012), a predictor-corrector step can be performed to obtain an approximate solution z k+1 ≈ z¯(ξ k+1 ) for a new parameter value ξ k+1 , given z k ≈ z¯(ξ k ). This combined predictor-corrector step takes the form  ∂F k −1  z k+1 = z k − (z ) F (z k ) + Cξ k ∂z  ∂F k −1  k+1 (5) (z ) C ξ − ξk − ∂z  ∂F k −1  (z ) F (z k ) + Cξ k+1 . = zk − ∂z As the parameter ξ enters linearly in Eq. (3), the above predictor-corrector step corresponds to a standard Newton step (4) applied directly to the problem with the new value ξ k+1 . This concept of introducing the parameter ξ linearly in order to simplify the continuation procedure is also referred to as parameter embedding in (Diehl, 2002).

13731

Proceedings of the 20th IFAC World Congress 13190 Andrea Zanelli et al. / IFAC PapersOnLine 50-1 (2017) 13188–13193 Toulouse, France, July 9-14, 2017

Note that, instead of using the exact Jacobian J(z k ) := ∂F k ∂z (z ) and its factorization in (5), it is common practice to use a Jacobian approximation Mk ≈ J(z k ) in order to reduce the overall computational burden (Deuflhard, 2011). We further consider the general class of Newtontype methods, e.g., including Gauss-Newton or quasiNewton Hessian approximations.

When solving neighboring instances of (1) for different initial conditions x ˆ0 with an interior-point method, information available from previously computed solutions is difficult to exploit for warm-starting in general. If an approximate solution z˜(ˆ x0 , 0) to equation (3) is used to initialize the Newton-type iterates for a different x ˆ0 , a number of problems can arise (Benson and Shanno, 2008). These issues are generally related to the need to bring the barrier parameter to some positive value τ0 in order to be able to adjust the iterates, without violating the positivity constraints imposed on inequality multipliers ν and slacks s. In particular, certain components of ν and s in z˜(ˆ x0 , 0) might be very close to zero and the enforcement of the positivity constraints can lead to very small steps. Moreover, due to the presence of the complementarity conditions, equations (3) become highly nonlinear for small values of τ , resulting in a poor performance of Newtontype algorithms. Algorithm 1 Homotopy method: Phase I

3: 4: 5: 6: 7: 8: 9:

z˜(ˆ x− 0 , τ0 )

0 input: ξ 0 = (ˆ x− ˜(ξ 0 ), x ˆ+ 0 , τ0 ), z = z 0 for k = 0, . . . , k1max x ˆk+1 ← φ1 (ξ k , z k ) 0 k+1 ξ ← (ˆ xk+1 , τ0 )  0  k+1 z ← z k − Mk−1 F (z k ) + Cξ k+1   if F (z k+1 ) + Cξ k+1 < 1 and x ˆk+1 =x ˆ+ 0 0 return z k+1 end end

Phase I

z˜(ˆ x+ 0 , τ0 ) Phase II

z¯(ˆ x0 , τ0 )

∗ z˜(ˆ x+ 0 ,τ )

z¯(ˆ x0 , 0) x ˆ− 0

3. THE HOMOTOPY-BASED ALGORITHM

1: 2:

z

x ˆ+ 0

x ˆ0

Fig. 1. This figure illustrates the two phases of the homotopy-based interior-point algorithm. During Phase I, the barrier parameter τ = τ0 is kept fixed and the initial state of the system x ˆ0 is updated. Once the approximate solution z˜(ˆ x+ 0 , τ0 ) is obtained, Phase 2 is used to compute a solution to the original problem. second phase in which τ is shrunk according to a standard barrier strategy for interior-point methods. Before doing so, the intermediate solution z˜(ˆ x+ 0 , τ0 ) is stored in order to be able to warm-start the algorithm for the next instance. This final procedure is described in Algorithm 2 and illustrated in Figure 1. Note that the performance of both Algorithm 1 and 2 depends on the policy used to update the homotopy parameter ξ, which is respectively defined by the functions φ1 (·) and φ2 (·). Advanced strategies can be used for this purpose, which are known in the context of path-following or continuation methods. A detailed discussion on the design of these update policies is outside the scope of this paper and the interested reader is referred to (Allgower and Georg, 1990; Deuflhard, 2011). Algorithm 2 Homotopy method: Phase II 1: 2: 3: 4: 5: 6:

In order to be able to exploit solutions available from previous instances of the OCP, a homotopy-based interiorpoint method is proposed in the following. Assume that problem (3) for x ˆ0 = x ˆ− 0 has been previously solved and a ˆ+ new solution has to be computed for x ˆ0 = x 0 . The main additional ingredient with respect to a standard warmstarting procedure, where z˜(ˆ x− 0 , 0) would be used, is the use of an approximate solution z˜(ˆ x− 0 , τ0 ) for some τ0 > 0. The idea consists in splitting the iterates on the relaxed problem (3) into two different phases. In the first phase, Newton-type iterates are performed on (3) for τ = τ0 and x ˆ0 approaching x ˆ+ ˆ− 0 , starting from x 0 . This configuration might be interpreted as the Newton-type method being applied to a smooth, unconstrained problem and initialized with z 0 = z˜(ˆ x− ˆ0 , the 0 , τ0 ). For modest perturbations of x iterates would quickly converge to a solution of the intermediate barrier problems. Algorithm 1 summarizes this first phase of the scheme, which is additionally illustrated in Figure 1. Once an approximate solution to (3) for τ = τ0 and x ˆ0 = x ˆ+ 0 has been obtained, the algorithm switches to a

7: 8: 9:

0 input: ξ 0 = (ˆ x+ ˜(ξ 0 ) 0 , τ0 ), z = z for k = 0, . . . , k2max τ k+1 ← φ2 (ξ k , z k ) k+1 ξ k+1 ← (ˆ x+ ) 0 ,τ  k+1 k z  ← z − Mk−1 F(z k ) + Cξ k+1 if F (z k+1 ) + Cξ k+1  < 1 and τ k+1 < τ ∗ return z k+1 end end

Remark 1: In Algorithm 1, the initial state x ˆ0 is potentially updated several times according to a given policy φ1 (·). However, in NMPC applications with sufficiently high sampling rates, it might be sufficient to perform the ˆ+ homotopy from x ˆ− 0 to x 0 in one iteration of Phase I. The numerical results in Section 5 will indeed use this approach to solve a series of neighboring optimization problems in an NMPC implementation. Remark 2: Notice that Algorithms 1 and 2 can be seen as special cases of a single algorithm in which the homotopy parameter ξ is updated, according to some policy φ. In particular, as both x ˆ0 and τ enter the problem linearly, the analysis presented in the following section holds for both algorithms. On the one hand, it provides convergence results for Phase I during which x ˆ0 is updated. On the other hand, the same results apply to Phase II, where the barrier parameter τ is changed.

13732

Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 Andrea Zanelli et al. / IFAC PapersOnLine 50-1 (2017) 13188–13193

∆zk+1 = Mk−1 (Mk − J(z k ))(z k − z¯k )

4. LOCAL CONVERGENCE RESULTS

− Mk−1 (Mk − J(z k ))(¯ z k+1 − z¯k )  1 − Mk−1 (J(¯ z k + t(z k − z¯k )) − J(z k ))(z k − z¯k )dt 0  1 −1 +Mk (J(¯ z k + t(¯ z k+1 − z¯k ))−J(z k ))(¯ z k+1 − z¯k )dt.

In this section, theoretical results regarding the convergence of the proposed method are derived. In particular, we consider the Newton-type predictor-corrector step in line 5 of Algorithm 1 and 2. As x ˆ0 and τ enter the problem linearly, the following results hold for both Phase I and Phase II of the algorithm. 4.1 Local Contraction Theorem For a compact notation, the solution to (3) for ξ = ξ k will be referred to as z¯k . Consider the Newton-type update z k+1 = z k − Mk−1 (F (z k ) + Cξ k+1 ),

(6)

where a Jacobian approximation Mk is used in (5). Notice that, at the solution z¯k+1 , the following holds: k+1

z¯

k+1

= z¯

k+1

−

Mk−1 (F (¯ z k+1 )

+ Cξ

k+1

),

(7)

k+1

) + Cξ = 0. because F (¯ z Assumption 1. (Lipschitz continuity). There exists a constant σ ≥ 0 such that for every solution z¯k and z¯k+1 , associated with ξ k and ξ k+1 , respectively, the following inequality holds:  k+1    z¯ − z¯k  ≤ σ ξ k+1 − ξ k  .

Assumption 2. (ω- and κ-conditions). There exist ω < ∞ and κ < 1 such that, for any given solution z¯k , iterate z k and iteration matrix Mk satisfy   (1) M −1 (J(z k ) − Mk ) ≤ κ k

    z k )) ≤ ω z − z¯k  , ∀z. (2) Mk−1 (J(z) − J(¯

The following Theorem provides a convergence proof for the Newton-type predictor-corrector method in (6), based on the results in (Tran-Dinh et al., 2012). Theorem 3. Let Assumptions 1 and 2 hold. The following  inequality holds for the sequence z k k≥0 , generated by the Newton-type predictor-corrector iterations (6):   ω ∆zk+1  ≤ (κ + ωσ ξ k+1 −ξ k  + ∆zk ) ∆zk  2 (8)   k+1 k  ωσ 2  k+1 k ξ + (κσ + −ξ ) ξ −ξ  , 2 k+1 := − z¯k+1 and ∆zk := z k − z¯k . where ∆zk+1 z Proof. Using (6) and (7), ∆zk+1 can be rewritten as ∆zk+1 = Mk−1 (Mk z k − F (z k ) − Cξ k+1 )

− Mk−1 (Mk z¯k+1 − F (¯ z k+1 ) − Cξ k+1 ),   zk ) adding and subtracting Mk−1 Mk z¯k + F (¯

∆zk+1 = Mk−1 (Mk (z k − z¯k ) − F (z k ) + F (¯ z k ))

− Mk−1 (Mk (¯ z k+1 − z¯k ) − F (¯ z k+1 ) + F (¯ z k ))

= Mk−1 Mk (z k − z¯k ) − Mk−1 Mk (¯ z k+1 − z¯k )  1 − Mk−1 J(¯ z k + t(z k − z¯k ))(z k − z¯k )dt 0  1 + Mk−1 J(¯ z k + t(¯ z k+1 − z¯k ))(¯ z k+1 − z¯k )dt, 0

zk ) adding and subtracting Mk−1 J(z k )(z k + z¯k+1 − 2¯

13191

0

Then, using the κ- and ω-conditions in Assumption 2     ∆zk+1  ≤ κ z k − z¯k  + κ z¯k+1 − z¯k   1     + ω z¯k + t(z k − z¯k ) − z k  dt z k − z¯k  0  1     + ω z¯k + t(¯ z k+1 − z¯k ) − z k  dt z¯k+1 − z¯k  0

ω ∆zk ) ∆zk  2    ω + (κ + z¯k+1 − z¯k  + ω ∆zk ) z¯k+1 − z¯k  2 and, finally, due to the regularity Assumption 1   ω ∆zk+1  ≤ (κ + ωσ ξ k+1 − ξ k  + ∆zk ) ∆zk  2    ωσ 2  ξ k+1 − ξ k ) ξ k+1 − ξ k  . + (κσ +  2 ≤ (κ +

Theorem 3 shows how the update of the parameter ξ affects contraction of the Newton-type iterates. In particular, it can be expected that, for small enough perturbations, the optimal manifold can be tracked by the predictor-corrector scheme. 4.2 An Illustrative Example In order to illustrate the results of Theorem 3, consider the nonlinear root-finding problem (3) with   20z1 z2 − z2 := (9) F (z) log(3z2 + 3) + sin(z1 ) + log(z1 )

and C = 3 · I2 . Figure 2 shows the results obtained by applying the homotopy method, with exact Jacobians, to Eq. (9) for 50 iterations with

(ξf − ξ0 ) · k , k = 1, . . . , 50, 50 where ξ0 = [ 0 0 ]T and ξf = [ 5 5 ]T . Every 5 iterations, the parameter is frozen and five exact Newton steps are taken to illustrate the locally quadratic convergence. ξk = ξ0 +

5. NUMERICAL RESULTS In the following, the computational advantages of the proposed method are assessed on a non-trivial numerical example. The nonlinear nonconvex optimization problems arising from an NMPC formulation will be solved with both a cold-started and a homotopy-based interior-point method. For this purpose, the presented homotopy-based algorithm has been implemented in the software package FORCES NLP (Zanelli et al., 2016) that uses a structureexploiting primal-dual interior-point method for multistage nonlinear nonconvex problems. The benchmark consists in the swing-up of an inverted pendulum, described by the differential equations: 13733

Proceedings of the 20th IFAC World Congress 13192 Andrea Zanelli et al. / IFAC PapersOnLine 50-1 (2017) 13188–13193 Toulouse, France, July 9-14, 2017

4

z1 z2 z¯1 z¯2

0

states

solution

1 0.5 −0.5 −1

0

5

10

15

20 25 30 35 homotopy iterations

40

45

∆zk+1 2 κ ¯

0 0

1

2

3

4

5

6

7

8

9

10

11

0

1

2

3

4

5

6

7

8

9

10

11

10

F

5

0

5

10

15

20 25 30 35 homotopy iterations

40

45

0 −5

50

−10

0.8

Fig. 3. Closed-loop trajectories for the pendulum example: both the cold-started and the homotopy-based algorithm result in the same swing-up performance.

0.6 0.4 0.2

2

50

0

10 10−4 10−8 10−12 10−16

p θ

0

5

10

15

20 25 30 35 homotopy iterations

40

45

50

Fig. 2. Convergence of the homotopy method on the simple root-finding problem described by (9). Every five Newton iterations, ξ is fixed to illustrate the locally quadratic convergence (dashed red). The ∆z 2 quantity κ ¯ := ∆zk+1 shows the contraction rate k 2 of the iterations, where ∆zk := z k − z¯k .  v   ω     −lmsθ ω 2 + F + gmcθ sθ ,  x˙ =  2  M + m − mc θ    −lmcθ sθ ω 2 + F cθ + gmsθ + M gsθ  l(M + m − mc2θ ) 

(10)

where x = (p, θ, v, ω) is the state of the system, in which p and v are the linear position and velocity of the cart and θ and ω are the angle and angular velocity of the pendulum. The input to the system is the force F applied to the cart, while m, l, M and g are fixed parameters representing the mass of the pendulum, its length, the mass of the cart, and gravity respectively. Note that the compact notation sθ := sin(θ) and cθ := cos(θ) is used in Eq. (10). An OCP of the form in (1) is considered, where f (·) represents the discretized dynamics obtained by applying the explicit Runge-Kutta scheme of order four with fixed step-size and ten intermediate integration steps. A control horizon T = 2s is used and the trajectories are discretized using N = 70 shooting nodes (Bock and Plitt, 1984). Simple bounds are imposed on the input: −10 N ≤ F ≤ 10 N

(11)

and a quadratic cost  1 T li (xi , ui ) = xi Qxi + uTi Rui + q T xi + rT ui 2 1 T T lN (xN ) = xN QN xN + qN xN , 2 has been used, with Q = diag( 0.5, 1, 0.002, 0.002),

R = 0.001

and q = −Qxr , r = 0 and qN = −QN xr . The reference xr (t) is defined as follows:  T t ≤ 2s [0 π 0 0] xr (t) = T [0 0 0 0] t > 2s. The proposed homotopy-based implementation uses a fixed τ0 = 0.0001, a single step in Phase I and a monotone strategy (Fiacco and McCormick, 1990) in Phase II. The cold-started algorithm uses instead an adaptive barrier strategy that, at every step, adjusts τ according to a measure of progress on the complementarity condition (Vanderbei, 1999). Notice that, for the cold-started algorithm, the adaptive strategy has been chosen over the monotone one, since the latter led to poor performance in the numerical experiments. Both implementations use a blocked BFGS Hessian approximation described in (Bock and Plitt, 1984). Figure 3 shows the closed-loop trajectories obtained with FORCES NLP using both algorithms, while Figure 4 compares the two strategies in terms of number of iterations required to solve the optimization problems and timings. Note that a disturbance is applied to the system between time t1 = 6.0s and t2 = 6.2s by replacing the optimal input u∗0 with the perturbed input u∗0 + 8. Using the homotopybased approach the number of iterations can be largely reduced, leading to considerable speedups. The worst-case number of iterations of 280 is reduced to 57 iterations and, for a considerable part of the scenario, a speedup of more than an order of magnitude is achieved. 6. CONCLUSIONS AND OUTLOOK A homotopy-based interior-point algorithm for NMPC applications that can exploit warm-starts has been proposed. Theoretical results that provide a contraction estimate for the algorithm are derived. In particular, it is shown that the optimal manifold can be tracked with a single Newtontype iteration for every homotopy parameter update. The method has been implemented in the software package FORCES NLP and the advantages of the algorithm have been assessed on a non-trivial example where a speedup of more than an order of magnitude can be achieved with respect to a cold-started implementation.

13734

speedup

CPU time [s]

iterations

Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 Andrea Zanelli et al. / IFAC PapersOnLine 50-1 (2017) 13188–13193

300 200 100 0

FORCES NLP - homotopy FORCES NLP - cold-started

0

1

2

3

4

5 6 time [s]

7

8

9

10

11

0

1

2

3

4

5 6 time [s]

7

8

9

10

11

0

1

2

3

4

5 6 time [s]

7

8

9

10

11

10−1 10−2 10−3

20 10 1

Fig. 4. Number of iterations and computation times for the cold-started and homotopy-based interior-point method, based on a BFGS Hessian approximation. The number of iterations can be largely reduced, resulting in a speedup of about an order of magnitude. An additional implementation of the proposed approach that exploits the hardware-tailored linear algebra routines available in the software package HPMPC (Frison et al., 2014) is part of ongoing development. REFERENCES Allgower, E.L. and Georg, K. (1990). Introduction to Numerical Continuation Methods. Colorado State University Press. Benson, H.A. and Shanno, D. (2008). Interior-point methods for nonconvex nonlinear programming: regularization and warmstarts. Computational Optimization and Applications, 40(143), 143–189. Bock, H.G. and Plitt, K.J. (1984). A multiple shooting algorithm for direct solution of optimal control problems. In Proceedings of the IFAC World Congress, 242–247. Pergamon Press. Deuflhard, P. (2011). Newton methods for nonlinear problems: affine invariance and adaptive algorithms, volume 35. Springer. Diehl, M. (2002). Real-Time Optimization for Large Scale Nonlinear Processes, volume 920 of Fortschritt-Berichte VDI Reihe 8, Meß-, Steuerungs- und Regelungstechnik. VDI Verlag, D¨ usseldorf. PhD Thesis. Diehl, M., Findeisen, R., and Allg¨ ower, F. (2007). A stabilizing real-time implementation of nonlinear model predictive control. In L. Biegler, O. Ghattas, M. Heinkenschloss, D. Keyes, and B. van Bloemen Waanders (eds.), Real-Time and Online PDE-Constrained Optimization, 23–52. SIAM. Diehl, M., Bock, H.G., Diedam, H., and Wieber, P.B. (2006). Fast Motions in Biomechanics and Robotics, volume 340, chapter Fast Direct Multiple Shooting Algorithms for Optimal Robot Control, 65–93. Springer. Ferreau, H.J., Houska, B., Geebelen, K., and Diehl, M. (2011). Real-time control of a kite-model using an autogenerated nonlinear MPC algorithm. In Proceedings of the 18th IFAC World Congress.

13193

Fiacco, A. and McCormick, G.P. (1990). Nonlinear Programming: Sequential Unconstrained Minimization Techniques. SIAM publications. Frasch, J.V., Gray, A.J., Zanon, M., Ferreau, H.J., Sager, S., Borrelli, F., and Diehl, M. (2013). An auto-generated nonlinear MPC algorithm for real-time obstacle avoidance of ground vehicles. In Proceedings of the European Control Conference (ECC), 4136–4141. Frison, G., Sorensen, H.B., Dammann, B., and Jørgensen, J.B. (2014). High-performance small-scale solvers for linear model predictive control. In Proceedings of the European Control Conference (ECC), 128–133. Garc´ıa, C., Prett, D., and Morari, M. (1989). Model Predictive Control: Theory and Practice – a Survey. Automatica, 25, 335ff. Gondzio, J. and Grothey, A. (2006). A new unblocking technique to warmstart interior point methods based on sensitivity analysis. Siam J. Control Optim., 19(3), 1184–1210. Mayne, D.Q., Rawlings, J.B., Rao, C.V., and Scokaert, P.O.M. (2000). Constrained model predictive control: Stability and optimality. Automatica, 26(6), 789–814. Nocedal, J. and Wright, S.J. (2006). Numerical Optimization. Springer Series in Operations Research and Financial Engineering. Springer, 2 edition. Ohtsuka, T. (2004). A continuation/GMRES method for fast computation of nonlinear receding horizon control. Automatica, 40(4), 563–574. Qin, S. and Badgwell, T. (2000). An overview of nonlinear model predictive control applications. In F. Allg¨ ower and A. Zheng (eds.), Nonlinear Predictive Control, volume 26 of Progress in Systems Theory, 370–392. Birkh¨auser, Basel Boston Berlin. Qin, S. and Badgwell, T. (2003). A survey of industrial model predictive control technology. Control Engineering Practice, 11, 733–764. Shahzad, A. and Goulart, P. (2011). A new hot-start interior-point method for model predictive control. In Proceedings of the IFAC World Congress. Shahzad, A., Kerrigan, E., and Constantinides, G. (2010). A warm-start interior-point method for predictive control. Technical report, Imperial College London. Tran-Dinh, Q., Savorgnan, C., and Diehl, M. (2012). Adjoint-based predictor-corrector sequential convex programming for parametric nonlinear optimization. SIAM J. Optimization, 22(4), 1258–1284. Vanderbei, R.J. (1999). Loqo: An interior point code for quadratic programming. Optimization methods and software, 11(1-4), 451–484. W¨achter, A. and Biegler, L.T. (2006). On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming. Mathematical Programming, 106(1), 25–57. Yildirim, E. and Wright, S.J. (2002). Warm-start strategies in interior-point methods for linear programming. SIAM Journal on Optimization, 12(3), 782–810. Zanelli, A., Domahidi, A., Jerez, J., and Morari, M. (2016). FORCES NLP: An efficient implementation of interior-point methods for multistage nonlinear nonconvex programs. International Journal of Control: Special Issue on MPC Algorithms and Applications. (accepted for publication).

13735