Minimization with Equality Constraints

Transcription

1 Path Constraints and Numerical Optimization Robert Stengel Optimal Control and Estimation, MAE 546, Princeton University, 2015 State and Control Equality Constraints Pseudoinverse State and Control Inequality Constraints Numerical Methods Penalty function and collocation Neighboring optimal Quasilinearization Gradient Newton-Raphson Steepest descent Optimal reatment of an Infection Copyright 2015 by Robert Stengel. All rights reserved. For educational use only. Minimization with Equality Constraints

2 Minimization with Equality Constraint on State and Control min u(t ) J = x(t ) f + t f t o L [ x(t),u(t) ] dt subject to Dynamic Constraint x(t) = f[x(t),u(t)], x( t o ) given dim(x) = n 1 dim(f) = n 1 dim(u) = m 1 State/Control Equality Constraint cx(t),u(t) [ ] 0 in (t o, t f ); dim(c) = ( r 1) ( m 1) Aeronautical Example: Longitudinal Point-Mass Dynamics 1 V = C D 2 V 2 S = 1 1 C L V 2 V 2 S h = V sin r = V cos ( )( ) m = SFC m gsin m gcos x 1 = V : Velocity, m/s x 2 = : Flight path angle, rad x 3 = h : Height, m x 4 = r : Range, m x 5 = m : Mass, kg

3 Aeronautical Example: Longitudinal Point-Mass Dynamics u 1 = : hrottle setting, % u 2 = : Angle of attack, rad = maxsl ( e h ) : hrust, N ( ) Drag coefficient C D = C Do + C L 2 C L = C L = Lift coefficient S = Reference area, m 2 m = Vehicle mass, kg = Air density = SL e h,kg/m 3 g = Gravitational acceleration, m/s 2 SFC = Specific Fuel Consumption, g/kn-s Path Constraint Included in the Cost Function Hamiltonian Dimension of the constraint dimension of the control J 1 = x(t f ) + t f t o { L + 1 (t)[ f x(t) ]+µ c} cx(t),u(t) [ ] 0 in (t o, t f ) he constraint is adjoined to the Hamiltonian dt H L + 1 f +µ c dim(x) = dim(f) = dim() = n 1 dim(u) = m 1 dim(c) = dim(µ) = r 1, r m

4 Euler-Lagrange Equations Including Equality Constraint (t f ) = [x(t )] f x = H[x,u,,c,µ,t] x = L x + f c x +µ x = L x + F + c x µ H[x,u,,c,µ,t] u L = u + G + c u µ = 0 cx(t),u(t) [ ] 0 in (t o, t f ) dim(x) = dim() = n 1 dim( F)= n n dim( G)= n m dim(u) = m 1 dim(c) = dim(µ) = r 1, r m No Optimization When r = m m unknowns, m equations cx(t),u(t) [ ] 0 u(t) = fcn[ x(t] Example c = Ax()+ t Bu()= t 0; dim(x) = n 1; dim(c) = dim(u) = m 1 dim(a) = m n; dim(b) = m m u()= t B 1 Ax() t Constraint Lagrange multiplier is irrelevant but can be expressed from dh/du = 0, µ = c u L u + G c u is square and non-singular

5 No Optimization When r = m MAINAIN CONSAN VELOCIY AND FLIGH PAH ANGLE cx(t),u(t) [ ] 0 = ( ) 1 2 V 2 S 2 0 = V = max C Do + C L m gsin 0 = = 1 C L 1 V 2 V 2 S m gcos u(t) = fcn[ x(t] V ( 0)= V desired ( 0)= desired Effect of Constraint Dimensionality: r < m MINIMIZE FUEL AND CONROL USE WHILE MAINAINING CONSAN FLIGH PAH ANGLE min u(t ) t f ( ) J = q m 2 + r 1 u r 2 u 2 2 t o dt c[ x(t),u(t) ] 0 = = 1 V t () C L 1 2 V 2 m ()S t () gcos t ( 0)= desired

6 Effect of Constraint Dimensionality: r < m dim(x) = n 1 dim(u) = m 1 dim(c) = r 1 c u is not square when r < m c u is not strictly invertible hree approaches to constrained optimization Algebraic solution for r control variables using an invertible subset of the constraint Pseudoinverse of control effect Soft constraint dim(u) = m 1 dim(c) = r 1 Effect of Constraint Dimensionality: r < m Algebraic solution for r control variables using an invertible subset of the constraint dim(x) = n 1; Example1 dim(a r ) = r n dim(u) = m 1; dim(u r ) = r 1; dim(b r ) = r r c = A r x()+ t B r u r ()= t 0; det(b r ) 0 ()= t B 1 r Ax() t u r Example 2 dim(u) = m 1; dim(u r ) = r 1; dim(b 1 ) = r r c = B 1 B 2 u r u mr u r = B 1u r + B 2 u mr = 0; det(b 1 ) 0 () ()= t B 1 1 B 2 u mr t

7 Second Approach: Satisfy Constraint Using Left Pseudoinverse: r < m c u L is the left pseudoinverse of control sensitivity dim c u Lagrange multiplier L = r m c µ L = u L L u + G Pseudoinverse of Matrix y = Ax dim(x) = r 1 ( m 1)= ( m r) ( r 1) r = m, A is square and non-singular x = A 1 y ( r 1)= ( r m) ( m 1)= ( r r) ( r 1) r m, A is not square Use pseudoninverse of A dim(y) = m 1 x = A # y = A y ( r 1)= ( r m) ( m 1) Maximum rank of A is r or m, whichever is smaller See

8 Left Pseudoinverse dim(x) = r 1 dim(y) = m 1 Maximum rank of A is r or m, whichever is smaller dim( A A)= r r dim( AA )= m m r < m, Left pseudoinverse is appropriate Ax = y A Ax = A y Averaging solution ( ) 1 A y ( ) 1 A x = A A A L A A x = A L y ( r 1)= ( r m) ( m 1) dim(x) = r 1 dim(y) = m 1 Right Pseudoinverse r > m, Right pseudoinverse is appropriate Ax = y = Iy Ax = ( AA )( AA ) 1 y = Iy Minimum Euclidean error norm solution Ax = AA ( AA ) 1 y x = A ( AA ) 1 y A R A ( AA ) 1 x = A R y dim( A A)= r r dim( AA )= m m ( m 1)= ( m r) ( r 1)

9 Left Pseudoinverse Example Ax = y, x = A A x = 1 3 r < m ( ) 1 A y 1 3 x = = 1 ( 10 20)= 2 Unique solution Right Pseudoinverse Example Ax = y, r > m x = A ( AA ) 1 y x 1 x 2 Ax = y; 1 3 x 1 x 2 = = ( 14)= = x 1 x 2 = 2 4 x 1 x 2 At least two solutions satisfies the equation, but x 2 = 20 = 1.4 and x = 19.6 Minimum - norm solution

10 Necessary Conditions Use Left Pseudoinverse for r < m (t f ) = [x(t f )] x Optimality conditions L u = L x + F + + G + c u with cx(t),u(t) [ ] 0 µ = 0 c x µ hird Approach: Penalty Function Provides Soft State-Control Equality Constraint: r < m L L original + c c : Scalar penalty weight Euler-Lagrange equations are adjusted accordingly L orig u (t f ) = [x(t )] f x + c 2c u + G = 0 = L x + F = L orig x + c 2c x cx(t),u(t) [ ] 0 + F

11 Equality Constraint on State Alone cx(t) [ ] 0 in (t o, t f ) J 1 = x(t f ) + t f t o { L + 1 (t)[ f x(t) ]+µ c} Hamiltonian H L + 1 f +µ c Constraint is insensitive to control c = c x x + c u u = 0 c x x dt Example of Equality Constraint on State Alone MINIMIZE FUEL AND CONROL USE WHILE MAINAINING CONSAN ALIUDE min u(t ) t f ( ) J = q m 2 + r 1 u r 2 u 2 2 t o dt c[ x(t),u(t) ]= c[ x(t) ]= 0 = ht ()h desired

12 Introduce ime-derivative of Equality Constraint Equality constraint has no effect on optimality condition H u L = u L = u + G + c u + G µ Solution: Incorporate time-derivative of c[x(t)] in optimization Introduce ime-derivative of Equality Constraint c[x(t)] as the zeroth-order equality constraint cx(t) [ ] c (0) [ x(t) ] 0 Compute dc (0) [ x(t) ] dt [ = c(0) x(t) ] t + [ c(0) x(t) ] f[ x(t),u(t) ] x c (1) [ x(t),u(t) ]= 0

13 ime-derivative of Equality Constraint Optimality condition now includes derivative of equality constraint H u L = u Subject to + G + c(1) u c (0) [ x(t o )] 0 or c (0) x(t f ) 0 µ trajectory, c (1) = 0 If c (1) /u = 0, differentiate again, and again, State Equality Constraint Example dc (0) c[ x(t) ] c ( 0) [ x(t) ]= 0 = ht ()h desired No control in the constraint; differentiate [ x(t) ] [ = c(0) x(t) ] dt t c (1) [ x(t) ]= 0 = h t + [ c(0) x(t) ] f[ x(t),u(t) ] x ()= V()sin t () t Still no control in the constraint; differentiate again

14 State Equality Constraint Example Still no control in the constraint; differentiate again dc (1) [ x(t) ] dt [ = c(1) x(t) ] t [ ] + c(1) x(t) x f[ x(t),u(t) ] c (2) [ x(t) ]= 0 = ht ()= dv ()sin t ()+ t V() t d sin t dt dt 1 = maxsl ( e h ) C D 2 V 2 S m gsin sin () t + cos t () C L 1 2 V 2 S m gcos () State Equality Constraint Example Control appears in the 2 nd -order equality constraint c (2) = maxsl ( e h ) C D + cos t x(t),u() t = 0 ( ) 1 2 h () C L 1 2 h ( )V 2 t ( )V 2 t ()S ()S H L + 1 f +µ c ( 2) () mt ()gsin t () mt ()gcos t sin t () 0 th - and 1 st -order some point on the trajectory (e.g., t 0 ) c ( 0) [ x(t 0 )]= 0 ht ( 0 )= h desired [ ]= 0 ( t 0 )= 0 c () 1 x(t 0 )

15 Minimization with Inequality Constraints Hard Inequality Constraints

16 Inequality Constraints Inequality Constraints

17 Inequality Constraints Soft Control Inequality Constraint L Loriginal + c c Scalar Example c [ u(t)] = (u : Scalar penalty weight umax ) 2, umin < u < umax 0 (u umin ), u umax 2, u umin

18 Numerical Optimization Numerical Optimization Methods

19 Parametric Optimization min u(t ) J = x(t ) f + t f t o subject to x(t) = f[x(t),u(t)], [ ] L x(t),u(t) dt x(t o ) given k No adjoint equations Examples u(t) = k u(t) = k 0 + k 1 t + k 2 t 2 +, k = k 0 k 1 k m u(t) = k u(t) = k 0 + k 1 t + k 2 t 2 +, k = k 0 k 1 k m t u(t) = k 0 + k 1 sin t f t 0 + k t 2 cos t f t 0, k = k 0 k 1 k 2 Parametric Optimization min u(t ) J = x(t ) f + subject to x(t) = f[x(t),u(t)], t f t o [ ] L x(t),u(t) dt x(t o ) given for a minimum minimizing control parameter, k J k = 0 2 J k > 0 2

20 min u(t ) J = x(t ) f + t f t o subject to x(t) = f[x(t),u(t)], [ ] L x(t),u(t) dt x(t o ) given ut ()( k 0 + k 1 t + k 2 t 2 ) Parametric Optimization Example 1 V = max C D 2 V 2 S = 1 C L k 0 + k 1 t + k 2 t 2 V h = V sin r = V cos ( )( ) m = SFC m gsin ( ) 1 2 V 2 S m gcos a = J k = 0 k 0 k 1 k 2 2 J k 2 > 0 Legendre Polynomials Solutions to Legendre s differential equation

21 Legendre Polynomials Polynomials can be generated by Rodriques s formula P n ( x)= 1 d n x n n! dx n ( ) n Optimizing Control: Find minimizing values of k n u( x)= k 0 P 0 ( x)+ k 1 P 1 ( x)+ k 2 P 2 ( x) +k 3 P 3 ( x)+ k 4 P 4 ( x)+ k 5 P 5 ( x)+ t x t f t o P 5 P 4 P 2 P 3 P 0 P 1 ( x)= 1 ( x)= x ( ) ( x)= 1 2 3x2 1 ( ) ( x)= 1 2 5x3 3x ( ) ( x)= x4 30x ( ) ( x)= x5 70x 3 +15x Control History Optimized with Legendre Polynomials Could be expressed as a Simple Power Series u *( x)= k * 0 P 0 ( x)+ k * 1 P 1 ( x)+ k * 2 P 2 x +k * 3 P 3 ( x)+ k * 4 P 4 ( x)+ k * 5 P 5 ( x)+ u *( x)= a * 0 + a * 1 x + a * 2 x 2 +a 3 * x 3 + a 4 * x 4 + a 5 * x 5 + a * 0 = k * * 1 * 3 0 k k a * 1 = k * * 3 * 15 1 k k a * * 3 * 30 2 = k 2 2 k ( ) P 5 P 4 P 2 P 3 P 0 P 1 ( x)= 1 ( x)= x ( ) ( x)= 1 2 3x2 1 ( ) ( x)= 1 2 5x3 3x ( ) ( x)= x4 30x ( ) ( x)= x5 70x 3 +15x

22 Parametric Optimization: Collocation Admissible controls occur at discrete times, k Cost and dynamic constraint are discretized Pseudospectral Optimal Control: State and adjoint points may be connected by basis functions, e.g., Legendre polynomials Continuous solution approached as time interval decreased min u k J = x k f + k f 1 k =0 L [ x,u k k ] subject to x k +1 = f k [x k,u k ], x 0 given Penalty Function Method Balakrishnans Epsilon echnique No integration of the dynamic equation Parametric optimization of the state and control history x(t) x(k x,t) u(t) u(k u,t) dim(k x ) n dim(k u ) m Augment the integral cost function by the dynamic equation error min J = x(t ),t f f wrt u(t ),x(t ) + t f t o L[ x(t),u(t),t]+ 1 { f[ x(t),u(t),t] x(t) } ( {} ) dt 1/ is the penalty for not satisfying the dynamic constraint

23 Penalty Function Method Choose reasonable starting values of state and control parameters e.g., state and control satisfy boundary conditions Evaluate cost function J 0 = x 0 (t f ) + Update state and control parameters (e.g., steepest descent) k xi+1 = k xi J t f t o L[ x 0(t),u (t) ]+ 1 { f[ x 0 (t),u 0 (t)] x 0 (t)} ( {} ) dt k x k x =k xi k ui+1 = k ui J k u k u =k ui aylor, Smith Iliff, 1969 Re-evaluate cost with higher penalty Repeat to convergence x i+1 (t) x(k xi+1,t) u i+1 (t) u(k ui+1,t) J i J i+1 J*, 0, f[ x(t),u(t),t] x(t) Neighboring Extremal Method Shooting Method: Integrate both state and adjoint vector forward in time x k +1 (t) = f[x k +1 (t),u k (t)], k +1 (t) = L t x k with x 0 (t 0 ) given, initial guess for u 0 (t) ()+ k +1 ()F t k () t u k +1 (t) defined by H[x k (t),u k (t), k +1 (t),t] u = L uk (t) + k +1 ()G t k (t) = 0... but how do you know the initial value of the adjoint vector?, k (t 0 ) given

24 Neighboring Extremal Method All trajectories are optimal (i.e., extremals) for some cost function because H u = H u = L u + G = 0 Integrating state equation computes a value for t f x(t f ) = x(t 0 ) + f[x k+1 (t),u k (t)]; x(t f ) x(t ) f x t 0 = ( t f ) Use a learning rule to estimate the initial value of the adjoint vector, e.g., ( ) x t f k +1 ( t 0 )= k ( t 0 ) k ( t f ) desired Gradient-Based Methods

25 Gradient-Based Search Algorithms Gradient-Based Search Algorithms Steepest Descent H u k +1 (t) = u k (t) k u t () u k +1 (t) = u k (t) 2 H () t u 2 k Newton Raphson 1 k H u t () k Generalized Direct Search H u k +1 (t) = u k (t) K () k u t k

26 Numerical Optimization Using Steepest-Descent Algorithm Iterative bidirectional procedure Forward solution to find the state, x() t Backward solution to find the adjoint vector, t () () Steepest-descent adjustment of control history, u t x k (t) = f[x k (t),u k 1 (t)], x(t o ) given Use educated guess for u 0 Numerical Optimization Using Steepest-Descent Algorithm k (t) = H x (t f ) = [x(t f )] x k = L x (t) + ()F(t) t k, [ E L #2 and #1] Use x k-1 (t) and u k-1 (t) from previous step

27 Numerical Optimization Using Steepest-Descent Algorithm H u k = L u (t) + ()G(t) t k [ E L #3] u k+1 (t) = u k (t) H u u(t )=uk (t ) = u k (t) L u + ()G(t) t k Use x(), t (), t and u() t from previous step Finding the Best Steepest-Descent Gain J 1k u k t J 2k u k t J k+1 u k+1 t J 0k u k t (), 0< t < t f : Best solution from the previous iteration Calculate the gradient, H k u H () k (), t 0< t < t k f u H ()2 k (), t 0< t < t k f J 0k J 1k J 2k = u (), t in 0 < t < t f : Steepest - descent calculation of cost (1) : Steepest - descent calculation of cost (2) J ( )= a 0 + a 1 + a 2 2 a 0 + a 1 ( 0)+ a 2 ( 0) 2 ( ) 2 a 0 + a 1 ( k )+ a 2 k a 0 + a 1 ( 2 k )+ a 2 2 k ( ) = 1 ( k ) k Solve for a 0, a 1, and a 2 Find * that minimizes J ( ) H ()= u k () t * k (), t 0< t < t k f u ( ) 2 1 ( 2 k ) ( 2 k ) a 0 a 1 a 2 : Best steepest - descent calculation of cost Go to next iteration

28 Steepest-Descent Algorithm for Problem with erminal Constraint min u(t ) t f J = x(t ) f + L[ x(t),u(t) ] dt x(t f ) t o 0 (scalar) H C u = H 0 u a b H 1 u = 0 see Lecture 3 for a and b Chose u k+1 (t) such that u k+1 (t) = u k (t) H C u u(t )=uk (t ) = u k (t) L u + G (t) 0 () t k 1 G k(t) 1 () t k x(t f ) b k Zero Gradient Algorithm for Quadratic Control Cost min u(t ) J = x(t ) f + t f t o L [ x(t) ]+ 1 2 u ()Ru t t () dt [ ] = L x(t) H x(t),u(t), (t) [ ]+ 1 2 u ()Ru t t H u ()= t H u ()= t u ()R t + () Optimality condition: + (t)f x(t),u(t) () ()G t t 0 [ ]

29 Zero Gradient Algorithm for Quadratic Control Cost H u ()= t H u ()= t u ()R t + () ()G t t 0 Optimal control, u*(t) u* ()R t = ()G t *() t u* ()= t R 1 G * () t () t But G k () t and k () t are sub-optimal before convergence, and optimal control cannot be computed in single step Chose u k+1 (t) such that u k+1 (t) = ( 1 )u k (t) R 1 G k t Relaxation parameter < 1 () k () t Stopping Conditions for Numerical Optimization Computed total cost, J, reaches a theoretical minimum, e.g., zero Convergence of J is essentially complete Control gradient, H u (t), is essentially zero throughout [t o, t f ] erminal cost/constraint is good enough t f t o H uk+1 H uk+1 J k+1 = 0 + J k+1 > J k ()= t 0 ± in t o,t f or ()H t uk+1 ()dt t = 0 + k+1 ( t f )= 0 +, or k+1 t f t f ( )= 0 ±, and L x(),u t () t dt < t o

30 Optimal reatment of an Infection Model of Infection and Immune Response x 1 = Concentration of a pathogen, which displays antigen x 2 = Concentration of plasma cells, which are carriers and producers of antibodies x 3 = Concentration of antibodies, which recognize antigen and kill pathogen x 4 = Relative characteristic of a damaged organ [0 = healthy, 1 = dead]

31 Infection Dynamics Fourth-order ordinary differential equation, including effects of therapy (control) x 1 = (a 11 a 12 x 3 )x 1 + b 1 u 1 + w 1 x 2 = a 21 (x 4 )a 22 x 1 x 3 a 23 (x 2 x 2 *) + b 2 u 2 + w 2 x 3 = a 31 x 2 (a 32 + a 33 x 1 )x 3 + b 3 u 3 + w 3 x 4 = a 41 x 1 a 42 x 4 + b 4 u 4 + w 4 Uncontrolled Response to Infection

32 Cost Function to be Minimized by Optimal herapy J = 1 2 p 11x 2 2 ( 1f + p 44 x 4 f ) t f t o ( q 11 x q 44 x ru 2 )dt Cost function includes weighted square values of Final concentration of the pathogen Final health of the damaged organ (0 is good, 1 is bad) Integral of pathogen concentration Integral health of the damaged organ (0 is good, 1 is bad) Integral of drug usage Examples of Optimal herapy Unit cost weights u 1 = Pathogen killer u 2 = Plasma cell enhancer u 3 = Antibody enhancer u 4 = Organ health enhancer

33 Effects of Increased Drug Cost r = 100 Next ime: Minimum-ime and -Fuel Problems Reading OCE: Section 3.5, 3.6

34 Supplemental Material Examples of Equality Constraints cx(t),u(t) [ ] 0 Pitch Moment = 0 = fcn(mach Number, Stabilator rim Angle) cu(t) [ ] 0 Stabilator rim Angle constant = 0 cx(t) [ ] 0 Altitude constant = 0

35 Minimum-Error-Norm Solution dim(x) = r 1 dim(y) = m 1 r > m Euclidean error norm for linear equation Ax y 2 2 = Ax y [ ] [ Ax y] [ ] = 2 AA AA 2 Ax y = 2[ y y] = 0 Necessary condition for minimum error x Ax y 2 = 2 Ax y 2 ( ) 1 y { } [ ] = 0 Express x as right pseudoinverse y {( )( AA ) 1 y y} = 2 AA herefore, x is the minimizing solution, as long as AA is non-singular