WASP Autonomous Systems Course: Module Control and Decision Making

Transcription

1 WASP Autonomous Systems Course: Module Control and Decision Making Daniel Axehill Automatic Control Linköping University

2 Introduction Two parts Modeling, control and optimal control: Daniel Axehill Planning: Jonas Kvarnström

3 A comment on your background A group with varying knowledge in control. Some of you might think that some parts are new to you. Other might think this is trivial and well-known! The objective with this lecture is to be broad rather than deep; don t feel sorry if you don t get all the details!

4 A comment on my background PhD in Automatic Control Optimization for Model Predictive Control of hybrid systems Postdoc at the Automatic Control Laboratory at ETH Zürich for 2 years. Associate Professor at the Division of Automatic Control, LiU. Fast and real-time optimization for embedded decision making. Autonomous systems, e.g., together with SAAB and Scania.

5 The control problem To make a system behave such that a given goal is achieved, despite disturbances and uncertainty. A very broad problem with applications everywhere. Fits well with what one would expect from an autonomous system. The fundamental concepts are not implementation specific: electronic, mechanical, pneumatic, software, human mind,...

6 Typical applications of automatic control

7 Typical applications of automatic control Literally Rocket science!

8 Show movie! Space shuttle launch

9 Less visible applications of automatic control Electrical grid DC/DC converter Production automation in general Climate control Toilet Process control for fuel etc....

10 1 Part I: Modeling and fundamental properties 2 Part II: Basic control 3 Part III: Optimal Control

11 Outline Part I: Modeling and fundamental properties Modeling What is dynamic systems and why do we need models? Differential equations Transfer functions Frequency properties State-space representation Time-discretization Hybrid systems Fundamental properties Observability Controllability Poles and zeros Stability

12 Outline Part I: Modeling and fundamental properties Modeling What is dynamic systems and why do we need models? Differential equations Transfer functions Frequency properties State-space representation Time-discretization Hybrid systems Fundamental properties Observability Controllability Poles and zeros Stability

13 Dynamic systems Systems that can be said to have a memory Example: A car remembers its position and velocity from one point in time to the next. If you would like the car to stop in front of a pedestrian you cannot just assign position := 1 m away from pedestrian and velocity := 0 without taking into account the cars current state. The actions used to control the system becomes state-dependent. The system s output/state does not only depend on the current input, but on all previous inputs to the system. You might have to pay in the future for mistakes made now and earlier.

14 Why are models useful in control and related areas? State-estimation makes use of models of system and disturbances. High-performance control design and analysis systems with intricate dynamics and/or are multivariable can often not be hand-tuned with acceptable result. to make good decisions one needs a good state-estimate. models are typically necessary for performing various types of analysis, e.g., stability, robustness, controllability, observability,... to obtain a priori certificates for success are sometimes crucial, but can be very difficult. Simulation of open and closed-loop system e.g. Simulink, Modelica,... evaluate hardware in hardware-in-the-loop simulations. a must for dangerous and costly experiments.

15 Example: Anyone up for trial and error hand-tuning? When we are dealing with expensive things that we don t want to break, it is highly interesting to be able to a priori prove that the design is sound.

16 The world is nonlinear, why consider linear models and systems? Main motivation: well-developed, comparatively simple, mathematical theory for linear systems poles, zeros invariance under scaling, additivity, frequency fidelity; superposition principle often relevant calculations and properties boil down to linear algebra Sufficiently close to an operating point, many (most) systems can be considered as linear. Errors due to a linear approximation can be considered using robustness analysis. Typically an idea with a system controlled by a feedback control loop is to maintain it close to an operating point, so it makes sense to assume that the system is close to this point. A control system developed using linear methods can (should) still be evaluated using nonlinear simulations.

17 Differential equations Linear differential equation d n y dt n +a d n 1 y 1 dt n a dy n 1 dt +a d m y ny = b 0 dt m +...+b du m 1 dt +b mu (1) where y is the output and u is the input. The relation contains derivatives, since a dynamic system. Looks formally the same in the Multiple Input Multiple Output (MIMO) case, but it becomes a vector-differential equation and the coefficients become matrices. Important special case: Linear differential equation with constant coefficients Linear Time-Invariant (LTI) system.

18 Linear differential equations: solution Homogeneous solution where y h (t) = C 1 e r 1t C n e rnt r 1, r 2,... are roots to the characteristic equation ( poles ). C 1, C 2,... are chosen to satisfy the initial conditions. Particular solution can be found, e.g., using the method of undetermined coefficients. The solution is given by superposition Y (t) = y h (t) + y p (t) Note that the solution contains terms of the principle form e pole t!

19 Transfer functions Apply Laplace transform (assuming all initial conditions zero) to the differential equation in (1) (s n + a 1 s n a n )Y (s) = (b 0 s m b m )U(s) (2) where Y (s) = L{y(t)} and U(s) = L{u(t)}. After introducing G(s) = the input-output-relation can be written as b 0 s m b m s n + a 1 s n a n (3) Y (s) = G(s)U(s) (4) In the MIMO case, G(s) becomes a transfer matrix.

20 Fourier transform and frequency response For a stable LTI system with transfer function G(s), it holds Y (iω) = G(iω)U(iω) (5) where Y (iω) and U(iω) are the Fourier transforms of the output and input, respectively. The magnitude G(iω) and phase arg G(iω) of G(iω) can be visualized as a Bode diagram. 100 Bode Diagram Magnitude (db) Phase (deg) Frequency (rad/s)

21 Sigma plots and gain for MIMO systems For MIMO systems, one Bode diagram would be required for each input-output combination Plot the singular values for the transfer matrix instead: sigma plot 70 Singular Values Singular Values (db) Frequency (rad/s)

22 Sigma plots and gain for MIMO systems The gain of a system at frequency ω with transfer matrix G(iω) lies between the smallest and largest singular value Y (iω) σ(g(iω)) = G(iω) (6) σ(g(iω)) U(iω) The gain of the system is G = max ω G(iω), which corresponds to the peak in the Bode diagram for SISO systems. 70 Singular Values Singular Values (db) Frequency (rad/s)

23 State-space representation State-space representation of an LTI system d x(t) = Ax(t) + Bu(t) dt y(t) = Cx(t) + Du(t) where x R n, u R m, y R p, A R n n, B R n m, C R p n, and D R p m. The corresponding transfer matrix can easily be obtained as (7) G(s) = C(sI A) 1 B + D (8) The state-space representation of an input-output relation is non-unique, there are infinitely many such representations related through a similarity transformation: ξ = T AT 1 ξ + T Bu y = CT 1 ξ + Du with ξ(t) = T x(t) and T invertible.

24 Conversion to state-space representation Representing a given transfer function on state-space form is often more complicated than the other way around. From simpler physical system: choose states from physical insight (position, velocity,...) From differential equation: choose the derivatives as states. From transfer function: Map transfer function coefficients to Diagonal canonical form Controllable canonical form Observable canonical form... MIMO case is harder.

25 Continuous time vs. discrete time Why consider discrete time? Computer based systems can typically only measure and manipulate outputs at certain time instants. Some systems are by construction of a discrete/stage-wise character; inventory systems, financial systems, computer programs,... Time not necessarily time. Most properties and tools that we mention in continuous time have discrete-time counter-parts.

26 Discrete-time state-space representation State-space representation of a discrete-time LTI system x t+1 = Ax t + Bu t y t = Cx t + Du t (9) where x t R n, u t R m, y t R p, A R n n, B R n m, C R p n, and D R p m. More general variants for time-varying and nonlinear variants exist. Under the assumption, e.g., the input u(t) is piecewise constant over the sample interval T, it is straightforward to compute a discrete-time representation in the form (9) given a continuous time system in the form (7).

27 Discrete-time hybrid systems Hybrid systems are systems consisting of 1. continuous dynamics: described by one or more difference (or differential in general) equations, states belong to a continuum. 2. discrete events: state variables assume discrete values from a countable set, e.g. binary digits {0, 1}, natural numbers N, integers Z, rational numbers Q,... finite set of symbols Dynamical systems whose state evolution depends on an interaction between continuous dynamics and discrete events are known as hybrid systems. Examples: systems with switches, valves, mechanical systems, computer programs etc.

28 Motivating example: Mechanical system with backlash δ ε x 1 x x Continuous dynamics: states x 1, x 2, ẋ 1, ẋ 2. Two discrete modes : a) contact mode mechanical parts are in contact and the force is transmitted. b) backlash mode mechanical parts are not in contact.

29 Motivating example: Autonomous vehicle Plan a trajectory through a world containing obstacles. Discrete decision: Should I turn left or right?

30 Examples of hybrid models: Piecewise Affine Systems Piecewise Affine (PWA) systems are defined by: polyhedral partition of the (x, u)-space: { D i } {[ ] } D i=1 = xt P i u xx t + Puu i t Pc i t affine dynamics and output in each region: { xt+1 = A i x t + B i u t + f i } y t = C i x t + D i u t + g i if x t D i where x R n, u R m. Important applications: PWA approximations of a nonlinear system. closed-loop MPC system for linear constrained systems.

31 Examples of hybrid models: Discrete Hybrid Automata xc(k) δe(k) EVENT GENERATOR (EG) uc(k) δe(k) u b (k) FINITE STATE MACHINE (FSM) uc(k) SYS 1... SYS s x b (k) u b (k) δe(k) MODE SELECTOR (MS) i(k) SWITCHED AFFINE SYSTEM (SAS) Interconnection between switched affine system: continuous dynamics. finite state machine: discrete events. Auxiliary components Event generator: generates discrete events that triggers FSM mode switches. Mode selector: selects affine subsystem.

32 Examples of hybrid models: Discrete Hybrid Automata Switched Affine System: State update equation: x c (k + 1) = A j x c (k) + B j u c (k) + f j, if i(k) = j Finite State Machine: Discrete time dynamic process: x b (k + 1) = f F SM (x b (k), u b (k), δ e (k)) x b X b {0, 1} n b (binary state). u b U b {0, 1} m b (binary input). δ e D {0, 1} ne (binary event from EG). f F SM : X b U b D X b is a deterministic logic function.

33 Examples of hybrid models: Systems Mixed Logical Dynamical Several relevant hybrid model classes can be shown, under certain assumptions, to be equivalent. Logic rules can be translated into Linear Integer Inequalities. Mixed Logical Dynamical (MLD) x t+1 = Ax t + B 1 u t + B 2 δ t + B 3 z t y t = Cx t + D 1 u t + D 2 δ t + D 3 z t E 2 δ t + E 3 z t E 4 x t + E 1 u t + E 5 where x R nc {0, 1} n b, u R mc {0, 1} m b y R pc {0, 1} p b, δ {0, 1} r b and z R rc. Looks at first sight like a linear state-space model with linear inequality constraints, however, there are integers present. Turns out to be suitable for hybrid MPC.

34 Outline Part I: Modeling and fundamental properties Modeling What is dynamic systems and why do we need models? Differential equations Transfer functions Frequency properties State-space representation Time-discretization Hybrid systems Fundamental properties Observability Controllability Poles and zeros Stability

35 Observability Definition (Observability) The state x 0 is called unobservable if, when u(t) = 0, t 0 and x(0) = x, the output is y(t) 0, t 0. The system is called observable if it lacks unobservable states. Theorem The unobservable states of the system (7) of order n, form a linear subspace, i.e., the null space of the observability matrix C CA O(A, C) = CA 2 (10). CA n 1 The system is observable iff O has full rank.

36 Controllability Definition (Controllability) The state x is called controllable if there is an input that in finite time transfers the system to state x from the initial state x(0) = 0. The system is called controllable of all states are controllable. Theorem The controllable states of the system (7) of order n, form a linear subspace, i.e., the range of the controllability matrix S(A, B) = [ B AB A 2 B... A n 1 B ] (11) The system is controllable iff S has full rank.

37 Minimal realization Definition A state-space representation that is both controllable and observable is called a minimal realization of the system. There does not exist any state-space representation with lower dimension that realizes the same input-output relation. No pole/zero cancellations can be made when the transfer operator is computed for a minimal realization of a SISO-system.

38 Poles LTI systems in continuous time Definition (Poles of a system in state-space form) The poles of a system are the eigenvalues (with multiplicity) of the system matrix A in a minimal state-space realization of the system. The pole polynomial is the characteristic polynomial for the matrix A, i.e., det(λi A). Theorem (Poles of a transfer matrix) The pole polynomial for a system with transfer matrix G(s) is the least common denominator of all minors to G(s). The poles of the system are the zeros of the pole polynomial.

39 Stability for continuous time LTI systems Definition (Stability region) For a continuous time system the stability region is equal to the left half plane, not including the imaginary axis. Theorem (Input-output stability) An LTI system is input-output stable iff its poles are inside the stability region. Theorem (Stability of solutions) An LTI system in state space form (7) is asymptotically stable iff all eigenvalues of the matrix A are inside the stability region. If the system is stable, all the eigenvalues are inside the stability region or on its boundary.

40 Poles vs. eigenvalues of A The notion poles is used to characterize input-output properties. Eigenvalues of A can beyond input-output properties also characterize internal properties of the state-space realization. In a minimal state-space representation, the poles of the system coincides with the eigenvalues of A. In a non-minimal representation, not all eigenvalues show up as poles which means that the system might explode internally even though the poles are stable.

41 Zeros LTI systems in continuous time Definition (Zeros of a system in state-space form) The (transmission) zeros are those s for which [ ] si A B M(s) = C D (12) does not have full rank. The polynomial which zeros coincide with those s where M(s) looses rank is called the zero polynomial. Theorem (Zeros of a transfer matrix) The zero polynomial of G(s) is the greatest common divisor for the numerators of the maximal minors of G(s), normalized to have the pole polynomial of G(s) as denominator. The zeros of the system are the zeros of the zero polynomial.

42 Importance of stability Why all this fuss about stability?! Lack of stability means that a small perturbation of a system in equilibrium will make the system leave that equilibrium. if this equilibrium point is the desired state/setpoint of the system, the behavior is obviously undesirable. The equilibrium point might potentially be left very quickly......and the system might very quickly be out of the region of attraction of the desired state. The system might literally explode...

43 Show movie! Gripen 1993

44 Equilibrium A (potentially nonlinear) system where the dynamics is given by ẋ(t) = f(x(t), u(t)) is said to have an equilibrium x 0, u 0 if f(x 0, u 0 ) = 0 E.g. if the system is started with x(0) = x 0 and u(t) = u 0 it holds that x(t) = x 0, t 0.

45 Linearization Given a system in the form ẋ = f(x, u) y = h(x) with an equilibrium x 0, u 0 it holds that f(x 0, u 0 ) = 0, y 0 = h(x 0 ) Then the linearization of the nonlinear system at x 0, u 0 is where d dt (x x 0) = A(x x 0 ) + B(u u 0 ) y y 0 = C(x x 0 ) A = f x (x 0, u 0 ), B = f u (x 0, u 0 ), C = h x (x 0 )

46 Stable equilibria Definition An equilibrium x 0 is stable if there for each ɛ > 0 exists a δ > 0 such that x(t) x 0 < ɛ for all t > 0 as soon as x(0) x 0 < δ. Example: Pendulum without friction around the lower position.

47 Stable equilibria, cont d Interpretation: Given a requirement that the solution has to stay within the distance ɛ from the equilibrium x 0 for all future, we should always be able to find a ball centered in x 0 with radius δ such that the requirement is satisfied for all starting points in this ball. x(t)-x 0 x(0)-x 0 x(0) x 0 x(t)

48 Asymptotically stable equilibria Definition An equilibrium x 0 is asymptotically stable if it is stable and additionally there exists a δ > 0 such that x(t) x 0, t as soon as x(0) x 0 < δ. Example: Pendulum with viscous friction around the lower position.

49 Globally asymptotically stable equilibria Definition An equilibrium x 0 is globally asymptotically stable if δ above can be made arbitrarily large. I.e. independently of from where the system is started, it will eventually converge to x 0 when t. Compare linear asymptotically stable systems.

50 Lyapunov functions: distance to target Let a function V (x) denote a (generalized) distance to an equilibrium x 0. The distance has to be non-zero until the system has arrived at the right place : V (x 0 ) = 0, V (x) > 0, x x 0 V (x) should indicate if the system runs away : V (x) x The distance should monotonically decrease until the end destination has been reached: d dt V (x(t)) = V x(x(t))ẋ(t) = V x (x(t))f(x(t)) < 0, x(t) x 0 Lyapunov functions can sometimes be given an energy interpretation, i.e., the total energy of the system has to decrease in order to obtain stability.

51 Lyapunov functions Formally: V (x 0 ) = 0, V (x) > 0, x x 0 V (x) x V x (x)f(x) < 0, x x 0 implies that ẋ = f(x) has a globally asymptotically stable equilibrium x(t) = x 0. It is sufficient that V x (x)f(x) 0 and that no solution (except x(t) = x o ) completely stays in the set where V x (x)f(x) = 0. If V has the above properties only in a neighborhood around x 0, then the equilibrium is locally asymptotically stable.

52 Linearization and stability Using Lyapunov function arguments it can be shown that Theorem If a linearization of a system at an equilibrium is asymptotically stable, then also the original system is asymptotically stable in a neighborhood around the point of the equilibrium. Conclusion: We can obtain stability properties of equilibria for a nonlinear system by studying stability properties of its linearization.

53 1 Part I: Modeling and fundamental properties 2 Part II: Basic control 3 Part III: Optimal Control

54 Outline Part II: Basic control Introduction The closed loop system Feedforward and feedback Sensitivity and robustness PID Cascade control Multivariable control Decentralized and decoupled control LQG Feedback linearization Fundamental limitations

55 Automatizing the choice of input: Controllers A controller is a device that automatically generates inputs to a controlled system, with the objective that it behaves in a way such that it achieves a given goal. A controller can be seen as a real-time decision maker: which input should be applied to the system at this instant in order to achieve the goal? Seen from the outside (the reference) a controlled system can behave completely differently compared to the original system, its new dynamics has been synthesized by the control design.

56 Controllability and observability revisited Theorem Let A R n n and B R n m. The constant matrix L R m n can be selected such that A BL obtains arbitrary predetermined eigenvalues (complex eigenvalues assumed to appear in conjugated pairs) iff the controllability matrix S has full rank. Theorem Let A R n n and C R p n. The constant matrix K R n p can be selected such that A KC obtains arbitrary predetermined eigenvalues (complex eigenvalues assumed to appear in conjugated pairs) iff the observability matrix O has full rank. Bottom line: Using feedback, the dynamics of a controllable system can be chosen arbitrarily (fast). the estimation error dynamics of an observable system can be chosen arbitrarily (fast).

57 Two fundamental concepts The closed loop system Feedforward and feedback

58 The closed loop system: important signals u, control input z, controlled variable r, reference signal, desired value of z y, measured output Disturbances w u, disturbance on the input w, disturbance on the output n, measurement disturbance w u w n r u z y F r Σ G Σ Σ F y Often we measure what we would like to control, i.e., y = z + n. For linear systems, u is in the form u = F r (s)r F y (s)y.

59 The closed loop system: important transfer functions Transfer functions: Closed loop system: G c = (I + GF y ) 1 GF r Sensitivity function: S = (I + GF y ) 1 Input sensitivity function: S u = (I + F y G) 1 Complementary sensitivity function: T = (I + GF y ) 1 GF y Signal relations: z = G c r + Sw T n + GS u w u u = S u F r r S u F y (w + n) + S u w u

60 Feedforward and feedback F f u f r y r Σ F u s Σ u G y G m 1 Feedforward from reference and/or measurable disturbance Compute an expression for how u should depend on r and/or w, n in order to obtain y = r. Feedforward from reference: Implement in F f. Pros/cons + Feedforward can be used to obtain very fast responses since a correct u is immediately applied, without need for aggressive feedback. An exact model of the system is necessary. The concept is sensitive to model errors and unmodeled system disturbances.

61 Feedforward and feedback F f u f r y r Σ F u s Σ u G y G m Feedback Compute the controlled input u based on measurements of the output y. Implement in F. Pros/cons + Exact knowledge/model of the system not necessary. + The result of non-measurable disturbances are compensated for if they influence y. + Unstable systems can be stabilized using feedback. Instability can be introduced by an improper feedback... A good mix of feedforward and feedback is the key to a high-performing control system. 1

62 Sensitivity and robustness Assume a relative model error: G true = (I + G )G model Sensitivity function S The transfer function between a disturbance on the output w and the controlled variable z. Measures the gain between the model error G and the relative output error z : z = S true G Complementary sensitivity function T The transfer function between a measurement disturbances n and the controlled variable z. The closed loop system is stable if This is satisfied if T (iω) < G T < 1 1, for all ω G (iω)

63 PID: A success story dating back to 1788 Boulton and Watt 1788: speed control of steam engine, mechanical implementation. Hydraulic and pneumatic implementations: late 1800s. Electronic implementations: 1930s Computer implementations: 1950s PID-on-a-chip : 1990s. Applications: all.

64 Show movie! Centrifugal governor

65 PID, cont d Is limited to systems with one input and one output (SISO). If there are more inputs and/or outputs, they have to be paired. Interpretation in Bode diagram: phase advancing and phase retarding. Can be tuned using intuition and manual experiments. Result: usually not that impressive (except for very simple cases). Systematic analysis (poles, zeros, S, T,...) can give controller parameters with very good performance. First systematic approach (poles): Maxwell Robust loop-shaping: Åström and Hägglund 2006.

66 A useful controller structure: Cascade control Plant y r z r u z y F 1 F 2 G 2 G 1 1 Several levels of control loops are cascaded: the output from the outer controller is used as a setpoint to the inner one. Makes use of available extra measurements (z above).

67 A useful controller structure: Cascade control Plant y r z r u z y F 1 F 2 G 2 G 1 1 The innermost loop is the fastest one The outer controller can consider the inner loop as perfect, i.e., z(t) z r (t). The fast inner loop can compensate for disturbances and model errors without the outer loop noticing it. A typical application is flow control through a valve; the flow requested by the outer loop is achieved by the inner loop.

68 A useful controller structure: Cascade control Plant y r z r u z y F 1 F 2 G 2 G 1 1 More general: Control hierarchy can be used to separate time scales Scheduling/planning (weeks) Site-wide optimization (day) Local optimization (hour) Supervisory control (MPC, minutes) Regulatory control (seconds)

69 Outline Part II: Basic control Introduction The closed loop system Feedforward and feedback Sensitivity and robustness PID Cascade control Multivariable control Decentralized and decoupled control LQG Feedback linearization Fundamental limitations

70 Multivariable control Multivariable control: control of systems with multiple inputs and/or outputs. Many properties generalize in principle, but there are details to be careful with. As we have seen; poles and zeros are significantly more complicated to compute for a transfer matrix.

71 Show movie! ABB Fanta Can Challenge

72 Several inputs and outputs. Interaction If there are many inputs and outputs it can happen that the inputs and outputs are cross-coupled in a non-trivial way. The control design will become significantly simpler if the system can be decomposed in subsystems with little interaction for which SISO design techniques can be individually employed. Alternatively, there are methods that can be applied directly to multivariable problems.

73 Interaction/cross-coupling Two-hand water tap, a system with annoying cross-coupling. Several inputs affect (considerably) one output. Several outputs are affected (considerably) by one input. Angle cold water handle Angle warm water handle Temperature Water flow

74 Interaction/cross-coupling Two-hand water tap, a system with annoying cross-coupling. Several inputs affect (considerably) one output. Several outputs are affected (considerably) by one input. Angle cold water handle Angle warm water handle Temperature Water flow

75 Interaction/cross-coupling, cont d. One-hand water tap, a system with kind cross-coupling. Every input affect (almost) only one output. Every output is (almost) only affected by one input. Angle temperature Angle flow Temperature Water flow

76 Interaction/cross-coupling, cont d. One-hand water tap, a system with kind cross-coupling. Every input affect (almost) only one output. Every output is (almost) only affected by one input. Angle temperature Angle flow 0 0 Temperature Water flow

77 Decentralized control Idea: Design a controller for a MIMO system by assigning one input to be controlled by each output. The result is a number of single variable loops u i = F i rr j F i yy j Works better the smaller the cross-couplings in the system are. One would like to pair the strongest coupled inputs and outputs: the pairing problem. One mathematical tool to measure the amount of interaction/cross-coupling between inputs and outputs is called Relative Gain Array (RGA).

78 Decoupled control What do we do if there are no obvious input-output pairs that account for the dominating dynamics? Create them! Perform variable transformations on inputs and outputs ỹ = W 2 y ũ = W 1 1 u A new virtual system is obtained: G(s) = W2 (s)g(s)w 1 (s) Select W 1 and W 2 such that G(s) becomes as close to diagonal (decoupled) as possible. Design a diagonal controller F y (s). Resulting controller: F y (s) = W 1 (s) F y (s)w 2 (s) Decoupling is usually performed approximately, e.g., the choice W 1 = G 1 (0) and W 2 = I performs decoupling at steady-state. With a suitable choice of W 1 and W 2 you can make a two-hand tap behave as one-hand tap. Much easier to control!

79 Linear Quadratic control: Problem formulation minimize s.t. { } E x(t) T Q 1 x(t) + u(t) T Q 2 u(t) dt 0 ẋ(t) = Ax(t) + Bu(t) + Nv 1 (t) z(t) = Mx(t) y(t) = Cx(t) + v 2 (t) (13) Assumption Assume that Q 1 0, Q 2 0, (A, B) stabilizable, and (A, M T Q 1 M) detectable. Furthermore, assume (A, C) detectable, (A, R 1 ) stabilizable, and v 1 and v 2 are Gaussian distributed white noise with intensities R 1 and R 2 respectively. Problem: Find the controller that minimizes the criterion in (13) among all possible causal controllers.

80 Linear Quadratic control: Solution Theorem (Linear Quadratic Gaussian (LQG)) The controller that minimizes the criterion in (13) under the assumptions above among all possible causal controllers (including nonlinear ones) is linear and given by u(t) = Lˆx(t) ˆx(t) = Aˆx(t) + Bu(t) + K (y(t) C ˆx(t)) (14) where L = Q 1 2 BT S with S as the symmetric pos. def. solution to A T S + SA + M T Q 1 M SBQ 1 2 BT S = 0 and K = P C T R 1 2 with P as the symmetric pos. def. solution to AP + P A T + NR 1 N T P C T R 1 2 P CT = 0

81 Linear Quadratic control: Solution structure The result shows that the separation principle holds for this problem The optimal solution can be split into two independent parts. Optimal observer: Kalman filter (gain K). Optimal feedback: LQ controller (gain L). The solution has the structure of feedback from estimated states. The separation principle does not hold for nonlinear problems, still the problem is usually separated into state-estimation and control in practice.

82 Feedback linearization: Mechanics x 1 position, x 2 velocity: x 2 ẋ 1 = x 2 ẋ 2 = k(x 1 ) b(x 2 ) + u m = 1 u x 1 where k(x 1 ) is a nonlinear position dependent force ( spring, gravitation, etc.). b(x 2 ) is a nonlinear velocity dependent force ( damping, friction). u control signal.

83 Feedback linearization: Mechanics x 2 x 1 position, x 2 velocity: m = 1 u x 1 ẋ 1 = x 2 ẋ 2 = k(x 1 ) b(x 2 ) + u Try with u = ū + k(x 1 ) + b(x 2 )

84 Feedback linearization: Mechanics u = ū + k(x 1 ) + b(x 2 ) results in a linear system x 2 m = 1 u x 1 ẋ 1 = x 2 ẋ 2 = ū with a new virtual control signal ū. The system is said to be feedback linearized / exactly linearized!

85 Feedback linearization: Aircraft speed control x 1 speed, x 2 engine thrust: where ẋ 1 = D(x 1 ) + x 2 ẋ 2 = x 2 + u D(x 1 ) aerodynamic drag. u engine throttle.

86 Feedback linearization: Aircraft speed control x 1 speed, x 2 engine thrust: ẋ 1 = D(x 1 ) + x 2 ẋ 2 = x 2 + u How can we choose u here in order to exactly linearize the system?

87 Feedback linearization: Aircraft speed control x 1 speed, x 2 engine thrust: ẋ 1 = D(x 1 ) + x 2 ẋ 2 = x 2 + u How can we choose u here in order to exactly linearize the system? Not as obvious... A more general theory is necessary!

88 Control affine form A nonlinear system in the form ẋ = f(x) + ug(x) y = h(x) where f, g and h are assumed infinitely times differentiable functions. Furthermore, u and y are assumed scalars. Hence, we require a certain simplifying structure of how u enters the system.

89 How does y depend on u? ẋ = f(x) + ug(x) y = h(x) In order to be able to linearize the relation between u and y, it is necessary to find an explicit expression for it. y = h(x) does not contain it. Differentiate! ẏ = h x f + uh x g Relation is found if h x g 0. ÿ = (h x f) x f + u(h x f) x g Relation is found if (h x f) x g 0. y (3) = ((h x f) x f) x f + u((h x f) x f) x g Relation is found if ((h x f) x f) x g 0. y (4) =...

90 Relative degree ẋ = f(x) + ug(x) y = h(x) Differentiate y w.r.t. time. Then the relative degree is the number of times the output needs to be differentiated before a direct dependence on the input is found. smallest ν for which y (ν) = r ν (x) + us ν (x) with s ν (x) 0. Strong relative degree, if it also holds that s ν (x) 0, x.

91 Exact input-output-linearization Consider a system with strong relative degree ν. Then it holds that y (ν) = L ν f h + ul gl ν 1 f h, where L g L ν 1 f h 0 The relation between a new virtual control signal ū and the output y can be made linear by choosing u = The linear relation becomes 1 L g L ν 1 f h (ū Lν f h) y (ν) = ū

92 But......if the relative degree ν < dim(x). Then there are nonlinear dynamics called zero dynamics that are not visible in the output. If it is stable, it is not a problem, if it is unstable, the proposed linearization will not work. Sometimes it is possible to get rid of the zero dynamics by choosing another output such that a strong relative degree equal to dim(x) is obtained.

93 Feedback linearization: Aircraft speed control, cont d x 1 speed, x 2 engine thrust: ẋ 1 = D(x 1 ) + x 2 ẋ 2 = x 2 + u How can we choose u here in order to exactly linearize the system? Choose u as u = D (x 1 )D(x 1 ) + D (x 1 )x 2 + x 2 + ū with a new virtual control signal ū.

94 Fundamental limitations Some examples of fundamental limitations: Unstable systems Systems with time delays Non-minimum phase systems...

95 Fundamental limitation: Unstable system Intuition: A stabilizing controller has to be faster than the system runs away. A more rigorous estimate suggests that for an unstable real pole at p 1 a rule of thumb is that ω B > 2p 1 (bandwidth twice as high as the pole location). Furthermore: Reliability of the controller is crucial, if it breaks...

96 Fundamental limitation: Time-delays Intuition: With a delay T d seconds it takes T d seconds to compensate for a disturbance, hence, we would like to avoid fast scenarios. A more rigorous estimate suggests that for a time-delay T d a rule of thumb is ω B < 1/T d.

97 Fundamental limitation: Non-minimum phase systems Non-minimum phase systems have at least one zero in the right-hand plane. a step response that starts in the wrong direction. Intuition: The system s gain has opposite signs for slow and fast scenarios. Good control for low frequencies might lead to feedback with wrong sign at high frequencies which might destabilize the system. Conclusion: avoid fast scenarios by designing a system with low bandwidth. Amplitude Step Response A more rigorous estimate suggests that for a real right-hand plane zero at z 0 a rule of thumb is that ω B z 0 / Time (sec)

98 Fundamental limitations: Summary Unstable system: lower bound on bandwidth. Time-delays: upper bound on bandwidth. Non-minimum phase systems: upper bound on bandwidth. Observation: Controlling a system which is unstable system and has time-delays or non-minimum phase behavior is fundamentally hard (theory confirms intuition).

99 1 Part I: Modeling and fundamental properties 2 Part II: Basic control 3 Part III: Optimal Control

100 Outline Part III: Optimal control Introduction Continuous time optimal control Discrete time optimal control problem Model predictive control A tractable form of real-time optimal control Re-optimization introduces feedback Linear MPC Hybrid MPC Explicit MPC On-line implementation via point-location Solving optimization problems in real-time is non-trivial

101 What is optimal control? A tool for making systems behave such that a given performance measure is optimized, without violating imposed constraints on inputs and behaviors Many applications Convenient and powerful controller synthesis tool for multivariable system (stability, uniqueness,...): LQ, H 2, H,... Energy optimal control Time optimal control Path planning... Optimal control was born in 1697, when Bernoulli published his solution to the brachystochrone problem (minimum time). Today: key component in autonomous systems.

102 The Goddard rocket problem Around 1910, Goddard invented a liquid propellant rocket whose thrust could be controlled. At that time, it was not self-evident to reach high altitudes. How should the thrust vary with time in order to reach the highest possible altitude? In presence of aerodynamic drag, the problem becomes complicated and interesting.

103 The Goddard rocket problem The problem can be formulated as an optimal control problem in the form maximize s.t. h(tf ) 1 (u D(v, h)) g m h = v v = m = γu 0 u umax v(0) = 0, h(0) = 0 m(0) = m0, m(tf ) m1 where g gravitational acceleration, u engine thrust (control input), D(v, h) aerodynamic drag, v velocity, and h altitude. The final time tf is a part of the optimization problem.

104 Minimum time on the racetrack Objectives: Compute an optimal reference trajectory minimizing the lap time. Follow the pre-computed reference trajectory using LQ control. Ongoing student project: overtake vehicles and avoid obstacles along the track.

105 Show movie! OSARR

106 Example: Motion planning Reverse with a truck and trailer through a maze

107 Show movie! Motion planning for a reversing general 2-trailer configuration using Closed-Loop RRT

108 Continuous time optimal control problem minimize u t0 +T t 0 L(t, x(t), u(t))dt + φ(x(t 0 + T )) s.t. ẋ = f(t, x, u) u U x X (15) x(t 0 ) x, x(t 0 + T ) X T Performance measure is an integral in time + an end penalty. The solution should satisfy a differential equation. Variants include the final time being an optimization variable or it can be infinite. The LQ problem is a special case of this problem! Solution forms Optimal feedback/policy Optimal open loop signal: generate reference trajectories.

109 Continuous time optimal control problem, cont d For most problems of practical interest, it is not possible to obtain a solution as a closed form analytical expression. Solve numerically. Methods for (mainly numerically) solving optimal control problems: Hamilton-Jacobi-Bellman equation, Dynamic programming (in discrete time) Smart exhaustive search Feedback solution. Indirect methods, Pontryagin s minimum principle Solve necessary conditions of optimality. Numerically: First optimize, then discretize. Open loop solution. Direct methods (most common method today) Numerically: First discretize, then optimize. Reduce the infinite dimensional problem into a finite dimensional one and solve using an NLP solver. Methods: Direct Single Shooting, direct collocation, Direct multiple shooting.

110 Discrete time optimal control problem CFTOCP: minimize u t 0 +T 1 t=t 0 L(x t, u t ) + φ(x(t 0 + T )) s.t. x t+1 = f(x t, u t ) u U x X x t0 = x, x t0 +T X T The integral has been replaced by a sum, the differential equation has been replaced by a difference equation. The discrete time optimal control problem is a multi-stage decision problem. A common form of stage is time, but can be anything that gives a new opportunity to revise our decision of the input. The actual solution method depends on the problem class, but often the problem is basically already in the form of an NLP.

111 Model predictive control: tractable real-time optimal control minimize u 0,u 1,... s.t. L(x t, u t ) t=0 x 0 = x x t+1 = f(x t, u t ) u U x X A controller is usually something that is supposed to run forever, hence, performance should be optimized from now (t = 0) to infinity. Denote the optimal policy (state feedback) µ(x). (16)

112 Model predictive control: tractable real-time optimal control minimize u 0,u 1,... s.t. T 1 L(x(t), u(t)) + L(x t, µ(x t )) t=0 x 0 = x x t+1 = f(x t, u t ) u U x X t=t (16) Split the infinite horizon in two parts, where the optimal policy is inserted in the tail-part. Denote the cost for the optimal tail φ(x(t )): φ(x(t )) = L(x t, µ(x t )) t=t

113 Model predictive control: tractable real-time optimal control minimize u 0,u 1,... T L(x t, u t ) + φ(x T ) t=0 s.t. x t+1 = f(x t, u t ) u t U x t X x 0 = x, x T X T (16) We are back in the form of CFTOCP, solvable using of-the-shelf NLP optimization routines, under the assumption that φ(x), f(x, u), U, and X are in a form supported by a solver. Typically φ(x T ) is approximated in practice, e.g., chosen as the quadratic function obtained when µ(x) = Lx and L is the LQ solution for the system linearized around the origin.

114 Receding horizon control An approximate feedback solution to the infinite time problem can be obtained using a receding horizon control strategy: At time t 0 solve CFTOCP with prediction horizon T and with initial state x = x(t 0 ). Apply only first computed control input u 0 to plant. Repeat procedure in next sample. x(t) u(t) Prediction N steps t t

115 Receding horizon control An approximate feedback solution to the infinite time problem can be obtained using a receding horizon control strategy: At time t 0 solve CFTOCP with prediction horizon T and with initial state x = x(t 0 ). Apply only first computed control input u 0 to plant. Repeat procedure in next sample. x(t) u(t) Prediction N steps t t

116 Receding horizon control An approximate feedback solution to the infinite time problem can be obtained using a receding horizon control strategy: At time t 0 solve CFTOCP with prediction horizon T and with initial state x = x(t 0 ). Apply only first computed control input u 0 to plant. Repeat procedure in next sample. x(t) u(t) Prediction t t N steps

117 Re-optimization introduces feedback Since the optimization problem to be solved in real-time on-line in MPC is parameterized in the initial state x, it follows that u. is a function of x. u ( x) means that the obtained solution is a state feedback. Since MPC implements a feedback control law, it is not as sensitive to model errors and disturbances as an open loop solution would have been. As with all feedback approaches, MPC can destabilize the closed loop system if not done properly. Since u ( x) is computed pointwise by the numerical optimization routine, no closed form expression for the feedback/policy u ( x) is obtained.

118 How does MPC work in the loop? Measurement Controlled system. Obtained measurements influence parameters in the optimization problem regarding the system s predicted future behavior. The new control signal is obtained from the solution to this optimization problem. Repeat: Measure, update parameters and re-optimize in each sample. MIQP Non-convex Lin. inequality constr. D N Eq. QP Convex Lin. equality constr. D N On-line MPC controller Branch and bound Divide into convex QP-problems Optimization Active Set QP Divide into equality constrained QP-problems Controlled system Ineq. QP Convex Lin. inequality constr. D N Control signal

119 Special case: MPC for discrete-time linear systems Given the current state x, solve in each sample: minimize u s.t. N 1 V T (x(n)) + L(x t, u t ) t=0 x t+1 = Ax t + Bu t H x x t + H u u t + h 0 x 0 = x, x N X T (17) Stage cost: L(x t, u t ) = x T t Qx t + u T t Ru t Terminal cost: V T (x N ) = x T N P x N Terminal set: X T (polyhedral) Optimization problem: convex QP with parameter x. Linear MPC is the most common MPC formulation today in applications.

120 Formulation as QP The linear MPC problem can be formulated as a QP in standard form minimize z s.t. 1 2 zt Hz + f T z A E z = S E x + b (18) E A I z b I where z is decision variable and x parameter. Can for a given fixed value of the parameter x be solved numerically using, e.g., Active-set methods Interior-point methods Accelerated gradient methods Software: CPLEX, Gurobi, CVXGEN, FORCES, qpoases,...

121 Hybrid MPC for systems in MLD form minimize u s.t. N 1 V T (x(n)) + L(x t, u t ) t=0 x t+1 = Ax t + B 1 u t + B 2 δ t + B 3 z t y t = Cx t + D 1 u t + D 2 δ t + D 3 z t E 2 δ t + E 3 z t E 4 x t + E 1 u t + E 5 x t,b, u t,b, δ t,b {0, 1} x 0 = x, x N X T Looks similar to the linear case, but there are binary variables. The QP has now been replaced by a Mixed Integer Quadratic Programming (MIQP) problem: NP-hard. The logical and continuous part of the OCP is solved in one shot. Solved using state-of-the-art MILP/MIQP solvers such as e.g. Gurobi and CPLEX.

122 Applications for MPC Originally for slow systems, e.g., process industry. Today increasingly faster applications. Progress has been made for advanced problems with nonlinear and hybrid dynamics. The development has required optimization algorithms with lower and more predictable computation times. I will outline some tools for achieving this.

123 Explicit linear MPC via mp-qp J ( x) = minimize z s.t. 1 2 zt Hz + f T z A E z = S E x + b E A I z b I where z is decision variable and x parameter. Instead of solving this problem pointwise in x, the parametric nature can be exploited and the problem can be solved as a multiparametric QP (mp-qp) for all x X. In the linear case, z ( x) is continuous polyhedral piecewise affine and J ( x) is continuous polyhedral piecewise quadratic. The same principle works also for hybrid systems: mp-miqp

124 Interpretation from a control perspective Control interpretation: Linear MPC is a continuous piecewise affine state feedback u ( x t ) = F i x t + g i, when H i x t k i where x t is a measurement or estimate of the state of the system. The computational effort is moved off-line off-line: solve mp-qp, computationally intensive. on-line: the solution is looked-up in a table, fast. Pros Capable of short sampling times. Bounded on-line computation time: real-time guarantees. Explicit expression for the feedback available for analysis etc. Cons Complexity (number of regions) of the explicit solution typically grows quickly with the state dimension, prediction horizon and number of constraints. The computational complexity off-line can become intractable.

125 Example: MPC control of a double integrator Optimal parametric solution u ( x) Velocity x 2 Position x 1 Cost function: x T T P x T + T 1 i=0 x i u2 i with P from Riccati solution. Prediction horizon: T = 10. Constraints: u 1 and x 5.

126 Explicit MPC inside and outside the loop Convex Convex Divide into equality constrained QP-problems Measurement Off-line Set of all possible measurements On-line Explicit MPC controller Ineq. QP Lin. inequality constr. D N Eq. QP Lin. equality constr. D N Active Set QP Optimization Control signal Controlled system Control signal for all possible measurements

127 Looking up the solution on-line: Point-location Given a polyhedral collection {R i } n i=1, find a polyhedron ( region ) that contains the query point x. } R i = {x R d : H i x k i Once the region is known, the control signal can be computed as u ( x t ) = F i x t + g i, when H i x t k i *

128 Point-location: simple approach (sequential search) Search sequentially through the list of polyhedra for the one containing x. Computation time grows as the total number of dividing hyperplanes: intractable for large problems. *

129 Point-location: balanced binary search tree Use the hyperplanes as decision criteria in a binary search tree. Ideally: every decision cuts away half the parameter space. Computation time grows logarithmically in number of regions. Works also for larger problems if the tree can be computed off-line in reasonable time. *

130 Solving optimization problems in real-time is non-trivial Approaches to reduce the computational burden: Warm-starts In receding horizon control the parameter x does not change that much from sample to sample. Let the optimization algorithm start with the old solution as an initial guess. Very effective for active-set methods and first-order methods. Less effective for interior-point methods. Problem structure exploitation Utilize structure from dynamics in the KKT system for Newton step (search direction) computations. Linear complexity in the length of prediction horizon. Parallel computations Perform numerical linear algebra in parallel. Logarithmic complexity in the length of prediction horizon. Code generation Automatically generate an optimized C-code for the specific problem to be solved on-line.

131 Solving optimization problems in real-time is non-trivial Approaches to reduce the computational burden: Warm-starts In receding horizon control the parameter x does not change that much from sample to sample. Let the optimization algorithm start with the old solution as an initial guess. Very effective for active-set methods and first-order methods. Less effective for interior-point methods. Problem structure exploitation Utilize structure from dynamics in the KKT system for Newton step (search direction) computations. Linear complexity in the length of prediction horizon. Parallel computations Perform numerical linear algebra in parallel. Logarithmic complexity in the length of prediction horizon. Code generation Automatically generate an optimized C-code for the specific problem to be solved on-line.

132 Karush-Kuhn-Tucker system for Newton step The search directions are found from a linear system in this form: 0 I I Qx Qxu A T Qxu T Qu B T A B 0 I I Qx Qxu A T Qxu T Qu B T A B 0 I λ 0 x 0 u 0 λ 1 x ˆx 1 u 0 1. λ 2 =. I.. I 0. I Qx Qxu A T u Qxu T Qu B T N 1 λ A B 0 I N xn I Qx Almost block diagonal. Can be solved using a Riccati recursion with linear computational complexity growth in the number of stages in the problem.

133 Thank you Thank you for your attention! Questions?

134 Literature Francesco Borrelli, Alberto Bemporad, and Manfred Morari. Predictive control for linear and hybrid systems, October Stephen Boyd and Lieven Vandenberghe. Convex Optimization. Cambridge University Press, Torkel Glad and Lennart Ljung. Control Theory Multivariable and Nonlinear Methods. Taylor & Francis, Torkel Glad and Lennart Ljung. Reglerteori Flervariabla och olinjära metoder. Studentlitteratur, Torkel Glad and Lennart Ljung. Reglerteknik Grundläggande teori. Studentlitteratur, Thomas Kailath. Linear systems. Prentice Hall, Hassan K. Khalil. Nonlinear systems. Prentice Hall, Karl J. Åstrom and Tore Hägglund. Advanced PID control. ISA, Kemin Zhou, John C. Doyle, and Keith Glover. Robust and optimal control. Prentice Hall,