Optimal Control Simulation Problem

Transcription

1 ECE-S Optimal Control Midterm his solution was prepared by Rob Allen on Feb 9, 009 he basic questions are given on the following page the solution presented includes a variety of {Q, R} which include the values in our exam Optimal Control Simulation Problem page 1 of 1

2 he solutions follow this form of the exam. ECE-S Midterm 1 Solutions Fall 009 he LIV inematic equations for one dimensional motion are given as: x v + 1 = 1 x + v 0 1 u We can consider this as the model of a falling object (maybe on another planet) where: x() = position, v() = velocity, and u() an acceleration input. Assume that the initial conditions are x0 = 5000 feet and v0 = -500 feet/sec use = 0.5 second. a) Using the technique in the least squares handout find an open loop control to bring the plant to the origin x() = 0, v() = 0 when =10 seconds. Provide plots of x(), v() and u(). he horizontal axis should be calibrated in time increments which is. Compute the energy expended. For parts b, c, and d use the notation and formulations in Lewis. b) Formulate a performance index using.- as the model and simulate the fixed final state open loop control problem to bring the plant to the origin x() = 0, v() = 0 when =10 seconds. We wish to minimize the control energy. Provide plots of x(), v() and u() and the cost J. he horizontal axis should be calibrated in time increments which is. Explain your choices for S, Q and R and any constraints that may be required. c) Using the same choice of S, Q and R simulate the system using free final state closed loop control. Provide plots of x(), v() and u() and the cost J and anything else that may be important. d) Since this is a LIV system find a constant feedbac gain matrix (.4-14). Find the limiting solution to.4. and the solution to the ARE. Again simulate the system showing plots of x(), v() and u() and the cost J and anything else that may be important. Comment on the results of parts a, b, c, and d. A table may be a good presentation method. page of 1

3 Given Problem: Part (a) Solution As stated in the least squares handout when driving a discrete time system to the origin in >n steps, there are many solutions. he problem is underdetermined, in that there are n equations, or constraints, specified by the physics of the problem with unnowns (discrete control inputs). he numbers of unnowns are specified by the finite time specified to drive the system from an initial state to the origin. What follows is an attempt to show that the pseudo-inverse 1 can be thought of a constrained minimum energy optimization problem and can be turned into an unconstrained optimization problem using Hamiltonian based approach taught in this course. he results here will be utilized in the discussion to compare simulation results from least squares and fixed-final state open loop control methods. As shown in the handout the problem can be written as Y = C U (1) he Euclidian, or -norm can be thought of as the square root of the total energy in a signal. Let the cost function, or performance index, be the square of the -norm of the control energy ( u + u + u +... ) = ( u + u +... ) = + J = U u () ow constrain U so that it must also satisfy the physics of the problem, which are specified by a system of discrete time equations, with initial and terminal conditions, as in the handout, page 3 of 1

4 Y C U = 0 (3) o transform this problem into an unconstrained optimization problem, introduce the Hamiltonian (scalar function) as H = U - λ Τ ( Y C U) (4) where λ is a Lagrangian multiplier. o be a stationary point, a necessary condition is that H U = U C λ = 0 (5) Multiplying both sides by C and using (1) enables the solution for λ to be written as λ 1 = (C C ) Y (6) Plugging this into (5) and solving for U, yields U = C (C C ) 1 Y (7) he handout refers to the terms proceeding the Y term as a pseudo-inverse. [C (C C ) 1 ] (8) o chec that the solution for U is indeed a minimum, the Hessian must be positive definite. he Hessian is, H U = > 0 (9) hus, the pseudo inverse solution for U, minimizes the control energy required to tae the system from x 0 to x in steps (>n). Simulation results are shown on the following page. ote what might be a drawbac to this method is that although the control energy is minimized, large state excursions are not penalized. For example at 0.5 seconds till the object lands, the object is traveling at approximately 100 feet per second and undergoing rapid deceleration! page 4 of 1

5 Figure 1 Open-loop least squares solution for discrete time model of falling object. Figure. Cost of least squares solution as a function of time. otal cost or control energy expended is ote here that the cost used includes a ½ so another acceptable answer is page 5 of 1

6 Part (b) ECE-S Midterm 1 Solutions Fall 009 Solution o solve this problem the discrete-time linear quadratic regulator, fixed-final-state open-loop control was implemented in Matlab. It turns out that this is a minimum-control-energy solution. he performance index over the time interval [i ] is defined by x S x + = i ( x Q x + u R u ) J = (1) i, where, S is a terminal condition state weighting matrix, and Q and R are time varying state and control weighting matrices respectively. Since the final-state is fixed, the terminal weighting term, S is unnecessary and set to zero. Q is set to zero to mae the problem more easily handled (see Lewis). he performance index, or cost function for the final-state-fixed problem then reduces to 1 1 = i ( u R u ) J = () i, he optimal control sequence is given as (equation.-38 in Lewis text) u * = R 1 B 1 1 ( A ) G0, ( r A x0 ) (3) where the weighted reachability gramian is 1 i 1 1 i 1 0, = A BR B ( A ) i= 0 G (4) For the following simulation results shown, R was set to a constant, 1. page 6 of 1

7 Figure 3 Fixed-final-state open-loop control simulation results for discrete time model of a falling object. Figure 4 Cost of least fixed-final-state solution as a function of time. otal cost or energy expended is , which is the same as the least squares solution. page 7 of 1

8 Part (c) ECE-S Midterm 1 Solutions Fall 009 Solution o solve this problem the discrete-time linear quadratic regulator, free-final-state closed-loop control simulation routine was implemented in Matlab (see Appendix). [x,u,k,s,j]= dlqropt(a,b,q,r,s,x0,); he cost function is x S x + = i ( x Q x + u R u ) J = (1) i, and the equation to solve for the optimal time-varying feedbac is u = K x () where the time varying gain is given by K ( B S B R ) B S + 1 = A (3) and the cost ernel is given by the following Riccati equation 1 [ S + 1 S + 1B B S + 1B + R ) B S 1 ] A Q S ( + (4) = A + o implement these equations the cost ernel (4) and time varying state feedbac gain (3) are computed beforehand and stored off-line. he cost ernel (4) must be solved bacwards in time, with an initial condition of S which is specified by the designer. After (3) and (4) are solved the governing discrete time state equations are solved forwards in time, utilizing the optimal time varying feedbac gain to compute the optimal control input at each time. hese are referred to as on-line, computations in the textboo (Lewis). o learn about the behavior of the weighting matrices in the cost equation (1), some limiting cases are presented. Zero-Input-Response(ZIR) ow that the boundary condition of a fixed final state has been removed that existed in the least squares and fixed-final-state solutions, the final-free-state closed loop solution can minimize control energy with no constraints. As an extreme example, setting Q = S = 0 and R very small will effectively have zero control energy and yields the Zero Input Response (ZIR) of the system. page 8 of 1

9 Figure 5 Free-final-state solution with Q=S =0 and R=.001 yields a zero input response (ZIR) which is an optimal solution to the minimum control energy problem with no constraints. Free-final-state closed-loop approximation of fixed-final-state open-loop control solution Or with appropriate choices for the weighting matrices, S, Q, and R, the results presented by the fixed-final-state open-loop solution can be approximated in a closed-loop manner. Figure 6 Free-final-state closed-loop solution simulation approximates fixed-final-state (minimum energy with boundary condition) open-loop solution for large R and S. Simulation is for S =[ ; ], Q=[0 0; 0 0], and R= page 9 of 1

10 ominal free-final-state closed-loop solution Weighting matrices are now selected to strie a balance between the competing and usually conflicting goals of minimizing control energy, state excursions while driving the system to a terminal state condition. Figure 7 Free-final-state closed-loop simulation for S =[000 0; ], Q=[1 0; 0 1], and R=10. Figure 8 ime varying gain and cost for free-final-state closed-loop simulation with S =[000 0; ], Q=[1 0; 0 1], and R=10. page 10 of 1

11 Figure 9 All four elements of the cost ernel matrix as a function of time for free-final-state closed-loop simulation with S =[000 0; ], Q=[1 0; 0 1], and R=10. Part (d) Solution o solve this problem the discrete-time linear quadratic regulator, free-final-state closed-loop infinite horizon routine was implemented in Matlab (see Appendix). [x,u,k,s,j]= dlqroptinf(a,b,q,r,s,x0,,kinf); he following values were utilized to compute the constant feedbac gain matrix S =[000 0; ], Q=[1 0; 0 1], and R=10, =00. he constant feedbac gain matrix becomes: K = (1) A solution to the Algebraic Ricatti Equation (ARE) exists if and only if the solution to the following Riccati equation, 1 [ S + 1 S + 1B B S + 1B + R ) B S 1] A Q S ( + () = A + page 11 of 1

12 converges to a finite solution. his equation was solved bacwards in time, to yield S = (3) Since the cost ernel matrix converges, a solution to the following ARE exists, S 1 = A [ S SB( B SB + R) B S] A (4) Using the Matlab dare() program to solve the ARE agreement is shown between the ARE and the time varying solution (3) for the cost ernel. [X,L,G] = dare(a,b,q,r); X = (5) Suboptimal Solution to the nominal free-final-state closed-loop solution in part (c) Figure 10 Suboptimal control for free-final-state closed-loop solution simulation with S =[000 0; ], Q=[1 0; 0 1], and R=10 shows similar results to part (c). page 1 of 1

13 Figure 11 Suboptimal fixed gains and cost for free-final-state closed-loop simulation with S =[000 0; ], Q=[1 0; 0 1], and R=10 shows results similar to part (c). he cost is slightly less, 60.65e6, versus 60.7e6 from part (c). Figure 1 All four elements of the cost ernel matrix as a function of time for the suboptimal free-final-state closed-loop simulation with S =[000 0; ], Q=[1 0; 0 1], and R=10. page 13 of 1

14 Discussion Methods (b), (c), and (d) all strive to minimize the cost function or performance index over the time interval i to defined by x S x + = i ( x Q x + u R u ) J = (1) i, where, S is a terminal condition weighting matrix, and Q and R are time varying state and control weighting matrices respectively. Values for the weighting matrices are chosen by the design engineer to give the system a desired performance. For the fixed-final-state open-poop control implemented in part (b) the cost function reduces (for scalar u) to J = = 1 ( u R u ) = ( R u + R u + R u +...) 1 1 i () Or, for constant weighting matrix, R, 1 J = R ( u1 + u + u3 +...) (3) As shown in part (a), the least squares solution, minimizes the square of the Euclidean, or -norm of the control energy, ( u1 + u + u ) = ( u1 + u ) U = u (4) If we assume the control weighting matrix, R does not vary with time, then () and (4) only differ by a scale factor and will have the same solution for the minimum cost. hus, it should be anticipated that the least squares method and the fixed-final-state open-loop control methods are equivalent and the implementations of the open-loop control methods will yield the same results only if R does not vary with time. For the free-final-state closed-loop control method algorithm of method (c) S is usually non-zero to drive the system to x, the terminal condition, and Q is also usually non zero to penalize excessive state excursions. Also, as shown by simulation, by as S and R become large (with Q set to zero), the results will approximate those of (a) and (b), since the terminal condition and the control energy expended are heavily weighted. But, the free-final-state method has the substantial benefit in that it is closed-loop, and will therefore will be more robust in that it will be more tolerant of un-modeled dynamics or disturbances. he free-final-state closed-loop control method is complicated by the fact that off-line computations must be done to compute and store the optimal, time-varying gain, K, for use in the closed-loop system implementation. A simplification made in method (d) is to utilize a constant feedbac gain, as goes to infinity (infinite horizon). his is referred to as a sub-optimal feedbac. However a performance penalty will be realized for this implementation and the designer must determine if the system performance is suitable for the application at hand. page 14 of 1

15 he simulation results are summarized in the table below. As expected the least squares and fixedfinal-state open-loop results are identical! As expected the free-final-state closed-loop results do not hit the terminal conditions exactly, as there are residual terminal velocity and position errors. Also note the fact that the open-loop methods effectively have no state weighting, Q, so the states can be large as terminal conditions are approached (note average acceleration is shown in table). But of course method free-final-state method uses more fuel. Since one is typically concerned with having a smooth landing, or minimizing certain states such acceleration, jer w, the impact of time-varying state weighting gain Q and penalize state excursions more heavily as terminal conditions are approached to further optimize the trajectory should be investigated. he table also shows the performance penalty realized for the simplicity of the sub-optimal control approach, in that the final velocity is excessive. Clearly this would mae for a rough landing and would not be suitable for implementation as it stands. Method Least Squares Open Loop Fixed Final State Open Loop Free Final State Closed Loop Suboptimal Control Closed Loop Final Position (f) Final Velocity (fps) Final Acceleration (fps^) Cost (Fuel) otal Cost Robust o o Yes Yes page 15 of 1

16 Appendix Code Listing for part (a): clear all close all %************** %*** PAR A *** %************** =0.5; % Sample ime x0= 5000; % f v0=-500; %fps A = [1 ; 0 1] B = [^/;] %Drive the system to the origin in 0 steps =0 %erminal condition x0=[x0; v0] x0=[0; 0] C=B; for i=1:-1 C = [C A^i *B]; end P_IV=C'*inv(C*C') u=(p_iv*(x0-a^*x0)); u=flipud(u) % Steps x = x0; traj = x0; for i=1: xnext = A*x + B*u(i); traj = [traj xnext]; x = xnext; J(i)= 1/ * u(i)^; end traj u' h = figure; subplot(3,1,1) stem([0:]*,traj(1,:),'linewidth',.0); ylabel('x()'); title(['on Optimal =0']); axis([-1* *(+1) ]) title('lunar Surface Landing:: Open Loop Least Squares Solution, =0 steps') subplot(3,1,) stem([0:]*,traj(,:),'linewidth',.0); ylabel('v()');axis([-1*,*(+1),-inf,inf]) subplot(3,1,3) stem([0:-1]*,u,'linewidth',.0); ylabel('u()'); xlabel('time(s)'); axis([-1*,*(+1),-inf,inf]) figure stem([0:-1]*,j,'linewidth',.0); ylabel('j()'); xlabel('time(s)'); axis([-1*,*(+1),-inf,inf]) title(['cost as a Function of time:: otal cost=' sprintf('%7.0f',sum(j))]) page 16 of 1

17 Code Listing for part (b): clear all close all %************** %*** PAR B *** %************** % Fixed Final State, Open Loop Control clear all =0 R=1 =0.5; % Sample Ime x0= 5000; % f v0=-500; %fps A = [1 ; 0 1] B = [^/;] %Drive the system to the origin in 0 steps =0 %erminal condition x0=[x0; v0] x0=[0; 0] r=x0 G = zeros(,); %Compute Controllability Grammian for i=0:-1 G=G + A^(-i-1) * B * inv(r) * B' * (A')^(-i-1); end Ginv = inv(g) % Steps x = x0; traj = x0; for i=0:-1 u(i+1) = inv(r) * B' * (A')^(-i-1) * Ginv * (r - A^*x0) xnext = A*x + B*u(i+1); traj = [traj xnext]; x = xnext; J(i+1)= R/ * u(i+1)^; end traj u' h = figure; subplot(3,1,1) stem([0:]*,traj(1,:),'linewidth',.0); ylabel('x()'); title(['on Optimal =0']);axis([-1,+1,- inf,inf]) axis([-1* *(+1) ]) title('lunar Surface Landing::Fixed Final State, Open Loop Control, =0 steps') subplot(3,1,) stem([0:]*,traj(,:),'linewidth',.0); ylabel('v()');axis([-1*,*(+1),-inf,inf]) subplot(3,1,3) stem([0:-1]*,u,'linewidth',.0); ylabel('u()'); xlabel(''); axis([-1*,*(+1),-inf,inf]) figure stem([0:-1]*,j,'linewidth',.0); ylabel('j()'); xlabel(''); axis([-1*,*(+1),-inf,inf]) title(['cost as a Function of time:: otal cost=' sprintf('%7.0f',sum(j))]) sum(j) page 17 of 1

18 Code Listing for part (c): clear all close all %************** %*** PAR C *** %************** clear all =0.5; % Sample Ime x0= 5000; % f v0=-500; %fps A = [1 ; 0 1] B = [^/;] %Drive the system to the origin in 0 steps =0 %erminal condition x0=[x0; v0] x0=[0; 0] r=x0 Q=[1 0; 0 1]; R=10; s=[000 0; ]; [x,u,k,s,j]= dlqropt(a,b,q,r,s,x0,); ECE-S Midterm 1 Solutions Fall 009 h = figure; subplot(3,1,1) stem([0:]*,x(1,:),'linewidth',.0); ylabel('x()'); title(['on Optimal =0']);axis([- 1*,*(+1),-inf,inf]) title('lunar Surface Landing:: Closed Loop, Free Final State =0 steps') subplot(3,1,) stem([0:]*,x(,:),'linewidth',.0); ylabel('v()');axis([-1*,*(+1),-inf,inf]) subplot(3,1,3) stem([0:-1]*,u,'linewidth',.0); ylabel('u()'); xlabel('time(s)'); axis([-1*,*(+1),-inf,inf]) h = figure; subplot(3,1,1) stem([0:-1]*,k(:,1),'linewidth',.0); ylabel('k1()');axis([-1*,*(+1),-inf,1.1*max(k(:,1))]) title(['closed Loop, Free Final State: Gains and Cost=' numstr(sum(j))]) subplot(3,1,) stem([0:-1]*,k(:,),'linewidth',.0); ylabel('k()');axis([-1*,*(+1),-inf,inf]) subplot(3,1,3) stem([0:]*,j,'linewidth',.0); ylabel('j()');axis([-1*,*(+1),-inf,inf]);xlabel('time(s)'); h= figure subplot(4,1,1) stem([0:]*,s(:,1,1),'linewidth',.0); ylabel('s11()');axis([-1*,*(+1),-inf,inf]) title('lunar Surface Landing:: Closed Loop, Free Final State =0 steps') subplot(4,1,) stem([0:]*,s(:,1,),'linewidth',.0); ylabel('s1()');axis([-1*,*(+1),-inf,inf]) subplot(4,1,3) stem([0:]*,s(:,,1),'linewidth',.0); ylabel('s1()');axis([-1*,*(+1),-inf,inf]) subplot(4,1,4) stem([0:]*,s(:,,),'linewidth',.0); ylabel('s()');axis([-1*,*(+1),- inf,inf]);xlabel('time(s)'); K sum(u.^)/ sum(j) page 18 of 1

19 function [x,u,k,s,j]=dlqropt(a,b,q,r,s,x0,) % Program to Cumpute and Simplate Optimal Feedbac Control %Compute and Store Optimal Feedbac Sequence n=length(a); S=zeros(,n,n); K=zeros(,n); S(+1,:,:)=s; for =:-1:1 K(,:)=inv(B'*s*B+R)*(B'*s*A); s=a' *(s-s*b*inv(b'*s*b+r)*b'*s) * A + Q; % Q+(R*s*a^)/(r+s*b^); S(,:,:)=s; end %Apply Optimal Control to Plant (Forward Iteration) x=zeros(n,); x(:,1)=x0; for =1: %Update Optimal Control Input u()=-k(,:)*x(:,); %Update Plant State J() = 0.5 * (x(:,)'*q*x(:,) + u()'*r*u()); x(:,+1)=a*x(:,)+b*u(); end J(+1)=0.5*x(:,)'*s*x(:,); page 19 of 1

20 Code Listing for part (d): clear all close all %************** %*** PAR D *** %************** clear all =0.5; % Sample Ime x0= 5000; % f v0=-500; %fps A = [1 ; 0 1] B = [^/;] %Drive the system to the origin in 0 steps =0 %erminal condition x0=[x0; v0] x0=[0; 0] r=x0 Q=[1 0; 0 1]; R=10; Kinf = [ ]; s=[000 0; ]; [x,u,k,s,j]= dlqroptinf(a,b,q,r,s,x0,,kinf); ECE-S Midterm 1 Solutions Fall 009 h = figure; subplot(3,1,1) stem([0:]*,x(1,:),'linewidth',.0); ylabel('x()'); title(['on Optimal =0']);axis([- 1*,*(+1),-inf,inf]) title('lunar Surface Landing:: Closed Loop, Free Final State =0 steps') subplot(3,1,) stem([0:]*,x(,:),'linewidth',.0); ylabel('v()');axis([-1*,*(+1),-inf,inf]) subplot(3,1,3) stem([0:-1]*,u,'linewidth',.0); ylabel('u()'); xlabel('time(s)'); axis([-1*,*(+1),-inf,inf]) h = figure; subplot(3,1,1) stem([0:-1]*,k(:,1),'linewidth',.0); ylabel('k1()');axis([-1*,*(+1),-inf,1.1*max(k(:,1))]) title(['closed Loop, Free Final State: Gains and Cost=' numstr(sum(j))]) subplot(3,1,) stem([0:-1]*,k(:,),'linewidth',.0); ylabel('k()');axis([-1*,*(+1),-inf,inf]) subplot(3,1,3) stem([0:]*,j,'linewidth',.0); ylabel('j()');axis([-1*,*(+1),-inf,inf]);xlabel('time(s)'); h= figure subplot(4,1,1) stem([0:]*,s(:,1,1),'linewidth',.0); ylabel('s11()');axis([-1*,*(+1),-inf,inf]) title('lunar Surface Landing:: Closed Loop, Free Final State =0 steps') subplot(4,1,) stem([0:]*,s(:,1,),'linewidth',.0); ylabel('s1()');axis([-1*,*(+1),-inf,inf]) subplot(4,1,3) stem([0:]*,s(:,,1),'linewidth',.0); ylabel('s1()');axis([-1*,*(+1),-inf,inf]) subplot(4,1,4) stem([0:]*,s(:,,),'linewidth',.0); ylabel('s()');axis([-1*,*(+1),- inf,inf]);xlabel('time(s)'); sum(u.^)/ sum(j) %=00 % Sinf = % % [X,L,G] = dare(a,b,q,r) page 0 of 1

21 function [x,u,k,s,j]=dlqroptinf(a,b,q,r,s,x0,,kinf) % Program to Cumpute and Simplate Optimal Feedbac Control %Compute and Store Optimal Feedbac Sequence n=length(a); S=zeros(,n,n); K=zeros(,n); S(+1,:,:)=s; for =:-1:1 K(,:)=Kinf; s=a' *(s-s*b*inv(b'*s*b+r)*b'*s) * A + Q; S(,:,:)=s; end %Apply Optimal Control to Plant (Forward Iteration) x=zeros(n,); x(:,1)=x0; for =1: %Update Optimal Control Input u()=-k(,:)*x(:,); %Update Plant State J() = 0.5 * (x(:,)'*q*x(:,) + u()'*r*u()); x(:,+1)=a*x(:,)+b*u(); end J(+1)=0.5*x(:,)'*s*x(:,); page 1 of 1