Bounded iteration for multiple box constraints on linear complementarity model predictive control and its application to vehicle steering control

ABSTRACT This paper presents linear model predictive control (MPC) for multiple kinds of constraint based on the linear complementarity problem (LCP) that gives the explicit upper bound of computational complexity. MPC generally solves constrained optimization problems. Its computational time should be strictly bounded for real-time applications. In a previous study, we proposed MPC based on the LCP for which a modified n-step vector successfully limits the number of iterations for the combinatorial problem. However, its class of applications is limited due to the existence of the modified n-step vector. In addition, MPC with a time-varying system is not included in this class since the modified n-step vector must be found for each problem at the corresponding time instance. This paper introduces a perturbation on constraints and applies a sequential LCP algorithm that gives a priori knowledge of the explicit upper bound of computational complexity and the accuracy of the solution. The iteration bounds are evaluated using the steering control of an autonomous driving vehicle for an obstacle avoidance manoeuvre.


Introduction
Model predictive control (MPC) is commonly used in industrial applications, including autonomous driving systems [1][2][3][4].However, its online optimization demands high computational resources that require the upper bound of computational complexity to be determined for application to mass production.
Several studies have explicitly guaranteed the upper bound of computational complexity.Saito et al. [5] proposed a linear complementarity problem (LCP)-based solver for box-constrained MPC problems that reaches the solution within 3mN pivoting iterations, where m is the input dimension and N is the number of prediction horizons.Pivoting algorithms [6][7][8] that focus on the LCP do not give an explicit upper bound for the number of iterations.Wright et al. [9] presented an LCP-based algorithm that utilizes the interior point method, but they did not provide the number of iterations.Okawa et al. [10] presented a method that computes a quasi-optimal solution and can estimate the upper bound of computational complexity.They later proposed an LCP-based pivoting algorithm [11] with a bounded iteration method for box-constrained MPC problems.This algorithm can obtain the exact solution and complete an iteration within the number of points where the box constraint is imposed.Its computation speed is competitive with that of other MPC algorithms.However, the algorithm is limited to the class of problems for which time-invariant linear programing is feasible.Stine and Adegbege [12] proposed an LCP solver that is less limited than that in [11], but their solver does not meet the application conditions of Algorithm 1 (see below), unlike the solver described in the present study.Thus, there are no guarantees on the upper bound of computational complexity.
This paper presents an algorithm with a predetermined finite number of iterations for MPC with multiple kinds of constraint.This algorithm removes the limitation of our previous algorithm [11] and provides an explicit trade-off relation between computational complexity and accuracy.In addition, the conditions imposed in [11] are relaxed based on [13] to deal with the time-varying problem.We apply the proposed algorithm to the steering control of an autonomous driving vehicle for an obstacle avoidance manoeuvre to evaluate its computational cost.
In our previous study [14], we experimentally demonstrated the applicability of the proposed method to autonomous driving.However, we did not present the theoretical development and specific numerical values for estimating the upper bound of computational complexity.Here, we present the theory of the proposed method and show through simulations that the computational cost can be estimated under the given assumption.

Model predictive control formulation
be the state and input, respectively.With the coefficient matrices set to A ∈ R n×n and B ∈ R n×m , the discretetime linear system can be expressed as follows: where k is the time step.By denoting the reference state as x t , the deviation from the reference state is expressed as e := x t − x.Then, the evaluation function J is formulated as follows: where N is the number of horizons, Q is the semidefinite symmetric weight matrix, and R is the positive definite symmetric weight matrix.P is the solution for the discrete-time algebraic Riccati equation for A, B, Q, and R, which guarantee system stability.In addition, the box constraints on the states and inputs are set as

Quadratic programing problem formulation
The linear MPC is transformed into a quadratic programing (QP) problem.First, the state X ∈ R nN , the reference state X t ∈ R nN , the input U ∈ R mN , and the matrices Ā ∈ R nN×n and B ∈ R nN×mN are respectively defined as where x t, * is the reference state at each time step k.
From (1), the following equality condition is obtained for X and U: With this equality condition and the removal of the constant term, (2) is transformed into where R := diag(R, . . ., R). (13) By lumping the box constraints for each prediction point, (3) is rewritten as follows: where Note that X u is a vector of upper constraints and X l is a vector of lower constraints.These vectors are composed of the state and input constraints at each prediction point, namely x u k and u u k for X u and x l k and u l k for X l .Āc and Bc are submatrices of Ā and B, respectively.Here, the size of X u and X l , the number of all box constraints, is defined as L. Hence, MPC is transformed into the following QP problem.minimize J subject to (14)

Linear complementarity problem formulation
The QP problem is transformed into an LCP under the Karush-Kuhn-Tucker (KKT) conditions.Optimizing the evaluation function in (9) in terms of the decision variable U under the inequality constraint in (14) gives the following relationship.
where λ * is the Lagrange multiplier.Furthermore, from (16) and the KKT conditions, we obtain where z ∈ R 2L , M ∈ R 2L×2L , and q ∈ R 2L .Hence, the QP problem can be expressed as the following LCP.

Previous study[11]
In this section, we introduce the n-step pivoting scheme, which requires the existence of a special parameter vector called an n-step vector, and describe how to apply the scheme to MPC with single-box constraints.
Assumption 3.1: It is assumed that the problem is a linear MPC problem with state box constraints, input box constraints, or mixed box constraints, where the upper and lower constraints are not all activated at a given time step k.Here, B is assumed to be full-rank.
Since H is a positive definite symmetric matrix, the matrix M is a P-matrix.The definitions of an Mmatrix, a P(P 0 )-matrix, a Q-matrix, an S-matrix, and a Z-matrix are given in [11].

Definition 3.2:
A positive vector p∈R is called an nstep vector for M if it satisfies the following relationship for all index sets α 1 .
The n-step pivoting scheme is an efficient method for solving an LCP that focuses on the structure of the problem.
Algorithm 1 n-step pivoting scheme [15] Require: M, q, p Ensure: The scheme is applicable if there is an n-step vector p for M.
Since the method for searching for n-step vectors proposed by Pang et al. [13] requires M to be an Smatrix, it is difficult to apply it to MPC that does not satisfy Assumption 3.1.[11] introduced a modified nstep vector and removed the S-matrix condition.Definition 3.3: A vector p is called a modified n-step vector for M if the positive vector p∈R exists for every index set α such that M αα is a P-matrix that satisfies the following relationship.11,13]): Let M be a square matrix and X and Y be Z-matrices that satisfy Then, if a positive vector p that satisfies exists, it is a modified n-step vector for M.
However, the domain of the modified n-step vector is problem-dependent and strictly limited in terms of the number of prediction points and prediction time.In addition, multiple box constraints cannot be considered simultaneously due to the assumption that B is full-rank [11].

Extension to multiple box constraints
The method described in Section 3 cannot deal with multiple box constraints at a given time step k due to Assumption 3.1.In the present section, we focus on another property of the n-step vector and extend the n-step scheme to MPC with multiple box constraints.Theorem 4.1: [16] If the LCP matrix M has positive diagonal entries and is strictly row-diagonally dominant, then where the n-step vector p is given in the form This theorem indicates that the (row) diagonal dominance of M is a sufficient condition for the existence of an n-step vector.Based on this property, the matrix is perturbed as follows: where ε is a positive scalar and D is a positive diagonal matrix.Since (25) holds for sufficiently large ε > 0, M has an n-step vector and Algorithm 1 is applicable.
Although LCP( M, q) is different from LCP(M, q), an algorithm exists for solving the original LCP (it solves the perturbed problem LCP( M, q) sequentially) [15].
(28) Here, (28) is rewritten as the following recursive formula: Algorithm 2 Sequential LCP Require: , , q Ensure: λ * if q ≥ 0 then Terminate and output the solution The convergence condition for Algorithm 2 is derived from the following theorem.

Theorem 4.2 ([15]):
If the symmetric nondegenerate matrix M is separated into Q-matrix and , and − is positive definite, the vector λ k generated by Algorithm 2 for q and an arbitrary initial vector λ 0 ≥0 converges to a solution for LCP(M, q).Based on Theorem 4.2 and (27), the matrix M is divided into = M + εD and = −εD.Although M is a P 0 -matrix, not a P-matrix as assumed in Assumption 3.1, is still a P-matrix.In addition, since M is a semi-definite symmetric matrix, − = M + 2εD is a positive definite matrix.Furthermore, for MPC with soft constraints, M is a nondegenerate matrix 2 that satisfies Theorem 4.2.Therefore, the evaluation function is represented as follows by redesigning the MPC with soft constraints. with where S x and S u are positive definite diagonal weight matrices.v x and v u are the constraint violations of the state and input, respectively, at each prediction point.The superscripts u and l represent the upper and lower bounds, respectively, and the subscripts x and u are the state and the input, respectively.To transform the MPC into an LCP, the following vectors that compose After the constant term is removed, the evaluation function and constraints become By converting the MPC to an LCP as done in Section 2, we obtain Thus, the matrix in (36) is the sum of the positive semi-definite matrix in (18) and the positive definite diagonal matrix S−1 .Hence, the matrix M s is positive definite and nondegenerate, and thus Algorithm 2 is proved to be feasible.In this way, Algorithm 1 is applicable to MPC with multiple soft box constraints through Algorithm 2.

Time-varying systems
Since B has the system matrix B as a submatrix, if B is time-varying, then M s that includes B is also timevarying.Furthermore, since n-step vectors must be found for each M s , the n-step vector may change for a time-variant M s .To deal with time-varying systems, the problem is perturbed while ensuring the existence of the n-step vector.
Theorem 5.1: Suppose that the matrices X and MX are Z-matrices.Then, the vector e is an n-step vector for M if X satisfies e X≥e . (37) Proof: Under (24), M is a Z-matrix if there exist positive vectors r and s that satisfy the following inequality condition [13].
Transposing (38) and setting r = e and s = 0, the following sufficient condition is derived.
where e is a positive vector.From (24), this means that set p = e.Since (39) is invariant for row scaling, the following relation is derived with a positive diagonal matrix .

X e≥e, (40)
where the diagonal elements of are selected to be sufficiently large.If X is a Z-matrix, X is also a Zmatrix.Therefore, X can replace X, and we obtain Theorem 5.1.
Hence, by replacing (24) with (37) and searching for a problem that contains the n-step vector p = e, we can treat n-step vectors as a time-invariant problem.In other words, for dealing with time-variant systems, we only need to find ε such that n-step vectors exist for each time-variant quantity.

Estimation of computational cost
To guarantee the maximum computational cost, we refer to the convergence property of Algorithm 2. From (29), = M s + εD and = −εD, and thus we obtain Here, the upper bound of the norm of the difference between ( 17) and ( 41) is defined as Hereafter, the norm in the equation is assumed to be the 2-norm.The upper bound of λ k+1 − λ k at the k-th iteration is given by the following Lemma.
Applying the norm and quadratic form properties to (45) yields 3 By rearranging (46), we obtain (43) and the proof is completed.
From Lemma 6.1, the maximum number of iterations can be derived if λ 0 and λ 1 are determined.In this study, we assume that λ 0 = 0. Based on this assumption, the Lemma for determining λ 1 is given as follows.
Lemma 6.2:With Algorithm 2 applied to the perturbed problem (41), λ k at the k-th iteration satisfies where (−q) + means the vector in which the negative elements of −q are replaced by zero.
Proof: From (41) and the complementarity condition, we obtain Here, transforming q and focusing on the non-negative condition in (21), we obtain Then, applying the properties of the norm and quadratic forms, the following relationship is derived.
Next, the theorem on the maximum number of iterations is derived with (51).Theorem 6.4:With Algorithm 2 applied to the perturbed problem (41) under the assumption λ 0 = 0, the maximum number of iterations k max for d < δ is determined in advance as follows: where δ is the accuracy of the solution 4 .
Proof: From (42) and (51), we obtain Taking the natural logarithm of both sides yields (55) Since d converges to 0 when Theorem 4.2 holds, we get σ max (εD)/σ min (M s + εD) < 1 and obtain (56) Then, by rearranging for k, we obtain (57) shows the minimum number of iterations k required to satisfy d < δ.In other words, k indicates the number of iterations required to obtain a solution with the designed accuracy, and we obtain (53).
Remark 6.1:The fact that ( 17) is z = q at λ 0 = 0 implies that the optimal trajectory is derived via a modification of the optimal trajectory without constraints using λ.Therefore, the maximum (−q) + is the constraint violation amount of the optimal trajectory without constraints.However, for some MPC problems, it might be difficult to estimate (−q) + because q is time-varying.Thus, we take a sufficiently large (−q) + for the problem.Figure 1 shows an example of the geometric interpretation of (−q) + via obstacle avoidance.The corresponding steering problem is described in Section 7.

Control model
The proposed method allows us to guarantee the computational cost under some parameter assumptions.In this section, it is applied to MPC with a time-varying system and its effectiveness is confirmed for obstacle avoidance control of a vehicle in a numerical simulation.
Figure 2 shows the vehicle model.Let the input be the rate of curvature deviation u = s (κ − κ r ) and the state be x = z θ κ − κ r , where z is the deviation from the reference path, θ is the angular deviation, and κ − κ r is the deviation between the curvature of the road and that of the vehicle.Discretizing the state equation of the triple integrator system with a discrete distance interval s and zero-order hold yields the following lateral dynamics [17]. where Since ( 59) is time-varying, A N in ( 7) is replaced by

Simulation conditions
A numerical simulation was conducted to verify the applicability of the proposed method for autonomous    driving MPC with multiple box constraints and a timevarying system.The simulation scenario is shown in Figure 3.As shown in this figure, the parked vehicle was set at 60 m and the reference velocity of the ego vehicle was 5.56 m/s (20 km/h).The ego vehicle was set to slow down to 2.78 m/s (10 km/h) at −0.981 m/s 2 for s≥40 m, and to recover its original velocity at 0.981 m/s 2 for s≥80 m.Moreover, a soft constraint was implemented on the position z and the input u to avoid the parked vehicle (a margin of 2.0 m was kept).As shown in Figure 4, the perturbation parameter ε was computed at each 0.0278 m/s (0.1 km/h) step in the assumed velocity range.This parameter was set by spline interpolation with a sufficiently small margin.In addition, (−q) + was set to 52.5 so that it would be large enough to reflect the deviation of q from the optimal trajectory without constraints.Here, the number of prediction points was 15 and the width of the road was 3.5 m.Furthermore, using the discrete time t = 0.1 and the current velocity v 0 , we define s = v 0 t .This means that s is time-varying and the control target has a time-varying system.For a solution accuracy δ = 0.1, the maximum number of iterations k max was computed using (53) to be 39.We compare the optimal trajectories obtained using the proposed method and the QP solver implemented in MATLAB to validate the trajectories.

Results and discussion
Figures 5 and 6 show the trajectory of the ego vehicle and snapshots of the simulation, respectively.The red lines in Figure 5 are the upper and lower soft constraints on the position, the black line is the trajectory of the ego vehicle, and the dashed line is the reference trajectory.The red lines in Figure 6 are the upper and lower soft constraints on the position, the blue dots are the optimal trajectory, and the dashed   line is the reference trajectory.These figures show that obstacle avoidance was achieved by reflecting the soft constraints.Furthermore, Figure 6 shows that the ego vehicle avoided the parked vehicle from t = 6.3 s to t = 22.3 s.Figures 7(a)-(d) show the time responses of velocity, longitudinal distance, steering angle, and rate of curvature deviation, respectively.In the time interval t = 6.3 s to t = 11.9 s, Figures 6 and 7 show that the ego vehicle started to avoid the parked vehicle by steering with gradual deceleration.Figures 6,7(b), and 7(c) show that the ego vehicle drove alongside the parked vehicle at a constant velocity from t = 11.9 s to t = 16.5 s.Furthermore, Figures 6 and 7(c) show that the ego vehicle started returning to its own lane from t = 16.5 s.Each trajectory approximately agrees with the trajectory of the QP solver, which indicates that the optimization was sufficiently accurate.
Figure 8 shows the iteration count and the computation time.The maximum computation time required to generate the trajectory is about 2.2 ms, which is sufficiently short given the control period of 100 ms.The proposed method is mostly superior to the QP solver in terms of computational time, which indicates that the computational speed sufficient for practical applications.The maximum number of iterations required to satisfy the specified accuracy δ is 22, which is fewer than the maximum number of iterations computed based on the given assumption (i.e.39 iterations).Since the proposed method modifies the trajectory based on constraint violations, the increase in computational cost is negligible even for 39 iterations.Hence, the proposed method is expected to always quickly generate trajectories for the considered scenario and parameters.
The results show that the proposed method is effective as a trajectory generation method with a guaranteed computational complexity and that it has potential for autonomous driving.

Conclusion
In this study, we proposed an LCP-based method for solving a finite number of pivot operations for MPC with multiple box constraints and a time-varying system.The results show that the optimal trajectory with the specified accuracy can be quickly solved under the guaranteed computational cost.It was confirmed that for the proposed method, the upper bound of computational complexity can be determined in advance, which allows us to determine the computational hardware requirements.

Figure 4 .
Figure 4. Relationship between velocity and perturbation parameter.

Figure 8 .
Figure 8. Iteration count and computation time.