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
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: http://dx.doi.org/10.1007/s10107-007-0096-6
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
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
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
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
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))
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
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.
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
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 1 0 0 = M 1,0 0 0.......... 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.
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 ) + 1 2 ˆ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}
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
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
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
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.
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.
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.
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
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
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, µ κ )
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: 0 500 A J 1 J2... JlN J1 B1 J2 B2... JlN ba xa x b 1 1 x2 = b2...... bln xln BlN 1000 1500 2000 2500 3000 3500 4000 4500 5000 0 1000 2000 3000 4000 5000 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
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 0.026 0.047 0.021 0.033 4 states, 8 stages 0.201 0.213 0.089 0.117 4 states, 12 stages 0.977 2.199 0.287 2.926 4 states, 16 stages 3.871 39.83 0.128 10.93 4 states, 20 stages 12.99 76.46 1.128 31.03 8 states, 4 stages 0.886 4.869 0.181 1.130 8 states, 8 stages 7.844 49.15 0.842 19.59 8 states, 12 stages 49.20 303.7 2.949 131.6 8 states, 16 stages 210.5 x 7.219 x 8 states, 20 stages 501.7 x 13.14 x 12 states, 4 stages 4.866 24.66 0.428 6.007 12 states, 8 stages 95.84 697.1 3.458 230.5 12 states, 12 stages 672.2 x 11.86 x 12 states, 16 stages x x 33.04 x 12 states, 20 stages x x 79.06 x
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 10 3 10 2 Solution Time (seconds) 10 1 10 0 10 1 10 2 n = 2 n = 4 n = 8 n = 12 N 3 /1000 5 10 20 50 Horizon Length N
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 22 21 20 19 18 Number of iterations 17 16 15 14 13 12 n = 2 n = 4 11 n = 8 n = 12 10 5 10 20 50 Horizon Length
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: http://dx.doi.org/10.1007/s10107-007-0096-6