Efficient robust optimization for robust control with constraints Paul Goulart, Eric Kerrigan and Danny Ralph

Transcription

1 Efficient robust optimization for robust control with constraints p. 1 Efficient robust optimization for robust control with constraints Paul Goulart, Eric Kerrigan and Danny Ralph

2 Efficient robust optimization for robust control with constraints p. 2 Outline Problem definition LTI system with bounded state disturbance Convex constraints on state and input Optimization over affine state feedback policies Optimization problem is non-convex Optimization over affine disturbance feedback policies Equivalent to affine state feedback, but convex Efficient solution by decomposition Optimization using QP for polytopic constraints Highly structured if -norm bounded disturbances Solvable in O(N 3 ) time Mathematical Programming, ONLINE FIRST:

3 Efficient robust optimization for robust control with constraints p. 3 What Do We Want to Do? For the discrete-time LTI system x i+1 = Ax i + Bu i + w i, i = 0, 1,... u i = µ i (x 0,...,x i ), i = 0, 1,... with given initial state x = x 0, find a feedback policy π := {µ 0 ( ), µ 1 ( ),...} which guarantees satisfaction of mixed convex constraints (x i, u i ) Z i = 0, 1,... for all sequences {w 0, w 1,...}, where w i W, W bounded

4 Efficient robust optimization for robust control with constraints p. 4 Why Do We Need A Feedback Policy? Without disturbances, the state trajectory can be computed exactly for a given open-loop policy. constraint constraint Nominal Trajectory constraint constraint time

5 Efficient robust optimization for robust control with constraints p. 4 Why Do We Need A Feedback Policy? With disturbances, an open-loop policy can accumulate large uncertainties over the prediction horizon. Open Loop Policy Envelope constraint constraint Nominal Trajectory constraint constraint time

6 Efficient robust optimization for robust control with constraints p. 4 Why Do We Need A Feedback Policy? A feedback policy can control the growth of uncertainty over the planning horizon. Open Loop Policy Envelope Feedback Policy Envelope constraint constraint Nominal Trajectory constraint constraint time

7 Efficient robust optimization for robust control with constraints p. 5 Optimization Over Feedback Policies Optimizing over arbitrary feedback policies is hard: Optimization is over functions: µ i : R n R n R m Infinite number of decision variables: π = {µ 0 ( ), µ 1 ( ),...} Infinite number of constraints: (x i, u i ) Z, w W := W N Compromise Solution: Finite control horizon, i.e. π = {µ 0 ( ), µ 1 ( ),...,µ N 1 ( )} Parameterize π in terms of affine state feedback laws u i = µ i (x 0,...,x i ) = g i + i L i,j x j j=0 Implement receding horizon control (RHC) law: u(k) = µ 0 (x(k))

8 Efficient robust optimization for robust control with constraints p. 6 A Bit of Notation Write state feedback policies in matrix form: so that u 0.. u N 1 L 0,0 0 0 = L N 1,0 L N 1,N 1 0 u = Lx + g and let A, B and E be matrices such that x 0 x 1.. x N + x = Ax + Bu + Ew = (I BL) 1 (Ax + Bg + Ew), where the initial state x := x 0 is known g 0.. g N 1

9 Efficient robust optimization for robust control with constraints p. 7 Choices of Cost Function for RHC Quadratic cost with u = Lx + g V N (x,l,g,w) := N 1 i=0 ( xi 2 Q + u i 2 ) R + xn 2 P, (Q, R, P) 0 Minimize the expected value: min E[V N (x,l,g,w)] (L,g) admissible Input-to-state stable RHC. Recovers LQR solution far from constraints. Minimize a worst-case cost: min (L,g) admissible max V N(x,L,g,w) w W N 1 i=0 γ 2 w i 2 2 RHC with l 2 gain γ. Recovers H solution far from constraints.

10 Efficient robust optimization for robust control with constraints p. 8 Non-Convexity in State Feedback A problem: The optimization problem is nonlinear in the feedback parameter L. A bigger problem: Example: The set of constraint admissible feedback policies is non-convex. x + = x + u + w u 3, w 1 When g = 0 and L 2,1 = 0, constraints are satisfied iff L 1,1 3 L 2,2 (1 + L 1,1 ) + L 2,2 3

11 Efficient robust optimization for robust control with constraints p. 9 An Alternative Feedback Policy Parameterize control as an affine function of disturbances: u i = v i + i 1 j=0 M i,j w j, i = 0,...,N 1 Full state feedback w i = x i+1 Ax i Bu i. u 0 u 1.. u N = M 1, M N 1,0 M N 1,N 2 0 w 0 w 1.. w N 1 + v 0 v 1.. v N 1 or u = Mw + v *State and disturbance feedback parameterizations are equivalent. **The set of admissible disturbance feedback policies is convex.

12 Efficient robust optimization for robust control with constraints p. 10 Implementing a Receding Horizon Controller Assume polytopic constraints on the states and inputs Z := {(x, u) Cx + Du b} X f := {(x, u) Y x z } Optimize (M, v) over feasible control policies, using the nominal cost function V (x,v) = 1 2 (ˆx T i Qˆx i + vi T Rv i ) ˆxT NP ˆx N N 1 i=0 This is particularly easy to do when the disturbances are -norm bounded W := {w w = Ed, d 1}

13 Efficient robust optimization for robust control with constraints p. 11 Implementing a Receding Horizon Controller The robust RHC law can be implemented by solving a QP at each time instant min M,Λ,v 1 2 ( Bv 2 Q + v 2 R) + (Ax) T Bv subject to Fv + (FMJ + GJ)d c + Tx, d 1 where matrices R, Q, F, J, G, and T and vector c are constructed from the problem data

14 Efficient robust optimization for robust control with constraints p. 11 Implementing a Receding Horizon Controller The robust RHC law can be implemented by solving a QP at each time instant min M,Λ,v 1 2 ( Bv 2 Q + v 2 R) + (Ax) T Bv subject to Fv + abs(fmj + GJ)1 c + Tx where matrices R, Q, F, J, G, and T and vector c are constructed from the problem data

15 Efficient robust optimization for robust control with constraints p. 11 Implementing a Receding Horizon Controller The robust RHC law can be implemented by solving a QP at each time instant min M,Λ,v 1 2 ( Bv 2 Q + v 2 R) + (Ax) T Bv subject to Fv + Λ1 c + Tx Λ (FMJ + GJ) Λ where matrices R, Q, F, J, G, and T and vector c are constructed from the problem data Complexity There are O(N 2 ) variables to choose in M An interior point method will require O(N 6 ) time to solve

16 Efficient robust optimization for robust control with constraints p. 11 Implementing a Receding Horizon Controller The robust RHC law can be implemented by solving a QP at each time instant min M,Λ,v 1 2 ( Bv 2 Q + v 2 R) + (Ax) T Bv subject to Fv + Λ1 c + Tx Λ (FMJ + GJ) Λ This problem can be decomposed into three parts Nominal Problem The first constraint is equivalent to a conventional RHC problem with no disturbances if Λ = 0. Constraint Contraction The term δc := Λ1 represents a modification to the nominal constraints. Perturbation Problems The second constraint represents a set of subproblems, each contributing to the total constraint contraction.

17 Efficient robust optimization for robust control with constraints p. 12 Decomposition: Nominal Problem The nominal states ˆx i are the expected states given no disturbances. Reintroduce these states - the first constraint Fv + δc c + Tx can be rewritten in terms of the nominal controls v i and states ˆx i ˆx 0 = x, ˆx i+1 Aˆx i Bv i = 0, i {0,..., N 1} Cˆx i + Dv i + δc i b, i {0,..., N 1} Y ˆx N + δc N z, Can be written in matrix form with banded coefficients.

18 Efficient robust optimization for robust control with constraints p. 13 Decomposition: Perturbation Problems Each of the t columns of the second constraint represents a subproblem Λ (FMJ + GJ) Λ Define y p as the output of some system in response to a unit disturbance at some time k, or y p = (FMJ + GJ)e p, for each p {1,...,t} As in the nominal problem, reintroduce states into each subproblem (u p i, xp i, yp i ) = 0, i {0,..., k} x p k+1 = E (j), x p i+1 Axp i Bup i y p i Cxp i Dup i = 0, i {k + 1,..., N 1} = 0, i {k + 1,..., N 1} y p N Y xp N = 0, Can also be written in matrix form with banded coefficients.

19 Efficient robust optimization for robust control with constraints p. 14 Decomposition: Complete Problem Decomposition yields a QP with a lot of structure: Constraint contraction term δc is the sum of the absolute values of the outputs y p Each of the subproblems has tightly banded coefficients Nominal problem and perturbation problems are coupled by the constraint contraction terms δc Complexity Still O(N 2 ) variables to choose Problem is now highly structured - can be solved efficiently in O(N 3 ) time using a sparse interior point method Can guarantee O(N 3 ) behavior at each interior point step using an appropriate factorization method

20 Efficient robust optimization for robust control with constraints p. 15 Primal-Dual Interior Point Methods for QP KKT conditions: 1 min θ 2 θ Qθ subject to Aθ = b, Cθ d Qθ + A π + C λ = 0 Aθ b = 0, Cθ + d z = 0 (λ, z) 0 λ z = 0

21 Efficient robust optimization for robust control with constraints p. 15 Primal-Dual Interior Point Methods for QP Relax complementarity condition: 1 min θ 2 θ Qθ subject to Aθ = b, Cθ d Qθ + A π + C λ = 0 Aθ b = 0, Cθ + d z = 0 λ i z i = µ, i Sequence of (θ κ, π κ, λ κ, z κ ) are obtained by solving following as µ κ 0: Q A C θ r Q A π = r A ( ) C Λ 1 Z λ rc Λ 1 r Z (r Q, r A, r C ) are residuals of 1 st three constraints, r Z := (ΛZ1 1 µ), µ (0, µ κ )

22 Sparsity Pattern of LHS Matrix One can order the primal and dual variables of the decomposed problem to get a symmetric, block-bordered, banded diagonal set of equations: A J 1 J2... JlN J1 B1 J2 B2... JlN ba xa x b 1 1 x2 = b bln xln BlN Using a Schur complement technique, one can show that above can be solved in O((m + n)3 N 3 ) operations Factorization of Bp based on a Riccati recursion Efficient robust optimization for robust control with constraints p. 16

23 Efficient robust optimization for robust control with constraints p. 17 Results - Average Solution Times (sec) Original Decomposition Problem Size OOQP PATH OOQP PATH 4 states, 4 stages states, 8 stages states, 12 stages states, 16 stages states, 20 stages states, 4 stages states, 8 stages states, 12 stages states, 16 stages x x 8 states, 20 stages x x 12 states, 4 stages states, 8 stages states, 12 stages x x 12 states, 16 stages x x x 12 states, 20 stages x x x

24 Efficient robust optimization for robust control with constraints p. 18 Results: solution times vs. horizon length Solution time vs. horizon length using decomposition, n = number of states Solution Time (seconds) n = 2 n = 4 n = 8 n = 12 N 3 / Horizon Length N

25 Efficient robust optimization for robust control with constraints p. 19 Results: no. iterations vs. horizon length No. iterations vs. horizon length using decomposition, n = number of states Number of iterations n = 2 n = 4 11 n = 8 n = Horizon Length

26 Efficient robust optimization for robust control with constraints p. 20 Closing Remarks Affine state feedback policies Optimization problem is non-convex Affine disturbance feedback policies Equivalent to affine state feedback Optimal policies can be found using convex optimization Controller Implementation Implemented as an RHC by solving a single QP at each step QP requires O(N 6 ) time for polytopic W Problem Decomposition For -norm bounded disturbances, reduces complexity from O(N 6 ) to O(N 3 ) by introducing additional variables to improve structure Can guarantee O(N 3 ) using the right factorization method, but off-the-shelf IP solvers do just as well Mathematical Programming, ONLINE FIRST: