Linear-Quadratic Control

Jenny Hong Nicholas Moehle Stephen Boyd

EE103Stanford University

November 30, 2016

Outline

Linear dynamical system

Control

Variations

Examples

Linear quadratic regulator

Linear dynamical system 2

Linear dynamical system

xt+1 = Axt +But, t = 1, 2, . . .

I n-vector xt is state at time t

I m-vector ut is input at time t

I n× n matrix A is dynamics matrixI n×m matrix B is input matrixI sequence x1, x2, . . . is called state trajectory

Linear dynamical system 3

Simulation

I given x1, u1, u2, . . . find x2, x3, . . .

I can be done by recursion: for t = 1, 2, . . .,

xt+1 = Axt +But

Linear dynamical system 4

Vehicle example

consider a vehicle moving in a plane:

I sample position and velocity at times τ = 0, h, 2h, . . .

I 2-vectors pt and vt are position and velocity at time ht

I 2-vector ut gives applied force on the vehicle time ht

I friction force is −ηvtI vehicle has mass m

I for small h,

mvt+1 − vt

h≈ −ηvt + ut,

pt+1 − pth

≈ vt

I we use approximate state update

vt+1 = (1− hη/m)vt + (h/m)ut, pt+1 = pt + hvt

Linear dynamical system 5

I vehicle state is 4-vector xt = (pt, vt)

I dynamics recursion is

xt+1 = Axt +But,

where

A =

1 0 h 00 1 0 h0 0 1− hη/m 00 0 0 1− hη/m

, B =

0 00 0

h/m 00 h/m

Linear dynamical system 6

Outline

Linear dynamical system

Control

Variations

Examples

Linear quadratic regulator

Control 7

Control

I x1 is given

I choose u1, u2, . . . , uT−1 to achieve some goals, e.g.,

– terminal state should have some fixed value: xT = xdes

– u1, u2, . . . , uT−1 should be small, say measured as

‖u1‖2 + · · ·+ ‖uT−1‖2

(sometimes called ‘energy’)

I many control problems are linearly constrained least-squaresproblems

Control 8

Minimum-energy state transfer

I given initial state x1 and desired final state xdes

I choose u1, . . . , uT−1 to minimize ‘energy’

minimize ‖u1‖2 + · · ·+ ‖uT−1‖2subject to xt+1 = Axt +But, t = 1, . . . , T − 1

xT = xdes

variables are x2, . . . , xT , u1, . . . , uT−1I roughly speaking: find minimum energy inputs that steer the state

to given target state over T periods

Control 9

State transfer example

vehicle model with T = 100, x1 = (10, 10, 10,−5), xdes = 0

x

0 5 10 15 20 25

-5

0

5

10

y

Control 10

t

0 50 100

u1

u2

-4

-2

0

2

4

u

t

0 50 100

x1

x2

x3

x4

-10

0

10

20

30

x

Control 11

Outline

Linear dynamical system

Control

Variations

Examples

Linear quadratic regulator

Variations 12

Output tracking

I yt = Cxt is output (e.g., position)

I yt should follow a desired trajectory, i.e., sum square tracking error

‖y2 − ydes2 ‖2 + · · ·+ ‖yT − ydesT ‖2

should be small

I the output tracking problem is

minimize∑T

t=2 ‖yt − ydest ‖2 + ρ∑T−1

t=1 ‖ut‖2subject to xt+1 = Axt +But, t = 1, . . . , T − 1

yt = Cxt, t = 1, . . . , T − 1

variables are x2, . . . , xT , u1, . . . , uT−1, y2, . . . , yTI parameter ρ > 0 trades off control ‘energy’ and tracking error

Variations 13

Output tracking example

vehicle model with T = 100, ρ = 0.1, x1 = 0, yt = pt (positiontracking)

x

0 1 2 3 4 5 6

p

p desired

0

1

2

3

4

5

6

y

Variations 14

t

0 50 100

u1

u2

-10

0

10

20

30

40

50

u

t

0 50 100

x1

x2

0

1

2

3

4

5

6

x

Variations 15

Waypoints

I using output, can specify waypoints

I specify output (position) w(k) at time tk at K total places

minimize ‖u1‖2 + · · ·+ ‖uT−1‖2subject to xt+1 = Axt +But, t = 1, . . . , T − 1

Cxtk = w(k), k = 1, . . . ,K

variables are x2, . . . , xT , u1, . . . , uT−1

Variations 16

Waypoints example

I vehicle model

I T = 100, x1 = (10, 10, 20, 0), xdes = 0

I K = 4, t1 = 10, t2 = 30, t3 = 40, t4 = 80

I w(1) =

[3.53.5

], w(2) =

[1.54

], w(3) =

[4.51

], w(4) =

[00

]

Variations 17

Waypoints example

x

0 2 4 6 8

-4

-2

0

2

4

6

y

Variations 18

t

0 50 100

u1

u2

-15

-10

-5

0

5

10

u

t

0 50 100

x1

x2

waypoints

waypoints

-5

0

5

10

x

Variations 19

Outline

Linear dynamical system

Control

Variations

Examples

Linear quadratic regulator

Examples 20

Rendezvous

I we control two vehicles with dynamics

xt+1 = Axt +But, zt+1 = Azt +Bvt

I final relative state constraint xT = zTI formulate as state transfer problem:

minimize∑T−1

t=1 (‖ut‖2 + ‖vt‖2)subject to xt+1 = Axt +But, t = 1, . . . , T − 1,

zt+1 = Azt +Bvt, t = 1, . . . , T − 1,xT = zT

variables are x2, . . . , xT , u1, . . . , uT−1, z2, . . . , zT , v1, . . . , vT−1

Examples 21

Rendezvous example

x1 = (0, 0, 0,−5), z1 = (10, 10, 5, 0)

x

0 5 10 15 20 25

-15

-10

-5

0

5

10

y

Examples 22

Formation

I generalize rendezvous example to several vehicles

I final position for each vehicle defined relative to others (e.g., relativeto a ’leader’)

I leader has a final velocity constraint

Examples 23

Formation example

x

-5 0 5 10 15

-10

0

10

20

30

40

y

Examples 24

Outline

Linear dynamical system

Control

Variations

Examples

Linear quadratic regulator

Linear quadratic regulator 25

Linear quadratic regulator

I minimize energy while driving state to the origin:

minimize∑T

t=2 ‖xt‖2 + ρ∑T−1

t=1 ‖ut‖2subject to xt+1 = Axt +But, t = 1, . . . , T − 1

variables are x2, . . . , xT , u1, . . . , uT−1

I∑T

t=2 ‖xt‖2 is (sum square) regulationI x = 0 is some desired (equilibrium, target) state

I parameter ρ > 0 trades off regulation versus control ‘energy’

Linear quadratic regulator 26

I LQR problem is a linearly constrained least-squares problem:

minimize ‖Fz‖2subject to Gz = d

I variable z is (x2, . . . , xT , u1, . . . , uT−1)

I F , G depend on A, B, ρ; d depends (linearly) on x1I solution is ẑ = Hd for some H

I optimal first input û1 is a linear function of x1, i.e.,

û1 = Kx1

for some m× n matrix K (called LQR gain matrix)I finding K involves taking correct ‘slice’ of inverse KKT matrix

I entries of K depend on horizon T , and converge as T grows large

Linear quadratic regulator 27

State feedback control

I find K for LQR problem (with large T )

I for each t,

– measure state xt– implement control ut = Kxt

I with ut = Kxt is called state feedback control policy

I combine with (‘open-loop dynamics’) xt+1 = Axt +But to getclosed-loop dynamics

xt+1 = (A+BK)xt

I we can simulate open- and closed-loop dynamics to compare

Linear quadratic regulator 28

Example: longitudinal flight control

body axis

horiztheta

variables are (small) deviations from operating point or trim conditions;

state is xt = (wt, vt, θt, qt):

I wt: velocity of aircraft along body axis

I vt: velocity of aircraft perpendicular to body axis (down is positive)

I θt: angle between body axis and horizontal (up is positive)

I qt = θ̇t: angular velocity of aircraft (pitch rate)

input is ut = (et, ft):

I et: elevator angle (et > 0 is down)

I ft: thrust

Linear quadratic regulator 29

Linearized dynamics

for 747, level flight, 40000 ft, 774 ft/sec, dynamics arext+1 = Axt +But, where

A =

.99 .03 −.02 −.32.01 .47 4.7 .00.02 −.06 .40 −.00.01 −.04 .72 .99

, B =

0.01 0.99−3.44 1.66−0.83 0.44−0.47 0.25

I units: ft, sec, crad (= 0.01rad ≈ 0.57◦)I discretization is 1 sec

Linear quadratic regulator 30

LQR gain

gain matrix K converged for T ≈ 30

T

0 50 100

-0.2

-0.1

0.0

0.1

0.2

0.3

0.4

K

Linear quadratic regulator 31

LQR for 747 model

I LQR gain matrix (for T = 100, ρ = 100) is:

K =

[−.038 .021 .319 −.270−.061 −.004 −.120 .007

]I e.g., K14 = −.27 is gain from pitch rate ((xt)4) to elevator angle

((ut)1)

Linear quadratic regulator 32

747 simulation

ut = 0 (’open loop’)

t

0 50 100

0u

t

0 50 100

-5

0

5

x

Linear quadratic regulator 33

747 simulation

ut = Kxt (’closed loop’)

t

0 50 100

-0.1

0.0

0.1

0.2

0.3

u

t

0 50 100

-3

-2

-1

0

1

x

Linear quadratic regulator 34

Linear dynamical systemControlVariationsExamplesLinear quadratic regulator