However, a typical model also shows a lot of structure that can be exploited in a rather elegant and efficient way. The focus of this paper is on nonlinear models with linear subsystems.

Efficient NMPC for nonlinear models with linear subsystems

Rien Quirynen, S´ebastien Gros and Moritz Diehl

Abstract— Real-time optimal control algorithms for fast, mechatronic systems need to be run on embedded hardware and they need to respect tight timing constraints. When using nonlinear models, the simulation and generation of sensitivities forms a computationally demanding part of any algorithm.

Automatic code generation of Implicit Runge-Kutta (IRK) methods has been shown to reduce its CPU time significantly.

However, a typical model also shows a lot of structure that can be exploited in a rather elegant and efficient way. The focus of this paper is on nonlinear models with linear subsystems.

With the proposed model formulation, the new auto generated integrators can be considered a powerful generalization of other solvers, e.g. those that support quadrature variables. A speedup of up to 5 − 10 is shown in the integration time for two examples from the literature.

Keywords : structure exploitation, code generation, IRK methods, NMPC, embedded optimization

I. INTRODUCTION

Nonlinear Model Predictive Control (NMPC) [1] and Moving Horizon Estimation (MHE) [2] are popular ap- proaches for real-time optimal control and estimation, since they can explicitly handle constraints and nonlinear dy- namics. In the case of a system with fast dynamics, the high computational burden forms a major challenge. An Optimal Control Problem (OCP) needs to be solved at each sampling time, respecting the hard timing constraints which are imposed by real-time applications. Recent algorithmic progress [3], [4], [5] allows to consider NMPC and MHE also for fast systems. Among the available online algorithms, the Real-Time Iteration (RTI) scheme [6] has been proposed as a highly competitive approach.

In addition to using an online algorithm, efficient imple- mentations are needed in a specific programming language to allow running it in real-time on embedded control hardware.

One way to achieve this is by automatic code generation, i.e.

by exporting a customized solver. Significant improvements

R. Quirynen, S. Gros and M. Diehl are with the Op- timization in Engineering Center (OPTEC), KU Leuven, Kasteelpark Arenberg 10, B-3001 Leuven-Heverlee, Belgium.

rien.quirynen@esat.kuleuven.be

* This research was supported by Research Council KUL: PFV/10/002 Optimization in Engineering Center OPTEC, GOA/10/09 MaNet and GOA/10/11 Global real- time optimal control of autonomous robots and mechatronic systems. Flemish Government: IOF / KP / SCORES4CHEM, FWO: PhD/postdoc grants and projects: G.0320.08 (convex MPC), G.0377.09 (Mechatronics MPC); IWT: PhD Grants, projects: SBO LeCoPro;

Belgian Federal Science Policy Office: IUAP P7 (DYSCO, Dynamical systems, control and optimization, 2012-2017); EU: FP7- EMBOCON (ICT- 248940), FP7-SADCO ( MC ITN-264735), ERC ST HIGHWIND (259 166), Eurostars SMART, ACCM. The first author holds a PhD fellowship of the Research Foundation – Flanders (FWO). The authors also thank M.

Vukov for fruitful discussions and joint development of the ACADO code generation tool.

in the computation time can also be obtained by removing unnecessary computations, optimizing the memory access and cache usage, and by exploiting the problem structure and sparsity patterns. This rather old idea has become quite popular for e.g. convex optimization [7]. It is now possible to assemble a complete algorithm for solving an OCP, starting from different components of which each could be code generated when advantageous. This is the approach pursued by the ACADO code generation tool, which exports highly efficient C-code based on the RTI scheme [8]. It has a MATLAB interface to export and use the same code in simulations [9].

A rather important component in case of NMPC is the one that handles integration and sensitivity generation for the nonlinear model. It forms typically a major or even dominating factor in the total computation time, while the accuracy of the derivatives also strongly defines the suc- cess of the optimizer. Because of the need for a more or less deterministic runtime in case of real-time applications, adaptivity is avoided where possible. The assumption in this paper is therefore that the step size and order of the integration method are fixed. It has been shown that code generation for Implicit Runge-Kutta (IRK) methods with a tailored approach for computing the sensitivities can be very efficient [10]. This is important because of the attractive properties of A-stable IRK methods and their natural exten- sion from systems of Ordinary Differential Equations (ODE) to Differential Algebraic Equations (DAE) of index 1. In the specific case of collocation methods, extra output functions can be evaluated on an arbitrary grid and this at a rather low cost [9].

In this paper, the following OCP formulation is addressed min

x(·),u(·) Z t+T

t (kx(τ) − x ref (τ)k ² _Q + ku(τ) − u ref (τ)k ² _R ) dτ + kx(t + T ) − x _ref (t + T )k ² _P

s.t. x(t) = ¯ x t ,

x(τ) = f (τ, x(τ), u(τ)), ˙ u ¯ ≤ u(τ) ≤ ¯u,

x ¯ ≤ x(τ) ≤ ¯x, ∀τ ∈ [t,t + T ], (1)

in which x and u are the states and controls, x _ref and u _ref are

the state and control references, t denotes the current time

and T the length of the prediction horizon. Of particular

interest is the nonlinear function f which defines the system

dynamics. It is known that the models used in applications,

either ODE or DAE systems, often have a clear structure

resulting in e.g. a sparse Jacobian matrix. A scalable way to

Fig. 1. Schematic illustrating the three stages, together with the workflow in the structure exploiting integrators.

handle this is by using sparse solutions for the linear algebra operators when that is advantageous. It will however create little to no gain for small to medium scale systems which appear in the problems that are targeted by this paper. The goal is therefore to have a closer look at the structure specific to these systems and to propose an alternative approach for structure exploitation, that will be shown to perform very well. The addressed model class contains as a subclass the Wiener-Hammerstein models which are since a long time used in system identification [11].

Contribution: This paper presents algorithms to exploit a frequently occurring model structure, resulting in very efficient auto generated integrators.

The paper is organized as follows. In Section II, a three stage model formulation will be proposed. Section III sum- marizes the used framework of collocation methods with sensitivity generation. Then, the issue of how to exploit the three stage structure in these IRK methods, is addressed by Section IV. Finally, some real-world applications are used in Section V to illustrate the structure exploiting integrators and their performance.

II. T HE THREE STAGE STRUCTURE

In this section, a typical structure consisting of three different stages in the system of differential equations is discussed.

A. Model formulation

When modeling for example a mechatronic system, the result is typically a set of nonlinear differential equations with possibly some algebraic states. In the case of an explicit ODE such as ˙ x = f (x, u) from the OCP in (1), one would often recognize one or more of the following three subsystems in this specific order

˙

x ^[1] = A ₁ x ^[1] + B ₁ u, (linear input system) x ˙ ^[2] = f ₂ (x ^[1] , x ^[2] , u), (nonlinear system)

˙

x ^[3] = A ₃ x ^[3] + f ₃ (x ^[1] , x ^[2] , u), (linear output system) (2) with the matrices A ₁ , B ₁ and A ₃ and the nonlinear functions f 2 and f ₃ defining the subsystems. Figure 1 illustrates this chain of three subsystems. In what follows, N _x ^[1] , N _x ^[2]

and N _x ^[3] denote the number of differential states in each of the three subsystems. As mentioned earlier, this class comprises Wiener-Hammerstein models which consist of a linear dynamic system followed by a static nonlinearity and

another linear dynamic system. In our notation, they are of form (2) with N _x ^[2] = 0. Let us generalize this structure to an implicit DAE system of index 1 as follows:

C 1 x ˙ ^[1] = A ₁ x ^[1] + B ₁ u, (3a) 0 = f ₂ (x ^[1] , x ^[2] , ˙ x ^[1] , ˙ x ^[2] , z, u), (3b) C ₃ x ˙ ^[3] = A ₃ x ^[3] + f ₃ (x ^[1] , x ^[2] , ˙ x ^[1] , ˙ x ^[2] , z, u), (3c) with invertible matrices C ₁ and C ₃ and function f ₂ satisfying

∂ f 2

∂ (z, ˙ x ^[2] ) invertible.

B. Motivation

The introduced structure is not something that comes up only in rare occasions. These stages almost always arise naturally when modeling for control. A linear input system (3a) can result from

• some partially linear dynamics which are independent of the states in the nonlinear equations. A classical example would be the double integrator

˙ x ₁ = u

˙

x ₂ = x ₁ (4)

but any linear variant of this is possible.

• any linear high order differential equation, which would be transformed into a set of first order equations

d ⁿ x

dt ⁿ = h(t, u(t), x(t), . . . , d ⁿ⁻¹ x dt ⁿ⁻¹ )

⇓ x 1 = x, . . . , x _n = d ⁿ⁻¹ x dt ⁿ⁻¹

˙ x ₁ = x ₂

. . .

˙

x _n = h(t, u(t), x ₁ (t), . . . , x _n (t)),

(5)

with h a linear function.

• implementing a filter on the controls or input states. In optimal control, it e.g. makes sense to extra penalize the high frequency content. Therefore, a High-Pass RC filter such as

dx(t)

dt = −ω _C x(t) + du(t)

dt (6)

can be used, with ω C = _RC ¹ = 2π f C in which f _C is called the cutoff frequency.

The linear output system in (3c) is somewhat similar to the input system in (3a), but with a nonlinear relation with respect to the previous states and the control inputs. An output system can therefore result from some partially linear dynamics which also depend on the previous states or it can implement a filter for these states. In the latter cases, the matrix A ₃ will generally be nonzero. When A ₃ = 0 and C 3

is an identity matrix, Equation (3c) reduces to

˙

x ^[3] = f ₃ (x ^[1] , x ^[2] , ˙ x ^[1] , ˙ x ^[2] , z, u) (7)

which are known in a different context as quadrature states

[12]. They are typically used to formulate objective and

constraint functions in an OCP.

III. A UTO GENERATED IRK METHODS

This section compactly describes the general implementa- tion of auto generated IRK methods, presented earlier in [10]

and [9]. Their formulation is handled in Subsection III-A, for ODE as well as DAE systems of index 1. Some comments on the implementation are repeated in Subsection III-B and the used methods for efficient generation of continuous output and sensitivities are respectively described by Subsection III- C and III-D.

A. Formulation

The general IRK methods from [9] solve the following Initial Value Problem (IVP) over a certain time:

0 = f (t, ˙ x(t), x(t), z(t), u(t)),

x(0) = x 0 , (8)

with x(t) a vector of N _x differential states, ˙ x(t) the corre- sponding time derivatives, z(t) a vector of N _z algebraic states and u(t) a vector of control inputs so that f defines a system of N _x + N _z equations. Note that N _x = N _x ^[1] + N _x ^[2] + N _x ^[3] in case of the structure in (3). These methods handle models ranging from explicit ODE until fully implicit DAE systems.

The Jacobian matrix ^{∂ f}

∂ (z, ˙ x) needs to be invertible, so that the algebraic states z and the differential state derivatives ˙ x are uniquely defined, i.e. the DAE system should be of index 1 [14].

Because of their high order of accuracy and good stability properties, the focus of this paper will be on A-stable IRK methods such as the Gauss methods [15]. Applied to the system in (8) for the integration from t _n until t n+1 = t _n + h, an s-stage IRK method can be formulated as follows:

0 = f t _n + c _i h, k _i , x _n + h

s

∑ j=1

a _{i j} k _j , Z _i

!

, i = 1, . . . , s,

x n+1 = x _n + h

s i=1 ∑

b _i k _i ,

(9)

with Z _i the stage values of the algebraic states, k _i the stage values of the differential state derivatives and the coefficients c i , b _i and a _{i j} are defined by the Butcher table. The method results in a nonlinear system of (N _x + N _z ) × s equations that can be solved using a Newton method. Using this scheme, consistency of the state values is only assured at the s collocation nodes. For stiffly accurate methods such as the Radau IIA methods, the endpoint is included in these nodes.

B. Implementation

With real-time applications in mind, the ACADO code generation tool exports a customized integrator with a deter- ministic runtime. The step size, the order of the method and the number of Newton iterations are therefore fixed. Let us write the nonlinear system from (9) as G(x _n , K) = 0, resulting in the following Newton-type iterations:

M = ∂ G

∂ K (x _n , K ^[0] ),

K ^[i] = K ^[i−1] − M ⁻¹ G(x _n , K ^[i−1] ), i = 1, . . . , L,

(10)

with M the evaluation of the Jacobian ∂ G/∂ K in the ini- tialization point and the variables K = (k ₁ , . . . , k _s , Z ₁ , . . . , Z _s ).

Note that one Jacobian evaluation and a small amount of iterations L is sufficient if this initialization K ^[0] is good enough. The evaluation of the Jacobian can be done using Al- gorithmic Differentiation (AD) [16]. A custom linear solver can even be exported, e.g. based on the LU decomposition of the matrix M.

C. Continuous output

Promising possibilities of auto generated IRK methods with continuous output have already been mentioned and illustrated in [9]. In the case of collocation methods, a specific family of IRK methods, this continuous exten- sion comes naturally. Using the collocation variables K = (k ₁ , . . . , k _s , Z ₁ , . . . , Z _s ), a polynomial is defined for x(t), ˙ x(t) and z(t):

x(t _n + ch) ≈ x _n + h

s i=1 ∑

k _i Z _c

0

l _i (τ) dτ,

˙

x(t _n + ch) ≈

s i=1 ∑

l i (c)k _i ,

z(t _n + ch) ≈

s i=1 ∑

l _i (c)Z _i ,

(11)

where l _i (t) = ∏ j6=i t−c _j

c i −c _j are the Lagrange interpolating poly- nomials. In the case of an s-stage IRK method of order p, the order of this approximation is p ^∗ = min(p, s + 1) for x(t) and p ^∗ − 1 for ˙x(t) and z(t) [17]. Output functions as general as

y = ψ(t, ˙x(t), x(t), z(t)), (12) can efficiently be evaluated on an arbitrarily fine grid.

D. Sensitivity generation

In the context of dynamic optimization, sensitivities with respect to all the independent variables w _n = (x, u) _n are needed in addition to the simulated values of states and outputs. In the case of extra outputs, forward techniques to compute the sensitivities ∂ (x n+1 , z _n+1 )/∂ w n are recom- mended. A thorough discussion on such techniques of sen- sitivity generation for IRK methods can be found in [13].

The conclusion there is that the most efficient method is to apply the Implicit Function Theorem (IFT) to G(w _n , K) = 0, resulting in

dK

dw _n = − ∂ G

∂ K

−1 ∂ G

∂ w n

, (13)

which can then be used to compute the sensitivities of the states and outputs. This direct approach can provide very accurate sensitivities, which is important for optimization.

The Jacobian evaluation in (13) and its factorization can be

reused in (10) for the next integration step [10]. Algorithm 1

compactly describes the resulting implementation.

Algorithm 1 The implementation of one step Input: w _n , initial K ^[0] , LU factorization of M Output: (x, ∂ x/∂ w n ) _n+1 and (z, ∂ z/∂ w n ) _n

1: for i = 1 → L do

2: K ^[i] ← K ^[i−1] − M ⁻¹ G(w _n , K ^[i−1] )

3: end for

4: x n+1 ← x _n + h ∑ ^s _i=1 b _i k _i

5: z _n ← ∑ ^s i=1 l _i (0)Z _i with l _i (t) = ∏ j6=i t−c _j c i −c _j 6: M ← ^{∂ G}

∂ K (w _n , K ^[L] )

7: compute _dw ^dK

n using M in (13)

8: ∂ x n+1 /∂ w n ← ∂ x n /∂ w n + h ∑ ^s _i=1 b _{i dw} ^{dk i}

n

9: ∂ z n /∂ w n ← ∑ ^s _i=1 l i (0) _dw ^{dZ i}

n

10: compute outputs and sensitivities using (K, _dw ^dK

n )

IV. S TRUCTURE EXPLOITING IRK METHODS

This section starts by adapting the IRK methods to the specific three stage structure in Subsection IV-A, followed by some remarks in Subsection IV-B. The availability of these methods in the ACADO Toolkit is briefly illustrated in Subsection IV-C.

A. Implementation details

The methods from Section III could be applied to the sys- tem in (3), ignoring the structure that is present. This paper, however, proposes an implementation which is completely equivalent with the latter, while exploiting the structure as much as possible. As illustrated in Figure 1, the scheme can be represented by four blocks in total including the one that handles the extra outputs and their sensitivities. The idea is to compute all the collocation variables K and their derivatives

dK

dw n , so that the outputs and the sensitivities can still be generated in a similar way as before.

a) Linear input system: Let us define the collocation variables K ₁ = (k ^[1] ₁ , . . . , k _s ^[1] ) for the linear input system. The resulting equations in (9) will also be linear, namely

M ₁ = ∂ G ₁

∂ K 1

=







C ₁ − ha ₁₁ A ₁ · · · −ha _1s A ₁ .. . . . . .. .

−ha _s1 A 1 · · · C 1 − ha _ss A 1





 (14)

is a constant sN _x ^[1] × sN _x ^[1] matrix in which G ₁ defines the collocation equations. Instead of performing L Newton it- erations like in (10), the variables can now be computed by K ₁ = −M ₁ ⁻¹ G ₁ (x _n ). The inverse matrix M ₁ ⁻¹ can be precomputed offline, which leaves us with only a matrix- vector multiplication. Even the derivatives _dw ^{dK 1}

n are constant and will therefore be precomputed.

b) Nonlinear system: For the implicit nonlinear system, the methods are still applied as described in Section III for the variables K ₂ = (k ^[2] ₁ , . . . , k _s ^[2] , Z ₁ , . . . , Z _s ) of dimension s(N _x ^[2] + N _z ). The only difference is that the derivatives ^dK _dw ¹

n

are now needed to evaluate ^{∂ G 2}

∂ w n for the sensitivity generation with G ₂ defining the nonlinear collocation equations. In Figure 1, evaluations of the function f ₃ in (3c) and of its

Jacobian are also considered to be part of this block so that it handles all nonlinear parts.

c) Linear output system: Completely similar to the in- put system, the variables K ₃ = (k ^[3] ₁ , . . . , k ^[3] _s ) can be computed by K ₃ = −M ₃ ⁻¹ G ₃ (x _n ) in which M 3 = ^{∂ G 3}

∂ K ₃ is a sN _x ^[3] × sN _x ^[3]

matrix and G ₃ defines the collocation equations for the linear output system. The inverse matrix M ₃ ⁻¹ and the derivatives of K ₃ with respect to x ^[3] _n are constant and can be precomputed.

The rest of the derivatives are computed by the matrix-vector multiplication ^dK _dw ³

n = −M ₃ ^{−1 ∂ G 3}

∂ w n , using the derivatives _dw ^{dK 1}

n

and _dw ^{dK 2}

n in the evaluation of ^{∂ G 3}

∂ w n . B. Some small remarks

First of all, the Jacobian matrix _dw ^dK

n has a clear sparsity structure

dK dw n

=







dK ₁

dx ^[1] 0 0 ^dK _du ¹

dK 2

dx ^[1]

dK 2

dx ^[2] 0 ^dK _du ²

dK 3

dx ^[1]

dK 3

dx ^[2]

dK 3

dx ^[3]

dK 3

du





 , (15)

which is exploited in the implementation of the integrators where possible. In addition, it is interesting to note that the in- and output system actually have an analytical solution since they consist of a set of linear differential equations with constant coefficients. Using this would however have little impact on the computational complexity and the overall accuracy of the integration method for the complete system.

C. The ACADO software

The presented integrators are part of the open source ACADO code generation tool [8]. It is implemented in C++ and efficient C-code is exported, tailored to a specific problem. But a brief glance at this tool is made possible by its MATLAB interface. Consider a simple artificial system

C ₁ x ˙ ₁ = A ₁ x ₁ + B ₁ u, (16a)

˙

x ₂ = u x ₁ , (16b)

C 3 x ˙ 3 = A ₃ x 3 + x ² ₂ , (16c) where the A, B and C matrices are constant, x 1 , x 2 and x 3

are vectors of each 2 differential states, u is the control input and (16a)-(16c) corresponds to the three stages from Section II. Let us now export a standalone integrator that exploits the structure in this model using ACADO from MATLAB:

1

DifferentialState x1(2) x2(2) x3(2);

2

Control u;

3

4

A1 = [ ]; A3 = [ ]; % 2x2 matrix

5

C1 = [ ]; C3 = [ ]; % 2x2 matrix

6

B1 = [ ]; % 2x1 vector

7

sim = acado.SIMexport( 0.1 );

8

9

sim.setLinearInput(C1, A1, B1);

10

sim.setModel(dot(x2) == [u*x1]);

11

sim.setLinearOutput(C3, A3, [x2.ˆ2]);

12

13

sim.set( 'INTEGRATOR_TYPE', 'INT_IRK_GL4' );

14

sim.set( 'NUM_INTEGRATOR_STEPS', 2 );

15

sim.exportCode('export')

Note that it is also possible to export these integrators directly as part of an NMPC algorithm, which is done in the next section.

V. A PPLICATIONS IN O PTIMAL C ONTROL

The goal of this section is both to illustrate the reasoning behind the proposed three stage structure in II-A and to present speedups that could be expected from the structure exploiting integrators. To strengthen this message, two real- world problems will be tackled. An application of NMPC on an overhead crane is discussed in Subsection V-A, followed by a quadcopter example in Subsection V-B. The experiments presented in this section are performed using the ACADO code generation tool on an ordinary computer (Intel i7- 3720QM 6MB cache, 2.60 GHz, 64-bit Ubuntu 12.04 and Clang 3.0).

A. NMPC of an overhead crane

A dynamic model very similar to the one in [18] is used for the same overhead crane. Let us immediately write down the equations in the presented three-stage format. The linear input system consists of

x ˙ T = v _T v ˙ T = a _T u ˙ T = u _{T R}

˙

x _L = v _L v ˙ _L = a _L u ˙ _L = u _LR , (17) followed by two nonlinear equations:

θ = ω ˙ ω = − ˙ 1

x L

(g sin(θ ) + a T cos(θ ) + 2v L ω ),

(18)

with:

a _T = − 1

τ ₁ v _T + A 1

τ ₁ u _T a _L = − 1

τ ₂ v _L + A 2

τ ₂ u _L . (19) Note that there is no linear output system and the different parameters are defined in [18]. The NMPC algorithm can now use an OCP formulation similar to (1) with this model for e.g. a simple point-to-point motion. Table I illustrates the speedup for one iteration of the RTI scheme implemented in ACADO. Using 10 control intervals over a horizon of 1s and 4 integration steps of the 4 ^th order Gauss method per interval, it presents the average computation times for the different components with and without structure exploitation in the auto generated integrator. For this relatively small model, a speedup factor of 3 can already be observed for the total computation time spent in integration and sensitivity generation.

It has been illustrated that a linear input system can appear naturally. Consider now the situation where one would like to extra penalize the high frequency content in the states. One way to do this is by implementing a High-Pass RC filter such as in (6) for each of the states. This results in the following 6 equations being added to the input system:

˙

x ^HP _T = ˙ x _T − ω C x ^HP _T x ˙ ^HP _L = ˙ x _L − ω C x ^HP _L v ˙ ^HP _T = ˙ v T − ω C v ^HP _T v ˙ ^HP _L = ˙ v L − ω C v ^HP _L

˙

u ^HP _T = ˙ u _T − ω C u ^HP _T u ˙ ^HP _L = ˙ u _L − ω C u ^HP _L , (20)

unstructured structured

integration method 220 µs 67 µs

condensing 6 µs 6 µs

QP solution (qpOASES) 16 µs 16 µs

remaining operations 3 µs 3 µs

one real-time iteration 245 µs 92 µs

TABLE I

A VERAGE COMPUTATION TIMES FOR NMPC OF AN OVERHEAD CRANE .

0 1 2 3 4 5 6

−0.2

−0.1 0 0.1 0.2 0.3 0.4 0.5

t [s]

vT [m/s]

Velocity trolley

0 1 2 3 4 5 6

−0.35

−0.3

−0.25

−0.2

−0.15

−0.1

−0.05 0 0.05 0.1

t [s]

vL [m/s]

Velocity cable

Fig. 2. Closed-loop trajectories for the velocity of both trolley and cable, before (dashed) and after extra penalization of high frequencies (solid).

but also in a linear output system consisting of

θ ˙ ^HP = ˙ θ − ω C θ ^HP ω ˙ ^HP = ˙ ω − ω C ω ^HP . (21) The new states in these equations are modeling the high frequency content in the corresponding states from the orig- inal system, leading to a complete system of 16 differential states. Weighting these high frequency states in the NMPC formulation causes a certain smoothing of the closed-loop trajectories, as depicted in Figure 2. Similar to before, Table II now presents a speedup factor of 6 for the integration time due to the structure exploitation on this extended model.

B. NMPC of a quadcopter

In this second example, NMPC will be applied to a Quadcopter (see e.g. [19]). This paper uses a simple Quad-

unstructured structured integration method 1380 µs 238 µs

condensing 25 µs 25 µs

QP solution (qpOASES) 12 µs 12 µs remaining operations 13 µs 13 µs one real-time iteration 1430 µs 288 µs

TABLE II

A VERAGE COMPUTATION TIMES FOR NMPC OF AN OVERHEAD CRANE ,

WITH EXTRA PENALIZATION OF HIGHER FREQUENCIES .

copter model, where it is assumed that low-level speed controllers ensure the tracking of the reference velocities of the propellers, which are computed by the NMPC scheme.

The model equations are briefly summarized by ω ¨ _k ^ref = U _k , ω ˙ _k = τ ⁻¹ 

ω _k ^ref − ω k



, (22)

˙ q = 1

2 E ^T Ω, Ω = J ˙ ⁻¹ (T + Ω × JΩ) , (23)

v ˙ = m ⁻¹ RF − g1 _z , (24)

F =

4 k=1 ∑

1

2 ρ AC L ω _k ² 1 _z , T =

4 k=1 ∑

(−1) ^k

2 ρ AC D ω _k ² 1 _z , (25)

p ˙ = v, I ˙ p = p − p _ref , (26)

where U _k for k = 1, . . . , 4 are the control inputs, commanding the 2 ^nd time derivative of the propeller reference velocities ω _k ^ref , ω k are the actual propeller velocities, q ∈ R ⁴ is the quaternion vector used to represent the orientation, Ω is the main body angular velocity in the body frame, v is the linear velocity in the inertial frame, p is the position and I _p ∈ R ³ the integral of the position error. Matrix R = EG ^T is the rotation matrix between the body frame and the inertial frame, with

G =









−q ₁ −q ₂ −q ₃ q ₀ −q ₃ q ₂ q ₃ q ₀ −q ₁

−q ₂ q ₁ q ₀







 , E =









−q ₁ −q ₂ −q ₃ q ₀ q ₃ −q ₂

−q ₃ q ₀ q ₁ q ₂ −q ₁ q ₀







 .

Parameter τ is the time constant of the speed control loops, J ∈ R ^3×3 is the inertia matrix of the Quadcopter, and m its total mass. Coefficients C _L and C _D are respectively the lift and drag coefficients, A is the individual area of the propellers, ρ is the air density, and 1 z = 

0 0 1  T

. It can be observed that (22) forms a 12 states linear input system, (23)-(24) forms a 10 states nonlinear dynamic system and (26) is a 6 states linear output system. The input dynamics ¨ ω _k ^ref = U _k are used to implement a penalty on the high-frequency components in the control input, i.e. the Lagrange term

Π _input =

4

∑

k=1

W 1 U _k ² +W ₂  ω ˙ _k ^ref

 2

is added to the objective, with positive weights W ₁ and W ₂ . Moreover, the integral of the position error ˙ I _p = p − p _ref is introduced and penalized in order to eliminate steady-state errors in the Quadcopter position, which typically result from constant disturbances and model errors.

The positive definite weighting matrices Q, R and P from the OCP formulation in (1) are chosen to achieve quick point-to-point motions of the quadcopter while taking into account its limitations. A possible closed-loop trajectory for the position and orientation of the quadcopter is presented in Figure 3, in the event of a motion from the point (1, 1, 1) to the origin. It shows the position as well as the orientation of the quadcopter at multiple time instants, which are 0.4s apart from each other.

Table III shows a speedup factor of 12 for the total integration time. It even seems to cause condensing to

0 0.2

0.4 0.6

0.8

1 0

0.2 0.4

0.6 0.8

1 0

0.2 0.4 0.6 0.8 1

x y

z

Fig. 3. Illustration of a closed-loop trajectory of the position and orientation of the quadcopter.

unstructured structured integration method 35.8 ms 3.1 ms

condensing 2.6 ms 2.6 ms

QP solution (qpOASES) 0.1 ms 0.1 ms remaining operations 0.2 ms 0.2 ms one real-time iteration 38.7 ms 6.0 ms

TABLE III

A VERAGE COMPUTATION TIMES FOR NMPC OF A QUADCOPTER .

become a more dominating computational cost in one real- time iteration. For these experiments, a horizon of length 5s with 25 control intervals has been used and 2 integration steps of the 6 ^th order Gauss method are performed per interval.

VI. C ONCLUSIONS & F URTHER DEVELOPMENTS

This paper proposed a new format for defining nonlinear dynamic models and showed how the three-stage structure therein can be strongly exploited by IRK methods. The gen- eral idea of code generation for IRK methods with efficient generation of sensitivities and continuous output was first briefly repeated. The mentioned structure in the differential equations has then been introduced and motivated, followed by a discussion on the consequences for the implementation when exploiting this. Two problems, namely the real-time control of an overhead crane and of a quadcopter, illustrated the relevance of the proposed three-stage structure. Numer- ical experiments have shown that important speedups can be achieved with these auto generated, structure exploiting integrators which are available in the ACADO toolkit [20].

It is useful to note that the proposed three-stage structure

could be detected and exploited automatically in any nonlin-

ear model. Future work could include implementing such an

automatic structure detection.

R EFERENCES

[1] D. Mayne, J. Rawlings, C. Rao, and P. Scokaert, “Constrained model predictive control: stability and optimality,” Automatica, vol. 26, no. 6, pp. 789–814, 2000.

[2] C. Rao and J. Rawlings, “Nonlinear Moving Horizon State Estima- tion,” in Nonlinear Predictive Control (F. Allg¨ower and A. Zheng, eds.), (Basel Boston Berlin), pp. 45–69, Birkh¨auser, 2000.

[3] C. Jones and M. Morari, “Polytopic approximation of explicit model predictive controllers,” IEEE Transactions on Automatic Control, vol. 55, no. 11, pp. 2542–2553, 2010.

[4] C. Kirches, L. Wirsching, S. Sager, and H. Bock, “Efficient numerics for nonlinear model predictive control,” in Recent Advances in Opti- mization and its Applications in Engineering (M. Diehl, F. F. Glineur, and E. J. W. Michiels, eds.), pp. 339–357, Springer, 2010.

[5] M. Diehl, H. J. Ferreau, and N. Haverbeke, Nonlinear model predictive control, vol. 384 of Lecture Notes in Control and Information Sciences, ch. Efficient Numerical Methods for Nonlinear MPC and Moving Horizon Estimation, pp. 391–417. Springer, 2009.

[6] M. Diehl, H. Bock, and J. Schl¨oder, “A real-time iteration scheme for nonlinear optimization in optimal feedback control,” SIAM Journal on Control and Optimization, vol. 43, no. 5, pp. 1714–1736, 2005.

[7] J. Mattingley and S. Boyd, Convex Optimization in Signal Processing and Communications, ch. Automatic Code Generation for Real-Time Convex Optimization. Cambridge University Press, 2009.

[8] H. Ferreau, Model Predictive Control Algorithms for Applications with Millisecond Timescales. PhD thesis, K.U. Leuven, 2011.

[9] R. Quirynen, S. Gros, and M. Diehl, “Fast auto generated ACADO integrators and application to MHE with multi-rate measurements,” in Proceedings of the European Control Conference, 2013.

[10] R. Quirynen, M. Vukov, and M. Diehl, “Auto Generation of Implicit Integrators for Embedded NMPC with Microsecond Sampling Times,”

in Proceedings of the 4th IFAC Nonlinear Model Predictive Control Conference, Noordwijkerhout, The Netherlands (M. Lazar and F. All- gower, eds.), vol. 4, 2012.

[11] M. Boutayeb and M. Darouach, “Recursive identification method for MISO Wiener-Hammerstein model,” Automatic Control, IEEE Transactions on, vol. 40, no. 2, pp. 287–291, Feb 1995.

[12] R. Serban and A. Hindmarsh, “CVODES: the sensitivity-enabled ODE solver in SUNDIALS,” in Proceedings of IDETC/CIE 2005, 2005.

[13] R. Quirynen, “Automatic code generation of implicit runge-kutta integrators with continuous output for fast embedded optimization,”

Master’s thesis, KU Leuven, 2012.

[14] R. Findeisen and F. Allg¨ower, “Nonlinear model predictive con- trol for index–one DAE systems,” in Nonlinear Predictive Control (F. Allg¨ower and A. Zheng, eds.), (Basel Boston Berlin), pp. 145–

162, Birkh¨auser, 2000.

[15] E. Hairer and G. Wanner, Solving Ordinary Differential Equations II – Stiff and Differential-Algebraic Problems. Berlin Heidelberg: Springer, 2nd ed., 1991.

[16] A. Griewank and A. Walther, Evaluating Derivatives. SIAM, 2 ed., 2008.

[17] N. H. Cong and L. N. Xuan, “Parallel-Iterated RK-type PC Methods with Continuous Output Formulas,” International Journal of Computer Mathematics, vol. 80:8, pp. 1025–1035, 2003.

[18] M. Vukov, W. V. Loock, B. Houska, H. Ferreau, J. Swevers, and M. Diehl, “Experimental Validation of Nonlinear MPC on an Overhead Crane using Automatic Code Generation,” in The 2012 American Control Conference, Montreal, Canada., 2012.

[19] G. Hoffmann, H. Huang, S. Waslander, and C. Tomlin, “Quadrotor Helicopter Flight Dynamics and Control: Theory and Experiment,”

in AIAA Guidance, Navigation and Control Conference and Exhibit, 2007.

[20] Sourceforge, “Acado toolkit.” URL: http://sourceforge.net/

p/acado/wiki/Home/, last checked on June 18, 2013.