Optimal Control. Lecture 3. Optimal Control of Discrete Time Dynamical Systems. John T. Wen. January 22, 2004

Transcription

1 Optimal Control Lecture 3 Optimal Control of Discrete Time Dynamical Systems John T. Wen January, 004

2 Outline optimization of a general multi-stage discrete time dynamical systems special case: discrete time linear quadratic regulator (LQR) Ref: Bryson & Ho, Ch.. January, 004Copyrighted by John T. Wen Page 1

3 Last Time Static optimization with equality and inequality constraints Lagrange multiplier and Hamiltonian Penalty function method (interior and exerior) MATLAB optimization toolbox (see lec03.m for example): Unconstrained optimization: fminunc Constrained optimization (equality or inequailty): fmincon We will use these functions to solve nonlinear optimal control problems. Single stage discrete time optimal control: treat the state evolution equation as an equality constraint and apply the Lagrange multiplier and Hamiltonian approach. January, 004Copyrighted by John T. Wen Page

4 Last Time (cont.) Single stage optimal control: model: x 1 = f 0 [x 0,u 0 ] Optimization index: J = φ(x 1 ) + L 0 [x 0,u 0 ] Hamiltonian : H 0 = L 0 + λ T 1 f 0 First order condition : 0 = H0 u 0 = L0 u 0 + λ T 1 f 0 u 0 State equation : x 1 = H0 = f 0 [x 0,u 0 ] λ 1 ( ) H 0 T [ ] f 0 T [ L 0 Costate equation : λ 0 = = λ 1 + x 0 x 0 x 0 [ ] φ T Boundary condition : λ 1 =. x 1 ] T January, 004Copyrighted by John T. Wen Page 3

5 Last Time (Single Stage: LQR) x 1 = A 0 x 0 + B 0 u 0, x 0 given J = (x 1 x d ) T Q 1 (x 1 x d ) + (x 0 x d ) T Q 0 (x 0 x d ) + u T 0 R 0 u 0. January, 004Copyrighted by John T. Wen Page 4

6 Optimal control for general discrete time systems We now extend the single step optimization to a general finite-horizon optimization. Consider the discrete dynamical system over N steps: x(i + 1) = f i [x(i),u(i)]; i = 0,...,N 1 where x(i) R n and u(i) R m. Example: linear time varying system x(i + 1) = A(i)x(i) + B(i)u(i). Consider the N-step look-ahead optimal control problem: N 1 J = φ[x(n)] + i=0 L i [x(i),u(i)]. Example: J = x T (N)Q(N)x(N) + N 1 ( i=1 x T (i)q(i)x(i) + u T (i)r(i)u(i) ). There are n N (constraint) equations and m N control variables. January, 004Copyrighted by John T. Wen Page 5

7 Lagrange Multiplier Approach Apply Lagrange multiplier as in single-step case. Define Hamiltonian and augmented cost function N 1 H = J + i=0 N 1 J = H i=0 λ T (i + 1) f i [x(i),u(i)] λ T (i + 1)x(i + 1). January, 004Copyrighted by John T. Wen Page 6

8 First Order Condition for Optimality Solve for Lagrange multipliers: J x(i) = H x(i) λt (i) = 0; i = 1,...,N. This becomes: Solve for control variable: λ T (N) = φ x(n) λ T (i) = Li x(i) + λt (i + 1) f i x(i). J u(i) = H u(i) = 0 Li u(i) + λt (i + 1) f i u(i) = 0. January, 004Copyrighted by John T. Wen Page 7

9 First Order Optimality Condition Complete Solution: Given x(0), the optimal control solution must satisfy x(i + 1) = f i [x(i),u(i)] 0 = Li u(i) + λt (i + 1) f i u(i) λ(i) = λ(n) = solve u(i) in terms of x(i) and λ(i + 1) [ ] f i T [ ] L i T λ(i + 1) + x(i) [ ] φ T. x(n) x(i) Dynamical equations now involve n variables (state x and co-state λ). Half of the boundary conditions are initial conditions on x, the other half are terminal conditions on λ. This is called a two-point boundary value problem (TPBVP). January, 004Copyrighted by John T. Wen Page 8

10 Discrete Time Linear Quadratic Regulator (LQR) Consider a linear time varying dynamics with quadratic optimization index: x(i + 1) = A(i)x(i) + B(i)u(i); x(0) = x 0 J = x T N 1( (N)Q(N)x(N) + x T (i)q(i)x(i) + u T (i)r(i)u(i) ) i=0 with R(i) > 0 and Q(i) > 0 Solution: Form Hamiltonian: H = J + N 1 i=0 λt (i + 1)(A(i)x(i) + B(i)u(i)). Use H u(i) = 0 to solve for u(i): u(i) = 1 R 1 (i)b T (i)λ(i + 1) Use λ(i) = [ H x(i)] T to obtain co-state dynamics: λ(i) = A T (i)λ(i + 1) + Q(i)x(i) λ(n) = Q(N)x(N). January, 004Copyrighted by John T. Wen Page 9

11 Discrete Time LQR Substituting optimal control back into the state equation we obtain: x(i + 1) = A(i)x(i) B(i)R 1 (i)b T λ(i + 1) (i) λ(i) Put them together, we have x(i + 1) = A(i) Q(i) λ(i) = A T λ(i + 1) (i) + Q(i)x(i). B(i)R 1 (i)b T (i) A T (i) x(i) λ(i+1) with the boundary condition How do we solve this? x(0) = x 0 ; λ(n) = Q(N)x(N). January, 004Copyrighted by John T. Wen Page 10

12 Solution of Discrete Time LQR Then λ(n) = Q(N)x(N), i.e., λ(n) and x(n) are linearly related. Now we show by induction that λ(i) and x(i) are also linearly related. Supposed that λi+1 = P(i + 1)x(i + 1). Then the state equation becomes: x(i + 1) = A(i)x(i) B(i)R 1 (i)b T (i)p(i + 1)x(i + 1) = (I + B(i)R 1 (i)b T (i)p(i + 1)) 1 A(i)x(i). We now substitute this into the co-state equation: λ(i)/ = A T (i)p(i + 1)x(i + 1) + Q(i)x(i) = [ A T (i)p(i + 1)(I + B(i)R 1 (i)b T (i)p(i + 1)) 1 A(i) + Q(i) ] x(i). }{{} P(i) January, 004Copyrighted by John T. Wen Page 11

13 Solution of Discrete Time LQR (Cont.) Since λ(n) and x(n) are linear related, by induction, λ(i) and x(i) are linearly related for i = 0,...,N. Furthermore, by letting P(N) = Q(N), we have the recursive equation of P(i) (this is called the discrete time time-varying Riccati Equation): P(i) = A T (i)p(i + 1)(I + B(i)R 1 (i)b T (i)p(i + 1)) 1 A(i) + Q(i). This is called the sweep method of solving the TPBVP. The recursive equation computing P(i) s is called the discrete-time time-varying Riccati Equation (quadratic dynamics). The optimal control is now in the feedback form: u(i) = R 1 (i)b T (i)p(i + 1)x(i + 1) = (R(i) + B T (i)p(i + 1)B(i)) 1 B T (i)p(i + 1)A(i)x(i) but P(i) needs to be pre-computed. The feedback gain is sometimes called the Kalman gain. K(i) = (R(i) + B(i) T P(i)B(i)) 1 B T (i)p(i + 1)A(i) January, 004Copyrighted by John T. Wen Page 1

14 Application Consider a nonlinear system ẋ = f (x,u), x(0) = x o. Suppose that a nominal trajectory has already been generated, i.e., there exists (x (t),u (t)), t [0,T ], such that ẋ = f (x,u ), x (0) = x o. To generate a feedback control law to follow the nominal trajectory, we can first linearize the nonlinear system about the nominal trajectory: δx = f (x (t),u (t)) x δx + f (x (t),u (t)) δu; δx(0) = 0 u where δx = x x and δu = u u. This can be further discretized to a linear time varying system, e.g., δx((k+1)t s ) = (I +t s f (x (kt s ),u (kt s )) x )δx(kt s )+ f (x (kt s ),u (kt s )) δu(kt s ); δx(0) = 0. u The discrete time LQR can now be applied to generate the optimal feedback corrective control δu. January, 004Copyrighted by John T. Wen Page 13