9 The LQR Problem Revisited

Transcription

1 9 he LQR Problem Revisited Problem: Compute a state feedback controller u(t) = Kx(t) that stabilizes the closed loop system and minimizes J := lim E [ z(t) z(t) ] t for the LI system Assumptions: a) D zu D zu 0; b) (A, B u ) stabilizable; ẋ(t) = Ax(t) + B u u(t) + B w w(t), x(0) = 0 z(t) = C z x(t) + D zu u(t). c) w(t) is a Gaussian white noise vector with zero mean and covariance W 0. Solution: As before, restate the cost function with the help of the Gramian J = trace ( ) PB w WBw where (A + B u K) P + P(A + B u K) + (C z + D zu ) (C z + D zu ) = 0. Use the Comparison Lemma to conclude that for any K such that (A + B u K) is Hurwitz then Y P = trace ( Y B w WB w) J where Y satisfies the inequality (A + B u K) Y + Y (A + B u K) + (C z + D zu K) (C z + D zu K) 0. MAE 280 B 124 Maurício de Oliveira

2 9.1 LQR revisited: solution by congruence + change-of-variables Recipe I (congruence + change-of-variables): First apply Schur complement (A + B u K) Y + Y (A + B u K) + (C z + D zu K) (C z + D zu K) 0 [ ] (A + Bu K) Y + Y (A + B u K) Cz + K Dzu 0 C z + D zu K I Under the assumption that (A, B u ) is stabilizable and Dzu D zu 0 we have that Y P 0 so that we can apply the congruence transformation [ ][ ][ ] Y 1 0 (A + Bu K) Y + Y (A + B u K) Cz + K Dzu Y I C z + D zu K I 0 I [ (A + Bu K)Y 1 + Y 1 (A + B u K) Y 1 Cz + Y ] 1 K Dzu C z Y 1 + D zu KY 1 0 I Apply change-of-variables X := Y 1, L := KY 1 to obtain the LMI [ AX + XA + B u L + L Bu XCz + ] L Dzu 0 C z X + D zu L I As for the cost function trace ( X 1 B w WB w) = trace ( Y Bw WB w) J Now introduce an extra variable Z such that herefore Z B wx 1 B w trace(zw) = trace ( B w X 1 B w W ) J Using Schur Complement Z B wx 1 B w [ ] Z B w 0. X B w MAE 280 B 125 Maurício de Oliveira

3 9.1.1 Summary: LQR revisited (first form) he state feedback controller u(t) = Kx(t), K = LX 1 where X S n, L R m n and Z S r solve the SDP min X S n,l R m n,z S r s.t. trace(zw) [ ] AX + XA + B u L + L Bu XCz + L Dzu 0, C z X + D zu L I [ ] Z B w 0, X 0. X B w stabilizes the closed loop system and minimizes the cost function J := lim E [ z(t) z(t) ] t for the LI system under the assumptions a) D zud zu 0; b) (A, B u ) stabilizable; ẋ(t) = Ax(t) + B u u(t) + B w w(t), x(0) = 0 z(t) = C z x(t) + D zu u(t) c) w(t) is a Gaussian white noise vector with zero mean and covariance W 0. MAE 280 B 126 Maurício de Oliveira

4 9.2 Elimination Lemma Let x R n, Q S n, B R m n and B R p n such that rank(b) < n and rank(c) < n. he following statements are equivalent: i) B QB 0 and C QC 0 ii) µ R : Q µb B 0, and Q µc C 0. iii) X R p m : Q + C X B + B X C 0. Remark: Proofs can be found in Skelton, Iwasaki and Grigoriadis (heorem , p. 29.) or Boyd, El Gahoui, Feron & Balakrishnan (p. 32). None as nice as the next one :). Proof: ii) and iii) i) is as in Finsler s Lemma, just multiply by B and C. i) ii) use Finsler s Lemma to show that i) implies the existence of scalars µ 1 and µ 2 such that Q µ 1 B B 0, Q µ 2 C C 0. hen µ = max{µ 1, µ 2 } is such that ii) holds for some µ. ii) iii) requires an explicit construction of X. MAE 280 B 127 Maurício de Oliveira

5 9.2.1 Construction of X Assume that ii) holds. If µ 0 then X = 0 solves the problem. So assume µ > 0. Now let ρ 2 = µ so that Define hen Q ρ 2 B B 0, Q ρ 2 C C 0. Φ := Q ρ 2 B B ρ 2 C C Φ + ρ 2 C C 0, Φ + ρ 2 B B 0. Also notice that Φ 0 because Φ := 1 2 [( Q 2ρ 2 B B ) + ( Q 2ρ 2 C C )] and 2ρ 2 > ρ 2 = µ. herefore, using Schur Complement [ ] Φ ρc Φ + ρ 2 C C 0 0. ρc I Schur Complement one more time [ ] Φ ρc 0 ρc I I ρ 2 CΦ 1 C 0. Likewise Φ + ρ 2 B B 0 I ρ 2 BΦ 1 B 0. We will now prove that X = ρ 2 Y Y = ρ 2 C Φ 1 B satisfies iii). With that in mind notice that [ ] I ρ 2 B Φ 1 B Y ρ 2 B Φ 1 C Y ρ 2 C Φ 1 B I ρ 2 C Φ 1 C 0. MAE 280 B 128 Maurício de Oliveira

6 Using Schur Complement this is equivalent to Φ ρ B ρ C ρ B I Y 0 ρ C Y I Using Schur Complement one more time [ ] [ ] Φ + ρ 2 C C ρ B + ρ C Y Q ρ ρ B + ρy C I + Y = 2 B B ρ B + ρ C Y Y ρ B + ρy C I + Y 0 Y and once again Q ρ 2 B B + ρ 2 (B + C Y)(I Y Y) 1 (B + Y C) 0 and I Y Y 0. Using the Lemma of the Matrix Inverse 2 so that (I Y Y) 1 = I + Y (I YY ) 1 Y 0 Q ρ 2 B B + ρ 2 (B + C Y)(I Y Y) 1 (B + Y C) = Q + ρ 2 C YB + ρ 2 B Y C + ρ 2 C YY C hence iii) holds because + ρ 2 (B + C Y)Y (I Y Y) 1 Y(B + Y C) ρ 2 C YY C + ρ 2 (B + C Y)Y (I Y Y) 1 Y(B + Y C) 0. 2 Verify that [ I + Y (I YY ) 1 Y ] (I Y Y) = I Y Y + Y (I YY ) 1 Y(I Y Y) = I Y Y + Y (I YY ) 1 (I YY )Y = I Y Y + Y Y = I. MAE 280 B 129 Maurício de Oliveira

7 9.3 LQR revisited: solution by Elimination Lemma Recipe II (existence conditions): As before we start with [ ] (A + Bu K) Y 0, Y + Y (A + B u K) Cz + K Dzu 0 C z + D zu K I Note that we are using strict inequalities! Now rewrite [ ] [ ] [ ] A Y + Y A Cz Y Bu [ ] I + C z I D zu }{{} K I 0 + }{{} 0 }{{}}{{} X C }{{} Q B C We need a more general version of Finsler s Lemma. K }{{} X Computing B and C under no assumptions [ B u Y Dzu] 0. }{{} B First compute C = [ I 0 ] = [ 0 I ]. he problem of computing a basis for 0 = Bx = [ [ ] Bu Y Dzu] [ ] x = B u Dzu Y 0 x 0 I is more involved. As before, because Y is nonsingular [ ] [ B Y 0 u Dzu] y = 0, y = 0 I x 0. Let [ E1 E 2 ] := [ ] Bu Dzu so that B = [ ] Y 1 0 [B u 0 I D zu [ ] ] Y 1 E = 1. E 2 MAE 280 B 130 Maurício de Oliveira

8 his choice produce the inequalities and 0 B QB = [ ] [ ][ ] E1 Y 1 E2 A Y + Y A Cz Y 1 E 1 C z I E 2 = [ ] [ ][ ] E1 E2 Y 1 A + AY 1 Y 1 Cz E1 C z Y 1. I 0 C QC = [ 0 I ] [ ][ ] A Y + Y A Cz 0 C z I I = I which is trivially satisfied. he first inequality, along with the cost inequality derived previously can be linearized with the change-of-variables X := Y 1. E 2 MAE 280 B 131 Maurício de Oliveira

9 9.3.2 Summary: LQR revisited (second form) he optimal state feedback controller u(t) = Kx(t) which can be computed from the solution to the SDP in the variables X S n, Z S r solve the SDP min X S n,z S r s.t. trace(zw) [ ] [ E 1 E2 AX + XA XCz C z X I [ ] Z B w 0, X 0. X B w ][ E1 E 2 ] 0, where [ E1 E 2 ] = [ ] Bu Dzu stabilizes the closed loop system and minimizes the cost function J := lim E [ z(t) z(t) ] t for the LI system under the assumptions a) D zu D zu 0; b) (A, B u ) stabilizable; ẋ(t) = Ax(t) + B u u(t) + B w w(t), x(0) = 0 z(t) = C z x(t) + D zu u(t) c) w(t) is a Gaussian white noise vector with zero mean and covariance W 0. MAE 280 B 132 Maurício de Oliveira

10 9.3.3 Computing B under the assumption D zu D zu 0 Proceed as before but now notice that if Dzu D zu 0 then [ ] [ ] E1 I 0 = E 2 D zu (DzuD zu ) 1 Bu (Dzu) = [ ] Bu Dzu is a possible choice for E 1 and E 2 so that, using X := Y 1 we have where 0 B QB = [ ] [ ][ ] E1 E2 AY 1 + Y 1 A + Y 1 Cz E1 C z Y 1 I E 2 [ ] I 0 = D zu (Dzu D zu) 1 Bu (Dzu [ ) ][ ] AX + XA XCz I 0 C z X I D zu (Dzu D zu) 1 Bu (Dzu [ ] ) I 0 = D zu (DzuD zu ) 1 Bu (Dzu) [ ] AX + XÃ XCz (Dzu) C z X + D zu (Dzu D zu) 1 Bu (Dzu [ÃX ) ] + X Ã B u (Dzu D zu) 1 Bu XCz (D zu ) = (D zu ) C z X (D zu ) (D zu ) Ã := A B u (D zud zu ) 1 D zuc z. MAE 280 B 133 Maurício de Oliveira

11 9.3.4 Summary: LQR revisited (third form) he optimal state feedback controller u(t) = Kx(t) which can be computed from the solution to the SDP in the variables X S n, Z S r solve the SDP min X S n,z S r s.t. trace(zw) ] [ÃX + X à B u (Dzu D zu) 1 Bu XCz (D zu ) (Dzu) C z X (Dzu) (Dzu) 0, [ ] Z B w 0, X 0. X B w where à := A B u (D zud zu ) 1 D zuc z, stabilizes the closed loop system and minimizes the cost function J := lim E [ z(t) z(t) ] t for the LI system under the assumptions a) D zu D zu 0; b) (A, B u ) stabilizable; ẋ(t) = Ax(t) + B u u(t) + B w w(t), x(0) = 0 z(t) = C z x(t) + D zu u(t) c) w(t) is a Gaussian white noise vector with zero mean and covariance W 0. MAE 280 B 134 Maurício de Oliveira

12 9.4 Example: satellite in circular orbit r u 1 u 2 m θ e Satellite of mass m with thrust in the radial direction u 1 and in the tangential direction u 2. Continuing... m( r r θ 2 ) = u 1 km r 2 + w 1, m(2ṙ θ + r θ) = u 2 + w 2, where w 1 and w 2 are independent white noise disturbances with variances δ 1 and δ 2. As before, putting in state space and linearizing around r ) ( ) x(t) = ωt (ū1 0, ū = w1 = 0, w = = 0, ω = k/ r ū 2 w 3 2 ω one obtains the linearized system ẋ = 3 ω 2 2 r ω 2 ω/ r 0 Problem: Given x+ 1/m 0 0 1/(m r) ( u1 u 2 ) + 1/m 0 0 1/(m r) m = 100 kg, r = R km, k = GM where G N m 2 /kg 2 is the universal gravitational constant, and M kg and R km are the mass and radius of the earth. If the variances δ 1 and δ 2 are 0.1N find solutions to the LQR control problem where Q = I, using u 2 only first, then using u 1 and u 2. R = ρi, MAE 280 B 135 Maurício de Oliveira ( w1 w 2 ).

13 % MAE 280 B - Linear Control Design % Mauricio de Oliveira % % LMI LQR Control % m = 100; % 100 kg r = 300E3; % 300 km R = 6.37E6; % ˆ3 km G = 6.673E-11; % N mˆ2/kgˆ2 M = 5.98E24; % ˆ24 kg k = G * M; % gravitational force constant w = sqrt(k/((r+r)ˆ3)); % angular velocity (rad/s) v = w * (R + r); % "ground" velocity (m/s) % linearized system matrices A = [ 1 0; 0 1; 3*wˆ2 2*(r+R)*w; -2*w/(r+R) 0]; Bu = [; ; 1/m 0; 0 1/(m*r)]; Bw = [; ; 1/m 0; 0 1/(m*r)]; % noise variances W = 0.1 * eye(2) W = e e-01 % scale = diag([1 r 1 r]) = % similarity transformation At = * A / At = e e e e e-04 0 But = * Bu But = e e-02 Bwt = * Bw Bwt = e e-02 MAE 280 B 136 Maurício de Oliveira

14 % optimal LQR state feedback control (using u2 only) n = size(at,1); m = 1; % LQR solution Cz = [eye(n); zeros(m,n)]; Dzu = [zeros(n,m); eye(m)]; [K,X,S] = lqr(at, But(:,2), Cz * Cz, Dzu * Dzu); Klqr = - K Klqr = e e e e+01 % LMI solution % declare variables X = sdpvar(n,n, symmetric ); Z = sdpvar(m,m, symmetric ); L = sdpvar(m,n); % declare LMIs LMI1 = [At*X+X*At +But(:,2)*L+L *But(:,2), X*Cz +L *Dzu ;... Cz*X+Dzu*L, -eye(m+n)]; LMI2 = [Z, Bwt(:,2) ; Bwt(:,2), X]; LMI3 = X; LMI = set(lmi1 < 0) + set(lmi2 > 0) + set(lmi3 > 0); options = sdpsettings( solver, sedumi ); solution =solvesdp(lmi,trace(w(2,2)*z),options) SeDuMi 1.1R3 by AdvOL, 2006 and Jos F. Sturm, Alg = 2: xz-corrector, theta = 0.250, beta = eqs m = 15, order n = 19, dim = 123, blocks = 4 nnz(a) = , nnz(ada) = 217, nnz(l) = 116 it : b*y gap delta rate t/tp* t/td* feas cg cg prec 0 : 4.49E : -6.05E E E+00 2 : -1.66E E E-01 3 : -3.52E E E-01 4 : -1.59E E E-01 5 : -2.29E E E-01 6 : -3.45E E E-01 7 : -4.71E E E-01 8 : -5.65E E E-01 9 : -7.25E E E : -8.33E E E : -1.07E E E : -1.23E E E : -1.40E E E : -1.52E E E : -1.63E E E : -1.70E E E : -1.76E E E : -1.79E E E : -1.82E E E : -1.84E E E-07 MAE 280 B 137 Maurício de Oliveira

15 21 : -1.84E E E : -1.85E E E : -1.85E E E : -1.85E E E : -1.85E E E : -1.85E E E : -1.85E E E : -1.85E E E-10 iter seconds digits c*x b*y Inf e e-02 Ax-b = 8.8e-10, [Ay-c]_+ = 3.0E-10, x = 5.0e+03, y = 2.4e-01 Detailed timing (sec) Pre IPM Post 1.500E E E-02 Max-norms: b = e-01, c = 1, Cholesky add =0, skip = 0, L.L = solution = yalmiptime: e-01 solvertime: e-01 info: Numerical problems (SeDuMi-1.1) problem: 4 dimacs: [7.9553e e e e-08] % Construct K and check closed loop stability Klqr Klqr = e e e e+01 K = double(l) / double(x) K = e e e e+01 Acl = At + But(:,2)*K; eig(acl) ans = e e-02i e e-02i e e-05 % optimal LQR state feedback control (using u1 and u2) n = size(at,1); m = 2; % LQR solution Cz = [eye(n); zeros(m,n)]; Dzu = [zeros(n,m); eye(m)]; [K,X,S] = lqr(at, But, Cz * Cz, Dzu * Dzu); Klqr = - K Klqr = e e e e e e e e+01 MAE 280 B 138 Maurício de Oliveira

16 % LMI solution % declare variables X = sdpvar(n,n, symmetric ); Z = sdpvar(m,m, symmetric ); L = sdpvar(m,n); % declare LMIs LMI1 = [At*X+X*At +But*L+L *But, X*Cz +L *Dzu ;... Cz*X+Dzu*L, -eye(m+n)]; LMI2 = [Z, Bwt ; Bwt, X]; LMI = set(lmi1 < 0) + set(lmi2 > 0); options = sdpsettings( solver, sedumi ); solution =solvesdp(lmi,trace(w*z),options) SeDuMi 1.1R3 by AdvOL, 2006 and Jos F. Sturm, Alg = 2: xz-corrector, theta = 0.250, beta = eqs m = 21, order n = 17, dim = 137, blocks = 3 nnz(a) = , nnz(ada) = 393, nnz(l) = 207 it : b*y gap delta rate t/tp* t/td* feas cg cg prec 0 : 5.86E : -1.21E E E+00 2 : -3.20E E E-01 3 : -5.95E E E-01 4 : -3.86E E E-01 5 : -5.46E E E-01 6 : -7.75E E E-01 7 : -1.09E E E-03 8 : -1.38E E E-04 9 : -1.74E E E : -2.02E E E : -2.38E E E : -2.60E E E : -2.78E E E : -2.84E E E : -2.86E E E : -2.86E E E : -2.86E E E-11 iter seconds digits c*x b*y Inf e e-02 Ax-b = 4.4e-11, [Ay-c]_+ = 2.0E-11, x = 1.6e+02, y = 2.9e-01 Detailed timing (sec) Pre IPM Post 1.000E E E-02 Max-norms: b = e-01, c = 1, Cholesky add =0, skip = 0, L.L = solution = yalmiptime: e-02 solvertime: e-01 info: No problems detected (SeDuMi-1.1) problem: 0 dimacs: [3.9717e e e e-10] MAE 280 B 139 Maurício de Oliveira

17 % Construct K and check closed loop stability Klqr Klqr = e e e e e e e e+01 K = double(l) / double(x) K = e e e e e e e e+01 Acl = At + But*K; eig(acl) ans = e e-02i e e-02i e e-02i e e-02i diary off MAE 280 B 140 Maurício de Oliveira

18 Let s solve it again with the second form. % MAE 280 B - Linear Control Design % Mauricio de Oliveira % % LMI LQR Control % m = 100; % 100 kg r = 300E3; % 300 km R = 6.37E6; % ˆ3 km G = 6.673E-11; % N mˆ2/kgˆ2 M = 5.98E24; % ˆ24 kg k = G * M; % gravitational force constant w = sqrt(k/((r+r)ˆ3)); % angular velocity (rad/s) v = w * (R + r); % "ground" velocity (m/s) % linearized system matrices A = [ 1 0; 0 1; 3*wˆ2 2*(r+R)*w; -2*w/(r+R) 0]; Bu = [; ; 1/m 0; 0 1/(m*r)]; Bw = [; ; 1/m 0; 0 1/(m*r)]; % noise variances W = 0.1 * eye(2) W = e e-01 % scale = diag([1 r 1 r]) = % similarity transformation At = * A / At = e e e e e-04 0 But = * Bu But = e e-02 Bwt = * Bw Bwt = e e-02 MAE 280 B 141 Maurício de Oliveira

19 % optimal LQR state feedback control (using u1 and u2) n = size(at,1); m = size(but,2); % LQR solution Cz = [eye(n); zeros(m,n)]; Dzu = [zeros(n,m); eye(m)]; [K,X,S] = lqr(at, But, Cz * Cz, Dzu * Dzu); Klqr = - K Klqr = e e e e e e e e+01 % LMI solution (second form) E = null([but Dzu ]); % declare variables X = sdpvar(n,n, symmetric ); Z = sdpvar(m,m, symmetric ); % declare LMIs LMI1 = E *[At*X+X*At, X*Cz ;... Cz*X, -eye(m+n)]*e; LMI2 = [Z, Bwt ; Bwt, X]; LMI = set(lmi1 < 0) + set(lmi2 > 0); options = sdpsettings( solver, sedumi ); solution =solvesdp(lmi,trace(w*z),options) SeDuMi 1.1R3 by AdvOL, 2006 and Jos F. Sturm, Alg = 2: xz-corrector, theta = 0.250, beta = eqs m = 13, order n = 15, dim = 101, blocks = 3 nnz(a) = , nnz(ada) = 169, nnz(l) = 91 it : b*y gap delta rate t/tp* t/td* feas cg cg prec 0 : 4.81E : -1.24E E E+00 2 : -2.72E E E-01 3 : -4.83E E E-01 4 : -3.67E E E-01 5 : -5.44E E E-01 6 : -7.84E E E-02 7 : -1.09E E E-04 8 : -1.39E E E-04 9 : -1.76E E E : -2.05E E E : -2.42E E E : -2.63E E E : -2.79E E E : -2.85E E E : -2.86E E E : -2.86E E E-10 iter seconds digits c*x b*y MAE 280 B 142 Maurício de Oliveira

20 Inf e e-02 Ax-b = 6.5e-10, [Ay-c]_+ = 3.1E-10, x = 1.6e+02, y = 2.9e-01 Detailed timing (sec) Pre IPM Post 1.400E E E-02 Max-norms: b = e-01, c = 1, Cholesky add =0, skip = 0, L.L = solution = yalmiptime: e-01 solvertime: e-01 info: No problems detected (SeDuMi-1.1) problem: 0 dimacs: [5.9141e e e e-09] % Construct K and check closed loop stability Y = inv(double(x)); QQ = [Y*At+At *Y, Cz ;... Cz, -eye(m+n)]; BB = [But *Y Dzu ]; CC = [eye(n) zeros(n,m+n)]; mu = 1e8 mu = max(eig(qq - mu*bb *BB)) ans = e+10 max(eig(qq - mu*cc *CC)) ans = e+00 Phi = QQ - mu*bb *BB - mu*cc *CC; XX = muˆ2 * CC * inv(phi) * BB ; % Check closed loop stability Klqr Klqr = e e e e e e e e+01 Klmi = XX Klmi = e e e e e e e e+01 Acl = At + But*Klmi; eig(acl) ans = e e-02i e e-02i e e-02i e e-02i diary off MAE 280 B 143 Maurício de Oliveira