Computation of the Constrained Infinite Time Linear Quadratic Optimal Control Problem

Transcription

1 Computation of the Constrained Infinite Time Linear Quadratic Optimal Control Problem July 5, Introduction Abstract Problem Statement and Properties In this paper we will consider discrete-time linear time-invariant systems x(t + ) = Ax(t) + Bu(t) (a) with n states and m inputs, i.e. A R n n and B R m n.. Finite-Time Unconstrained LQR (FTLQR) Consider the unconstrained finite-time optimal control problem N JN (x()) = min { U x(n) P x(n) + u(i) Ru(i) + x(i) Qx(i)} () for system () with Q, P and R. The solution to () yields the state-feedback optimal control law i= u (i) = K i x(i) i =,..., N (3) where the gain matrices K i are given by the equation K i = (B P i+ B + R) B P i+ A, (4) and where the symmetric positive semidefinite matrices P i are given recursively by the algorithm P N = Q (5) P i = A (P i+ P i+ B(B P i+ B + R) BP i+ )A + Q. (6) The optimal cost is given by J N (x()) = x() P x(). (7). Infinite-Time Unconstrained LQR (LQR) If in () we set N = + and we assume that (A, B) stabilizable and (A, C) detectable (Q = C C), then we obtain the standard infinite time linear quadratic regulator (LQR) problem J (x()) = min U { u(i) Ru(i) + x(i) Qx(i)} (8) i=

2 whose solution can be expressed as the state feedback control law where the gain matrix K is given by u (i) = Kx(i), i =,..., + (9) K = (B P B + R) B P A () and P is the unique solution of the algebraic matrix equation P = A (P P B(B P B + R) BP )A + Q () within the class of positive semidefinite symmetric matrices..3 Finite-Time Constrained LQR (FTCLQR) Assume now that the states and the inputs of system () are subject to the following constraints x H (a) u D (b) where H and D are polyhedral sets, and consider the finite-time constrained optimal control problem (FTCLQR) N JN (x()) = min { U x(n) P x(n) + u(i) Ru(i) + x(i) Qx(i)} (3a) subj. to x(i) H i [,..., N] (3b) i= u(i) D i [,..., N ] (3c) The solution to problem (3) has been studied by Bemporad et.al. in [?]. We will briefly summarize the main results. By substituting x(i) = A i x() + i j= Aj Bu(i j), problem (3) can be reformulated as J (x()) = x() Y x() + min U { U HU + x() F U} (4a) s.t. GU W + Ex() (4b) where the column vector U [u(),..., u(n ) ] R s, is the optimization vector, H = H, and H, F, Y, G, W, E are easily obtained from Q, R, P, () and () (see [?] for details). We denote with X f R n the set of initial states x for which the optimal control problem (3) is feasible, i.e. X f = {x R n U R s, GU W + Ex} and with U (x) the optimizer of (4) for x() = x. Before going further, we will introduce the following definitions Definition We define a polyhedral partition as follows... Definition The set of active constraints A(x) at point x of problem (3) is defined as A(x) = {i I G i U (x) W i E i x = }, I = {,,..., m G } (5) whereby G i, W i and E i denote the i-th row of the matrices G, W and E, respectively. Problem (4) depends on x(), therefore the implementation of FTCLQR can be performed either by: solving the quadratic program (4) for a given x(), or as shown in [?,?], by solving problem (4) for all x() within a given range of values, i.e., by considering (4) as a multi-parametric Quadratic Program (mp-qp). Analysis of the mp-qp solution [?] leads to the following results on the solution of (3)

3 Theorem Consider the FTCLQR (3). Then, the set of feasible parameters X f is convex, the optimizer U : X f R s is continuous and piecewise affine, i.e. U (x()) = F k x() + G k if x() P k = {x R n H k x K k }, k =,..., N r (6) and the optimal solution J : X f R is continuous, convex and piecewise quadratic. Furthermore, {P k } N r k= is a polyhedral partition of X f and the set of active constraints in each P k is constant, i.e. A(x) = A k, x P k, A k I, k =,..., N r Once a certain set of active constraints A k is identified, the authors in [?] use KKT conditions to compute the polyhedron P k and the feedback law F k, G k as described next. First, new matrices G Ak, W Ak and S Ak are formed by extracting the rows indexed by A k from G, W and S, where S = E + GH F. Then, the feedback law is given by F k = H G A k (G Ak H G A k ) S Ak H F (7) G k = H G A k (G Ak H G A k ) W Ak (8) (9) and the polyhedron P k is computed as [ ] G(Fk + H H k = F ) S (G Ak H G A k ) S Ak [ ] W GG K k = k (G Ak H G A k ) W Ak () () Various methods to identify the sets of active constraints A k, without enumerating them all, are proposed in the literature....4 Infinite-Time Constrained LQR (CLQR) If in (4) we set N = + and we assume that (A, B) stabilizable and (A, C) detectable (Q = C C), then we obtain the infinite-time constrained LQR (CLQR) problem: J (x()) = x() Y x() + min U { U HU + x() F U} (a) s.t. GU W + Ex() (b) where the column vector U [u(), u(),...] R s, is the optimization vector and all other matrices are computed as in (4). The following theorem is needed to establish the solutions properties of the CLQR: Theorem Given system () with (A, B) stabilizable and (C, A) detectable, there exists a finite horizon ˆN(x()) such that the finite-time solution to () and (3) matches the infinite-time solution of (8) and () for a give x(). Proof cite Kalman(?) or some other guy... In view of the results of the previous section and Theorem, the implementation of CLQR can be performed either by: solving the quadratic program (4) for a given x() with N = ˆN(x()), or we can solve problem () in a given compact set S of the initial conditions by solving the mp-qp (4) for N = ˆN S max x() S ˆN(x()). Theorem 3 Given a compact set S, the horizon ˆN S is finite and by theorem the state feedback solution of problem () in S is PWA. 3

4 Various methods have been proposed in literature which allow an estimation or computation of ˆNS and ˆN(x()). Chmielewski and Manousiouthakis presented an approach, which allows for a conservative estimate of the finite horizon N est ˆN S for a compact set S. They solve a single, finite dimensional, convex program of known size to obtain N est. Rawlings et.al. presented an algorithm which iteratively attempts to identify ˆN(x()). The key definitions and theorems will be reformulated here for completeness. Definition 3 X K R n denotes the set of states x t for which the control law given in (9) satisfies the constraints in () for all time. If the constraints in () are symmetric, X K is a positive invariant set containing an open neighborhood of the origin (cite 6 in rawlings). Theorem 4 The control laws and the associated cost for any given initial state described by (4) are equal to those for the infinite horizon case in () if the terminal state x(n) lies in the unconstrained positive invariant set X K for (4), i.e. if x(n) X K, then J N (x()) = J (x()). Proof This holds because the terminal cost x(n) P x(n) computed from () is equal to the cost for the horizon segment N [N +, ], if and only if x N X K : [ x(n) P x(n) = min U i=n ] x(i) Qx(i) + u(i) Ru(i) x N X K (3) Rawlings method solves the QP for a set horizon N. If the final state is not in X K m N is increased according to a predefined iteration law. One approach would be to continually increment N by, which would yield the minimal horizon N such that x(n) X K. An alternative is to increase N by a factor of at each iteration,i.e. N = N, which results in fewer QPs to be solved at the cost of a larger ˆN(x()). 3 Comparison of Available Techniques for solving the CLQR Based on two examples, some of the drawbacks of the approaches of Rawlings (cite) and Manousiouthakis (cite) will be illustrated. Example We will consider the following system presented in moraribemporad y(t) = s u(t) (4) + 3s + sampled with T =.s to obtain the state-space representation: ( ) ( ) x(t + ) = x(t) + u(t) (5) y(t) = [.44] x(t) The task is to regulate the system to the origin while fulfilling the input constraint u Example We will consider a standard double-integrator system y(t) = u(t) (6) s 4

5 and its equivalent discrete-time state-space representation: ( ) ( ) x(t + ) = x(t) + u(t) (7) y(t) = [ ] x(t) The task is to regulate the system to the origin while fulfilling the input constraint u In order to be able to apply the Manousiouthakis approach, we will first define the compact set S as S M = {x R 5 x i 5, i =, }. Using the method in (Manousiouthakis) we obtain N est = for Example and N est = 743 for Example. Obviously, solving quadratic programs with these horizons will not yield any results in reasonable time. Moreover, the true, non-conservative minimal horizon is ˆN S = 5 for Example and ˆN S = for Example. Rawlings approach is well suited for small compact sets. However, if the set S is larger, e.g. S R = {x R x i, i =, }, the necessary minimal horizon grows quickly, drastically increasing run-times. For random values of x() S R the run-times of the algorithm are presented for an increment policy of N = N + and N = N in the table below: W orst Case Average Case N = N + sec 5 sec N = N 8 sec 6 sec While May be insert comparison with N of manu.. and N of rawlings..., and tehrefore motivate our approach: that is: off-line computation of the PWA but with small N. Chmielewski and Manousiouthakis presented an approach, which allows for a conservative estimate of the finite horizon N. 4 Computation of the Constrained Infinite Time Linear Quadratic Optimal Control Problem In this section, the main contribution of this paper is presented. Our procedure is initialized by computing the positive-invariant unconstrained set X K such that: for x(t) X K x(t + ) X K if x(t + ) = Ax(t) + Bu(t) u(t) = K LQR x(t) The polyhedron X K can be computed by continually increasing the prediction horizon from N = until an additional increase has no effect on X K, i.e. until X K converges (reference Blanchini). X K is depicted in Figure (a). We can describe the polyhderon X K through the matrix-inequalities H X x K X. Note that this approach only works for the unconstrained region. The exploration algorithm finds a point x by stepping over a facet of X K with step-size ɛ (ɛ ). The solution of (??) with horizon N = for x = x when pb?? is fixed fix x to have the same name provides the active constraints that define the neighboring critical region CR (see Figure (b)). By Theorem 4, the finite time optimal solution computed above equals the infinite time optimal solution if x(n) X K, therefore we extract from CR the set of points which will enter X K at the next time-step, provided that 5

6 Feasible Region Fragments 3 Feasible Region Fragments 3 Feasible Region Fragments.5.5 Region for Horizon N=.5 Extract Reachability SubsetITCR Step over Favet θ θ θ epsilon step over facet Pos. Inv. Unconstrained Region Pos. Inv. Unconstrained Region θ θ θ (a) Compute pos.-inv. region X K and step over facet with step-size ɛ (b) Solve QP for new point with horizon N to create the CR (c) Compute reachability subset ITCR Figure : Constrained Infinite-Time Region Exploration the optimal control-law associated with CR (u (t) = F x(t) + G ) is applied. According to Lemma?? the open loop prediction x(t + N) = A N x(t) + [A N B,..., AB, B](F x(t) + G) (8) equals the closed loop response, and the Infinite-Time Critical Region (ITCR) is defined by the intersection of the following polyhedrons and half-spaces: H X x(t + N) K X (9a) Gz W + Sx (9b) λ (9c) Whereby (9b) and (9c) are identical to (??) and (??) which were derived in Section.3. The first identified region will be referred to as ITCR (see Figure (c)) according to the following convention. Definition 4 The region ITCR N i is the i-th Infinite-Time-Critical-Region, computed by optimizing over the horizon N and satisfying the N-steps reachability condition 9a. In the following, we will assume that ITCR N i is defined as the non-redundant set of inequalities Hi N x KN i. This polyhedron has two types of facets: Type I: The facet originated from constraint restrictions in (9b) and (9c). Type II: The facet originated from reachability restrictions in (9a). The exploration procedure continues for all the facets of Type I according to the same scheme: Subsequently this procedure is repeated for all facets, which originate from constraint restrictions (Type I). This also applies to all facets of X K. Type II facets are not considered, since, by definition, those constraints cut off the points that will not reach X K in N steps. Once all facets have been explored for the finite horizon N, the horizon is increased to N + and the entire procedure is restarted. For all regions which have been previously computed, only facets of Type II are considered while for all newly identified regions, only facets of Type I are examined. Theorem 5 The intersection of the interior of ITCR N i and ITCR M j is nonempty, if and only if i = j and M = N. Proof 3 If trivial. Only If, from Theorem 4 and (9), we can conclude that the ITCR region partition is identical to the infinite-time region partition. Therefore each region has a distinct set of active constraints and from Alberto we can conclude that if two IT CRs have a non-empty intersection then they are identical. 6

7 x Trajectory for initial state [.8;.8] Horizon = Horizon = Horizon = 3 Horizon = 4 Horizon = 5 Horizon = 6 Horizon = 7 Horizon = 8 Horizon = 9 Horizon = Horizon = Horizon = Horizon = 3 Horizon = 4 Horizon = x Figure : State trajectory for x = [.8;.8]. We summarize the ideas from Section and this Section in the following algorithm: Algorithm : function R = computec-lqr. ITCR = X K, N =, R = {};. R = R {ITCR, K LQR }; 3. forall ITCR M i R, M < N, explore(itcr M i,type II ); 4. forall ITCR M i R, M = N, explore(itcr M i,type I ); 5. if N < N max, N = N + goto 3, else, return(r). Algorithm : function explore(itcr,type);. forall f facets(itcr),. if f Type, goto 9; 3. Step over f and get x; 4. if ITCR N i R s.t. x ITCR N i, i = i +, goto 9; 5. solve (??) for x = x; 6. compute ITCR new according to (9); 7. compute F and G according to (??); 8. R = R {ITCR new, F, G}; 9. end forall. If the system is subject to symmetric constraints, the computation time can be almost halved, since the region partition is also symmetric to the origin (cite tondels friend). 7

8 R_ Reachability N R_ R_3 R_4 Reachability N- Figure 3: Region R and R are connected to R 4 and region R 3 is adjacent to R 4. Theorem 6 If all regions with a horizon smaller than N have been computed using algorithm, the set of those regions is positive invariant. Proof 4 Follows from Theorem??. The state will always move to a region with horizon N at the next time step until the unconstrained region X K is reached. Theorem 7 In the ITCOC region partition, each region with a reachability of N is adjacent or connected to a region with a reachability of N. We define two regions as connected, if they re not directly adjacent but reachable by moving through adjacent regions with same horizon as the originating region (see Figure 3). Proof 5 Assume that the entire region partition of set S for an arbitrary horizon of N is computed. Subsequently we compute the reachability subset for a horizon of N over the entire set S. Now we recompute the partition and the reachability set for a horizon of N. Obviously the reachability set for horizon N is a subset for the reachability set for horizon N. Therefore all infinite horizon regions with reachability N are adjacent are connected to a region with reachability N. By computing all regions with a reachability horizon of N = and by applying Theorem 7, we can show that the presented algorithm provides the minimal solution to the problem at hand. The solution is minimal in the sense that the smallest possible horizon is always used to compute the QPs, i.e. if a given point can reach X K in N steps, the QP solved for that point will never be computed with a horizon larger than N. Therefore this algorithm is also an efficient method to compute ˆN S for any set sets without having to rely on conservative estimates. 5 Results Figure 4 shows the regions which were computed by using Algorithm on the double integrator example presented in morari et.al.. The times needed to compute the regions using various algorithms with a.ghz System are given in the table below. Inf-MPQP denotes algorithm presented in the previous section; MPQP using N min signifies that the MPQP presented in Section 4 is solved for the lowest possible horizon setting; MPQP using N estimate signifies the the horizon estimates from Chmielewski et.al. were adopted. 8

9 θ Feasible Region Fragments Horizon = Horizon = Horizon = 3 Horizon = 4 Horizon = 5 Horizon = 6 Horizon = 7 Horizon = 8 Horizon = 9 Horizon = Horizon = Horizon = Horizon = 3 Horizon = 4 Horizon = θ Figure 4: Regions computed by Algorithm θ Feasible Region Fragments Horizon = Horizon = Horizon = 3 Horizon = 4 Horizon = 5 Horizon = 6 Horizon = 7 Horizon = 8 Horizon = 9 Horizon = Horizon = Horizon = Horizon = 3 Horizon = 4 Horizon = θ Figure 5: Computation of the invariant set which covers the black 5x5 region. 9

10 explored state-space Inf-MPQP MPQP using N min MPQP using N estimate 5x5.5 sec sec 8 sec x sec sec sec tracking not possible could be used in combination with rawlings point to numerical problems? 6 Conclusions