Embedded Optimization for Model Predictive Control of Mechatronic - PowerPoint PPT Presentation

Embedded Optimization for Model Predictive Control of Mechatronic Systems Moritz Diehl Systems Control and Optimization Laboratory Department of Microsystems Engineering (IMTEK) & Department of Mathematics University of Freiburg ETH Zürich, July 14, 2016

Complex Sensor Actuator Systems SENSORS ACTUATORS • GPS • flight surfaces • acceleration • steering wheel • radar • motor speeds How to connect ? • vision • joint torques • ... • ... M. Diehl 2

Classical Filters Maps from one time series into another 3 M. Diehl 3

Classical Filters Maps from one time series into another 3 Special case: linear time invariant (LTI) filters action output = weighted sum of past measurement inputs M. Diehl 4

Linear Filters are Everywhere… AUDIO SYSTEMS: - Dolby, - Echo and other effects - active noise control ... FEEDBACK CONTROL: - PID controller, - Kalman filter, - LQR, ... M. Diehl 5

Linear Filters are Everywhere… AUDIO SYSTEMS: - Dolby, - Echo and other effects - active noise control ... FEEDBACK CONTROL: - PID controller, - Kalman filter, - LQR, ... …but they need lots of tuning to cope with constraints and nonlinearities. M. Diehl 6

Alternative: Embedded Optimization IDEA: Solve, in real-time and repeatedly, an optimization problem that depends on the incoming stream of input data, to generate a stream of output data. M. Diehl 7

Alternative: Embedded Optimization IDEA: Solve, in real-time and repeatedly, an optimization problem that depends on the incoming stream of input data, to generate a stream of output data. Example: Parametric Quadratic Programming M. Diehl 8

Embedded Optimization: a CPU-Intensive Map EMBEDDED OPTIMIZATION Surprisingly powerful! Nearly every map of interest can be generated by embedded convex optimisation… M. Diehl 9

The ubiquity of parametric convex optimization THEOREM [Baes, D., Necoara, 2008] Every continuous map µ : R n x R n u ! u = µ ( x ) 7! x can be represented as parametric convex program (PCP): µ ( x ) = arg min u g ( u , x ) s.t. ( u , x ) 2 Γ PCP: objective and feasible set jointly convex in parameters and variables ( x , u ).

Overview • Embedded Optimization • Time Optimal Motions in Mechatronics • Real-Time Optimization Methods and Software • Four Experimental NMPC Applications M. Diehl 11

Time-Optimal Point-To-Point Motions [PhD Vandenbrouck 2012] Fast oscillating systems (cranes, plotters, wafer steppers, …) Control aims: • reach end point as fast as possible • do not violate constraints • no residual vibrations Idea: formulate as embedded optimization problem in form of Model Predictive Control (MPC) M. Diehl 12

Model Predictive Control (MPC) Always look a bit into the future Example: driver predicts and optimizes, and therefore slows down before a curve M. Diehl 13

Optimal Control Problem in MPC For given system state x , which controls u lead to the best objective value without violation of constraints ? simulated state trajectory controls (unknowns / variables) prediction horizon (length also unknown for time optimal MPC) M. Diehl 14

Optimal Control Problem in MPC For given system state x , which controls u lead to the best objective value without violation of constraints ? simulated state trajectory controls (unknowns / variables) prediction horizon (length also unknown for time optimal MPC) M. Diehl 15

Time Optimal MPC of a Crane SENSORS MPC ACTUATOR • line angle • cart motor • cart position Hardware: xPC Target. Software: qpOASES [Ferreau, D., Bock, 2008] M. Diehl 16

Time Optimal MPC of a Crane Univ. Leuven [Vandenbrouck, Swevers, D.] M. Diehl 17

Optimal solutions varying in time (inequalities matter) Solver qpOASES [PhD H.J. Ferreau, 2011], [Ferreau, Kirches, Potschka, Bock, D. , A parametric active-set algorithm for quadratic programming, Mathematical Programming Computation, 2014] M. Diehl 18

Time Optimal MPC at ETEL (CH): 25cm step, 100nm accuracy TOMPC at 250 Hz (+PID with 12 kHz) Lieboud‘s results after 1 week at ETEL: - 25 cm step in 300 ms - 100 nm accuracy equivalent to: „fly 2,5 km with MACH15, stop with 1 mm position accuracy“ M. Diehl 19

Overview • Embedded Optimization • Time Optimal Motions in Mechatronics • Real-Time Optimization Methods and Software • Four Experimental NMPC Applications M. Diehl 20

A few nice MPC problems can be cast as convex problems… M. Diehl 21

A few nice MPC problems can be cast as convex problems… … but we usually have nonlinear dynamics, leading to nonconvex optimal control problems. M. Diehl 22

Simplified Optimal Control Problem in ODE path constraints h ( x , u ) ≥ 0 6 states x ( t ) terminal initial value constraint r ( x ( T )) ≥ 0 x 0 r controls u ( t ) - p p 0 t T Z T minimize L ( x ( t ) , u ( t )) dt + E ( x ( T )) x ( · ) , u ( · ) 0 subject to x (0) − x 0 = 0 , (fixed initial value) x ( t ) − f ( x ( t ) , u ( t )) = 0 , ˙ t ∈ [0 , T ] , (ODE model) h ( x ( t ) , u ( t )) ≥ 0 , t ∈ [0 , T ] , (path constraints) r ( x ( T )) ≥ 0 (terminal constraints)

Optimal Control Solution Methods - Family Tree M. Diehl 24

Optimal Control Solution Methods - Family Tree (curse of dimensionality) M. Diehl 25

Optimal Control Solution Methods - Family Tree (curse of (bad inequality dimensionality) treatment) M. Diehl 26

Optimal Control Solution Methods - Family Tree (curse of (bad inequality dimensionality) treatment) (only for stable systems) M. Diehl 27

Optimal Control Solution Methods - Family Tree (curse of (bad inequality dimensionality) treatment) (only for stable systems) M. Diehl 28

Direct Multiple Shooting [Bock and Plitt, 1981] [Leineweber et al. 1999] I Discretize controls e.g. piecewise constant φ i ( x i , z i , u i , p ) Hans Georg Bock u ( t ) = u i for t ∈ [ t i , t i +1 ] @ @ R I Solve relaxed DAE on each interval [ t i , t i +1 ] numerically, r r r starting with artificial initial values x i , z i . Obtain trajectory r x i +1 r x i r pieces, and state at end of interval φ i ( x i , z i , q i , p ). r p p p p p p p p I Also numerically compute integrals N − 1 u i X minimize l i ( x i , z i , u i , p ) + E ( x N , p ) x , z , u , p Z t i +1 i =0 l i ( x i , z i , u i , p ) := L ( x , z , u , p ) dt subject to t i q q q x i +1 − φ i ( x i , z i , u i , p ) = 0 , i = 0 , . . . , N − 1 , (continuity) g ( x i , z i , u i , p ) = 0 , i = 0 , . . . , N − 1 , (algebraic consistency) h ( x i , z i , u i , p ) ≥ 0 , i = 0 , . . . , N , (discretized path constr.) r ( x 0 , x N , p ) ≥ 0 . (boundary conditions) 29

Real-Time Iterations [PhD Diehl 2001, Heidelberg] 1) Keep states in problem - use direct multiple shooting [1] 2) Exploit convexity via Generalized Gauss-Newton [2] 3) Use tangential predictors for short feedback delay [3] 4) Iterate while problem changes (Real-Time Iterations) [4] 5) Auto-generate custom solvers in plain-C [5,6] (no if, no malloc) [1] Bock & Plitt, IFAC WC, 1984 [2] Bock 1983 [3] Bock, D. et al, 1999 [4] D. et al., 2002 / 2005 [5] Mattingley & Boyd, 2009 [6] Houska et al.: Automatica, 2011. Open source toolkit: ACADO CodeGen [6] M. Diehl 30

Dynamic Optimization Problem in MPC Structured parametric Nonlinear Program (pNLP) Initial Value is often not known beforehand (“online data” in MPC) Discrete time dynamics come from ODE simulation (“multiple shooting”)

Dynamic Optimization Problem in MPC Summarize as with convex and

Nonlinear MPC = parametric Nonlinear Programming Solution manifold is piecewise differentiable (kinks at active set changes) NLP sensitivity Critical regions are non-polyhedral How to deal with a sequence of large parameter changes? NLP Pathfollowing!

Real-Time Iteration (Sequential Convex Programming) Step 1: Linearize nonlinear constraints at to obtain convex problem: Step 2: Get new value of parameter and solve convex problem - typically a quadratic program (QP) - to obtain next iterate. Go to step 1. [Diehl, Bock, Schloeder, Findeisen, Nagy, Allgower, JPC, 2002] [Zavala, Anitescu, SICON, 2010] [Tran Dinh, Savorgnan, Diehl, SIOPT, 2013]

Real-Time Iteration Tangential prediction even across active set changes Can divide computations in “preparation” and “feedback phase” [D. 2001]

Real-Time Iteration Contraction Estimate Contraction estimate for primal dual errors: Contraction depends on bounds on nonlinearity, Jacobian error, and on strong regularity. Contraction rate independent of active set changes! [Tran Dinh, Savorgnan, Diehl, SIOPT, 2013]

Computations in one Real-Time Iteration NLP 1) Linearize constraints: Integration & sensitivities Sparse QP 2) Condense sparse QP Condensed small QP 3) Solve condensed QP

Computations in one Real-Time Iteration NLP Can prepare without knowing “Preparation phase” 1) Linearize constraints: Integration & sensitivities Sparse QP 2) Condense sparse QP Condensed small QP 3) Solve condensed QP

Computations in one Real-Time Iteration NLP 1) Linearize constraints: Integration & sensitivities Sparse QP 2) Condense sparse QP Condensed small QP “Feedback phase” 3) Solve condensed QP