ECSE 644 Optimal Control Feb, 4 Due: Feb 17, 4 (Tuesday) Homework Solution # 3 1 (5%) Consider the discrete nonlinear control system in Homework # For the optimal control and trajectory that you have found in Homework #, design a linear quadratic regulator to track this solution The specific problems that you are asked to solve are: (a) (1%) Write the control law should be of the form u k = u k + u k where u k is the open loop optimal control that you found in Homework # Find u k by using the LQR covered in class (b) (5%) By using your simulation, compare the open loop optimal response (from Homework #) and the closed loop optimal response (solution in part 1a) under small perturbation of the initial condition (c) (1%) Simulate the complete nonlinear system with your controller and estimate how much error in initial state can be tolerated Solution: The solution script is in hw3am The nonlinear system is The optimal trajectory is given by Let x k+1 = x k + t s g(x k )u k x k+1 = x k + t s g(x k)u k x k := x k x k, u k := u k u k Then subtracting the optimal trajectory from the state trajectory and keeping the linear terms, we get x k+1 = x k + t s g(x k) u k + t s (g(x)u) x x=x k u=u k x k Define A k = I + t s (g(x)u) x x=x k u=u k, B k = t s g(x k) 1
For the unicycle case, we have A k = 1 + t s u k 1 1, B k = t s 1 1 x 1 k Choose Q and R >, then we can apply discrete LQR to find solution of Riccati equation, P k, feedback gains, K k, and the optimal control A sample state comparison for is shown below u k = u k K k (x k x k) Q = diag {5, 5, 5}, R = diag {5, 5} The input trajectory comparison is shown below
The convergence of the state depends strongly on the choice of Q and R (5%) Let x k+1 = 5 5 3 1 x k + 1 u k, x = 8 4 (a) (5%) Find the optimal open loop control, u k, k =, 1, that drives x to x = and minimizes J a = 1 1 u T k u 1 k k= Determine the corresponding optimal state trajectory and optimal cost (b) (5%) Find the optimal control, u k = Kx k that minimizes J b = 1 u T k k= 1 and the corresponding optimal state trajectory and optimal cost (c) (5%) Find the optimal open loop control, u k, k =, 1, that minimizes J c = 1x T x + 1 1 u T k k= u k, 1 and the corresponding optimal state trajectory and optimal cost (d) (5%) Find a constant gain full state feedback to achieve deadbeat control (ie, all closed loop poles at the origin) Find the closed loop state trajectory (e) (5%) Compare the solutions in parts a d and discuss if they make intuitive sense Solution: The solution script is in hw3bm u k, (a) The system is x k+1 = Ax k + Bu k, 3
with x and x given The optimization index is First form the Hamiltonian J = 1 1 u T k Ru k k= Optimal control is found from H k u k = : Co-state equation is given by H k = 1 ut k Ru k + λ T k+1(ax k + Bu k ) λ k = u k = R 1 B T λ k+1 ( ) T H = A T λ k+1 x k The two-point boundary value problem is then x k+1 = Ax k BR 1 B T λ k+1 λ k+1 = A T λ k Since N is only, this is easy to solve by hand: λ 1 = A T λ λ = A T λ x 1 = Ax BR 1 B T λ 1 x = Ax 1 BR 1 B T λ = A x ABR 1 B T λ 1 BR 1 B T λ = A x ABR 1 B T A T λ BR 1 B T A T λ Since x and x are given, we can solve for λ For 5 5 1 A =, B =, x 3 1 = we get λ = 19 637 We can calculate the optimal control to be 93 u =, u 6 1 = The corresponding x 1 is The optimal cost is x 1 = 15 585 J a = 3888 4 8 4 15 361, x =
(b) The steady state gain and solution to the Riccati equation are given by: P = A T (P P B(B T P B + R) 1 B T P )A + Q K = (B T P B + R) 1 B T P A They can be found numerically by using the MATLAB function dlqr: P = 1 1 5, K = 5 5 1 5 The first two control signals are: u = 3 6, u 1 = 15 3 The corresponding state sequence is x 1 = 1 6, x = Since the closed loop system is stable, the state sequence eventually converges to zero, but x is still fairly large The cost corresponding to part (a) is J b = 3375 1 The true (infinite horizon) cost is J = 5 x T P x = 36 (c) We have the same TPBVP as in part (a): with the boundary condition x = From the co-state equation: x k+1 = Ax k BR 1 B T λ k+1 λ k+1 = A T λ k 8 4, λ = Q x = 1 λ 1 = A T λ = A T Q x Substituting into the optimal control and state propagation, we get x 1 = Ax BR 1 B T λ 1 = Ax BR 1 B T A T Q x x = Ax 1 BR 1 B T λ = A x ABR 1 B T A T Q x BR 1 B T Q x 5
Now solve for x : x = (I + ABR 1 B T A T Q + BR 1 B T Q ) 1 Q x For this problem, We can then compute u = x = 9 615 18 7, u 1 = 1 355 The optimal trajectory is x 1 = The cost corresponding to part (a) is 998 583 J c = 3814 (d) We use the MATLAB place command to place both poles at the origin feedback gain is 15 5 K = 35 15 Then and u = Kx = x 1 = 1 ie, the state converges to in one step The cost corresponding to part (a) is J d = 353 (e) Part (a) has a tight constraint (exact terminal state constraint), therefore, the cost J a = 3888 is higher than J b = 3375 (asymptotic terminal constraint) and J c = 3814 (terminal cost) Note that x for parts (b) and (c) are not zero Part (b) has more control constraint but no state constraint In this case, the penalty on the final state is fairly heavy, therefore, the cost is higher over the first two steps Indeed, x in part (b) is much larger than x in part (c) Part (d) has no optimality consideration and is the most aggressive controller (driving x to origin in one step) Therefore, the cost is the highest J d = 3571, The 6
3 (5%) Given x() = 1, x(1) =, formulate the minimization of J = 1 ẋ dt as an optimal control problem Find and sketch the optimal x(t) Also find J Solution: Let ẋ = u Form Hamiltonian: H = u + λu First solve for u: H u = = u = λ Co-state equation: λ = H x = There λ = constant = c This implies u is also a constant Then x(t) = ut + 1 At t = 1, x(1) = u + 1 = Therefore, u = 1, and the optimal x is t + 1 The optimal cost is 1 4 (5%) Consider the following nonlinear control system: with the quadratic performance index ẋ = x 3 + u, x() = 1 J = 1 x () + 1 (x + u ) dt (a) (1%) Write the state and costate equations, stationarity condtion, and boundary conditions Eliminate u(t) in the state and costate equations by using the solution of the stationarity condition (b) (5%) Prove that if x(t) is small on,, then an approximate solution for the costate is λ = x x 3 (c) (1%) For this approximate costate, find the state solution x(t) and the approximate optimal control Solution: (a) Form the Hamiltonian H = 1 (x + u ) + λ( x 3 + u) 7
First find the optimal u: H u = u + λ = = u = λ The co-state equation is λ = H λ = x 3x λ The state equation with the optimal control is ẋ = x 3 λ (b) We would liket verify that λ x x 3 for x small First differentiate the approximate λ: λ = (1 3x )ẋ = (1 3x )( x 3 λ) Substitute in the approximate expression of λ again, we get λ = x 3x 3 Now substitute in the approximate expression of λ into the λ equation, we get λ = x 3x 3 + 3x 5 For x small, the two expressions for λ are approximately the same Therefore, the approximation for λ is valid (c) Substitute in the approximate expression of λ into ẋ, we get ẋ = x or x(t) = 5e t This means that u(t) 1 e t + 1 8 e 3t 8