# Patent

## Title
System and method for exploiting a good starting guess for binding constraints in quadratic programming with an infeasible and inconsistent starting guess for the solution

## Abstract

The present invention provides an algorithm that does not relax the problem at the very onset, even if xf is infeasible. Instead, it solves the EQP with the initial guess for the active set without relaxing the problem. If this solution to the first EQP is not optimal, but nevertheless feasible, we can use this as our guess for the feasible point. This has the advantage of being a feasible point that is consistent with the initial active set, whereas the initial guess used in the previous method is not necessarily so.

## Background
This invention was conceived in performance of U.S. Navy Contract No. N 00019-02-C-3003. The government may have certain rights in this invention.


BACKGROUND OF THE INVENTION
The present invention efficiently solves quadratic programs in real time. Such quadratic programs need to be solved to determine the best actuator commands to meet performance goals while satisfying constraints in Dynamic Inversion or Model Predictive Control of a dynamical system.
Time varying systems can be modeled by a set of linearized algebraic equations that relate the outputs and performance indices of the system to the inputs at every instant in time (in a nonlinear dynamical system, these equations change at every time instant). These inputs are formally known as control variables, because the values of these variables are under the control of the operator. The aim is to find the best sequence of control variables in time that maximize the performance indices of the system. In addition, the physical constraints of the system must be honored in order to prevent critical system failure. The problem of determining the best sequence of control variables requires solving a sequence of optimization problems in time, i.e., a new optimization problem at every instant in time. Each such optimization problem must be solved almost instantaneously. For the Dynamic Inversion application in systems with fast dynamics, this allows only a few milliseconds. Hence the requirement that the optimization technique come up with a solution swiftly is key in these applications. Two co-pending applications, “Real-Time Quadratic Programming For Control Of Dynamical Systems,” Ser. No. 10/308,285, filed Dec. 2, 2002, and “System and Method of Accelerated Active Set Search for Quadratic Programming in Real-Time Model Predictive Control,” Ser. No. 10/367,458, filed Feb. 14, 2003, the assignee of which is the assignee of the present invention, describe improved techniques for obtaining solutions efficiently.
The solution is restricted to a convex form, in general, and sometimes to quadratic programming, in order to ensure that a unique optimal solution exists, there are no local minima or maxima, and that the solution can be found with reasonable computation.
The algorithm also uses the active set method for solving realtime convex optimization problems, where the active set of a prior solution is “fedback” to initialize the search for the best active set in the current problem. This envisions application in a control system where the control solution is periodically updated at a high enough frequency that there is only little change from one update to the next. However, there are certain difficulties if the initial guess for the solution (the actuator commands) is infeasible and/or not consistent with the initial guess for the active set.
If the initial guess for the quadratic program is not feasible, the prior technique introduces an extra variable t (the big-K formulation). If a constraint that should be in the optimal active set is infeasible, it is relaxed, and the final solution from the big-K relaxed problem will always violate this constraint (setting the linear penalty with M>0 can correct for this).
There is a strong argument in favor of retaining consistency during the iterations, because otherwise the objective function may increase from one iteration to another, and a constraint may not satisfy the right hand side exactly even when it is declared active. Moreover, with an inconsistent start, if the solution to the first EQP violates any of the constraints, the current point becomes a convex combination of the EQP solution and xf (the initial guess for the feasible solution). Any constraint in the first active set that xf does not satisfy exactly must be dropped if consistency between the current iterate x and the active set were to be retained. This results in potentially promising constraints being dropped from the active set, thus losing the benefit of a good active set hot start.
SUMMARY OF THE INVENTION
The present invention provides an algorithm that does not relax the problem at the very onset, even if xf is infeasible. Instead, it solves the EQP with the initial guess for the active set without relaxing the problem. Call this solution x_eqp1. Very often, this turns out to be the optimal solution, because the initial guess for the active set is the one from the previous time point, and in a large proportion of the QPs, the previous active set is optimal.
If this solution to the first EQP, x_eqp1, is not optimal, but nevertheless feasible, we can use this as xf, our guess for the feasible point. This x_eqp1 has the advantage of being a feasible point that is consistent with the initial active set, whereas the xf used in the previous method is not necessarily so.
There are “critical constraints” that cannot be violated by the solution from the QP solver under any circumstance. It is evident that the set of critical constraints have to be such that a feasible solution exists in the first place. E.g., a set of such critical constraints can be the set of lower and upper limits on actuators, also known as ‘variable bound constraints’ in optimization terminology. It is a requirement imposed on the guess for the initial feasible point xf that it must not violate any critical constraint.
In the situation where the solution to the first EQP, x_eqp1 is neither optimal nor feasible, if it still satisfies the critical constraints we can use x_eqp1 as xf, and relax the non-critical constraints that are violated by introducing the extra variable t. This xf has the advantage of being consistent with the initial active set, while guaranteeing non-violation of the critical constraints. The next set of working constraints is chosen to be the initial working set, minus the constraints for which the multipliers are negative.
If x_eqp1 does violate the critical constraints, the projection of x_eqp1 onto the set of critical constraints is computed (call it xp), and this becomes the new guess for the feasible solution. Now, the elements of v (the vector denoting which constraints should be relaxed) and the value of t are manipulated in such a manner that the point xp is binding for as many of the constraints in the initial active set as possible, thereby retaining consistency of xp w.r.t. the original constraints in the guess for the active set for which xp is not strictly binding.



BRIEF DESCRIPTION OF THE DRAWINGS
Other advantages of the present invention will be readily appreciated as the same becomes better understood by reference to the following detailed description when considered in connection with the accompanying drawings wherein:
The FIGURE illustrates one type of control system that uses the quadratic programming method of the present invention.



DETAILED DESCRIPTION OF A PREFERRED EMBODIMENT
The Generic Problem
FIG. 1 is a generic model of a control system 10 using Model Predictive Control and of a type that would benefit from the present invention. The control system 10 includes a desired trajectory generator 12 which creates a desired profile of the outputs of the system 10. A linearization module 14 derives a linearized model about the desired trajectory from the desired trajectory generator 12. A quadratic Programming formulation module 16 forms a quadratic program to determine a control profile for best attaining the desired trajectory while respecting any constraints. The Quadratic Programming Solver 18 solves the optimization problem established by the formulation module 16 to generate a profile of the optimal controls. The Quadratic Programming Solver 18 is the focus of this invention. The profile of the optimal controls is sent to an actuation system 20, which acts on the plant 22 of the system 10. The sensor system 24 provides feedback from the plant 22 to the desired trajectory generator 12.
The forthcoming formulation of the optimization problem for the MPC is included here only for the sake of completeness. To further simplify the presentation, we have considered only a “one-time step ahead” MPC, or the constrained dynamic inversion problem, elaborated in U.S. Pat. No. 6,882,889. The full MPC problem can also be formulated and solved using the active set method described in “System and Method of Accelerated Active Set Search for Quadratic Programming in Real-Time Model Predictive Control,” Ser. No. 10/367,458, filed Feb. 14, 2003. The following formulation is only one possible representation of the Model-Predictive control problem, intended to illustrate how we get to a Quadratic Program, and not to claim an innovation in deriving the Quadratic Program.
Consider a dynamical system with state variables ξ, control variables u, and outputs y, described by the linearized system as below (this can be a linear system, or the linearized version of a nonlinear dynamical system):




  
    
      
        
          
            
              d
              ξ
            
            
              d
              t
            
          
          =
          
            
              
                A
                1
              
              ⁢
              ξ
            
            +
            
              
                B
                1
              
              ⁢
              u
            
          
        
        ⁢
        
          

        
        ⁢
        
          y
          =
          
            
              C
              ⁢
              
                
              
              ⁢
              ξ
            
            +
            Du
          
        
      
    
    
      
        (
        1
        )
      
    
  




We would like to determine how to deliver a change in control so that we achieve a desired change in the outputs. The problem is best handled in the discrete time version of the above linear system, where the subscript t represents each discrete time point:

Δξt=A1ξt−1+B1ut−1

Δyt=CΔξt+DΔut  (2)

Here Δut=ut−ut−1 represents the change in the control variable required to produce a desired change Δyt=ν in the outputs.
The quadratic objective we can minimize to achieve this is

(Δyt−ν)TV(Δyt−ν)

The minimization must be performed subject to the system equations (2) being satisfied. Here V represents a diagonal matrix of positive weights on the each of the output goals. We also add similar goal terms for the state variables and control variables. These secondary objectives have small, but always non-zero weights, to preserve strict convexity of the problem.
The equality constraints posed by the system equations (2) can be eliminated by substituting Δξt=A1ξt−1+B1ut−1 in the second equation in (2), thereby deriving

Δyt=DΔut+CA1ξt−1+CB1ut−1  (3)

This establishes that the change in outputs can be expressed solely in terms of Δut.
Additional inequality constraints on the outputs and inputs, including physical limits and rate limits, can also be expressed all in terms of Δut. in the form

AΔut≦b  (4)

The Quadratic Program

Combining the objective and the constraints, we arrive at a strictly convex quadratic program (QP). After replacing the optimization variable Δut with x to avoid cumbersome notation, we can write the QP as below:




  
    min
    ⁢
    
      
    
    ⁢
    
      1
      2
    
    ⁢
    
      x
      T
    
    ⁢
    Hx
  
  +
  
    
      c
      T
    
    ⁢
    x
  




subject to

.Ax≦b

Algorithm for Searching for the Optimal Active Set
Active set algorithms search iteratively for the set of binding constraints at optimality, usually referred to as the active set. For model-predictive control problems, the solution to the MPC problem in the current time step provides a guess for the optimal active set in the next time step. This particular guess is used to great advantage to cut down the number of iterations required for solving the QP to convergence, a feature that is practically indispensable in real-time control. The broad steps of the active set algorithm are sketched below, and the associated linear algebra is elaborated in the following section.

    
    
        Start off with a guess for an active set. We will denote the set of indices of constraints in the active set by W, and also characterize by E the rows in the constraint matrix A corresponding to the guessed active set. Assume a feasible point xf is known, i.e., Axf≦b.
        In iteration k, given a guess E for the active constraints, solve the Equality-Constrained QP (EQP)
    
    






  
    
      
        
          
            
              min
              x
            
            ⁢
            
              
            
            ⁢
            
              
                1
                2
              
              ⁢
              
                x
                T
              
              ⁢
              Hx
            
          
          +
          
            
              c
              T
            
            ⁢
            x
          
        
        ⁢
        
          

        
        ⁢
        
          
            
              s
              .
              t
              .
              
                
              
              ⁢
              Ex
            
            =
            r
          
          ,
        
      
    
    
      
        (
        7
        )
      
    
  




where r represents the sub-vector of right hand sides for the active constraints. The optimal solution x*, and Lagrange multipliers λ* are given by




  
    
      
        
          
            [
            
              
                
                  H
                
                
                  
                    E
                    T
                  
                
              
              
                
                  E
                
                
                  0
                
              
            
            ]
          
          ⁡
          
            [
            
              
                
                  
                    x
                    *
                  
                
              
              
                
                  
                    λ
                    *
                  
                
              
            
            ]
          
        
        =
        
          [
          
            
              
                
                  -
                  c
                
              
            
            
              
                r
              
            
          
          ]
        
      
    
    
      
        (
        8
        )
      
    
  




Ratio Test Determine largest α ε [0,1] such that xk−1+αs is feasible, where s=x*−xk−1, and xk−1 denotes the prior iterate. In other words, α is picked to be the largest value in [0,1] such that Axk−1+αAs≦b. This test need not be performed for constraint indices i that are in the active set, since they have Asi=0 (or Asi≦0 in iteration 1), and the prior iterate xk−1 is feasible. (note: It is possible to have degenerate constraints not in the active set that have Asi=0, and the ratio test over such constraints would also be skipped.) Thus α is given by




  α
  =
  
    
      min
      
        i
        ∉
        W
      
    
    ⁢
    
      
        
          b
          i
        
        -
        
          
            (
            
              Ax
              
                k
                -
                1
              
            
            )
          
          ⁢
          i
        
      
      
        As
        i
      
    
  




    
    
        Update the guess for the active set in the next iterate.
        
            Add to W the first index iB corresponding to which α achieves its minimum value in the ratio test, the guess for the active indices in the next iterate. The corresponding constraint is loosely referred to as the ‘tightest constraint’.
            If there exist constraints for which the Lagrange multiplier λ* in the EQP (7) is negative, drop the constraint with the most negative multiplier from the active set. This is the so-called steepest edge rule. Bland's Rule is deployed whenever degeneracy is detected in the active set, and in these instances, the first constraint with a negative multiplier is dropped. Details on treatment and detection of degeneracy appear later in this disclosure.
            Special case for first iteration Define a constraint in the initial active set to be consistent if the constraint is binding at xf. If in the first iteration α<1, then all constraints that are not consistent are dropped, irrespective of the sign of their multiplier. This is because if Exf≦r with strict inequality for some components, the updated iterate x1=xf+α(x*−xf) does not satisfy Ex1=r for all constraints in the active set for the next iteration, leading to a discrepancy. This is the consistent active set method (described in earlier patent) For example, if xf is in the strict interior of the feasible set, this could lead to dropping all the constraints and re-starting with an empty active set, thereby allowing little exploitation of ‘hot start’. However, if xf is the set of optimal controls obtained by solving the MPC problem at the prior time step, it is usually consistent with the initial guess for the active set.

The Inconsistent Active Set Method

        
        
    
    


In some cases, the known feasible point is largely inconsistent with the starting guess for the active set. This leads to many inconsistent constraints being dropped from the active set at the end of the first iteration. If the initial hot start guess is a good one, many iterations are wasted in picking these constraints back up. Thus another approach is to retain the inconsistent constraints in the active set and continue the search for the optimum even though the iterate and the active set disagree. This inconsistency disappears if in any iteration α=1. Moreover, the degree of inconsistency reduces at every iteration. The inconsistent method usually allows far more rapid convergence to the optimum than the consistent method.
Update the Iterate

xk=xk−1+αs.

Convergence to the global optimum is achieved when α=1 and λ*≧0, i.e., when the active set does not change.
If the allowed time within the real-time interval is exhausted before convergence is reached, the last updated iterate is returned as the solution.
This invention deals with the issue of retaining as much consistency as possible while hot starting, and exploiting the ‘big-M’ relaxation to keep as many constraints consistent as possible with respect to the iterate.
There is a strong argument in favor of retaining consistency during the iterations, because otherwise the objective function may increase from one iteration to another, and a constraint may not satisfy the right hand side exactly even when it is declared active.
Moreover, with an inconsistent start, if the solution to the first EQP violates any of the constraints, the current point becomes a convex combination of the EQP solution and xf (the initial guess for the feasible solution). Any constraint in the first active set that xf does not satisfy exactly must be dropped if consistency between the current iterate x and the active set were to be retained. This results in potentially promising constraints being dropped from the active set, thus losing the benefit of a good ‘active set hot start’.
This invention corrects for these deficiencies. Below is a description of the innovative approaches:
Let us first describe the “Big-K/M” relaxation needed for handling infeasible starts, described as part of an earlier patent. If the initial guess xf is not feasible, the problem is relaxed by introducing an extra variable t:

Minimize 0.5xTHx+cTx+0.5Kt2+Mt

x,t

subject to Ax−vt<=b

t>=0 (not needed if M=0)

This algorithm does not relax the problem at the very onset, even if xf is infeasible. Instead, it solves the EQP with the initial guess for the active set without relaxing the problem. Call this solution x_eqp1. Very often, this turns out to be the optimal solution, because the initial guess for the active set is the one from the previous time point, and in a large proportion of the QPs, the previous active set is optimal. The method practiced currently could end up relaxing some of these active constraints if xf is infeasible, and thus not arrive at the true optimum despite the fact that the initial guess for the active set is actually optimal.
If this solution to the first EQP, x_eqp1, is not optimal, but nevertheless feasible, we can use this as xf, our guess for the feasible point. This x_eqp1 has the advantage of being a feasible point that is consistent with the initial active set, whereas the xf used in the previous method is not necessarily so.
Honoring critical constraints while maintaining consistency: To further elaborate, let us introduce the notion of ‘critical constraints’. These are constraints that cannot be violated by the solution from the QP solver under any circumstance. It is evident that the set of critical constraints have to be such that a feasible solution exists in the first place. E.g., a set of such critical constraints can be the set of lower and upper limits on actuators, also known as ‘variable bound constraints’ in optimization terminology. (It is a requirement imposed on the guess for the initial feasible point xf that it must not violate any critical constraint.)
Now let us consider the case where the solution to the first EQP, x_eqp1 is neither optimal nor feasible. If it still satisfies the critical constraints we can use x_eqp1 as xf, and relax the non-critical constraints that are violated by introducing the extra variable t. This xf has the advantage of being consistent with the initial active set, while guaranteeing non-violation of the critical constraints. The next set of working constraints is chosen to be the initial working set, minus the constraints for which the multipliers are negative.
If x_eqp1 does violate the critical constraints, the projection of x_eqp1 onto the set of critical constraints is computed (call it xp), and this becomes the new guess for the feasible solution. Now, the elements of v (the vector denoting which constraints should be relaxed) and the value of t are manipulated in such a manner that the point xp is binding for as many of the constraints in the initial active set as possible, thereby retaining consistency of xp w.r.t. the original constraints in the guess for the active set for which xp is not strictly binding.
The logic for the last step can be described as below. For our implementation we have assumed that t=tf=1 in the initial iteration, even though it is not essential.

    
    
        If xp violates a constraint ai*xp<=bi in the initial active set, it is relaxed with a choice of vi>0 such that ai*xp−vi*tf=bi.
    
    


If xp violates a constraint ai*xp<=bi not in the initial active set, it is relaxed with a choice of vi>0 such that ai*xp−vi*tf<bi.
If xp does not violate a constraint it is not relaxed. If it happened to be in the initial active set, and is still binding, it is retained in the active set, and dropped otherwise.
If the number of constraints in the working set after the above procedure is more than the maximum allowed, the least infeasible constraints are dropped from the active set.
In accordance with the provisions of the patent statutes and jurisprudence, exemplary configurations described above are considered to represent a preferred embodiment of the invention. However, it should be noted that the invention can be practiced otherwise than as specifically illustrated and described without departing from its spirit or scope.

## Claims

1. A method for controlling a multivariable system including the steps of:
a) receiving a plurality of sensor signals indicating current conditions of the system;
b) receiving a plurality of commands;
c) determining the desired dynamic response of the system based upon the commands and the sensor signals;
d) in each of a plurality of time steps, formulating a problem of controlling the system to achieve the desired dynamic response as a solution to a quadratic programming problem;
e) solving the quadratic programming problem in each time step using an iterative algorithm which searches for an optimal active set, wherein the active set comprises a set of constraints that are binding at an optimized solution; and
f) in each subsequent time step of the plurality of time steps:
g) solving the quadratic programming problem based on a final active set of a prior time step of the plurality of time steps to obtain xf;
h) initializing a search for the optimal active set based on the final active set of the prior time step of the plurality of time steps and based upon the assumption that xf is feasible.

2. The method of claim 1 wherein said step h) is performed without relaxing any of the active constraints.

3. The method of claim 1 wherein said step h) is performed without relaxing a plurality of the active constraints.

4. The method of claim 3 further including the steps of, in each subsequent time step in said step f):
i) determining that xf is feasible, but not optimal, after said step g) and using xf in said step h) based upon the determination that xf is feasible.

5. The method of claim 1 wherein the constraints include a plurality of critical constraints and a plurality of non-critical constraints, the method further including the steps of, in one of the plurality of subsequent time steps in said step f):
i) determining that xf is not feasible, after said step g);
k) relaxing at least one of the plurality of non-critical constraints based upon the determination that xf is not feasible.

6. The method of claim 5 further including the steps of:
l) determining that xf does not violate the critical constraints; and
m) based upon said step l), using a next set of working constraints as the initial working set, minus the constraints for which an associated Lagrange multiplier is negative.

7. The method of claim 5 further including the steps of:
l) determining that xf violates at least one of the critical constraints; and
m) based upon said step l), using a projection of xf onto the plurality of critical constraints, xp, as a new guess for the solution.

8. The method of claim 7 wherein v is a vector denoting which constraints should be relaxed, and wherein the plurality of non-critical constraints are relaxed in said step k) by introducing an additional variable t, the method further including the steps of:
based upon the determination in said step l) that xp violates at least one of the critical constraints, altering the vector v and the value of t to maximize the number of constraints for which xp is binding in the initial active set.

9. A control system comprising:
a desired trajectory generator for creating a desired trajectory;
a linearization module deriving a linearized model about the desired trajectory;
a quadratic programming module in each of a plurality of time steps, formulating a problem of controlling the system to achieve the desired dynamic response as a solution to a quadratic programming problem;
a quadratic programming solver for solving an optimization problem established by the quadratic programming module to generate a profile of optimal controls, the quadratic programming solver solving the quadratic programming problem in each time step using an iterative algorithm which searches for an optimal active set and in each subsequent time step of the plurality of time steps, the quadratic programming solver in each subsequent time step of the plurality of time steps solving the quadratic programming problem based on a final active set of a prior time step of the plurality of time steps to obtain xf, the quadratic programming solver initializing a search for the optimal active set based on the final active set of the prior time step of the plurality of time steps and based upon the assumption that xf is feasible.

10. The system of claim 9 wherein the quadratic programming solver initializes the search for the optimal active set without relaxing any of the active constraints.

11. The system of claim 9 wherein the quadratic programming solver, in each subsequent time step, determines that xf is feasible, but not optimal.

12. The system of claim 9 wherein the constraints include a plurality of critical constraints and a plurality of non-critical constraints, the quadratic programming solver programmed to, in one of the plurality of subsequent time steps: determine whether xf is feasible, relax at least one of the plurality of non-critical constraints based upon the determination that xf is not feasible.

13. The system of claim 12 wherein the quadratic programming solver is programmed to determine whether xf violates the critical constraints and if not, to use a next set of working constraints as the initial working set, minus the constraints for which an associated Lagrange multiplier is negative.

14. The system of claim 12 wherein the quadratic programming solver is programmed to determine whether xf violates the critical constraints and if so, use a projection of xf onto the plurality of critical constraints, xp, as a new guess for the solution.

15. The system of claim 14 wherein v is a vector denoting which constraints should be relaxed, and wherein the plurality of non-critical constraints are relaxed by introducing an additional variable t, the quadratic programming solver programmed to alter the vector v and the value of t to maximize the number of constraints for which xp is binding in the initial active set based upon the determination that xp violates at least one of the critical constraints.