Method and apparatus for preconditioned predictive control of vehicle movement
10877445 ยท 2020-12-29
Assignee
Inventors
Cpc classification
B25J9/1607
PERFORMING OPERATIONS; TRANSPORTING
G06F17/16
PHYSICS
International classification
G06F17/16
PHYSICS
G06F17/11
PHYSICS
Abstract
A predictive controller controls a vehicle subject to equality and inequality constraints on state and control variables of the vehicle and solves a matrix equation of necessary optimality conditions to produce control inputs to control the vehicle. The controller represents the state variables as a function of the control variables using discrete-time dynamics and determines the control inputs iteratively using two levels of iterations. The first level of iterations selects active inequality constraints, updates a constraint Jacobian matrix with a low-rank update for a change in the set of active inequality constraints to include the active inequality constraints, and updates a preconditioning matrix with a low-rank factorization update, in response to the low-rank update of the constraint Jacobian matrix. The second level of iterations, nested in the first level, solves the matrix equation with the updated constraint Jacobian matrix using the updated preconditioning matrix to produce the control inputs.
Claims
1. A predictive controller for controlling a movement of a vehicle subject to constraints including equality and inequality constraints on state variables and control variables of the vehicle, the predictive controller including a processor configured to execute components of the predictive controller, comprising: an estimator to estimate a current state of the vehicle using measurements of the movement of the vehicle; and a controller to solve, at each control step, a matrix equation of necessary optimality conditions to produce control inputs as values of the control variables in a control solution and to control the movement of the vehicle using the control inputs to change a state of the vehicle, wherein the controller represents the state variables as a function of the control variables using discrete-time dynamics and determines the control inputs iteratively using two levels of iterations including a first level of iterations and a second level of iterations performed within an iteration of the first level of iterations, wherein the first level of iterations, performed until a first termination condition is met, selects active inequality constraints for each point of time within a control horizon, updates a constraint Jacobian matrix of the matrix equation with a low-rank update for a change in the set of active inequality constraints, to include the active inequality constraints, and updates a preconditioning matrix, with a low-rank factorization update, in response to the low-rank update of the constraint Jacobian matrix, wherein the second level of iterations, performed until a second termination condition is met, solves the matrix equation with the updated constraint Jacobian matrix using the updated preconditioning matrix to produce the control inputs.
2. The predictive controller of claim 1, wherein an inequality constraint is active at a point of time when a constraint function of the inequality constraint is equal to a constraint bound of the inequality constraint at the point of time, wherein the first level of iterations selects the set of active inequality constraints based on the current state of the vehicle and a current sequence of control inputs.
3. The predictive controller of claim 2, wherein the first termination condition includes a feasibility of the control solution and an optimality of the control solution, wherein the controller tests the optimality and the feasibility of the control solution and exits the first level of iterations when the first termination condition is met, otherwise, wherein the control solution is not optimal, the controller removes the active inequality constraint having a negative dual variable that prevents optimality of the control solution and repeats the first level of iterations, and otherwise, wherein the control solution is not feasible, the controller updates the current sequence of control inputs with feasible portion of the control solution, adds a blocking inequality constraint in the set of active inequality constraints, and repeats the first level of iterations.
4. The predictive controller of claim 3, wherein the controller, in response to adding the blocking inequality constraint, adds a row in the constraint Jacobian matrix with a first function of coefficients of the blocking inequality constraint and updates the preconditioning matrix with a second function of the coefficients of the blocking inequality constraint.
5. The predictive controller of claim 4, wherein the controller, in response to removing the active inequality constraint, removes a row in the constraint Jacobian matrix that corresponds to the removed active inequality constraint and updates the preconditioning matrix with a reverse of the second function.
6. The predictive controller of claim 1, wherein each change in the set of active inequality constraints corresponds to only one constraint being either added or removed, which leads to a rank-one factorization update for the preconditioning matrix, in response to the rank-one update of the constraint Jacobian matrix.
7. The predictive controller of claim 1, wherein the preconditioning matrix is updated or evaluated using a reduced arithmetic precision, compared to the arithmetic precision that is used for the matrix equation.
8. The predictive controller of claim 1, wherein the preconditioning matrix has a sparsity structure, wherein the controller updates the factorization of the preconditioning matrix while preserving the sparsity structure.
9. The predictive controller of claim 1, wherein the low-rank factorization update is a low-rank update of a Cholesky factorization.
10. The predictive controller of claim 1, wherein, for a current control step, the controller initializes the Jacobian matrix and the factorization of the preconditioning matrix with values of the Jacobian matrix and the factorized preconditioning matrix determined during a previous control step.
11. The predictive controller of claim 1, wherein the preconditioning matrix is an augmented Lagrangian block-diagonal matrix, and wherein the second level of iterations solves the matrix equation using a minimal residual method.
12. The predictive controller of claim 1, wherein the preconditioning matrix is a Schur complement block-diagonal matrix, and wherein the second level of iterations solves the matrix equation using a minimal residual method.
13. The predictive controller of claim 12, wherein the controller regularizes a Hessian of the matrix of the matrix equation to produce a positive definite approximation of the Hessian of the matrix, and wherein the controller determines elements of the Schur complement block-diagonal matrix based on the approximation of the Hessian of the matrix.
14. The predictive controller of claim 1, wherein the preconditioning matrix is a constraint preconditioner, and wherein the second level of iterations solves the matrix equation using a projected conjugate gradient method.
15. The predictive controller of claim 14, wherein the controller regularizes a Hessian of the matrix of the matrix equation to produce a positive definite approximation of the Hessian of the matrix, and wherein the controller determines elements of the constraint preconditioner based on the approximation of the Hessian of the matrix.
16. The predictive controller of claim 1, wherein the control inputs to the vehicle includes one or a combination of a velocity of the vehicle, an acceleration of the vehicle, a torque of one or more engines of the vehicle, a torque of one or more electric propulsion motors of the vehicle, a steering angle of one or more wheels of the vehicle, a torque of a power steering system of a vehicle, and a driving torque of one or more wheels of the vehicle.
17. A vehicle including the predictive controller of claim 1.
18. The vehicle of claim 17, further comprising: a low-level controller to accept the control inputs form the predictive controller to control at least one actuator of the vehicle, wherein the low-level controller includes one or combination of a steering controller to control a steering of the vehicle, a brake controller to control deceleration of the vehicle, a throttle controller to control an acceleration or a propulsion torque of the vehicle.
19. The vehicle of claim 17, further comprising: a motion planner configured to submit a desired motion command to the predictive controller, such that the control inputs determined by the predictive controller cause the vehicle to follow the desired motion command.
20. A method for controlling a movement of a vehicle subject to constraints including equality and inequality constraints on state and control variables of the vehicle, wherein the method uses a processor coupled with stored instructions implementing the method, wherein the instructions, when executed by the processor carry out steps of the method, comprising: estimating a current state of the vehicle using measurements of the movement of the vehicle; solving, at each control step, a matrix equation of necessary optimality conditions to produce control inputs as values of the control variables in a control solution; and controlling the vehicle using the control inputs to change a state of the vehicle, wherein the first level of iterations, performed until a first termination condition is met, selects active inequality constraints for each point of time within a control horizon, updates a constraint Jacobian matrix of the matrix equation with a low-rank update for a change in the set of active inequality constraints, to include the active inequality constraints, and updates a preconditioning matrix, with a low-rank factorization update, in response to the low-rank update of the constraint Jacobian matrix, wherein the second level of iterations, performed until a second termination condition is met, solves the matrix equation with the updated constraint Jacobian matrix using the updated preconditioning matrix to produce the control inputs.
Description
BRIEF DESCRIPTION OF THE DRAWINGS
(1)
(2)
(3)
(4)
(5)
(6)
(7)
(8)
(9)
(10)
(11)
(12)
(13)
(14)
(15)
(16)
(17)
(18)
(19)
(20)
(21)
(22)
(23)
(24)
DETAILED DESCRIPTION
(25) Some embodiments of the invention provide a system and a method for controlling an operation of a system or a system using a predictive controller. AN example of the predictive controller is a model predictive control (MPC) determining control inputs based on a model of the controlled system.
(26)
(27) The system 120, as referred herein, can be any machine or device controlled by certain manipulation input signals 111 (inputs), possibly associated to physical quantities such as voltages, pressures, forces, torques, and to return some controlled output signals 103 (outputs), possibly associated to physical quantities such as currents, flows, velocities, positions indicative of a transition of a state of the system from a previous state to the current state. The output values are related in part to previous output values of the system, and in part to previous and current input values. The dependency on previous inputs and previous outputs is encoded in the state of the system. The operation of the system, e.g., a motion of components of the system, can include a sequence of output values generated by the system following the application of certain input values.
(28) A model of the system 102 can include a set of mathematical equations that describe how the system outputs change over time as functions of current and previous inputs, and the previous outputs. The state of the system is any set of information, in general time varying, for instance an appropriate subset of current and previous inputs and outputs, that, together with the model of the system and future inputs, can uniquely define the future motion of the system.
(29) The system can be subject to physical limitations and specification constraints 104 limiting the range where the outputs, the inputs, and also possibly the states of the system are allowed to operate.
(30) The controller 110 can be implemented in hardware or as a software program executed in a processor, e.g., a microprocessor, which at fixed or variable control period sampling intervals receives the estimated state of the system 121 and the desired motion command 101 and determines, using this information, the inputs, e.g., the control signal 111, for operating the system.
(31) The estimator 130 can be implemented in hardware or as a software program executed in a processor, either the same or a different processor from the controller 110, which at fixed or variable control period sampling intervals receives the outputs of the system 103 and determines, using the new and the previous output measurements, the estimated state 121 of the system 120.
(32)
(33)
(34) In some embodiments, the solution of this inequality constrained optimization problem 350 uses the state and control values over the prediction time horizon from the previous control time step 310, which can be read from the memory. This concept is called warm- or hot-starting of the optimization algorithm and it can considerably reduce the required computational effort of the MPC controller in some embodiments. In a similar fashion, the corresponding solution vector 355 can be used to update and store a sequence of optimal state and control values for the next control time step 360.
(35)
(36) In some embodiments, e.g., based on a linear time-invariant (LTI) dynamical model and time-invariant constraints of the system, only the QP vectors 347 can change from one control time step to the next and the QP matrices 346 are constant instead. In this linear time-invariant case, some embodiments can use hot-starting of the optimization algorithm, based on the reuse of matrix factorizations that are related to the constant QP matrices in order to reduce the required computational effort of the MPC method at each control time step. In other embodiments, given a time-varying description of the dynamical model and/or constraints and limitations of the system, both the QP vectors 347 and the QP matrices 346 can change from one control time step to the next.
(37) Some embodiments are based on recognition that The second order optimization algorithm requires repeatedly solving a matrix equation Kz=b with the varying matrix K and the right-hand side vector b. However, iterative solvers tend to converge poorly without preconditioning. To that end, there is a design an efficient preconditioning matrix P to solve P.sup.1Kz=P.sup.1b with computational cost O(Nm.sup.2) per iteration.
(38) In general, computing a preconditioning matrix P can be a computationally demanding task. However, some embodiments are based on realization, that under some circumstances, the preconditioning matrix P can be replaced with a number of matrix updates. Moreover, using some sparse structure of the matrices, those multiple updates are of low rank, which leads to the desired computational cost of O(Nm.sup.2).
(39) Specifically, it is realized that the system control is subject to equality and inequality constraints on the states and control inputs of the system, while the computational complexity is mostly due to inequality constraints. At different times, different inequality constraints can be active (potentially violated) or inactive. The inactive inequality constraints can be ignored, while the active inequality constraints can be replaced with equality constraints forming an optimization problem subject only to equality constraints.
(40) Replacement of the active inequality constraints with equality constraints, results in an update of the matrix K and preconditioning matrix P for each active-set change. Thus, the computational demand lies not only in constructing the preconditioner P, but mostly in updating the matrix P in response to adding or removing a constraint from the current active set. In addition, replacement of the active inequality constraints with equality constraints, results in a KKT matrix K that includes a block-diagonal Hessian and a block-sparse Jacobian matrix. The matrix P can also be formed to have a particular block sparsity structure that is computationally cheaper to form, update, and to factorize than a dense matrix.
(41) In such a manner, it is realized that a change to the current active set results in a low-rank update to the block-sparse Jacobian matrix. In this context, an update to a matrix is considered to be of low rank when the rank of the difference between the original and the updated matrix is small, relatively to the dimensions of the matrix. As a special case, it is realized that adding or removing only one constraint to or from the current active set, results in a rank-one update to the block-sparse Jacobian matrix. In the case that exactly two constraints are either added or removed to or from the current active set, this results in a rank-two update to the block-sparse Jacobian matrix. Despite the fact that the preconditioner is a function of the entire KKT matrix K, the low-rank update of the constraint Jacobian matrix leads to a low-rank update of the entire preconditioner.
(42) To that end, some embodiments use active-set optimization with preconditioned iterative solver while preserving the sparsity structure of the preconditioning matrix P, e.g., a block-tridiagonal structure.
(43)
(44) The first level of iterations 375 is performed until a first termination condition is met, e.g., a converging criteria and/or a number of iterations is reached. The purpose of the first level of iterations is to update constraint Jacobian matrix and preconditioning matrix. For example, an iteration of the first level selects 385 active inequality constraints for each point of time within a control horizon, updates 386 the constraint Jacobian matrix, with a rank-one update for a single change in the set of active inequality constraints, to include the equality constraints and the active inequality constraints, and updates 387 a preconditioning matrix, with a rank-one factorization update, in response to the rank-one update of the constraint Jacobian matrix. In the case of multiple changes to the set of active inequality constraints, the low-rank update to the constraint Jacobian matrix 386 leads to a low-rank factorization update to the preconditioning matrix 387.
(45) The second level of iterations solves 377 the matrix equation with the updated constraint Jacobian matrix using the updated factorization of the preconditioning matrix to produce the control solution 380. The solution is found iteratively until a second termination condition is met, such as an optimality of the solution criteria and/or a number of iterations. If the control solution 380 is found to be optimal, the solution is used to control the system. Otherwise, the first and the second levels of iterations are repeated.
(46)
(47) In each iteration of an active-set method, the method solves the equality constrained QP that corresponds to the current guess of active inequality constraints 405. The required computational effort for the solution of an equality constrained QP is cheaper than the original solution of the inequality constrained QP 350. In case the resulting new control solution guess is both optimal and feasible 406, then the optimal solution 355 to the original inequality constrained QP 350 is found. If this is not the case, but one of the Lagrange multipliers corresponding to the set of active inequality constraints is negative 407, then the corresponding inequality constraint(s) can be removed from the current guess of the active set 410. Finally, if the new solution guess is not optimal and feasible but all Lagrange multipliers corresponding to the active inequality constraints are positive, then a blocking inequality constraint can be identified in order to compute a new primal feasible solution guess 415. This blocking inequality constraint(s) can then be added 420 to the current guess of the active set of inequality constraints and the optimal control solution guess can be updated accordingly 422.
(48) In some embodiments, the iterations of the active-set method continue until a termination condition is met, e.g., an optimal and feasible control solution is found 355 or the maximum number of iterations is reached 402. Note that the search direction (, ), that is obtained as the solution of the equality constrained QP in each active-set iteration 405, results in a family of candidate vectors + that are primal feasible with respect to the equality constraints and the inequality constraints that are contained in the active set, regardless of the value for . Therefore, some embodiments identify the most blocking constraint as the inequality constraint for which the step length >0 is the largest, such that the blocking inequality constraint is satisfied exactly as an equality constraint 421 and all other inequality constraints are satisfied too.
(49) In case that at least one of the Lagrange multipliers corresponding to the active inequality constraints is negative 407, then some embodiments remove the inequality constraint that corresponds to the most negative Lagrange multiplier from the current active set 410. However, some embodiments can alternatively decide to remove a different constraint, corresponding to a negative Lagrange multiplier, from the active set or some embodiments can decide to remove multiple constraints from the active set at once 410. Similarly, some embodiments can add multiple inequality constraints at once to the active set 420 in one iteration. And some embodiments reverse the order between deciding to remove 410 or add 420(a) constraint(s) from/to the active set, i.e., such embodiments first try to identify the block inequality constraint 415 before checking for negative Lagrange multipliers corresponding to the active inequality constraints 407.
(50)
(51) In some embodiments, the presence of state-dependent inequality constraints can prevent the initialization procedure from
(52)
(53)
(54) As shown in
(55)
(56) For a dense nn matrix, a matrix factorization generally has a computational complexity of O (n.sup.3), while a rank-one update to a precomputed matrix factorization corresponds to a computational complexity of only O (n.sup.2). In the case of a low-rank factorization update, the computational complexity is also O (n.sup.2), as long as the rank is small compared to the dimension n of the nn matrix. A low-rank factorization update can be carried out either directly or in the form of a sequence of rank-one factorization updates. In general, low-rank updates to matrix factorizations are therefore of a lower order computational complexity than the computation of a new matrix factorization after each update of the active set.
(57)
(58) Some embodiments are based on the realization that the linear system 707, that needs to be solved, has the particular saddle point KKT structure 706. More specifically, the matrix H in the (1,1)-block corresponds to the block-diagonal Hessian in the KKT matrix 708. Some embodiments rely on the property that each of the Hessian block matrices individually is positive (semi-)definite 708. In addition, the matrix A in the (2,1)-block, and its transpose A.sup.T in the (1,2)-block of the KKT matrix 706, defines the block-structured constraint Jacobian matrix in the saddle point linear KKT system. Some embodiments rely on the property that the constraint Jacobian is a matrix of full rank, i.e., each of the rows in the matrix A need to be linearly independent. Embodiments of this invention that are based on a primal feasible active-set method, as described in
(59)
(60)
(61) Each iteration of the solver requires at least one matrix-vector multiplication with the exact coefficient matrix K 805, which can be executed efficiently by exploiting the optimal control structured block-sparsity as shown in
(62) The overall computational cost for solving each saddle point linear system is proportional to the number of iterations. A stopping criterion 830 checks if the current approximation z satisfies a given tolerance or whether the maximum number of iterative steps k=k.sub.max is reached. If this stopping criterion is satisfied, then the solution z* is found 835, otherwise the approximation z is updated 840 and the iterations continue 845. Some examples of the iterative method are the preconditioned generalized minimal residual (PGMRES) method, the preconditioned minimal residual (PMINRES) and a projected preconditioned conjugate gradient method (PPCG).
(63) In some embodiments, an initial approximate solution z.sub.0 810 is used to start the iterative procedure for the solution of the saddle point linear system. This approximate solution guess can be based on the solution to the previous saddle point linear system 605, given the active-set change from one iteration of the active-set method to the next 601. Alternatively, some embodiments compute the change of the primal and dual variables in the saddle point linear system 706, such that a reasonable solution guess consists of zero values instead.
(64) Some embodiments determine or update 820 at least a part of a preconditioning matrix using either the exact coefficient matrix K 801 or an approximate coefficient matrix 815. The preconditioner can first be generated and then applied, e.g., in every step of the main iterative cycle 825. In some embodiments, the preconditioner is related to a preconditioning function that is generated 820 outside of the main iterative cycle, wherein the preconditioning function can be applied to a vector in order to return the product between the matrix inverse of the preconditioner P.sup.1 and that specific vector 825. For example, in some embodiments of the iterative method, the preconditioning function acts on the residual bKz in order to compute P.sup.1(bKz). Application of the preconditioner accelerates the convergence of the iterative method without significantly increasing the computational costs per iteration, thus, leading to an overall improved performance of the MPC controller.
(65) In some embodiments of this invention, a block-structured preconditioner is used such that its matrix factorization can be efficiently determined, updated 820 and applied 825. The block-structured matrix factorization therefore requires a computational cost of complexity O(Nm.sup.3), where N is the control horizon length and in is the number of state and control variables. In some embodiments, the computation of this matrix factorization, which is needed for the solution of each saddle point linear system, is however avoided by instead updating the matrix factorization for the preconditioner from one iteration of the active-set method to the next iteration 615. In case only one constraint is either added or removed to/from the active set in each iteration, such an update corresponds to a rank-one factorization update of complexity O(Nm.sup.2) that preserves the block-structured sparsity of the preconditioning matrix. Similarly, applying the inverse of the preconditioner P.sup.1 to a vector, given this block-sparse matrix factorization, requires a computational cost of complexity O(Nm.sup.2).
(66) If an approximate inverse of an approximate coefficient matrix is used for the matrix form of the preconditioner, i.e., P.sup.1K.sup.1, the number of iterations in the preconditioned iterative method is radically reduced. In an exact case, where the preconditioner 820 at a given time step coincides with the inverse of the exact coefficient matrix 801, i.e., P.sup.1=K.sup.1, the number of iterations in the preconditioned method is reduced to one. However, the exact case can be computationally expensive, thus, some embodiments are based on a recognition that, for the purpose of the preconditioning setup or update 820 and application 825, an approximate coefficient matrix 815 can be easier and faster to factorize, compared to their exact counterparts, while still leading to high-quality preconditioning. Similarly, determining, updating or applying the preconditioner 820 does not require full accuracy and can be done using a reduced number of arithmetic bits on the computer-based controller 110. Another embodiment can determine and/or apply the factorization of the preconditioner in the low precision arithmetic from the already computed approximate coefficient matrix.
(67)
(68)
floating point operations (FLOPS) in case P is a nn matrix, i.e., the computational complexity is O(n.sup.3). Instead, given a low-rank update to the preconditioning matrix 616, (a) rank-one update(s) of the Cholesky factorization of the matrix can be carried out with a computational complexity of O(n.sup.2) instead. The application of the preconditioner function 910 includes the following steps. First, the matrix equation L {tilde over (z)}32 b is solved by forward substitution 915, using the fact that the matrix L is lower-triangular. Then, the matrix equation L.sup.T z={tilde over (z)} is solved by back substitution 920, using the fact that the matrix L.sup.T is upper-triangular. Both the forward and backward substitution, using the Cholesky factors L and L.sup.T, can be carried out with a computational complexity of O(n.sup.2). Finally, the result of applying the preconditioner is determined 925 as P.sup.lb=(L L.sup.T).sup.1b=z.
(69)
(70) A Cholesky factorization P=L L.sup.T for ann matrix can be recovered in O(n.sup.2) computations after a rank-one modification {tilde over (P)}=P+.sup.T. This is referred to as an update if >0 and as a downdate if <0. In case of a block-tridiagonal Cholesky factorization, the particular block-sparsity structure of the Cholesky factors can be preserved as long as the matrix remains block-tridagonal after the rank-one modification {tilde over (P)}=P+.sup.T. As mentioned earlier, the computational complexity is O(Nm.sup.3) for the block-tridiagonal Cholesky factorization and O(Nm.sup.2) for the rank-one update based on a rank-one modification of the block-tridiagonal matrix. Note that also the insertion or deletion of a row-column pair in the symmetric matrix P corresponds to a particular type of a rank-one modification and the Cholesky factorization can be updated at the same computational complexity of O(Nm.sup.2). Some embodiments are based on a backward or reverse variant of the block-tridiagonal Cholesky factorization, which has particular advantages when performing the rank-one factorization updates in the context of an MPC controller, given the fact that most of the active-set changes occur during the time intervals earlier in the control prediction horizon.
(71)
(72)
(73) Given an update to the active set, where either an inequality constraint is added or removed 611, a rank-one update is computed for the Cholesky factorization of the preconditioning matrix while preserving the block-tridiagonal sparsity structure. More specifically, for the augmented Lagrangian type preconditioner 1011, adding or removing a constraint results in adding or removing a corresponding row and column of the symmetric weighting matrix =I since its dimensions correspond to the total number of equality constraints and active inequality constraints. In addition, adding or removing a constraint from the active set, corresponds to adding or removing a row in the constraint Jacobian A of the KKT matrix 706, which results in a rank-one update or downdate for the block-tridiagonal Cholesky factorization of the matrix H+A.sup.TA.
(74) For the specific embodiment in which =I, the inverse
(75)
is known such that the block-tridiagonal Cholesky factorization only needs to be computed and updated for the block matrix H+A.sup.TA 1022. The computational cost for computing or updating this factorization is, respectively, O(Nm.sup.3) or O(Nm.sup.2) where N is the control horizon length and in is the number of states and controls.
(76)
(77)
(78) In some embodiments, the Hessian matrix for the MPC problem formulation is a diagonal matrix 1125, such that the Hessian regularization H{tilde over (H)}>0 can be done efficiently, e.g., by approximating each of the diagonal elements such that they are individually larger than some positive value >0. In general, the Hessian approximation {tilde over (H)}H becomes closer for smaller values of >0 such that the iterative solver converges faster, but should be chosen sufficiently large in order to avoid ill-conditioning of the preconditioning matrix P.sub.s. In other embodiments, where the Hessian matrix for the MPC problem formulation is a general block-diagonal matrix with dense Hessian blocks, then the Hessian regularization can, for example, be performed heuristically or based on a factorization of the block matrices. Alternatively, a Riccati-based recursion can be performed to make the Hessian matrix positive definite in the range space of the system dynamics and/or of the active inequality constraints. However, such techniques generally require a computational complexity of O(Nm.sup.3), which is the same complexity as that of the block-tridiagonal Cholesky factorization itself.
(79) Given an update to the active set, where either an inequality constraint is added or removed 611, a rank-one update is computed for the Cholesky factorization of the preconditioning matrix while preserving the block-tridiagonal sparsity structure. More specifically, for the Schur complement type preconditioner 1111, adding or removing a constraint does not affect the Hessian matrix or its approximation {tilde over (H)}H. However, adding or removing a constraint from the active set, corresponds to adding or removing a row in the constraint Jacobian A of the KKT matrix 706, which results in adding or removing a row-column pair in the block matrix A{tilde over (H)}.sup.1A.sup.T 1126 with a block-tridiagonal sparsity structure 1127. This insertion or deletion then results in a rank-one update or downdate for the block-tridiagonal Cholesky factorization of the matrix A{tilde over (H)}.sup.1A.sup.T. Note that this update to the matrix A{tilde over (H)}.sup.1A.sup.T can be computed easily for a diagonal Hessian matrix {tilde over (H)}H but, even for a general block-diagonal Hessian matrix, this update can be computed effectively, e.g., based on a Cholesky factorization for the block matrices in the positive definite Hessian approximation H{tilde over (H)}>0.
(80)
(81) In the case where the exact Hessian matrix is invertible and the constraint preconditioner is based on the approximation {tilde over (H)}H, the resulting PPCG implementation corresponds to a direct linear system solution such that no more than one iteration is carried out in theory. In practice, however, multiple conjugate gradient iterations are often still necessary because of the use of a limited number of arithmetic bits on the computer-based controller 110, because of an inexact application of the constraint preconditioner and/or ill-conditioning of the problem data for practical MPC formulations.
(82)
(83) Note that the relatively high cost with complexity O(Nm.sup.3) for carrying out a block-tridiagonal Cholesky factorization can be avoided in the online computations of the MPC controller by performing a hot-start procedure. In some embodiments, based on a linear time-invariant MPC formulation, the Cholesky factorization is then reused from one control time step to the next, which is often referred to as a hot-starting of the active-set method. For this purpose, the factorization needs to be updated based on the active-set changes from the control solution at the previous control time step to the initial feasible starting point of the active-set optimization algorithm during the next control time step.
(84)
(85) The vehicle can also include an engine 1306, which can be controlled by the controller 1302 or by other components of the vehicle 1301. The vehicle can also include one or more sensors 1304 to sense the surrounding environment. Examples of the sensors 1304 include distance range finders, radars, lidars, and cameras. The vehicle 1301 can also include one or more sensors 1305 to sense its current motion quantities and internal status. Examples of the sensors 1305 include global positioning system (GPS), accelerometers, inertial measurement units, gyroscopes, shaft rotational sensors, torque sensors, deflection sensors, pressure sensor, and flow sensors. The sensors provide information to the controller 1302. The vehicle can be equipped with a transceiver 1306 enabling communication capabilities of the controller 1302 through wired or wireless communication channels.
(86)
(87) The above-described embodiments of the present invention can be implemented in any of numerous ways. For example, the embodiments may be implemented using hardware, software or a combination thereof. When implemented in software, the software code can be executed on any suitable processor or collection of processors, whether provided in a single computer or distributed among multiple computers. Such processors may be implemented as integrated circuits, with one or more processors in an integrated circuit component. Though, a processor may be implemented using circuitry in any suitable format.
(88) Also, the various methods or processes outlined herein may be coded as software that is executable on one or more processors that employ any one of a variety of operating systems or platforms. Additionally, such software may be written using any of a number of suitable programming languages and/or programming or scripting tools, and also may be compiled as executable machine language code or intermediate code that is executed on a framework or virtual machine. Typically, the functionality of the program modules may be combined or distributed as desired in various embodiments.
(89) Also, the embodiments of the invention may be embodied as a method, of which an example has been provided. The acts performed as part of the method may be ordered in any suitable way. Accordingly, embodiments may be constructed in which acts are performed in an order different than illustrated, which may include performing some acts concurrently, even though shown as sequential acts in illustrative embodiments.
(90) Although the invention has been described by way of examples of preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the invention. Therefore, it is the object of the appended claims to cover all such variations and modifications as come within the true spirit and scope of the invention.