Minimization with Equality Constraints!

Transcription

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

2 Minimization with Equality Constraint on State and Control min ut c xt,ut subject to [ ] J = " xt f L xt,ut xt = f[xt,ut], x t 0 t f t 0 Dynamic Constraint State/Control Equality Constraint given [ ] 0 in t 0, t f ; dimc = r "1 m "1 dt dimx = n 1 dimf = n 1 dimu = m 1 3 Aeronautical Example: Longitudinal Point-Mass Dynamics 1 V = C D 2 "V 2 S m gsin = 1 * 1 C L V 2 "V 2 -, S m gcos /. h = V sin r = V cos m = SFC 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 4

3 Aeronautical Example: Variables and Constants u 1 = : hrottle setting, u 2 = " : Angle of attack, rad = Air density = SL e "h,kg/m 3 = 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 g = Gravitational acceleration, m/s 2 SFC = Specific Fuel Consumption, g/kn-s 5 Path Constraint Included in the Cost Function Hamiltonian Constraint must be satisfied at every instant of the trajectory Dimension of the constraint " dimension of the control t f { } J 1 = " xt f L 1 t[ f xt ] µ c dt t 0 c[ xt,ut ] 0 in t o, t f he constraint is adjoined to the Hamiltonian H L 1 f µ c dimx = dimf = dim = n " 1 dimu = m " 1 dimc = dimµ = r " 1, r m 6

4 Euler-Lagrange Equations Including Equality Constraint t f = "[xt ] f "x = " H[x,u,,c,µ,t] x * = " L x f x c -, µ x. / * = " L x F 0 c x4 5, - µ./ H[x,u,",c,µ,t] u 0* L - =, u. / 12 G * " c -, u. / 3 µ 45 = 0 c[ xt,ut ] 0 in t o, t f dimx = dim = n " 1 dim F dim G = n " n = n " m dimu = m " 1 dimc = dimµ = r " 1, r m 7 No Optimization When r = m Control entirely specified by constraint m unknowns, m equations c[ xt,ut ] 0 " ut = fcn[ xt] Example : State Control c = Ax t Bu t = 0; dimx = n 1; dimc = dimu = m 1 dima = m n; dimb = m m = "B "1 Ax t u t Constraint Lagrange multiplier is unneeded but can be expressed: from dh/du = 0, µ = "c "u * "L "u, - G./ " c u is square and non-singular 8

5 Effect of Equality Constraint Dimensionality: r < m MINIMIZE FUEL AND CONROL USE WHILE MAINAINING CONSAN FLIGH PAH ANGLE min ut t f J = q m 2 r 1 u r 2 u 2 dt t 0 c[ xt,ut ] 0 = " = 1 V t, C L 1 2 V 2. ts *.. m -. / 1 gcos" t = desired 9 Effect of Constraint Dimensionality: r < m dimx = n 1 dimu = m 1 dimc = 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 if it exists Pseudoinverse of control effect [or weighted pseudoinverse BD] Soft constraint 10

6 1 st Approach: Algebraic Solution for r Control Variables Example 1:State Control dimx = n 1; dima r = r n dimu = m 1; dimu r = r 1; dimb r = r r c A r x t B r u r t = 0; detb r " 0 t = B 1 r Ax t u r Example 2 : Control only dimu = m 1; dimu r = r 1; dimb 1 = r r; dimb 2 = r m " r c B 1 B 2 u r u m"r u r = B u B u = 0; detb 0 1 r 2 m"r 1 t = "B "1 1 B 2 u m"r t 11 2 nd 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 / -. 12

7 Pseudoinverse of Matrix Matrix inverse if r = m, A is square and non-singular y = Ax x = A 1 y dimx = r 1 dimy = m 1 If r m, A is not square, maximum rank of A is r or m, whichever is smaller Use pseudoninverse of A x = A y = A y See 13 Left and Right Pseudoinverse r < m, use Left pseudoinverse Ax = y A Ax = A y 1 A y 1 A x = A A A L A A x = A L y Averaging solution dim A = m r = r r = m m dim A A dim AA r > m, use Right pseudoinverse Ax = y = Iy Ax = AA AA 1 y = Iy Ax = A " A AA 1 y x = A AA 1 y A R A AA 1 x = A R y Minimum-Euclidean-error-norm solution 14

8 Necessary Conditions Use Left Pseudoinverse for r < m Optimality conditions t f = "[xt ] f "x * = " L x F c, x µ L - /. " * L u G " c u plus [ ] 0 c xt,ut µ L,. = 0-15 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 t f = "[xt ] f "x u c 2"c u - G / = 0./ = " L x F = " / * / L orig x c, 2c x -. c[ xt,ut ] 0 F

9 Equality Constraint on State Alone t f { } J 1 = " xt f L x,u 1 t" f x,u xt µ tc x dt t 0 c[ xt ] 0 in t 0, t f Hamiltonian H L 1 f µ c Constraint is insensitive to control perturbations to first order c = "c "x x "c "u u = "c "x x 0 17 Equality Constraint Has No Effect on Optimality Condition " H u 0 L, = 2 * u L, = 2 * u -. 1 G / c, * u-. 3 G / µ 5 4 Solution: Incorporate time-derivative of c[xt] in optimization 18

10 Introduce ime-derivative of Equality Constraint Define c[xt] as the zeroth-order equality constraint dc 0 [ ] c 0 [ xt ] 0 c xt Compute first-order equality constraint [ xt ] dt [ = c0 xt ] t [ ] = c0 xt c0 xt t x c 1 xt,ut [ c0 xt ] dxt x dt [ ] [ ] = 0 f [ xt,ut ] 19 Include Derivative in Optimality Condition u H x,u," Includes derivative of equality constraint u * L x,u - =,. / G x,u" t * c1 x,u Must satisfy zeroth-order constraint at one point on path e.g., c 0 xt o [ ] 0 or c 0 " xt f, 0 u If c 1 /u = 0, differentiate again, and again, See Supplemental Material -. / µ 20

11 Minimization with Inequality Constraints 21 Hard Inequality Constraint Constraint not in force for entire trajectory rajectory must not pass beyond the constraint 22

12 Inequality Constraints Different constraints invoked at different times Some constraints never invoked 23 ransversality Corner Conditions Juncture between unconstrained and constrained points hree Weierstrass-Erdmann conditions = t 1 = H t 1 t 1 H t 1 "H "u t 1 = "H "u t

13 Corners and Discontinuities Continuous implies continuous ut at juncture u t 1 = u t 1 x t 1 = x t 1 Not a corner Discontinuous implies discontinuous ut at juncture u t 1 u t 1 " x t 1 x t 1 Corner 25 Soft Control Inequality Constraint L L original c c : Scalar penalty weight Scalar Example c[ ut ] = u u max u u min, u " u max, u min < u < u max, u u min 26

14 Numerical Optimization 27 Numerical Optimization Methods 28

15 Parametric Optimization min ut t f [ ] J = " xt f L xt,ut dt t 0 subject to xt = f[xt,ut], xt 0 given Control specified by a parameter vector, k No adjoint equations Examples ut = k ut = k 0 k 1 t k 2 t 2, k = k 0 k 1 k " m ut = k ut = k 0 k 1 t k 2 t 2, k = k 0 k 1 k " m t * ut = k 0 k 1 sin t f t, 0 k t * 2 cos t f t, 0, k = k 0 k 1 k " 2 29 Parametric Optimization min ut J = " xt f t f [ ] L xt,ut dt t 0 subject to xt = f[xt,ut], xt o given Necessary and sufficient conditions for a minimum Use static search algorithm to find minimizing control parameter, k J k = 0 2 J k >

16 min ut J = " xt f t f [ ] L xt,ut dt t 0 subject to xt = f[xt,ut], xt 0 given u t k 0 k 1 t k 2 t 2 Parametric Optimization Example 1 V = max C D 2 "V 2 S m gsin 1 2 "V 2 S = 1 0* 1 C L k 0 k 1 t k 2 t 2 V 2, h = V sin r = V cos m = SFC -./ m gcos a = " J k = 0 2 J k > 0 2 k 0 k 1 k 2 31 Penalty Function Method Balakrishnan s Epsilon echnique No integration of the dynamic equation Parametric optimization of the state and control history xt xk x,t ut uk u,t dimk x n dimk u m Augment the integral cost function by the dynamic equation error min J = " xt f,t f wrt ut,xt. / 0 L xt,ut,t t f 4 t o [ ] 1 { [ ] - xt } { } *, f xt,ut,t 1 / is the penalty for not satisfying the dynamic constraint dt 32

17 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. / L x 0t,u t 0 t f 4 t o [ ] 1 aylor, Smith Iliff, 1969 { [ ] - x 0 t} { } *, f x 0 t,u 0 t dt Update state and control parameters e.g., steepest descent k xi1 = k xi " J k x k x =k xi k ui1 = k ui " J k u k u =k ui Re-evaluate cost with higher penalty Repeat to convergence J i J i1 J*, " 0, f xt,ut,t x i1 t xk xi1,t u i1 t uk ui1,t [ ] xt 33 Neighboring Extremal Method Shooting Method : Integrate both state and adjoint vector forward in time x k1 t = f[x k1 t,u k t], x 0 t 0 given, initial guess for u 0 t k 1 t = " L t x k k 1 tf k t, k t 0 given Control satisfies necessary condition 3 { } u k1 t =fcn H[x t,u t," k k k1 t,t] u = arg L u k t " k1 tg k t 0... but how do you know the initial value of the adjoint vector? 34

18 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 " x t f t f xt f = xt 0 f[x k1 t,u k t]; " xt f " xt f x t 0 = * t f Use a learning rule to estimate the initial value of the adjoint vector, e.g., k 1 t 0 = k t 0 " k t f " desired 35 Multiple Shooting Method Divide the total time interval into n subintervals, defined by grid points t 0 < t 1 < t 2 < < t N1 < t N = t f Optimize between grid points, e.g., min J u t N n=1 min J u t n t n,t n"1 Constrain/correct/iterate final and initial state at grid points Option: Implement with discrete-time model 36

19 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, B-splines, Continuous solution approached as time interval decreased min u k J = " x k f k f 1 [ ] L x,u k k k =0 subject to x k 1 = f k [x k,u k ], x 0 given 37 Legendre Polynomials Solutions to Legendre s differential equation 38

20 Parametric Optimization with Legendre Polynomials Optimizing Control: Find minimizing values of k n = k 0 P 0 x k 1 P 1 x k 2 P 2 x k 4 P 4 x k 5 P 5 x u x k 3 P 3 x t x t f t o Can combine with optimal shooting method 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 2 3 x = x5 70x 3 15x 39 Gradient-Based Methods 40

21 Gradient-Based Search Algorithms 41 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 Newton Raphson k 1 k "H "u t Generalized Direct Search "H u k 1 t = u k t K k "u t k k 42

22 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 x k t = f[x k t,u k 1 t], xt o given Steepest-descent adjustment of control history, u t Use educated guess for u 0 t on first iteration 43 k t = " H x t f = *[xt f ]., / - x 0 Numerical Optimization Using Steepest-Descent Algorithm k = " L x t tft k, [ E " L 2 and 1] Use x k-1 t and u k-1 t from previous step 44

23 Numerical Optimization Using Steepest-Descent Algorithm " H u k = * L u t tgt,k [ E - L 3] u k1 t = u k t " H u ut =uk t = u k t " L u * tgt k Use x t, t, and u t from previous step 45 Finding the Best Steepest-Descent Gain J 1k u k t " J 0k " u k t, 0 < t < t f : Best solution from the previous iteration k H k Calculate the gradient, H k u t, in 0 < t < t f u t, 0 < t < t f : Steepest - descent calculation of cost 1 46

24 Finding the Best Steepest-Descent Gain J 2k u k t 2" k H k J 0k J 1k J 2k u t, 0 < t < t f : Steepest - descent calculation of cost 2 = a 0 a 1 a 0 a 1 = a 0 a 1 " a 2 " 2 J " a 0 a 1 0 a " k a 2 " k 2 2" k a 2 2" k = 1 " k " k Solve for a 0, a 1, and a 2 Find "* that minimizes J " 2 1 2" k 2" k a 0 a 1 a 2 47 Finding the Best Steepest-Descent Gain Best steepest - descent calculation of cost J k1 u k1 t H = u k t " * k k Go to next iteration u t, 0 < t < t f 48

25 min ut Steepest-Descent Algorithm for Problem with erminal Constraint t f dt xt f J = " xt f L xt,ut t o [ ] " 0 scalar H C u = H 0 u " a b H 1, u. * - = 0 Chose u k1 t such that see Lecture 3 for a and b definitions Bryson Ho, 1969 u k1 t = u k t " H C u ut =uk t = u k t " L u G t* 0 t k 1 G kt* 1 t b k k xt f 49 Optimal reatment of an Infection 50

26 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] 51 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 52

27 Uncontrolled Response to Infection 53 Cost Function to be Minimized by Optimal herapy J = 1 2 p 11x 2 2 1f p 44 x 4 f 1 2 t f t o q 11 x 2 1 q 44 x 2 4 ru 2 dt radeoffs between final values, integral values over a fixed time interval, state, and control 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 Drug cost may reflect physiological cost side effects or financial cost 54

28 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 55 Next ime: Minimum-ime and -Fuel Problems Reading OCE: Section 3.5,

29 Supplemental Material 57 Left Pseudoinverse Example A = " 1 3 ; y = " 2 6 Ax = y, x = A A r < m 1 A y " x = " 1 3 * 1 3 " x = " 1 3, " 1 3 " 2 6 = = 2 Unique solution 58

30 Minimum-Error-Norm Solution Euclidean error norm for linear equation Ax y 2 2 = Ax y [ ] [ Ax y] Necessary condition for minimum error x Ax " y 2 = 2 Ax " y 2 [ ] = 0 dimx = r 1 dimy = m 1 r > m [ ] = 2 A " A AA 2 Ax y Express x as right pseudoinverse 1 y { } [ ] = 0 = 2 y y y { AA 1 y y} = 2 AA herefore, x is the minimizing solution, as long as AA is non-singular 59 Right Pseudoinverse Example A = " 1 3 ; y = 14 Ax = y, r > m x = A AA 1 y " x 1 x 2 Ax = y; " 1 3 " x 1 x 2 = " 3 " * " 1 3 = " = " = , " 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 Right pseudoinverse provides minimum - norm solution 60

31 No Optimization When r = m MAINAIN CONSAN VELOCIY AND FLIGH PAH ANGLE c[ xt,ut ] 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 1 S / m gcos, * * 4 ut = fcn[ xt] V 0 = V desired = desired 0 61 Examples of Equality Constraints c[ xt,ut ] 0 Pitch Moment = 0 = fcnmach Number, Stabilator rim Angle c[ ut ] 0 Stabilator rim Angle constant = 0 c xt [ ] 0 Altitude constant = 0 62

32 Example of Equality Constraint on State Alone MINIMIZE FUEL AND CONROL USE WHILE MAINAINING CONSAN ALIUDE min ut t f J = q m 2 r 1 u r 2 u 2 dt t 0 [ ] = c[ xt ] = 0 = h t h desired c xt,ut 63 State Equality Constraint Example dc 0 c[ xt ] c 0 [ xt ] = 0 = h t h desired No control in the constraint; differentiate [ xt ] dt c 1 xt [ = c0 xt ] t [ ] = 0 = h " t [ c0 xt ] f [ xt,ut ] x = V tsin" t Still no control in the constraint; differentiate again 64

33 dc 1 State Equality Constraint Example Still no control in the constraint; differentiate again [ xt ] dt [ = c1 xt ] t = 0 = h t = dv dt [ c1 xt ] f [ xt,ut ] c 2 [ xt ] x tsin t V t d sin t " dt, 1 = maxsl e "h C D 2 V 2 /. S * m gsin 1-0 sin t cos t,. - C L V 2 S / * m gcos State Equality Constraint Example Success Control appears in the 2 nd -order equality constraint c 2 * = 1 maxsl e h C D, " cos0 t * 1, " xt,u t " = h C L 1 2 h V 2 - ts. / m t V 2 - ts. / m t H L 1 f µ c 2 gsin0 t gcos0 t 2 2 sin0 t 0 th - and 1 st -order constraints satisfied at some point on the trajectory e.g., t 0 [ ] = 0 h t 0 = h desired [ ] = 0 " t 0 = 0 c 0 xt 0 c 1 xt 0 66

34 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 * 4 P 4 x k * 5 P 5 x k 3 * P 3 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 2 2 k 4 8 a * 1 = k * * " 3 * " 15 1 k 3 2 k 5 8 a * * " 3 * " 30 2 = k 2 2 k 4 8 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 2 3 x = x5 70x 3 15x 67 Zero Gradient Algorithm for Quadratic Control Cost min ut J = " xt f t f L [ xt ] 1, 2 u tru t * dt t o [ ] = L xt H xt,ut,t " [ ] 1 2 u tru t H u t = H t = u u t Optimality condition: tf xt,ut R " tg t 0 [ ] 68

35 Zero Gradient Algorithm for Quadratic Control Cost H u t = H u t = u t t and k t R " tg t 0 Optimal control, u*t u* tr = " tg * t u* t = R 1 G * t" t are sub-optimal before convergence, But G k and optimal control cannot be computed in single step Chose u k1 t such that u k1 t = 1 " u k t " R 1 G k t k t " Relaxation parameter < 1 69 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 satisfied, and integral cost is good enough H uk1 t f H uk1 J k1 = 0 J k1 > J k " t = 0 ± in " t o,t f or t H uk1 tdt = 0 t o k1 t f = 0 ", or k1 t f = 0 ± ", and L x t t f t o,u t dt < 70

36 Effects of Increased Drug Cost r =