MAE546Lecture3 (1)

8/10/2019 MAE546Lecture3 (1)

1/15

Principles for Optimal Control ofDynamic Systems

Robert StengelOptimal Control and Estimation, MAE 546,

Princeton University, 2013 Dynamic systems

Cost functions

Problems of Lagrange, Mayer, and Bolza

Necessary conditions for optimality

Euler-Lagrange equations

Sufficient conditions for optimality Convexity, normality, and uniqueness

Copyright 2013 by Robert Stengel. A ll rights reserved. For education al use only.http://www.princeton.edu/~stengel/MAE546.html

http://www.princeton.edu/~stengel/OptConEst.html

!x(t) =dx(t)

dt= f[x(t),u(t)]

! Dynamic Process! Neglect disturbance effects, w(t)

! Subsume p(t)and explicit dependenceon tin the definition of f[.]

The Dynamic Process

Trajectory ofthe System

!x(t)=

dx(t)

dt=f

[x(t),u(t)]

! Integrate the dynamic equation to determine the trajectoryfrom original time, t0, to final time, tf

x(t) = x(t0 )+ f[x(!),u(!)]d!t0

t

" ,

given u(t) for t0 # t# tf

What Cost FunctionMight Be Minimized?

Minimize time required to go from A to B

J = Fuel-use Efficiency( )0

final range

! dR = Fuel Used

J = dt0

final time

! = Final time

J = Cost per hour( )dt0

final time

! = $$

Minimize fuel used to go from A to B

Minimize financial cost of producing a product

8/10/2019 MAE546Lecture3 (1)

2/15

Optimal System Regulation

J =1

T

x2(t)( )

0

T

! dt

J =1

Tx

T(t)x(t)!" #$

0

T

% dt=1

Tx

1

2+ x1

2+!+ x

n

2!" #$0

T

% dt

dim(x) = 1 x 1

dim(x) = nx 1

Minimize mean-square state deviations over a time interval:

Scalar variation of a single component

Sum of variation of all state elements

Weighted sum of state element variations

Why not use infinite control?

J =1

Tx

T(t)Qx(t)!" #$

0

T

% dt =1

Tx

1 x

2 x

3!"

#$

q11

q12

q13

q21

q22

q23

q31

q32

q33

!

"

&&&

#

$

'''

x1

x2

x3

!

"

&&&

#

$

'''

(

)**

+**

,

-**

.**

0

T

% dt

n = 3dim(x) = nx 1

dim(Q) = nx n

Tradeoffs Between State andControl Variations

Tradeperformance, x, against control usage, u

J = x2(t)+ru 2 (t)( )0

T

! dt, r >0

J = xT(t)x(t) +ru

T(t)u(t)( )

0

T

! dt, r > 0

dim(u) = 1 x 1

dim(u) = mx 1

J = xT(t)Qx(t) + u

T(t)Ru(t)( )

0

T

! dt, Q, R > 0 dim(R) = mx m

Weight the relative importance of state and control components

Minimizea cost function that contains state and control vectors

Examples

Effects of Control Weighting inOptimal Control

J= xT(t)Qx(t)+ u

T(t)Ru(t)( )

0

T

! dt, Q, R > 0 Q =1 0

0 1

!

"#

$

%&

R = r = 1 or 100

Optimal feedback

control stabilizes anunstable system

Smaller control weight

Allows larger controlresponse

Decreases statevariation

Larger control weight

conserves controlenergy

8/10/2019 MAE546Lecture3 (1)

3/15

Open- and Optimal Closed-LoopResponse to Disturbance

Q =100 0

0 100

!

"#

$

%&

R = 1

Stable 2nd-order linear dynamic system: dx(t)/dt=Fx(t)+Gu(t)

Optimal feedback control reduces response to disturbances

Time ResponsePhase-Plane Plot

(Rate vs. Displacement) Classical Cost Functionsfor Optimizing DynamicSystems

The Problem of Lagrange(c. 1780)

Examples of Integral Cost: theLagrangian

L x(t),u(t)[ ]= xT(t)Qx(t)+ uT(t)Ru(t)!" #$ Quadratic trade between state and control

= 1 Minimum time problem

= !m(t) = fcn x t( ),u t( )!" #$ Minimum fueluse problem

L x(s),u(s)[ ]= Change in area with respect to differential length, e.g., fencing, ds[Maximize]

minu

(t)

J = L x(t),u(t)[ ]to

tf

! dt

dim(x) = n !1

dim(f) = n !1

dim(u) = m !1

subject to

!x(t) = f[x(t),u(t)] , x(to)given

! x(tf)"# $% = xT(t)Px(t)

t=tfWeighted square - error in final state

= tfinal& tinitial( ) Minimum time problem

= minitial& mfinal( ) Minimum fuel problem

Examples of Terminal Cost

The Problem of Mayer(c. 1890)

minu(t) J =

! x

(tf)"#

$%

subject to

!x(t) = f[x(t),u(t)] , x(to)given

8/10/2019 MAE546Lecture3 (1)

4/15

The Problem of Bolza (c. 1900)The Modern Optimal Control Problem*

Combine the Problems of Lagrange and Mayer

minu(t)

J = ! x(tf)"# $% + L x(t),u(t)[ ]to

tf

& dt Minimize the sum ofterminal and integralcosts By choice of u(t)

Subject to dynamic

constraint

subject to

!x(t) = f[x(t),u(t)] , x(to )given

and with fixed end time, tf

Augmented Cost Function

JA = ! x(tf)"# $% + L x(t),u(t)[ ]+T(t) f[x(t),u(t)]'!x(t)[ ]{ }

to

tf

( dt

Adjoin the dynamic constraint to the integrand using a

Lagrange multiplier*to form the Augmented Cost Function, JA:

dim t( )"# $% = dim f x t( ),u t( ),t"# $%{ } = n &1

The Dynamic Constraint

The constraint = 0 when the dynamic equation is satisfied

dimT(t) f[x(t),u(t)]" !x(t)[ ]{ } = 1# n( ) n #1( ) = 1

f[x(t),u(t)]! !x(t)[ ] = 0 when !x(t) =f[x(t),u(t)] in t0, tf"# $%

* Lagrange multiplier is also called

Adjoint vector

Costate vector

Necessary Conditionsfor a Minimum

8/10/2019 MAE546Lecture3 (1)

5/15

Necessary Conditionsfor a Minimum

! Cost is insensitive to control-induced perturbations atthe final time

!

Satisfy necessary conditions for stationarityalongentire trajectory, from toto tf

! For integral to be minimized, integrand takeslowest possible value at every time

!Linear insensitivity to small control-inducedperturbations

!Large perturbations can only increase the integralcost

L x(t),u(t)[ ]+ T (t) f[x(t),u(t)]" !x(t)[ ]{ }

Integrand must be linearlyinsensitive to control-inducedperturbation

Larger perturbations can onlyincrease the integrand

Integrand

The Hamiltonian

H x(t),u(t),(t)[ ] = L x(t),u(t)[ ]+ T(t)f x(t),u(t)[ ]

L x(t),u(t)[ ]+ T (t) f[x(t),u(t)]"!x(t)[ ]{ } =

H x(t),u(t),(t)[ ]" T(t)!x(t){ }

Re-phrase the integrand by introducing the Hamiltonian

The Hamiltonian is a function of the Lagrangian,adjoint vector, and system dynamics

Incorporate the Hamiltonian inthe Cost Function

J = ! x(tf)"# $% + H x(t),u(t), (t)[ ]'T(t)!x(t){ }

to

tf

( dt

The optimal cost, J*, is produced by the optimal historiesof state, control, and Lagrange multiplier: x*(t),u*(t),and

Variations in the Hamiltonian reflectintegral cost

constraining effect of system dynamics

Substitute the Hamiltonian in the cost function

minu(t)

J = J* = ! x* (tf)"# $% + H x* (t),u* (t), * (t)[ ]' *T(t)!x *(t){ }

to

tf

( dt

* t( )

8/10/2019 MAE546Lecture3 (1)

6/15

8/10/2019 MAE546Lecture3 (1)

7/15

First-Order Insensitivityto Control Perturbations

1) !"

!x# T

%

&'(

)* t=tf= 0

!x 0( ) =f x 0( ),u 0( )!" #$ need not be zero, but

x 0( ) cannot change instantaneously unless control is infinite

% &x &u( )!" #$t=t0

' 0, so &Jt=0

= 0

2) !H

!x+

! T#

$%&

'(= 0 in t

0,tf( )

Individual terms of !J* must remain zero for arbitrary variations in !u t( )

3) !H

!u= 0 in t0 ,tf( )

Euler-LagrangeEquations

Euler-Lagrange Equations

1 (tf) ="#[x(tf)]

"x

$%

&

'(

)

T

Jacobian matrices

F(t) !!f

!xt( )

G(t) !!f

!ut( )

2 ! (t) =" #H[x(t),u(t), (t),t]

#x

$%&

'()

T

=" #L

#x+

Tt( )

#f

#x

*

+,-

./

T

=" Lx(t) +

Tt( )F(t)*+ -.

T

3) !H[x(t),u(t),(t),t]

!u=

!L

!u+

Tt( )

!f

!u

#

$%&

'(= L

u(t) +

Tt( )G(t)#$ &' = 0

Ordinary differential equation for adjoint vector

Boundary condition for adjoint vector

Optimality condition

Jacobian Matrices

8/10/2019 MAE546Lecture3 (1)

8/15

Jacobian MatricesExpress the SolutionSensitivity to Small Perturbations

Sensitivity to state perturbations:stability matrix

F(t) =!f

!x x=xN( t)u=uN(t)w=wN( t)

=

!f1!x1

!f1!x2

... !f1

!xn

!f2!x1

!f2!x2

... !f2

!xn

... ... ... ...

!fn!x1

!fn!x2

... !fn

!xn

"

#

$$$$$$$$

%

&

''''''''x=xN(t)u=uN( t)w=wN(t)

Nominal (reference) dynamic equation

!xN(t) =

dxN(t)

dt

=f[xN(t),u

N(t)]

Sensitivity to Small ControlPerturbations

Control-effect matrix

G(t) =!f

!u x=xN( t)u=uN( t)w=wN( t)

=

!f1

!u1

!f1

!u2

... !f

1

!um

!f2!u1

!f2!u2

... !f2

!um

... ... ... ...

!fn!u

1

!fn!u

2

... !fn

!um

"

#

$$$$$$$$

%

&

''''''''x=xN(t)u=uN(t)w=wN(t)

Jacobian Matrix Example

F t( ) =

0 1 0

!2a1 x

3Nt( )! x

1Nt( )"# $% !a2 a2 + 2a1 x3N t( )! x1N t( )"# $%

c1+ b

3u

1Nt( )"# $% c1 3c2x3N

2t( )

"

#

&&&&

$

%

''''

Jacobian matrices are time-varying

Original nonlinear equation describes nominal dynamics

G t( ) =

0 0

b1

b2

b3x

1N

t( ) 0

!

"

###

$

%

&&&

!xN t( ) =

!x1N

t( )

!x2N

t( )

!x3N t( )

!

"

###

#

$

%

&&&

&

=

x2N

t( )

a2 x

3N

t( )'x2N

t( )!" $%+ a1 x3N t( )'x1N t( )!" $%2

+b1u1N

t( )

c2x

3N

t( )3 + c1 x1N

t( )+ x2N

t( )!" $%+b3x1N t( )u1N t( )

+b2u2N

t( )

!

"

###

##

$

%

&&&

&&

Dynamic Optimization isa Two-Point Boundary

Value Problem

Boundary condition for the state equationis specified at t0

!x(t) = f[x(t),u(t)] , x(to )given

Boundary condition for theadjoint equationis specified attf

! (t) =" #L

#xt( )+ T t( )

#f

#xt( )

$

%&'

()

T

, (tf) =

#*[x(tf)]

#x

+,-

./0

T

8/10/2019 MAE546Lecture3 (1)

9/15

Sample Two-Point Boundary Value ProblemMove Cart 100 Meters in 10 Seconds

!x1

!x2

!

"

##

$

%

&&

=

x2

u

!

"##

$

%&& L =ru

2; ' =q x

1f(100( )

2

Cost function: tradeoff between

Terminal error squared

Integral cost of control squared

x1

x2

!

"

##

$

%

&&=

Position

Velocity

!

"##

$

%&&

H x,u,[ ] = L x,u[ ]+ Tf x,u[ ]

= ru2+ !

1 !

2"#

$%

x2(t)

u(t)

"

#&&

$

%''

J= q x1f

!100( )2

+ ru2 dtto

tf

"

Solution forAdjoint Vector

! (t) =" #H

#x

$%&

'()

T

=" #L

#x+

T#f

#x

*+,

-./

T

=" 0 + !1 !2( ) 0 10 0

0

123

45*

+,,

-

.//

T

(tf) =#6[x(tf)]

#x

$%&

'()

T

= 2q x1f

"100( ) 0*+,

-./

T

!1(t)

!2(t)

"

#

$$

%

&

''=

!1(tf)

!1(tf) tf( t( )

"

#

$$

%

&

''=

2q x1f

(100( )2q x

1f(100( ) tf( t( )

"

#

$$$

%

&

'''

!!1

!!2

"

#

$$

%

&

''= (

0

!1

"

#$$

%

&'';

!1

!2

"

#

$$

%

&

''

t=tf

=

2q x1f

(100( )0

"

#

$$

%

&

''

Solution forControl History

!H

!u"#$

%&'T

=!L

!u"#$

%&'T

+!f

!u"#$

%&'T

t( ))*++

,-..= 0

2ru(t)+ 0 1( )2q x

1f!100( )

2q x1f

! 100( ) tf!t( )

"

#

$$$

%

&

'''=0

Optimality condition

Optimal control strategy

u(t) =!q

r

x1f

!100( ) tf! t( ) !k1 + k2t

Cost Weighting Effects onOptimal Solution

u(t) =!q

rx

1f! 100( ) tf! t( ) !k1 + k2tx(t) = x(to ) + f[x(t),u(t)]dt, to! tf

to

t

"

x1(t)

x2(t)

#

$

%%

&

'

((=

k1t2 2 +k2t

3 6

k1t+ k2t

22

#

$

%%

&

'

((

For t= 10s, x!f

=

100

1+ 0.003r

q

8/10/2019 MAE546Lecture3 (1)

10/15

Typical Iteration to FindOptimal Trajectory

Calculate x(t)using prior estimate of u(t)

x(t) = x(to ) + f[x(t),u(t)]dt, to!tfto

t

"

(t) = (tf)" #L

#xt( )+ T t( )

#f

#xt( )

$

%&'

()

T

dttf

t

* , tf+to

Calculate adjoint vector using prior estimate of x(t)and u(t)

Typical Iteration to FindOptimal Trajectory

Calculate H(t)and #H/#uusing prior

estimates of state, control, and adjoint vector

H x(t),u(t),(t)[ ] =L x(t),u(t)[ ]+ T(t)f x(t),u(t)[ ]

"H

"u=

"L

"u+

Tt( )

"f

"u

#

$%&

'(, to)tf

Estimate new u(t)

unew

(t) = uold

(t) +!u"H(t)

"u

#

$%&

'(, t

o) t

f

Alternative NecessaryCondition for Time-Invariant Problem

Time-Invariant Optimization Problem

Time-invariant problem: Neither Lnor fis

explicitly dependent on time

H x(t),u(t),(t),t[ ] = L x(t),u(t)[ ]+ T(t)f x(t),u(t)[ ]

= H x(t),u(t),(t)[ ]

!x(t) =f[x(t),u(t),p(t),t] =f[x(t),u(t),p]

L x(t),u(t),t[ ] = L x(t),u(t)[ ]

Then, the Hamiltonian is

8/10/2019 MAE546Lecture3 (1)

11/15

Time-Rate-of-Change of theHamiltonian for Time-Invariant System

dH[x(t),u(t), (t)]

dt=

"H

"t+

"H

"x

"x

"t+

"H

"u

"u

"t+

"H

"

"

"t

dH

dt= L

x(t) +

Tt( )F(t)"# $%!x + Lu(t) +

Tt( )G(t)"# $%!u + f

T !

dH

dt= Lx(t) +

Tt( )F(t)( )+ ! #$ %&!x + Lu (t)+

Tt( )G(t)#$ %&!u

= 0[ ]!x + 0[ ]!u = 0 on optimal trajectory

from Euler-Lagrange Equations #2 and #3

Hamiltonian is Constant on theOptimal Trajectory

dH

dt= 0! H* = constant on optimal trajectory

For time-invariant system dynamics and Lagrangian

H* = constantis an alternative scalarnecessary condition for optimality

Open-End-TimeOptimization Problem

Open End-Time Problem

Final time, tf, is free to vary

J = ! x(tf)"# $% + H x(t),u(t),(t)[ ]'T(t)!x(t){ }

to

tf

( dt

tfis an additional control variable for minimizing J

!J = !J(tf) + !J(t0 )+ !J(t0 " tf)

!J(tf) = !J(tf)fixed tf

+

dJ

dt t=tf

!tf

Goal: tffor which sensitivity to perturbation in final time is zero

Final Time

Cost

8/10/2019 MAE546Lecture3 (1)

12/15

Additional NecessaryCondition for OpenEnd-Time Problem

Cost sensitivity to final time should be zero

!"*

!t=#H* at t =tf for open end time

Final Time

Cost

Additional necessary condition for stationarity

dJ

dt t=tf

=!"

!t+

!"

!x!x

#

$%&

'(+ H) T !x#$ &'

+,-

./0 t=tf

=!"

!t+

T!x

#

$%&

'(+ H) T !x#$ &'

+,-

./0 t=tf

=!"

!t+H

#$%

&'( t=tf

=0

H* = 0with Open End-Time

If terminal cost is independent of time,

andfinal time is open

dJ

dt t=tf

=!"

!t+H

#$%

&'( t= tf

= 0( )+H{ }t=tf

= 0

!H*t=tf

= 0

H* = 0with Open End-Time andTime-Invariant System

!H*t= tf

= 0

If terminal and integral costs are independentof time, and final time is open

H* = 0 in t0 !t!tf

dH

dt= 0! H* = constant on optimal trajectory

Examples of OpenEnd-Time Problems

Minimize elapsed time to achieve an

objective

Minimize fuel to go from one place toanother

Achieve final objective using a fixedamount of energy

8/10/2019 MAE546Lecture3 (1)

13/15

Sufficient Conditionsfor a Minimum

Sufficient Conditions fora Minimum

Euler-Lagrange equations are satisfied(necessary conditions for stationarity),plus proof of

Convexity

Controllability Normality

Uniqueness

Singular optimal control

Higher-order conditions

! Strengthened condition

!2H x*,u*, *( )

!u2

> 0 in t0, tf( )

Positive definite (m x m)Hessian matrix

throughout trajectory

! Weakened condition

!2H x*,u*, *( )

!u2

# 0 in t0, tf( )

Hessian mayequal zero at

isolated points

ConvexityLegendre-Clebsch Condition

Normality andControllability

Normality: Existence ofneighboring-optimal solutions

Neighboring vs. neighboring-optimal trajectories

Controllability: Ability to satisfya terminal equality constraint

Legendre-Clebsch conditionsatisfied

8/10/2019 MAE546Lecture3 (1)

14/15

Neighboring vs. Neighboring-Optimal Trajectories

Nominal (or reference) trajectory and control history

xN(t),uN(t){ } fortin [to, tf]

Trajectory perturbed by

Small initial condition variation

Small control variation

x(t), u(t){ } fortin [to, tf]

= xN(t)+ !x(t), uN(t)+ !u(t){ }

This a neighboring trajectory

but it is not necessarily optimal

Both Paths Satisfy theDynamic Equations

Alternative notation

!xN(t) = f[x

N(t),u

N(t)], x

N t

o( ) given

!x(t) = f[x(t),u(t)], x to( ) given

!xN(t) = f[x

N(t),u

N(t)]

!x(t) = !xN(t)+ !!x(t) = f[x

N(t)+ !x(t),u

N(t) + !u(t)]

!x(to) = x(t

o)" x

N(t

o)

!x(t) = x(t)" xN(t)

!x(t) = x(t)" xN(t)

!u(t) = u(t)" uN(t)

Neighboring-OptimalTrajectories

xN*(t)is an optimal solution to a cost function

!xN *(t) = f[xN *(t),uN *(t)], xN to( ) given

JN* = ! xN *(tf )"# $% + L xN *(t),uN *(t)[ ]to

tf

& dt

!x *(t) = f[x *(t),u*(t)], x to( ) given

J* = ! x *(tf )"# $% + L x *(t),u *(t)[ ]to

tf

& dt

If x*(t)is an optimal solution to the same cost function

Then xNand xare neighboring-optimal trajectories

UniquenessJacobi Condition

Finite state perturbation impliesfinite control perturbation

No conjugate points

Example: Minimum distance fromthe north pole to the equator

ConjugatePoint atNorth Pole!

x(t) < "{ } # !u(t) < "{ }

http://www.encyclopediaofmath.org/index.php/Jacobi_condition

http://en.wikipedia.org/wiki/Conjugate_points

8/10/2019 MAE546Lecture3 (1)

15/15

Next Time:

Principles for Optimal Control,Part 2

Reading:

OCE: pp. 222-231

!"##$%&%'()$ +)(%,-)$

Time-Invariant Example with Scalar ControlCart on a Track

H x,u,[ ] =ru(t)2 + 2q x1f "100( ) 2q x1f " 100( ) tf" t( )#

$%&

'(x

2 (t)

u(t)

#

$

%%

&

'

((

ru(t)2+ 2q x1f !100( ) tf! t( )u(t)+ 2q x1f !100( )x2(t) = Constant TBD( )

H x,u,[ ] =L x,u[ ]+ Tf x,u[ ] = Constant

=ru(t)2+ !1(t) !2(t)

"#

$%

x2(t)

u(t)

"

#

&&

$

%

''

=ru(t)2+!1(t)x2(t) +!1(t) tf( t( )u(t) = Constant

Cart on a Trackwith Scalar Controland Open End Time

H* =ru(t)2+!1(t)x2 (t)+!1(t) tf" t( )u(t) =0

Fixed end-timeresults (tf= 10 s)

Open end-timewould beimportant only ifq/ris small