embedded optimization for resource constrained...

Embedded Optimization forResource Constrained Platforms

Collaborative Project FP7-248940Seventh Framework Programme

ICT-2009.3.4 - Embedded Systems Design

Deliverable : D7.4

Documentation of Best Practice Guidance for Use of theEMBOCON Open Platform

Research leading to these results has received funding from the European CommunitySeventh Framework Programme, Grant Agreement Number 248940Written by F. Kehrle, M. Jung, T. Huschto, J. Frasch, M. Engelhart (OVGU)

Reviewed by A. Schild (BASF)

Delivery Date August 28, 2013

Work Package WP7: Integration of Embedded Optimization Modules for the Open Platform

Dissemination Level Public (PU)

EMBOCON: FP7-ICT-248940 August 28, 2013

Contents

1 Introduction 3

2 Problem Class 5

3 Test Example: Overhead Crane Setup 63.1 Model Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63.2 Nonlinear Model Predictive Controller Setup . . . . . . . . . . . . . . . . . . . . . . . . . . 8

4 ACADO Toolkit 104.1 Installation and setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104.2 Creating a crane controller with ACADO . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114.3 Coupling the created controller to GEMS . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

5 Multi-Parametric Toolbox 175.1 Installation and setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 175.2 Creating a crane controller with the Multi-Parametric Toolbox . . . . . . . . . . . . . . . . . 175.3 Coupling the created controller to GEMS . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

6 FORCES 216.1 Installation and setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 216.2 Creating a crane controller with FORCES . . . . . . . . . . . . . . . . . . . . . . . . . . . . 216.3 Coupling the created controller to GEMS . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

7 Coupling GEMS to an application/simulation 277.1 Implementation of the observer interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 277.2 Simulink Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

D.7.4: Documentation of Best Practice Guidance for Use of the EMBOCON Open Platform of 29


1 Introduction

The EMBOCON Software Platform offers a set of standardized software interfaces. The set of software in-terfaces includes the ModelInterface, the OptimizerInterface, the ObserverInterface andthe SimluationApplicationInterface. The interfaces form the basis for the interaction between thedifferent modules of the online part of the software platform. These interfaces specify sets of callable functionsfor the different modules to trigger computation routines in the corresponding modules. The set of specifiedfunctions of an interface includes standard functions which have to be implemented by the module and optionalfunctions which are used in special problem classes or for debugging purpose. The input and output parametersof the interface functions define the data content and the data structures which are exchanged when calling afunction.

The advantages of using common interfaces for the communication between the different modules of the onlinepart of the platform are flexibility and applicability. Flexibility is offered by the easy substitution of any modulewith another module implementing the same interface, e.g. the simulation environment can be exchanged byan application after running necessary test cases, or the optimizer can be replaced by another optimizer forbenchmarking performance using the same application. Applicability is offered by the set of common interfaceswhich, allows on the one hand, industrial users to implement a needed interface in a simulation environmentand/or an application to evaluate the performance of different state-of-the-art numerical algorithms supportedby the platform, and on the other hand allows algorithm developers to test their developed numerical algorithmson a set of different problems in form of models, simulations, or applications from different target sectors

Figure 1: Structure and Interfaces of the online part of the EMBOCON open platform.

The structure and the set of interfaces of the software platform is shown in Figure 1 and contains the followinginterfaces: The model interface, the optimizer interface, the observer interface, and the simulation-applicationinterface. The optimizer interface and the observer interface are common for all optimization algorithms usedby the platform, independent whether the development platform is used to implement the problem, or C code



generated by the Matlab by- pass is used. Both are used to connect the elements of the optimization routineto the supervisor. The model interface is used for the interaction between the elements of the optimizationalgorithm and C code models generated by the Modelica-Optimica-based online part of the platform. Thesimulation-application interface is used for the interaction between a simulation environment or an applicationand the supervisor. It contains two layers, one common information layer which defines what should be ex-changed via the interface and a tailored implementation layer which includes the actual implementation of theconnection to an application, or a simulation environment.

In this tutorial, we show how the EMBOCON software platform can be applied to the example of an overheadcrane. In section 2 we start with an abstract definition of the problem class considered in this report. Section3 gives an explanation of the overhead crane model. Implementation instructions for the optimizer interfacesare given in sections 4, 5, and 6 for the optimizers ACADO Toolkit, MPT, and FORCES. Finally, section 7describes how a simulation can be set up with a simple observer interface via Simulink in MATLAB.



2 Problem Class

In this report, we consider nonlinear MPC problems of the following form:

minx(·),u(·)

∫ t+T

t

(‖x(τ)− xref(τ)‖2

Q +‖u(τ)−uref(τ)‖2R)

dτ +‖x(t +T )− xref(t +T )‖2P (1a)

s.t. x(t) = xt (1b)

x(τ) = f (x(τ),u(τ)) (1c)

u ≤ u(τ) ≤ u (1d)

x ≤ x(τ) ≤ x for all τ ∈ [t, t +T ] (1e)

Here, t denotes the current time, x the differential state, u the control input, and T the fixed length of theprediction horizon. Variables xref and uref denote state and control references, respectively. As usual in trackingMPC formulations, the norms in the objective function can be weighted with positive-definite matrices Q, Rand P. Additionally, upper and lower bounds u, u and x, x on the controls and states, respectively, are to berespected by the controller. Note, that problem (1) can be regarded as a parametric nonlinear optimal controlproblem (OCP), where the parameter xt represents the state estimate at the current time instant t.



3 Test Example: Overhead Crane Setup

The example we consider in this report is an overhead crane which consists of a cart that can move in onehorizontal direction, with a pendulum hanging from this cart. It is a MIMO (Multiple Input Multiple Output)system with two inputs, i.e. control variables – translation of the cart uC and cable length uL – and three outputs,i.e. state variables – position of the cart xC, cable length xL, and angular deflection of the pendulum θ . Themotion of the cart is controlled using a linear motor and influences the motion of the pendulum, of which theline length is controlled by a winch. The position of the cart is measured using a fully integrated optical positionencoder. The angular deflection is measured by a rotary encoder. However, only the angular deflection of theleft cable α can be measured (see Figure 2), but the angular deflection of the pendulum θ can be expressed asa function of α .

Figure 2: Schematic representation of the overhead crane setup.

Figure 2 shows a schematic representation of the overhead crane. An overview of the variables and corre-sponding units can be found in Table 1. The aim is to perform point-to-point motions in the vertical plane withtracking MPC, using the automatic code generation features of ACADO Toolkit, MPT and FORCES.

Variable Description Unit

θ Output the angular deflection of the pendulum radxC Output the horizontal position of the cart mxL Output the length of the cable of the pendulum muC Input to the velocity controller of the cart VuL Input to the velocity controller of the winch V

Table 1: Outputs and inputs of the crane setup.

3.1 Model Equations

We introduce vC as the velocity of the cart and vL as the rate of change of the cable length, and thus have

xC(t) = vC(t) and xL(t) = vL(t). (2)

Furthermore, we introduce aC and aL as derivatives of vC and vL, which can be understood as accelerations, andω as the angular velocity of the pendulum,

vC(t) = aC(t), vL(t) = aL(t), θ(t) = ω(t). (3)



Together with an additional damping term,

− cm · xL(t)

θ(t), (4)

the equation of motion for the pendulum (see e.g. [?] for details) is

xC(t)cos(θ(t)) =−xL(t)θ(t)−2xL(t)θ(t)−gsin(θ(t))− cm · xL(t)

θ(t). (5)

Replacing xC = vC = aC, θ = ω , and xL = vL, we have

aC(t)cos(θ(t)) =−xL(t)ω(t)−2vL(t)ω(t)−gsin(θ(t))− cm · xL(t)

ω(t) (6)

⇐⇒ ω(t) =1

xL(t)

(−gsin(θ(t))−aC(t)cos(θ(t))−2vL(t)ω(t)− c ·ω(t)

m · xL(t)

). (7)

Considering the input to the velocity controller uC of the cart and the position of the cart xC, the followingdynamic relation holds,

xC(s)uC(s)

=AC

s(τCs+1), (8)

with gain AC and time-constant τC. For the input to the velocity controller of the winch motor uL and the cablelength xL the corresponding dynamic relation is

xL(s)uL(s)

=AL

s(τLs+1), (9)

with time-constant τL and gain AL. For the accelerations aT and aL, we now have

aC(t) =−1τC

vC(t)+AC

τCuC(t),

aL(t) =−1τL

vL(t)+AL

τLuL(t).

(10)

In order to be able to constraint the motor torques indirectly, we introduce control rates as actual controls,

uC(t) = uCR(t) and uL(t) = uLR(t). (11)

uCR denotes the control rate of the cart subsystem, while uLR denotes the control rate of the winch mechanism.Note, that now uC and uL are state variables, whereas uCR and uLR are the control variables.

Finally, the ODE model for the crane setup can be written as follows,

xC(t) = vC(t), vC(t) = aC(t), (12a)

xL(t) = vL(t), vL(t) = aL(t), (12b)

θ(t) = ω(t), ω(t) =1

xL(t)

(−gsin(θ(t))−aC(t)cos(θ(t)) (12c)

−2vL(t)ω(t)− cω(t)mxL(t)

),

uC(t) = uCR(t), uL(t) = uLR(t) (12d)

aC(t) =−1τC

vC(t)+AC

τCuC(t), aL(t) =

−1τL

vL(t)+AL

τLuL(t). (12e)



These equations will be summarized in the following as x = f (x,u) where

x := (xC,vC,xL,vL,θ ,ω,uC,uL)T and u := (uCR,uLR)

T . (13)

An overview of the states and controls in the model is given in Table 2. Estimates for time-constants and gains,as well as values for all other parameters are listed in Table 3.

Variable Description

xC State horizontal position of the cartvC State horizontal velocity of the cartxL State length of the cable of the pendulumvL State rate of change of the cable lengthθ State angular deflection of the pendulumω State angular velocity of the pendulumuC State input to the velocity controller of the cartuL State input to the velocity controller of the winchuCR Control auxiliary torque of the cart motoruLR Control auxiliary torque of the winch motor

Table 2: States and controls of the crane ODE model.

Parameter Description Value

m Mass of the pendulum 1.318 kgg Gravitational constant 9.81 m/s2

c Damping constant for the motion of the pendulum 0kgm2/s (*)AC Gain of the velocity controller of the cart 0.047418203070092 m/s/VτC Time constant of the velocity controller of the cart 0.012790605943772 sAL Gain of the velocity controller of the winch 0.034087337273386 m/s/VτL Time constant of the velocity controller of the winch 0.024695192379264 s

*: Damping was neglected during the identification phase, but it may be necessary to identify it in the future.

Table 3: Estimates of the parameter values.

3.2 Nonlinear Model Predictive Controller Setup

At each sampling time the nonlinear OCP (1) is solved with the following bounds on controls and states

−10V≤ uC(τ)≤ 10V,

−10V≤ uL(τ)≤ 10V,

−100V/s≤ uCR(τ)≤ 100V/s,

−100V/s≤ uLR(τ)≤ 100V/s.

The system inputs uC and uL are constrained by internal limitations of the corresponding velocity controller.Constraints for the control rates uCR and uLR are tuned such that saturations in the cart and winch actuators are



avoided. The state and control vectors x(·) and u(·) and the system description are defined as in (13) and (12),respectively. The control horizon has to be chosen suitable for the MPC method used. See also the followingsections for precise values.

The current MPC formulation (1) can incorporate references for full state and control vectors. We keep thereferences constant over the control horizon. Furthermore, only the references for the cart position and cablelength are changed online, while all other references are always set to zero:

xref := (xC,ref,0,xL,ref,0,0,0,0,0)T

and uref := (0,0)T .(14)



4 ACADO Toolkit

In this section we employ a nonlinear real-time iteration algorithm, which has been originally developed in [?].The basic strategy is to discretize the optimal control problem (1) with a multiple shooting discretization [?]using numerical integration and a piecewise constant control parameterization. This leads to a structured non-linear programming problem that can be solved with a sequential quadratic programming (SQP) method. Asthe objective consists of a least squares tracking term, it is reasonable to employ a Gauss-Newton method toapproximate the Hessian matrices [?]. Thus, the algorithm requires only first-order sensitivities of the statetrajectory with respect to the inputs.

The main idea of the real-time iterations is to perform only one SQP step per sampling time, i.e., the optimalcontrol problem (1) is only approximately solved in each real-time iteration. Due to the fact that the stateestimate xt enters the optimization problem only via the affine constraint x(t) = xt , we can integrate the dynamicsystem as well as the corresponding sensitivities without knowing the actual value of the system state. This partis called the preparation phase. As soon as an estimate of the system state is available we only need to solve asingle quadratic programming (QP) problem. Afterwards, the QP solution corresponding to the control inputsof the first time- interval can be immediately sent to the real process. This phase is called the feedback phaseand is typically much shorter than the overall SQP iteration. For the details of this approach, we refer to [?].Moreover, a mathematical foundation of the nonlinear real-time iteration scheme as well as stability results canbe found in [?, ?, ?].

ACADO Toolkit is a software environment and algorithm collection for automatic control and dynamic opti-mization. It provides a general framework for using a great variety of algorithms for direct optimal control,including model predictive control, state and parameter estimation and robust optimization. ACADO Toolkit isimplemented as self-contained C++ code.

4.1 Installation and setup

Here is explained how to download, compile and use ACADO Toolkit with a CMake build system in a Linuxoperating system.

Installation

As prerequisites, one needs to download a few packages via the apt-get package manager:

sudo apt-get install gcc g++ cmake git gnuplot doxygen graphviz

Download the ACADO Toolkit code:

git clone https://github.com/acado/acado.git -b stable ACADOtoolkit

Go to the ACADO Toolkit directory and create a build folder for an out-of-source build:

cd ACADOtoolkitmkdir buildcd build

Run CMake to generate makefiles and start the building process:

cmake ..make



Check whether the installation was successful by running an example:

cd ..cd examples/getting_started./simple_ocp

Set up the ACADO environment

The first thing you need to do is to make Linux environment aware of ACADO Toolkit. One way of doing thisis to add a simple bash script to your environment file. Open the corresponding file

sudo gedit ˜\bashrc

and append the following line to the file:

source <YOUR_ACADO_DIRECTORY>/build/acado_env.sh

Afterwards refresh the environment by typing in the terminal

. ˜/.bashrc

4.2 Creating a crane controller with ACADO

First of all we need to define the variables, describing the crane in ACADO syntax.

int main( void ){USING_NAMESPACE_ACADO

// Define the variables// ----------------------------------------------------------------DifferentialState xC; // the cart positionDifferentialState vC; // the cart velocityIntermediateState aC; // the cart accelerationDifferentialState xL; // the cable lengthDifferentialState vL; // the cable velocityIntermediateState aL; // the cable accelerationDifferentialState theta; // the excitation angleDifferentialState omega; // the angular velocity// ----------------------------------------------------------------

// Controller// ----------------------------------------------------------------DifferentialState uC; // the input to the cart velocityDifferentialState uL; // the input to the cable velocityControl duC;Control duL;// ----------------------------------------------------------------

// Additional parameters



// ----------------------------------------------------------------const double g = 9.81; // the gravitational constantconst double c = 0.0; // damping const. motion of pendulumconst double m = 1318.0; // the mass of the pendulum

const double tauC = 0.012790605943772; // cart time constantconst double aC = 0.047418203070092; // cart gainconst double tauL = 0.034087337273386; // cable time constantconst double aL = 0.024695192379264; // cable gain// ----------------------------------------------------------------

Then define the model equations as described in (12).

// Define the model equations// ----------------------------------------------------------------aC = -1.0 / tauC * vC + aC / tauC * uC;aL = -1.0 / tauL * vL + aL / tauL * uL;

DifferentialEquation f;

f << dot( xC ) == vC;f << dot( vC ) == aC;f << dot( xL ) == vL;f << dot( vL ) == aL;f << dot( theta ) == omega;f << dot( omega ) == 1.0 / xL * ( -g * sin(theta) - aC * cos(theta)

- 2 * vL * omega - c * omega / (m * xL) );

f << dot( uC ) == duC;f << dot( uL ) == duL;// ----------------------------------------------------------------

Now, we use this ODE formulation to setup the optimal control problem with the weighting matrices Q, P andR, to be solved at each MPC sampling instant (see (1a)).

// Define the weighting matrices// ----------------------------------------------------------------Matrix Q = eye( 8 );Matrix R = eye( 2 );Matrix P = eye( 8 );

P *= 1e-10;

//cartQ(0, 0) = 1e0;Q(1, 1) = 1e-2;//hoistingQ(2, 2) = 1e0;Q(3, 3) = 1e-2;// angle



Q(4, 4) = 1e-4;Q(5, 5) = 1e-2;// controlsQ(6, 6) = 1e-6;Q(7, 7) = 1e-6;// control ratesR(0, 0) = 1e-6;R(1, 1) = 1e-6;// ----------------------------------------------------------------

// Set up the MPC - Optimal control problem// ----------------------------------------------------------------OCP ocp(0.0, 1.2, 12);

ocp.minimizeLSQ( Q, R );ocp.minimizeLSQEndTerm( P );

ocp.subjectTo( f );ocp.subjectTo( -10.0 <= uC <= 10.0 );ocp.subjectTo( -10.0 <= uL <= 10.0 );ocp.subjectTo( -100.0 <= duC <= 100.0 );ocp.subjectTo( -100.0 <= duL <= 100.0 );// ----------------------------------------------------------------

Code generation in ACADO

In order to obtain a fast implementation of the outlined nonlinear MPC algorithm, we use the ACADO CodeGeneration tool [?, ?]. The idea behind this tool is to optimize the number of required operations in theevaluation of the nonlinear right-hand side of the differential equation as well as in the associated derivatives.In order to accomplish this, the tool makes use of a symbolic representation of the optimal control problem.The derivatives are symbolically simplified employing automatic differentiation tools and taking into accountzero-entries in the Jacobian. Similarly, a tailored Runge-Kutta method with fixed step-sizes for integratingthe model equations is generated in form of optimized C-code. The Runge-Kutta routine also integrates theassociated variational differential equations to obtain the first-order derivative information as needed withinthe Gauss-Newton real-time iteration algorithm. For more details on the implementation of the ACADO CodeGeneration tool we refer to [?].

Generally, the OCP formulation is passed to the MPCexport class, several export options are set and the codeis generated and exported to a given target directory.

// Define an MPC export module and generate the code// ----------------------------------------------------------------MPCexport mpc( ocp );

mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );mpc.set( INTEGRATOR_TYPE, INT_RK4 );mpc.set( NUM_INTEGRATOR_STEPS, 12 * 10 );



mpc.set( QP_SOLVER, QP_QPOASES );mpc.set( HOTSTART_QP, YES );mpc.set( GENERATE_TEST_FILE, NO );mpc.set( GENERATE_MAKE_FILE, NO );

mpc.exportCode( "./crane_nmpc_export" );// ----------------------------------------------------------------

return 0;}

The generator can be compiled and executed, following a simple procedure. After changing to the projectdirectory, we build the generator

cp <YOUR_ACADO_DIRECTORY>/cmake/FindACADO.cmake .mkdir buildcd buildcmake ..makecd ..

and generate the NMPC code:

./crane_nmpc_generator

4.3 Coupling the created controller to GEMS

Using the Simulink as a top layer of the application, GEMS serves as an intermediate layer between Simulinkand the auto-generated NMPC code. The code that couples the GEMS and the NMPC is encapsulated in the fileoptimizer.c. In particular, all calls to the auto-generated code are located in the function makeOptStep,which is explained at the crane example in the following. If you want the simulation to run using your ownoptimizer, all you need to do is implement your own MPC algorithm in the function makeOptStep.

int makeOptStep(emb_optimizer optim, emb_opt_context opt_context,const double xcur[], const double uprev[], double ucur[])

{

Get the number of state and control variables and define those.

emb_size_type nx = getOptModelStateCount(optim);emb_size_type nu = getOptModelInputCount(optim);

int i, j, status;static unsigned firstRun = 1;double feedback[ ACADO_NX ];

// Fill the reference for statesfor (i = 0; i < ACADO_N; ++i)

for (j = 0; j < ACADO_NX; ++j)acadoVariables.xRef[i * ACADO_NX + j] = xref[ j ];



// Fill the reference for controls -- always zero :)for (i = 0; i < ACADO_N; ++i)

for (j = 0; j < ACADO_NU; ++j)acadoVariables.uRef[i * ACADO_NU + j] = 0.0;

Initialize the shooting notes and the controls at the first cycle.

if ( firstRun ){

// Print header...

// Initialize the shooting nodes// ----------------------------------------------------------------for (i = 0; i < ACADO_N + 1; ++i){

acadoVariables.x[i * ACADO_NX + 0] = 0.1;acadoVariables.x[i * ACADO_NX + 1] = 0.0;acadoVariables.x[i * ACADO_NX + 2] = 0.7;acadoVariables.x[i * ACADO_NX + 3] = 0.0;acadoVariables.x[i * ACADO_NX + 4] = 0.0;acadoVariables.x[i * ACADO_NX + 5] = 0.0;acadoVariables.x[i * ACADO_NX + 6] = 0.0;acadoVariables.x[i * ACADO_NX + 7] = 0.0;

}// ----------------------------------------------------------------

// Initialize the controls to zero// ----------------------------------------------------------------for (i = 0; i < ACADO_N; ++i)

for (j = 0; j < ACADO_NU; ++j)acadoVariables.u[i * ACADO_NU + j] = 0.0;

// ----------------------------------------------------------------

firstRun = 0;}

For further iterations, shift the initialization of state and controls

else{

shiftStates( 0 );shiftControls( 0 );

}

The most important function calls for ACADO are preparationStep and feedbackStep. Those func-tions are responsible for computing the NMPC algorithm. Prepare the first/next iteration by

preparationStep();



At each sampling instant, the user obtains the current state measurement. This measurement is then passed tothe feedback step to obtain the optimized control inputs.

feedback[ 0 ] = xcur[ 0 ];feedback[ 1 ] = xcur[ 2 ];feedback[ 2 ] = xcur[ 1 ];feedback[ 3 ] = xcur[ 3 ];feedback[ 4 ] = xcur[ 4 ];feedback[ 5 ] = xcur[ 5 ];feedback[ 6 ] = xcur[ 6 ];feedback[ 7 ] = xcur[ 7 ];

status = feedbackStep( feedback );

// Assign to inputfor( i = 0; i < ACADO_NU; i++ ) {

ucur[ i ] = acadoVariables.u[ i ];}

return 0;}



5 Multi-Parametric Toolbox

The Multi-Parametric Toolbox (MPT) focuses on optimal control of constrained linear (CL) and constrainedpiecewise affine (CPWA) systems. The importance of these systems is due to the fact that many systemscan be linearized around an operating point and then be considered as linear in a constrained region, andthat a significant nonlinear behavior inside the constrained region is often well-captured by a piecewise affineapproximation of the dynamics.

The aim of the MPT is to provide efficient computational means to obtain feedback controllers for these typesof constrained optimal control problems in a MATLAB [?] programming environment. By multi-parametricprogramming, a linear or quadratic optimization problem is solved off-line. The associated solution takes theform of a piecewise affine (PWA) state feedback law, as detailed in deliverable D3.1 of work package WP3. Inparticular, the state-space is partitioned into polyhedral sets and for each of those sets the optimal control lawis given as one affine function of the state. In the on-line implementation of such controllers, computation ofthe controller action reduces to a simple set-membership test plus the evaluation of the corresponding affinefunction. This simplicity, together with the potential for a very high speed evaluation of the control law forCL and CPWA systems, is an important reason why this method has attracted so much interest in the researchcommunity.

This section describes how a problem can be implemented in the MPT and how it is connected to a GEMSsupervisor. A more detailed description of the mathematical framework of the MPT can be found in deliverableD7.1, section 3. The implementational details can be accessed through the manual [?] for the MPT 2 or throughMATLAB’s help function for the MPT 3. For this section, we limit ourselves to the syntax of MPT 3.


The MPT can be obtained at the MPT homepage through the script install mpt3.m, which automaticallyfetches and installs the corresponding packages inside MATLAB. A MATLAB version of R2011a or newer isrequired. Note that some features of the MPT 3 require MATLAB’s Control System Toolbox as well.

5.2 Creating a crane controller with the Multi-Parametric Toolbox

The MPT can only handle linear and piecewise-linear control systems (nonlinear systems can only be handledwith partial functionality) and hence the nonlinear ODE system has to either be linearized or to be decomposedinto several linearized formulations with distinct feasible regions in the state-input space. We decided to employthe most simple strategy by linearizing the right-hand side around the reference point xref, uref yielding the linearODE system

xC(t) = vC(t), vC(t) =−1τC

vC(t)+AC

τCuC(t), (15a)

xL(t) = vL(t), vL(t) =−1τL

vL(t)+AL

τLuL(t), (15b)



θ(t) = ω(t), ω(t) =1

xL,ref

[cos(θref)

τCvC(t)−2ωrefvL(t)−2vL,refω(t)

+1

xL,ref

(2cωref

mxL,ref+2vL,refωref +gsin(θref)

+ACuC,ref− vC,ref

τCcos(θref)

)xL(t)

+

(ACuC,ref− vC,ref

τCsin(θref)−gcos(θref)

)θ(t)

− AC

τCcos(θref)uC(t)

],

(15c)

uC(t) = uCR(t), uL(t) = uLR(t). (15d)

As the reference point is constant over time, we have a homogeneous ODE system after linearization, i.e.,(y(τ)

u(τ)

)= M

(y(τ)

u(τ)

),

y(t) = yt ,

u(t) = ut ,

where

M =

d fdy

(yref,uref)d fdu

(yref,uref)

0 0

=:

(A B

0 0

),

which is scaled to the correct stage length ts:

A = ts * A;B = ts * B;

Note that the stage length has to be chosen relatively short since the linearized system is only a reliable approx-imation on a short time horizon. Therefore, a time step length of 10 ms is chosen for this realization.

This homogeneous system’s analytic solution is known to be(y(τ)

u(τ)

)= eM(τ−t)

(yt

ut

).

Now, we use this solution to formulate the linear time-invariant (LTI) system with time step length ts.

model = LTISystem(’A’, A, ’B’, B, ’T_s’, ts);

At this point, alternatively to LTI systems, the MPT also supports discrete-time piecewise affine (PWA) systems,which are composed of a finite number of LTI systems. Each of these LTI systems is valid in a differentpolyhedral region of the state-input space. A third class of supported systems are mixed-logical dynamic (MLD)systems, which incorporate that some of the controls are binary decisions. An important part of the MPT is theHYSDEL compiler which allows a formulation of hybrid linear models in textual form to be translated into thecorresponding system matrices A, B, etc.

In the MPT, state and control bounds x,x,u,u can be implemented in the model as, e.g.



model.x.min = [-100*ones(nx-2,1); -10*ones(2,1)];model.x.max = [ 100*ones(nx-2,1); 10*ones(2,1)];model.u.min = -100*ones(nu,1);model.u.max = 100*ones(nu,1);

The reference points are simply added through

model.x.with(’reference’);model.x.reference = xref;model.u.with(’reference’);model.u.reference = uref;

If none were added, the default values 0 are taken for both reference trajectories.

The penalty matrices Q, R, from the tracking objective of (1) are employed as quadratic functions

model.x.penalty = QuadFunction(Q);model.u.penalty = QuadFunction(R);

In our case, the terminal penalty P takes into account the infinite-horizon solution of the Ricatti-equation

P = A>PA−A>PB(B>PB+R)−1B>PA+Q. (16)

The solution of this equation is obtained with the Control System Toolbox’s dlqr function in MATLAB. TheMPT implements this through

model.x.with(’terminalPenalty’);model.x.terminalPenalty = model.LQRPenalty;

Finally, we can create the MPC-Controller with a prediction horizon of N model stages, i.e. T = Nts.

controller = MPCController(model, N);

This controller could be used inside MATLAB to control the process on-line with the current states x0

u = controller.evaluate(x0);

where u is the feedback control given by the controller. However, as we are interested in a coupling to GEMS,we want to create exportable code. Therefore, we need to get an explicit formulation of the controller. Theseexplicit controllers can directly be exported to C and with default settings create the files mpt getInput.h,mpt getInput.c, mpt getInput sfunc.c.

exp_controller = controller.toExplicit;exp_controller.exportToC;

Note that creating the explicit formulation of the controller is rather time consuming but it needs to be doneonly once for each system in an off-line setup step.


The created C-code is almost ready to be coupled to GEMS. The missing link is the implementation of thecorresponding emb optimizer class and several functions that access it. These functions mostly provideinformation transfer as e.g.



emb_size_type getOptModelStateCount(emb_optimizer optim){

int ns = 8;return ns;

}

However, the most important part is the specification of the makeOptStep function, which returns the con-troller’s feedback to the GEMS supervisor. It uses the interface function mpt getInput(X,U) of the explicitcontroller obtained from MPT’s code generation to determine the control.

int makeOptStep(emb_optimizer optim, emb_opt_context opt_context,const double xcur[], const double uprev[],double ucur[])

{emb_size_type nx = getOptModelStateCount(optim);emb_size_type nu = getOptModelInputCount(optim);

static float region=1, X[MPT_NX], U[MPT_NU];int i;

// variables must be passed as floats to the explicit controllerfor (i=0; i<nx; i++) X[i] = (float)xcur[i];for (i=0; i<nu; i++) U[i] = (float)ucur[i];

// get control law through the created C-coderegion = mpt_getInput(X,U);

// pass the outputfor (i=0; i<nu; i++) ucur[i] = U[i];

// check if control law was found, if not, stopif (region<1) return 1;

return 0;}



6 FORCES

FORCES stands for Fast Optimization for Real-time Control on Embedded Systems and is a numerical opti-mization code generation framework for convex multistage problems. This problem class covers for examplesome Model Predictive Control (MPC) formulations as well as Moving Horizon Estimation (MHE) problems.The optimization code generated by FORCES is library-free ANSI-C code. It can directly be added to anyexisting software project.

FORCES supports convex quadratically constraint quadratic programming (QCQP) and allows parametric pro-gramming specifications. These problems are specified in a MATLAB interface and then sent to a server forthe code generation purpose. The generated code is slim and focuses solely on the optimization of the problemgiven through the interface. As a result, it is quite fast since it does not introduce unneeded features.

The algorithmic details of FORCES can be found in [?]. A FORCES problem over N stages is formulated to be

miny

N

∑i=1

12

z>i Hizi + f>i zi

s.t. zi ≤ z≤ zi, 1≤ i≤ N,

Aizi ≤ bi, 1≤ i≤ N,

z>i Qi, jzi +g>i, jzi ≤ r2i, j, 1≤ i≤ N,1≤ j ≤ qi,

Cizi +Di+1zi+1 = ci, 1≤ i≤ N−1,

where zi constitutes of all variables present in the i-th stage.


FORCES is a server based code generator that is implemented as a web service. As such, it requires a clientthat takes the problem formulation, connects to the server and retrieves the generated code. A Matlab client isprovided for this purpose, which can be requested after registration on the homepage.

The provided interface must be added to the MATLAB path to be usable in MATLAB – this can e.g. be doneonly temporarily for the problem setup as

addpath(’<YOUR_FORCES_DIRECTORY>’);

6.2 Creating a crane controller with FORCES

FORCES can only handle linear and convex quadratic constraints, hence the nonlinear ODE system has to belinearized for FORCES to be able to handle the problem. However, linearizing around the working point is areasonable assumption and makes the handling of the emerging system much easier. We decided to employ themost simple strategy by linearizing the right-hand side around the reference point xref, uref yielding the linearODE sytem (15).



As the reference point is constant over time, we have a homogeneous ODE system after linearization, i.e.,(y(τ)

u(τ)

)= M

(y(τ)

u(τ)

),

y(t) = yt ,

u(t) = ut ,

where

M =

d fdy

(yref,uref)d fdu

(yref,uref)

0 0

=:

(A B

0 0

),

which is scaled to the correct stage length ts:

A = ts * A;B = ts * B;

Note that the stage length has to be chosen relatively short since the linearized system is only a reliable approx-imation on a short time horizon. Therefore, a time step length of 10 ms is chosen for this realization.

This homogeneous system’s analytic solution is known to be(y(τ)

u(τ)

)= eM(τ−t)

(yt

ut

).

Now, we set up the penalty matrices Q, R, from the tracking objective of (1).

Q = 100*eye(nx);Q(end,end) = 1;Q(end-1,end-1) = 1;R = 1*eye(nu);[˜,P] = dlqr(A,B,Q,R);

Note that in this case, the terminal penalty P takes into account the infinite-horizon solution of the Ricatti-equation

P = A>PA−A>PB(B>PB+R)−1B>PA+Q,

which is obtained with the Control System Toolbox’s dlqr function in MATLAB.

We consider a multistage problem, where the first stage is used to give the control feedback and the other stagesare used as the horizon. For our problem, the controls and states are merged in z, only the last stage’s variableszN contain just state variables xN . This last stage is used to consider the infinite horizon effects.

The quadratic part of the objective is denoted with H and the linear part is denoted with f . For our setting, weget the objective contribution for each stage k < N to be the same

(xk− xref)>Q(xk− xref)+(uk−uref)

>R(uk−uref)

=(

xk uk)>( Q 0

0 R

)︸︷︷︸

=:H

(xk

uk

)+(

x>refQ>+ x>refQ u>refR

>+u>refR)︸︷︷︸

=: f

(xk

uk

)+ const,



and the terminal stage’s contribution

(xN− xref)>P(xN− xref) = x>N P︸︷︷︸

=:HN

xN +(

x>refP>+ x>refP

)︸︷︷︸

=: fN

xN + const,

To show more of the functionality of FORCES, we assume the objective could change and hence define it as aproblem parameter:

params(1) = newParam(’H’,1:N-1,’cost.H’);params(end+1) = newParam(’f’,1:N-1,’cost.f’);params(end+1) = newParam(’H_N’, N, ’cost.H’);params(end+1) = newParam(’f_N’, N, ’cost.f’);

A problem with N stages is realized through the command

problem = MultistageProblem(N);

The stage’s dimensions and variables are set up in the following way, note the special dimensions for the laststage which has no controls.

% state and control boundsxmin = [-100*ones(nx-2,1); -10*ones(2,1)];xmax = [ 100*ones(nx-2,1); 10*ones(2,1)];umin = -100*ones(nu,1);umax = 100*ones(nu,1);

for i = 1:(N-1)problem(i).dims.n = nx+nu; % number of stage variablesproblem(i).dims.l = nx+nu; % number of lower boundsproblem(i).dims.u = nx+nu; % number of upper bounds

% lower bound indices and valuesproblem(i).ineq.b.lbidx = 1:problem(i).dims.n;problem(i).ineq.b.lb = [xmin, umin];

% upper bound indices and valuesproblem(i).ineq.b.ubidx = 1:problem(i).dims.n;problem(i).ineq.b.ub = [xmax, umax];

end

problem(N).dims.n = nx; % last stage has no controlsproblem(N).dims.l = nx;problem(N).dims.u = nx;

% lower bound indices and valuesproblem(i).ineq.b.lbidx = 1:problem(i).dims.n;problem(i).ineq.b.lb = [xmin];

% upper bound indices and valuesproblem(i).ineq.b.ubidx = 1:problem(i).dims.n;problem(i).ineq.b.ub = [xmax];



In our case, only the constraint matrices C and D are used since we need neither affine linear constraintsnor quadratic constraints. We just use the coupling constraints for the different stages, which describe thediscretized ODE system and the initial conditions.

% initial and coupling constraints for first stage,% initial constraints take parameter x0problem(1).dims.r = 2*nx;problem(1).eq.C = [eye(nx), zeros(nx,nu); A, B];params(end+1) = newParam(’z1’,1,’eq.c’);

% coupling constraints for intermediate stagesfor i = 2:N-1

problem(i).dims.r = nx;problem(i).eq.C = [A, B];problem(i).eq.c = zeros(nx,1);

% special size for D_2 since it is coupled to first stageif( i==2 )

stages(i).eq.D = [zeros(nx,nx+nu); -eye(nx), zeros(nx,nu)];else

stages(i).eq.D = [-eye(nx), zeros(nx,nu)];end

end

% no additional constraints for last stage needed, only couplingproblem(N).dims.r = 0;problem(N).eq.D = -eye(nx);

for i = 1:Nproblem(i).dims.p = 0; % no affine linear constraintsproblem(i).dims.q = 0; % no quadratic constraints

end

The output of the controller, which is in our case the control of the first stage, is defined through the followingcommands

name = ’u1’;fromStage = 1;idxWithinStage = nx+1:nx+nu;outputs = newOutput(name, fromStage, idxWithinStage);

Since we declared the initial states x0 and the terms H and f of the objective to be parameters, we can nowcreate the controller.

% set name to be "myMPC"codeoptions = getOptions(’myMPC’);

% send to server to generate code and save resultsgenerateCode(stages,params,codeoptions,outputs);




The created C-code is almost ready to be coupled to GEMS. The missing link is the implementation of thecorresponding emb optimizer class and several functions that access it. The most important part is thespecification of the makeOptStep function, which returns the controller’s feedback to the GEMS supervisor.It uses the files myMPT.h and myMPT.c obtained from FORCES’s code generation to determine the control.The function’s initial part where

int makeOptStep(emb_optimizer optim, emb_opt_context opt_context,const double xcur[], const double uprev[],double ucur[])

{emb_size_type nx = getOptModelStateCount(optim);emb_size_type nu = getOptModelInputCount(optim);

unsigned int i, j;

Since the objective and the initial states were defined as parameters, they need to be specified before the con-troller is called. Since they do not change, they can–in this case–be defined with global variables Q, R, P,uref and xref with the same values as obtained during the problem design phase in MATLAB. These globalvariables could be set with the setParameter function of the GEMS optimizer interface.

myMPC_params params;// Set parameters H and H_Nfor (i = 0; i < (nx+nu)*(nx+nu); i++) params.H[i] = 0;for (i = 0; i < nx; i++) {

for (j = 0; j < nx; j++)params.H[i*(nx+nu)+j] = 2*Q[i][j];

}for (i = nx; i < nx+nu; i++) {

for (j = nx; j < nx+nu; j++)params.H[i*(nx+nu)+j] = 2*R[i-nx][j-nx];

}for (i = 0; i < nx; i++) {

for (j = 0; j < nx; j++)params.H_N[i*nx+j] = 2*P[i][j];

}

// Set parameters f and f_Nfor (i = 0; i < nx+nu; i++) params.f[i] = 0;for (i = 0; i < nx; i++) {

double sum = 0, sum_N = 0;for (j = 0; j < nx; j++) {

sum -= (xref[j]*Q[j][i] + xref[j]*Q[i][j]);sum_N -= (xref[j]*P[j][i] + xref[j]*P[i][j]);

}params.f[i] = sum;params.f_N[i] = sum_N;

}for (i = 0; i < nu; i++) {



double sum = 0;for (j = 0; j < nu; j++)sum -= (uref[j]*R[j][i] + uref[j]*R[i][j]);

params.f[nx+i] = sum;}

// Set parameter z1 for initial conditionsfor (i = 0; i < nx; i++) params.z1[i] = xcur[i];for (i = nx; i < 2*nx; i++) params.z1[i] = 0;

Now, as the problem is set up, the controller can be called and the control can be given to the supervisor.

// Solve MPC (call controller)myMPC_output output;myMPC_info info;int flag = myMPC_solve( &params, &output, &info);

// If error happened, stopif (flag != 1) return 1;

// Assign outputucur[0] = output.u1[0];ucur[1] = output.u1[1];

return 0;}



7 Coupling GEMS to an application/simulation

The supervisor of the online part of the EMBOCON software platform forms the communication managerbetween an optimizer, an observer and an application or simulation environment of the optimization routine.The supervisor is responsible for the initialization and freeing of the different elements and for triggering thefunctions calls of elements in the right order. The supervisor offers logging of data like controls, states etc. andmeasures computation times for all function calls. The measured timings together with the logged data can beused for testing, validation and performance evaluation of numerical algorithms.

The standard computation cycle of the supervisor starts with the setup of the elements of the online part byconnecting them to the supervisor. Next, the creation and initialization phase starts by initializing the appli-cation or simulation environment and getting the first measurements of the current state and the parametersof the system. These measurements are used to trigger the creation and initialization step of the observer andof the optimizer including the creation and initialization of the connected models. After the initialization ofall elements is done, the optimization is executed by calling the makeSimStep function of the SimulationApplication Interface. The resulting measurements of the makeSimStep call are passed to the observer andthe makeObsStep function is called. The resulting states of the observer call are passed to the optimizer bycalling the makeOptStep function of the Optimizer Interface. The optimizer calculates new control inputsfor the next step. These control inputs are applied to the connected simulation environment or application bycalling again the makeSimStep function of the Simulation Application Interface. Then new measurementsof the system response to the applied control actions are passed again to the observer and the makeObsStepfunction of the Observer Interface is called to calculate the next states. This cycle is repeated until a terminationcriterion is reached, e.g. a predefined final time of a control horizon. During the optimization cycle computationtimes for any function calls are measured and logged with all data exchanged via the supervisor.

In the following we describe how a simulation can be set up with a simple observer interface via Simulink inMATLAB.

7.1 Implementation of the observer interface

As with the optimizer interface, the majority of the functions in the observer interface are self-explanatory andeasy to implement. The most important function is makeObsStep, which triggers the observer to do the nextobservation step and communicates the data to the GEMS supervisor. The input parameters ucur and ycurcontain the measurements of the outputs and of the controls of an application or simulation environment. Theseare the measurements of the system at the current sample time. They are used together with the last estimate ofthe system states to create a new estimate xcur.

In a very simple setting, where the application/simulation directly measures the system states directly and nouncertainty is included, the observer simply pipes the measurements ycur to the new states xcur.

int makeObsStep(emb_observer obs, emb_obs_context obs_context,const double ucur[], const double ycur[],const double xprev[], double xcur[])

{emb_size_type nx = getObsModelStateCount(obs);for (unsigned int i = 0; i < nx; i++)

xcur[i] = ycur[i];return 0;

}



7.2 Simulink Example

The basic idea of this example is to control a crane model using a MPC which makes use of the GEMSinterfaces. The simulation consists of two major blocks. On the one hand there is the block Crane and on theother hand there is the block Controller.

Figure 3: Simulink model for the simulation.

The block Crane is the non-linear crane model described by the equations in section 3, illustrated in figure 5.

The Controller consists of a C s-function block named SGems SFunc. This block has two inputs namely themeasured states of the crane and the reference signal to which you want to steer the model to. This last oneis up to the implementer to choose. The output of this block is the input signal u which will be applied to thecrane model.

Figure 4: Simulink model for the controller.

This SGems SFunc calls SGems which stands for Simulink Gems. This function is basically a simple im-plementation of what Gems would do. It consists of two main function calls. The first one is makeObsStepwhich tells the observer to estimate the state. In this model it just sets the estimated state to the measured state.The second function that gets called is makeOptStep. This runs the MPC which returns the input values thatwill be applied to the system. Aside from these two functions it also sets the optimization parameter xref tothe user defined reference signal that it got as an input. Note that it uses the given xref for the entire horizon.

Simulink automatically calls the function mdlOutputs as part of the SGems SFunc block in the Controller.



Figure 5: Simulink model of the crane.

static void mdlOutputs(SimStruct *S, int_T tid){

const char *xref = (char *) ssGetInputPortSignal(S,0);const char *x = (char *) ssGetInputPortSignal(S,1);char *u = (char *) ssGetOutputPortSignal(S,0);

int_T* busInfo = (int_T *) ssGetUserData(S);

// Temporary bus copy declarationsxref_bus _u0BUS;x_bus _u1BUS;u_bus _y0BUS;

// Copy input bus into temporary structurex_ref_copy(_u0BUS, xref);x_copy(_u1BUS, x);

SGems(&_u0BUS,&_u1BUS, &_y0BUS);

// Copy output controls into control busu_copy(_y0BUS, u);

}



References

[1] F. Allgower, T.A. Badgwell, J.S. Qin, J.B. Rawlings, and S.J. Wright. Nonlinear predictive control andmoving horizon estimation – An introductory overview. In P.M. Frank, editor, Advances in Control,Highlights of ECC’99, pages 391–449. Springer, 1999.

[2] H.G. Bock. Recent advances in parameter identification techniques for ODE. In P. Deuflhard andE. Hairer, editors, Numerical Treatment of Inverse Problems in Differential and Integral Equations, pages95–121. Birkhauser, Boston, 1983.

[3] H.G. Bock and K.J. Plitt. A Multiple Shooting algorithm for direct solution of optimal control prob-lems. In Proceedings of the 9th IFAC World Congress, pages 242–247, Budapest, 1984. Pergamon Press.Available at http://www.iwr.uni-heidelberg.de/groups/agbock/FILES/Bock1984.pdf.

[4] M. Diehl. Real-Time Optimization for Large Scale Nonlinear Processes. PhD thesis, Universitat Heidel-berg, 2001.

[5] M. Diehl, R. Findeisen, and F. Allgower. A Stabilizing Real-time Implementation of Nonlinear ModelPredictive Control. In L. Biegler, O. Ghattas, M. Heinkenschloss, D. Keyes, and B. van Bloemen Waan-ders, editors, Real-Time and Online PDE-Constrained Optimization, pages 23–52. SIAM, 2007.

[6] A. Domahidi, A. Zgraggen, M.N. Zeilinger, M. Morari, and C.N. Jones. Efficient interior point methodsfor multistage problems arising in receding horizon control. In IEEE Conference on Decision and Control(CDC), pages 668–674, 2012.

[7] M. Fliess, J. Levine, P. Martin, and P. Rouchon. Flatness and defect of non-linear systems: introductorytheory and examples. International Journal of Control, 61:13–27, 1995.

[8] B. Houska, H.J. Ferreau, and M. Diehl. ACADO Toolkit – An Open Source Framework for AutomaticControl and Dynamic Optimization. Optimal Control Applications and Methods, 32(3):298–312, 2011.

[9] B. Houska, H.J. Ferreau, and M. Diehl. An auto-generated real-time iteration algorithm for nonlinearMPC in the microsecond range. Automatica, 47(10):2279–2285, 2011.

[10] M. Kvasnica, P. Grieder, M. Baotic, and F.J. Christophersen. Multi-parametric toolbox. Technical report,Swiss Federal Institute of Technology, Zurich, Switzerland, 2006.

[11] The MathWorks, Inc. http://www.mathworks.de/products/matlab/, State: 05.2011.

[12] V. M. Zavala and L. T. Biegler. Nonlinear programming strategies for state estimation and model predic-tive control. In Nonlinear Model Predictive Control, pages 419–432. Springer-Verlag, Berlin, 2009.


embedded optimization for resource constrained...

Documents