Autonomous Collision Avoidance Using MPC with LQR-Based Weight Transformation

Abstract

Model predictive control (MPC) is a multi-objective control technique that can handle system constraints. However, the performance of an MPC controller highly relies on a proper prioritization weight for each objective, which highlights the need for a precise weight tuning technique. In this paper, we propose an analytical tuning technique by matching the MPC controller performance with the performance of a linear quadratic regulator (LQR) controller. The proposed methodology derives the transformation of a LQR weighting matrix with a fixed weighting factor using a discrete algebraic Riccati equation (DARE) and designs an MPC controller using the idea of a discrete time linear quadratic tracking problem (LQT) in the presence of constraints. The proposed methodology ensures optimal performance between unconstrained MPC and LQR controllers and provides a sub-optimal solution while the constraints are active during transient operations. The resulting MPC behaves as the discrete time LQR by selecting an appropriate weighting matrix in the MPC control problem and ensures the asymptotic stability of the system. In this paper, the effectiveness of the proposed technique is investigated in the application of a novel vehicle collision avoidance system that is designed in the form of linear inequality constraints within MPC. The simulation results confirm the potency of the proposed MPC control technique in performing a safe, feasible and collision-free path while respecting the inputs, states and collision avoidance constraints.

1. Introduction

MPC has been recognised as one of the most powerful multi-objective optimal control techniques for a wide range of applications with its ability to formulate the system constraints as a finite-horizon constrained optimisation problem. MPC performance is highly reliant on the weights on each objective but the challenge is that there is no systematic procedure to select the weights to assure the best performance of MPC. Currently, suitable weight selection is a time consuming procedure that requires a large number of trial and error selections and many computer simulations [1,2,3]. The complexity increases with an increase in the number of control objectives [4]. Hence, the necessity of developing a precise tuning procedure for MPC weights has arisen. This has been investigated by previous research works using different techniques [5,6]. For example, [6] presented a solution for selecting weights in order to match the MPC as an LQR-based linear controller. In this work, an LMI-based LQR approach was chosen by recasting the DARE in the LMI form to calculate the weighting matrices of an unconstrained MPC. Although this approach was found as a solution for MPC control parameters, the cost function was limited to have zero cross state-space terms, such that only a few controllers could be matched. In [7], the matching controller, i.e., the task of matching the MPC performance with the performance of a favorite controller, has been addressed by presenting a systematic method to tune the parameters of generalized unconstrained MPC for SISO systems [8]. In this approach the tuning parameters were calculated based on the desired closed-loop transfer function in order to relate the tuning parameters to the desired specification of the system. However, in this algorithm, a higher control horizon leads to a set of inequality bilinear constraints in the unknown elements, which cannot be solved using convex optimisation for linear MPC. In [9], the authors proposed an analytical tuning method for the Dynamic Matrix Control (DMC) algorithm. DMC is one of the primary versions of MPC that utilizes the step response of the plant, which can be approximated by the first-order plus delay time (FOPDT) model. The proposed analytical method offers several advantages, such as low computational cost and the conservative behaviour of the control system in chemical plants. However, this approach fails to demonstrate the effectiveness of applying quadratic programming solvers or additional adaptive mechanisms within the controller architecture. Other tuning strategies have been investigated by researchers using intelligent learning algorithms [10,11]. In [12,13], the authors implemented neural networks and fuzzy-based decision making to tune the MPC parameters, without using the reference system model. The authors of [1] proposed an automated strategy for an MPC-based motion cueing algorithm optimized using a multi-objective genetic algorithm. This strategy found suitable MPC weights, aiming to demonstrate the superiority of the proposed tuning strategy over empirical tuning methods without the prior knowledge of the plant model. The general idea behind auto-tuning using learning and neural network algorithms is that the tuning parameters are updated along with the optimisation algorithm, and thus the solution is always set to the optimal value. Moreover, it is not required to have a great amount of knowledge about the system model to initialize the tuning procedure. However, it is needed to operate two optimisation procedures per time step (optimisation required by intelligent algorithms and additional optimisation required by the MPC controller) that may require overhead computational time. In [14], the authors proposed a full review of tuning strategies for different MPC formulations, such as dynamic matrix control, generalized predictive control and state-space representation of an MPC controller. Moreover, a review on a few autotuning strategies is available in this work. The authors mentioned that these strategies benefit from not having full knowledge of the model of the system to initialize the tuning procedure. However, the major disadvantage of the autotuning strategy is the computational overhead, which has to perform two optimizations compared to offline tuning strategies. In [15], the authors proposed an evolutionary algorithm using a non-dominated sorting genetic algorithm for a multi-objective optimisation problem within an MPC framework. This method offers simple procedure to handle MPC constraints. Moreover, this algorithm suggests a flexible representation of the decision variables that expedite the controller performance. However, this algorithm suffers from computation overhead, as it requires two optimisations per time step.

In this paper, the inverse optimal control problem [16,17,18] of a discrete time linear quadratic regulator (DLQR) is used for designing an MPC controller to perform collision avoidance manoeuvres by selecting appropriate weighting matrices. In general, the DLQR weights ($Q,R$) are used to solve the discrete algebraic Riccati equation (DARE) for discrete time systems in order to find a solution (P) that is a real symmetric positive definite matrix. Then, using P, an optimal state feedback control gain can be obtained from the DLQR control law. However, these weighting matrices obtained from DLQR can also be used to compute the controller of unconstrained MPC [6]. Therefore, using the aforementioned concept, the objective of this paper is to design the weighting matrix Q of the DLQR in such a way that Q can be used within the MPC framework, while assuring asymptotical stability of the system [19]. DLQR and unconstrained MPC can be related through DARE, but using the weighting matrix obtained from the inverse optimal control problem of DLQR in MPC design cannot ensure solving the reference tracking problem. To overcome this problem, we recast DLQR into a discrete time linear quadratic tracking (DLQT) problem [20] to design the weighting matrix Q, which can be used for MPC design to ensure solving the reference tracking problem. In order to do this, the dynamics of the vehicle and Ackerman pole-placement criteria are considered in the formulation of Q selection through DLQT, which will be used in MPC design for a trajectory planning task. In order to highlight the practical use of this analytical tuning technique, the proposed technique is implemented to generate safe, admissible and collision-free trajectories for autonomous vehicle collision avoidance systems. In this work, a collision avoidance system is formulated as a finite horizon optimisation problem with vehicle dynamics, while the collision avoidance constraints are formulated within the MPC algorithm. However, collision avoidance constraints are generally non-convex in the context of trajectory planning, which limits the feasibility and uniqueness of the solution of the optimisation problem. To tackle this problem, researchers provide solutions such as convexification [21], changing the reference frame [22,23] and affine linear collision avoidance constraints [24]. In this paper the collision avoidance constraints are formulated in the form of linear affine constraints to perform trajectory planning manoeuvres on a curve path. Moreover, in the formulation of the collision avoidance constraints, the longitudinal position of the vehicle is excluded, allowing the designers to reduce the state dimensions of the system. This is advantageous, as removing a state from the system model helps in lowering the computational overhead for solving the optimisation problem in MPC. The effectiveness of the entire framework for autonomous collision avoidance system on a curved road is validated in a co-simulation platform where a high-fidelity vehicle model is simulated in IPG-CarMaker while the proposed MPC design is implemented in MATLAB/Simulink.

The contributions of this paper are summarised as follows: (i) Design of an analytical tuning technique to perform a collision-free trajectory for autonomous vehicles, and (ii) development of a mathematical framework to design convex constraints in order to perform collision avoidance manoeuvres on a curved path.

2. Discrete Time Optimal Controller Design

In this section, the tuning strategy for designing the MPC controller with the appropriate selection of the weighting matrix Q is described. To design the tuning strategy, a discrete time inverse optimal control problem is formulated using a linear time invariant (LTI) system:

$$\overline{x}(k+1)=\overline{A_d}\overline{x}\left(k\right)+\overline{B_d}u\left(k\right)$$

(1)

The objective of this strategy is to design the Q matrix from a discrete time algebraic Ricatti equation, which will ensure almost equal optimality between the DLQR and the unconstrained MPC control problem. It is noted that in this design the weighting factor R is chosen as a fixed value as in [19]. Moreover, the weighting matrix Q obtained from the inverse DLQR problem should be designed in such a way that the closed-loop poles are placed in the desired location within the unit circle by satisfying user-defined specifications using Ackerman’s pole placement formula. It is noted that this design is a procedure for finding the MPC weighting matrix Q by obtaining the sub-optimal solution from the DLQR controller in the presence of the constraints.

In the following subsections a general discrete infinite linear quadratic regulator problem is briefly overviewed and then a recast in DLQR as a discrete linear quadratic tracking (DLQT) problem. Next, with the concept of the inverse DLQT control problem, a strategy for designing an analytical approach to select the Q matrix for the MPC control design will be discussed.

2.1. Discrete Time Linear Quadratic Regulator (DLQR)

To design the discrete time optimal controller, the standard infinite horizon quadratic cost needs to be minimized, corresponding to the discrete time system model (1):

$$\mathcal{J}=\sum _{k=0}^{\infty }\overline{x}\left(k\right)Q^T\overline{x}\left(k\right)+u\left(k\right)R^{T}u\left(k\right)$$

(2)

where $Q=Q^T\ge 0$ and $R=R^T=\left[\begin{array}{cc}10& 0\\ 0& 9\times {10}^{-2}\end{array}\right]>0$. The solution of the DARE can be obtained to minimize the performance index (2) and is given by:

$$P=Q+{\overline{A}}_d^{T}P{\overline{A}}_d-{\overline{A}}_d^{T}P{\overline{B}}_d{(R+{\overline{B}}_d^{T}P{\overline{B}}_d)}^{-1}{\overline{B}}_d^{T}P{\overline{A}}_d$$

(3)

The discrete time optimal controller gain $\overline{K}$ can be achieved using the matrix $P=P^T=\left[\begin{array}{ccc}{P}_{11}& P_{12}& P_{13}\\ P_{21}& P_{22}& P_{23}\\ P_{31}& P_{32}& P_{33}\end{array}\right]>0$ calculated from (3) to minimizes the cost function (2). The controller gain $\overline{K}$ can be written as:

$$\overline{K}={(R+{\overline{B}}_d^{T}P{\overline{B}}_d)}^{-1}{\overline{B}}_d^{T}P{\overline{A}}_d$$

(4)

which gives the control law:

$$u\left(k\right)=-\overline{K}\overline{x}\left(k\right)$$

(5)

2.2. Discrete Time Linear Quadratic Tracking (DLQT)

In this paper, an augmented system is presented based on the system dynamic (1) and the reference trajectory. The objective for the DLQT problem is to find the optimal control law in such a way that the designed controller can be ensured to track the reference trajectory for the linear system (1) within the predefined finite horizon performance index. The augmented system becomes:

$$\begin{array}{c}{X}_{k+1}=\left[\begin{array}{c}{\overline{x}}_{k+1}\\ r_{k+1}\end{array}\right]=\underset{\widehat{A}}{\underbrace{\left[\begin{array}{cc}\overline{A_d}& 0\\ 0& F\end{array}\right]}}\underset{X_k}{\underbrace{\left[\begin{array}{c}{\overline{x}}_k\\ r_k\end{array}\right]}}+\underset{\widehat{B}}{\underbrace{\left[\begin{array}{c}{\overline{B}}_d\\ 0\end{array}\right]}}u\left(k\right)\end{array}$$

(6)

where F is assumed to be Hurwitz. This strategy is adopted from [20], and for the brevity of the paper, an interested reader can refer to this technical paper for calculation of the DLQT algebraic Riccati equation:

$$Q_1-P_1+{\widehat{A}}^T{P}_1\widehat{A}-{\widehat{A}}^T{P}_1\widehat{B}{(R+{\widehat{B}}^T{P}_1\widehat{B})}^{-1}{\widehat{B}}^T{P}_1\widehat{A}=0$$

(7)

where $Q_1$ is the weighting matrix for the augmented system (1):

$$Q_1=\left[\begin{array}{cc}Q& 0\\ 0& Q\end{array}\right]$$

(8)

and $P_1$ is the solution of the algebraic Riccati equation:

$$P_1=P_1^T>0$$

(9)

Then the control law can be written as:

$$u\left(k\right)=-{(R+{\widehat{B}}^T{P}_1\widehat{B})}^{-1}{\widehat{B}}^T{P}_1\widehat{A}X\left(k\right)$$

(10)

where the value of the controller gain is:

$$\widehat{K}={(R+{\widehat{B}}^T{P}_1\widehat{B})}^{-1}{\widehat{B}}^T{P}_1\widehat{A}$$

(11)

For the reference tracking problem using MPC for the system (6), the Ricatti Equation (7) holds. The Q matrix obtained from the DLQR problem can be used for the DLQT control problem as shown in (8). Therefore, Q can be obtained by solving the inverse DLQR optimal control problem to use it in the reference tracking problem from the DLQT approach. Hence, we can use the Q matrix obtained from the inverse DLQR approach in the MPC control problem, since the MPC reference tracking problem is equivalent to the DLQT control problem. The following section describes the system model that has been used for the MPC tuning strategy.

3. System Model

In this paper, a kinematic vehicle model is used to capture the vehicle model in MPC [25]. This model assumes no slip between the tyre and the road, and is found to be a suitable model for trajectory planning for the application of collision avoidance manoeuvres [26,27]. Under the assumption of small angles approximation [28] the vehicle bicycle model is:

$$\dot{\xi }=v_x$$

(12)

$$\dot{y}=v_x\psi +\frac{l_r}{l_f+l_r}{v}_x{\delta }_f$$

(13)

$${\dot{v}}_x=a_x$$

(14)

$$\dot{\psi }=\frac{v_x{\delta }_f}{l_f+l_r}$$

(15)

where $\xi$ and y are the longitudinal and lateral displacement of the vehicle in inertial coordinates, respectively, $\psi$ is the inertial heading angle of the vehicle, $v_x$ is the longitudinal velocity of the vehicle, $l_f$ is the distance of the front axle from the centre of gravity (C.G), and $l_r$ is the distance of the rear axle from C.G. The control inputs are the steering angle ${\delta }_f$ and longitudinal acceleration $a_x$. For a given nominal velocity $v_{x,nom}$, the system in (12), (13), (14), (15) can be expressed using an LTI system using the compact notation given below:

$$\dot{x}=Ax+Bu$$

(16)

where $x\triangleq {[\xi ,y,v_x,\psi ]}^T\in \mathcal{X}\subseteq {\mathbb{R}}^4$ is the state vector and $u\triangleq {[{\delta }_f,a_x]}^T\in \mathcal{U}\subseteq {\mathbb{R}}^2$ is the input vector with $\mathcal{X}$ and $\mathcal{U}$ are polyhedron regions used as state and input constraints, respectively. The structure of the system matrix A and input matrix B are as follows:

$$\left[\begin{array}{c}\dot{\xi }\\ \dot{y}\\ {\dot{v}}_x\\ \dot{\psi }\end{array}\right]=\underset{A}{\underbrace{\left[\begin{array}{cccc}0& 0& 1& 0\\ 0& 0& 0& v_{x,\mathrm{nom}}\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]}}\left[\begin{array}{c}\xi \\ y\\ v_x\\ \psi \end{array}\right]+\underset{B}{\underbrace{\left[\begin{array}{cc}0& 0\\ \frac{v_{x,\mathrm{nom}}{l}_r}{(l_r+l_f)}& 0\\ 0& 1\\ \frac{v_{x,\mathrm{nom}}}{(l_r+l_f)}& 0\end{array}\right]}}\left[\begin{array}{c}\delta \\ a_x\end{array}\right]$$

(17)

where $v_{x,nom}$ is the nominal longitudinal velocity of the vehicle, which is chosen in the same way as the initial speed of the vehicle [3]. Then system (16) is descretised with a sampling time $t_s$ to obtain the linear time invariant discrete system shown below:

$$x(k+1)=A_{d}x\left(k\right)+B_{d}u\left(k\right)$$

(18)

where $A_d=e^{A{t}_s}=\left[\begin{array}{cccc}1& 0& t_s& 0\\ 0& 1& 0& v_{x,\mathrm{nom}}{t}_s\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ and $B_d=B{t}_s=\left[\begin{array}{cc}0& 0\\ \frac{v_{x,\mathrm{nom}}{l}_r{t}_s}{L}& 0\\ 0& t_s\\ \frac{v_{x,\mathrm{nom}}{t}_s}{L}& 0\end{array}\right]$

As the dynamics of the state $\xi$ in system (17) depends only on v, it is possible to further simplify the system for trajectory generation:

$$\overline{x}(k+1)=\overline{A_d}\overline{x}\left(k\right)+\overline{B_d}u\left(k\right)$$

(19)

where $\overline{x}={[y,v,\psi ]}^T$ is the system states, $u=[{\delta }_f,a_x]$ is the system inputs, and matrices ${\overline{A}}_d$ and ${\overline{B}}_d$ are obtained by extracting the appropriate rows and columns of $A_d$ and $B_d$, respectively, in (17).

4. Selection of Weighting Matrix $\mathbf{Q}$ for MPC Design

In order to obtain the appropriate Q matrix, for designing an MPC-based reference tracking controller, we have chosen $Q=diag(Q_{11},Q_{22},Q_{33})$ for system (19) and the optimal control gain $\overline{K}$ with a fixed weighting factor R. For the reference tracking problem, MPC can be recast as a DLQT problem as described in Section 2.2. Therefore, it can be inferred that the weighting matrix Q obtained from the inverse DLQT problem will provide almost the same controller gain for MPC, which is described in the following Theorem.

Theorem 1.

MPC will ensure the reference tracking for system (19), guaranteeing the sub-optimality of the DLQT control problem by satisfying (2)–(5) with $Q_1^T=Q_1\ge 0$, $R^T=R>0$ and $P_1^T=P_1>0$, where $Q_1=diag(Q,Q)$, $R_1=diag(R,R)$ and the controller gains $\widehat{K}=diag(\overline{K},\overline{K})$ with $Q=diag(Q_{11},Q_{22},Q_{33})$, and $P_1=diag(P,P)$ where P will be as (3) and $\overline{K}=\left[\begin{array}{ccc}{k}_1& k_2& k_3\\ k_4& k_5& k_6\end{array}\right]$, respectively.

Proof. 

Equation (3) can be recast as a DLQR problem:

$$Q=P-{\overline{A}}_d^{T}P{\overline{A}}_d+{\overline{A}}_d^{T}P{\overline{B}}_d{(R+{\overline{B}}_d^{T}P{\overline{B}}_d)}^{-1}{\overline{B}}_d^{T}P{\overline{A}}_d$$

(20)

where $P=P^T=\left[\begin{array}{ccc}{P}_{11}& P_{12}& P_{13}\\ P_{21}& P_{22}& P_{23}\\ P_{31}& P_{32}& P_{33}\end{array}\right]>0$, $R=\left[\begin{array}{cc}{R}_1& 0\\ 0& R_2\end{array}\right]>0$ and

$Q=Q^T=\left[\begin{array}{ccc}{Q}_{11}& Q_{12}& Q_{13}\\ Q_{21}& Q_{22}& Q_{23}\\ Q_{31}& Q_{32}& Q_{33}\end{array}\right]\ge 0$. The corresponding control law (4) can be written as:

$$\overline{K}=\left[\begin{array}{ccc}{k}_1& k_2& k_3\\ k_4& k_5& k_6\end{array}\right]={(R+{\overline{B}}_d^{T}P{\overline{B}}_d)}^{-1}{\overline{B}}_d^{T}P{\overline{A}}_d$$

(21)

Since the input $\overline{B_d}$ matrix in (19) includes only three non-zero elements, the three components of controller gains $(k_2,k_4,k_6)$ will be zero, and the rest of the gains will be non-zero elements. The following section describes the system model required for designing the weighting matrix within the MPC controller.    □

Now, using (19), (21) and the matrices (P,Q,R) in (20) yields the diagonal and off-diagonal elements of the Q matrix as shown in (25)–(27) and

$$Q_{12}=-k_2\left[P_{11}\frac{v{l}_r{t}_s}{L}+P_{13}\frac{v{t}_s}{L}\right]-P_{12}{t}_s{k}_5$$

(22)

$$Q_{13}=v{t}_s{P}_{11}-\left[k_3\left[P_{11}\frac{v{l}_r{t}_s}{L}+P_{13}\frac{v{t}_s}{L}\right]+P_{12}{t}_s{k}_6\right]$$

(23)

$$\begin{array}{c}{Q}_{32}=v{t}_s{P}_{12}-\left[\right[\frac{v{l}_r}{L}{t}_s(v{t}_s{P}_{11}+P_{13})\\ +\frac{v}{L}{t}_s(v{t}_s{P}_{13}+P_{33})]k_2+(v{t}_s{P}_{12}+P_{23})k_5]\end{array}$$

(24)

respectively. Since matrix Q is diagonal, all of the off-diagonal elements are equal to zero, i.e., $Q_{12}=Q_{13}=Q_{32}=0$. Therefore, using these conditions and gains $k_2=k_4=k_6=0$, the elements of the P matrix can be obtained, i.e., (22) yields (32). Using (32) in (23) yields (29), and using (32) in (24) results in (33). Now, using the elements (32), (33), (29) and the R, $B_d$ matrices in (21), Equations (34)–(36) will hold. Finally, by solving (34)–(36), Equations (28), (30) and (31) can be obtained. It is noted that the gain matrix $\overline{K}$ needs to be specified such that the closed-loop poles of the system (18) would be inside the unit circle. In this paper, Ackerman’s pole placement formula is used to obtain the gains ($\overline{K}$) by specifying the closed-loop pole location inside the unit circle. This method solves the solution of the Riccati equation to generate optimal weights using an LQR controller. The generated gains are then used in the MPC controller, which inherits all of the properties and optimal solutions from the LQR controller when MPC constraints are not active. However, when the constraints are active, the solution of the Riccati equation in LQR provides a sub-optimal solution for MPC, as it satisfies the constraints of the Quadratic Programming (QP) problem indirectly. In other words, the set of states $\overline{x}\left(k\right)$ where the matching occurs is the polyhedron, where the unconstrained optimizer (i.e., unconstrained MPC, therefore the DLQR controller) satisfies the constraints of the QP problem [6].

$$\begin{array}{cc}\hfill Q_{11}& =(P_{11}\frac{v{l}_r}{l_r+l_f}{t}_s+P_{13}\frac{v}{l_r+l_f}{t}_s)k_1\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill Q_{22}& =P_{22}{t}_s{k}_5\hfill \end{array}$$

(26)

$$\begin{array}{cc}\hfill Q_{33}& =-v{t}_s{P}_{13}+[\frac{v{l}_r}{l_r+l_f}{t}_s(v{t}_s{P}_{11}+P_{13})+\frac{v}{l_r+l_f}{t}_s(v{t}_s{P}_{13}+P_{33})]\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill P_{13}& =\frac{-\frac{R_1{k}_{1}v{t}_s}{L}[\frac{k_{3}v{t}_s}{L}+(1-\frac{v{t}_s}{L})]}{\frac{v^4{t}_s^4{k}_1}{L^3}[\frac{v{l}_r{t}_s{k}_3}{L}-l_r-v{t}_s](1-\frac{l_r{k}_3}{L})-\frac{v^3{t}_s^3}{L^2}\left[(1-\frac{v{l}_r{t}_s{k}_1}{L})(1-\frac{v{t}_s}{L})(1-\frac{l_r{k}_3}{L})\right]+\frac{v^2{l}_r{t}_s^2}{L}(\frac{l_r{k}_3}{L}-1)\frac{v^3{t}_s^3{k}_1{k}_3}{L^3}}\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill P_{11}& =-\frac{P_{13}\frac{v}{L}{t}_s{k}_3}{\frac{v{l}_r}{L}{t}_s{k}_3-v{t}_s}\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill P_{33}& =\frac{P_{11}[\frac{v{t}_s{l}_r}{L}-\frac{k_1{v}^2{l}_r^2{t}_s^2}{L^2}]-k_1{R}_1+P_{13}[\frac{v{t}_s}{L}-\frac{v^2{l}_r{t}_s^2{k}_1}{L^2}]}{\frac{k_1{v}^{2}t{s}^2}{L^2}}\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill P_{22}& =\frac{R_2{k}_5}{ts-t{s}^2{k}_5}\hfill \end{array}$$

(31)

$$\begin{array}{cc}\hfill P_{12}& =0\hfill \end{array}$$

(32)

$$\begin{array}{cc}\hfill P_{23}& =0\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill k_1& =\frac{\frac{P_{11}v{l}_r{t}_s}{L}+\frac{P_{13}v{t}_s}{L}}{R_1+\frac{v^2{l}_r^2{t}_s^2{P}_{11}}{L^2}+\frac{[\frac{P_{13}v{l}_r{t}_s}{L}+\frac{P_{33}v{t}_s}{L}]v{t}_s}{L}}\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill k_3& =\frac{\frac{P_{11}{v}^2{l}_r{t}_s}{L}+P_{13}\frac{[v{t}_s{P}_{13}+P_{33}]v{t}_s}{L}}{R_1+\frac{P_{11}{v}^2{t}_s^2}{L^2}+\frac{[\frac{P_{13}v{t}_s}{L}+\frac{P_{3}3v{t}_s}{L}]v{t}_s}{L}}\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill k_5& =\frac{P_{22}{t}_s}{R_2+P_{22}{t}_s^2}\hfill \end{array}$$

(36)

$$\begin{array}{cc}\hfill k_2& =k_4=k_6=0\hfill \end{array}$$

(37)

5. Constraint Design

In this section, the mathematical framework for computing the collision avoidance constraints that are used to plan trajectories on curved roads is presented. In the absence of traffic vehicles, a convex region is designed to ensure that the planned vehicle trajectories remain within the boundaries of the road. On the other hand, in the presence of obstacles, a convex region is designed to ensure that the planned trajectories remain within the road boundaries and maintain a safe distance from the obstacle. A detailed explanation of the aforementioned constraints designs is presented in the following subsections.

5.1. Road Boundary Constraints

Using the edges of a curved road as constraints within the MPC framework results in non-convex constraints, which is not suitable for a Quadratic Programming (QP) framework [29]. Consequently, boundary constraints are designed by assuming that the road segment in the immediate vicinity of the subject vehicle is part of a straight road (Figure 1). The edges of the virtual straight road are obtained by calculating the points $P_1$ and $P_2$ in Figure 1. These virtual straight lines can be constructed using the generated points $P_1$ and $P_2$, followed by the slope equal to the heading angle of the upper and lower road boundaries. Moreover, the equation of these lines can be formulated in the form of linear inequality constraints, which can ensure that the planned trajectories lie within the edges of this virtual road. The point $P_1$ is located at the intersection of the left (outer) road edge with an imaginary line passing through the subject vehicle’s centre of gravity (C.G) and perpendicular to the subject vehicle’s longitudinal axis. The equations of the outer lane boundary and the intersected line are:

$$y_{outer}=f\left(x_{outer}\right)$$

(38)

$$a_{outer}\xi +b_{outer}y+c_{outer}=0$$

(39)

where (38) is the equation of the outer lane boundary, (39) is the equation of the intersected line perpendicular to the outer lane boundary, and the parameters $a_{outer},b_{outer}$ and $c_{outer}$ in Equation (39) are the coefficients of the intersected line. The line passing through $P_1$ with a slope equal to the reference path heading angle ${\psi }_{ref}$ forms the outer edge (left line) of the virtual straight road (blue line in Figure 1) with the following equation:

$$m_{outer}\xi -y+P_{1y}-m_{outer}{P}_{1x}<0$$

(40)

where $m_{outer}$ is the slope of the equation of the outer edge road and $(P_{1x},P_{1y})$ are coordinate positions of point $P_1$. From Equation (12) it is evident that the evolution of $\xi$ depends on the longitudinal velocity of the vehicle. Therefore, the predicted longitudinal position $\xi$ can be estimated using the initial position ${\xi }_0$ and the predicted velocity for the entire prediction horizon $N_p$ using the equation bellow:

$$\overline{\xi }\left(j\right)=\left[{\xi }_0+\sum _{i=1}^j(\overline{v}\left(i\right)·t_s)\right];j=1,2,...,N_p$$

(41)

${\xi }_0$ is the current longitudinal position of the subject vehicle and $\overline{v}$ is the predicted nominal velocity. The equation above is substituted into Equation (40) to formulate the generalized constraint equation for $N_p$; different constraint equations given below:

$$m_{outer}\left[{\xi }_0+\sum _{i=1}^j(\overline{v}\left(i\right)·t_s)\right]-y\left(j\right)-m_{outer}{P}_{1x}+P_{1y}<0$$

(42)

where the predicted nominal velocity and predicted lateral position can be obtained from the prediction model in MPC. For different values of j in (41), Equation (42) represents the outer edge of the virtual straight road constraint, which solely depends on two states of the vehicle (longitudinal velocity and lateral position of the vehicle). Similarly, the line passing through $P_2$ with a slope ${\psi }_{ref}$ forms the right (inner) edge of the virtual straight road. The inner edge of the virtual straight road can be expressed as:

$$y_{inner}=f\left(x_{inner}\right)$$

(43)

$$a_{inner}\xi +b_{inner}y+c_{inner}=0$$

(44)

where (43) is the equation of the inner lane boundary and (39) is the intersected line perpendicular to the inner lane boundary. Consequently, in the same way as the upper bound, the lower hyperplane can be defined as:

$$m_{inner}\left[{\xi }_0+\sum _{i=1}^j(\overline{v}\left(i\right)·t_s)\right]-y\left(j\right)-m_{inner}{P}_{4x}+P_{4y}>0$$

(45)

To decouple the orientation of the virtual straight road with the subject vehicle’s orientation, the edges are parameterised using ${\psi }_{ref}$. It is noted that, this technique is suitable to be implemented for different range of curvature of the road. Moreover, it can be ensured that the optimization problem in MPC will always be feasible, and guarantee the convexity on the curve road.

5.2. Collision Avoidance Constraints

As mentioned above the collision avoidance constraints are designed to ensure the subject vehicle evades the obstacle vehicle without any collisions. In literature it is common to design ellipsoids/rectangles around the obstacle to obtain the collision avoidance constraints [30,31]. However, these techniques result in non-convex optimisation problem which limits the uniqueness and feasibility of the solution of the optimisation. In this paper, the collision avoidance constraints are generated using two lines entitled as forward collision avoidance constraints (FCC) and rear collision avoidance constraints (RCC) as shown in Figure 2. This technique is inspired from [24], where the collision avoidance constraints are used for only straight driving conditions. The purpose of generating the FCC is to prevent front end collision while performing a collision-free lane change and the purpose of the RCC is to prevent rear end collision while returning to the original lane. The activation of FCC is when the subject vehicle is behind the lead vehicle while RCC is activated when the subject vehicle crosses the lead vehicle. In this framework, three points ($P_3,P_4,P_5$) are required to generate collision avoidance constraints. These points can be calculated by intersecting virtual longitudinal and lateral axis of the lead vehicle (grey and yellow lines respectively) with a circle centred at the lead vehicle’s geometrical center, which provides the location of the points $P_3$, $P_4$, and $P_5$. The following subsections explain the procedure for generating collision avoidance constraints.

5.2.1. Forward Collision Avoidance Constraints

The virtual line on the road segment representing FCC is the line that passes through the points $(P_{3x},P_{3y})$ and $(P_{4x},P_{4y})$ in Figure 3. Points $(P_{3x},P_{3y})$ are obtained as a result of the intersection of the longitudinal axis of the lead vehicle (grey line in Figure 2):

$$a_{Lon}{x}_L+b_{Lon}{y}_L+c_{Lon}=0$$

(46)

with a circle centred at the the lead vehicle geometrical centre and radius of $L_x+l_{rL}$:

$${(x-x_L)}^2+{(y-y_L)}^2={(L_x+l_{rL})}^2$$

(47)

where $L_x$ is the safety distance, which can be defined as $L_x=v_{x}t+L_c$. The longitudinal velocity of the subject vehicle expressed as $v_x$, t is the desired time gap of the subject vehicle when approaching the lead vehicle, and finally $L_c$ is the lead vehicle length. Moreover, the parameters $l_{rL},a_{Lon},b_{Lon},c_{Lon}$ represent the distance of the centre of gravity of the vehicle to the rear wheels and coefficients of the intersected line, respectively. It is noted that at every time instant of MPC optimization, $L_x$ is updated according to the current value of the subject vehicle velocity ($v_x$). For generating FCC, another set of points is required $(P_{4x},P_{4y})$. These points are generated by means of the intersection of the lateral axis of the lead vehicle (yellow line in Figure 2) with a circle of radius W:

$${(x-x_L)}^2+{(y-y_L)}^2=W^2$$

(48)

the parameter $W=\frac{1}{2}{W}_c+W_L$, where $W_c$ is the width of the subject vehicle and $W_L$ is the lane width plus the distance from the subject vehicle to the centre line of the road [24]. After defining two sets of points, it would be trivial to find the equation of the line for forward collision avoidance constraints, where the final equation will be:

$$\underset{a_{FCC}}{\underbrace{m_{FCC}·t_s}}\left[\sum _{i=1}^j\left(\overline{v}\left(i\right)\right)\right]+(\underset{b_{FCC}}{\underbrace{-1}})y+\underset{c_{FCC}}{\underbrace{P_{1y}-m_{FCC}{P}_{1x}+m_{FCC}·{\xi }_0}}>0$$

(49)

where $m_{FCC}$ is the slope of the forward collision avoidance constraint. The generated FCC divides the $(x,y)$ plane into two regions. Region 1: $a_{FCC}x+b_{FCC}y+c_{FCC}>0$, which is the safe region, and Region 2: $a_{FCC}x+b_{FCC}y+c_{FCC}<0$, which represents the unsafe region. FCC forces the vehicle to be in a safe region while performing a lane change on a curved/straight road. Moreover, (49) represents a linear affine constraint approximation, which can be formulated in QP format.

5.2.2. Rear Collision Avoidance Constraints

Similar to FCC, RCC can be generated by following similar steps. The procedure for calculating the intersection point $P_3$ is identical to FCC, thus resulting in generating the equation of the RCC line (Figure 4):

$$\underset{a_{RCC}}{\underbrace{m_{RCC}·t_s}}\left[\sum _{i=1}^j\left(\overline{v}\left(i\right)\right)\right]+(\underset{b_{RCC}}{\underbrace{-1}})y+\underset{c_{RCC}}{\underbrace{P_{3y}-m_{RCC}{P}_{3x}+m_{RCC}·{\xi }_0}}<0$$

(50)

It is noteworthy that the aforementioned collision avoidance constraints can be adopted for the scenarios where (i) the subject vehicle needs to change a lane while performing the collision avoidance manoeuvre, and (ii) where more traffic members are present on the road that require multiple hyperplanes.

Remarks

• One of the benefits of the proposed approach lies in the fact that there is only one parameter to be tuned (desired time gap) and the rest of the parameters are based on the geometry of the lead vehicle.
• The design of the collision avoidance constrains is based on basic mathematical operation; therefore the aforementioned constraints for each traffic vehicle can be generated without any major computation overhead. Thus, it is suitable for real time implementation.
• Reducing a state dimension in the system model helps in bringing down the computation requirement for solving the optimisation problem.

It is noted that by using boundary constraints and the appropriate FCC/RCC based constraints, a convex region representing safe zones on a curved road is generated that forces MPC to plan a safe and collision-free trajectory on the road.

6. Trajectory Planning Controller

In general, autonomous highway driving involves motion planning and control of a subject vehicle in order to maintain the velocity and keep the vehicle away from lane boundaries while avoiding possible collisions. The choice of manoeuvre to perform is the result of the decision-making process with the objective of following the desired trajectory $x_{ref},y_{ref}$ (Section 6.1) while respecting physical and design limitations of the subject vehicle. In this section, using the definition of the linear model in (16) and linear system constraints in Figure 5, a constrained linear quadratic optimal control problem is formulated over a prediction horizon $N_p$.

6.1. Reference Trajectory Generator

The reference trajectory generator provides the longitudinal position $x_{ref}\left(s\right)$ and lateral positions $y_{ref}\left(s\right)$ as well as the orientation (${\psi }_{ref}\left(s\right)$) of the road for the vehicle to follow. The generated paths are parametrized by curvature as a function of the distance s along the path. The equation of curvature can be formulated as follows:

$$\kappa =\frac{{\dot{x}}_{ref}{\ddot{y}}_{ref}-{\dot{y}}_{ref}{\ddot{x}}_{ref}}{{({\dot{x}}_{ref}^2+{\dot{y}}_{ref}^2)}^{\frac{3}{2}}}$$

(51)

The reference trajectories are formulated using the clothoids method, which is expressed by Fresnel integrals [32,33]. The trajectories are parametrized by curvature $\kappa$ as a function of distance s along the path as follows:

$$x_{ref}\left(s\right)={\int }_0^{s}cos{\psi }_{ref}\left(x\right)dx$$

(52)

$$y_{ref}\left(s\right)={\int }_0^{s}sin{\psi }_{ref}\left(x\right)dx$$

(53)

where (52) and (53) are constructed using the polynomial integration method of the Fresnel function applied to clothoid [34], and ${\psi }_{ref}$ is calculated as follows:

$${\psi }_{ref}\left(s\right)=2{tan}^{-1}\left(\frac{y_{ref,n}-y_{ref,n-1}}{x_{ref,n}-x_{ref,n-1}}\right)$$

(54)

where $\left(x_{ref,n}\right),y_{ref,n}$, $n=1...N$ are the sequence of N, the sampled points on the trajectory [35].

6.2. Trajectory Planning: MPC

For the trajectory planning using linear MPC, the system states and inputs are subjected to the following state and input constraints:

$$\overline{x}\in \overline{\mathcal{X}},u\in \mathcal{U}$$

(55)

where $\overline{\mathcal{X}}=\{\overline{x}\in {\mathbb{R}}^3:{\overline{x}}_{min}$≤x≤${\overline{x}}_{max}\}\subset {\mathbb{R}}^3$ and $\mathcal{U}=\{u\in {\mathbb{R}}^2:u_{min}$≤u≤$u_{max}\}\subset {\mathbb{R}}^2$ are states and inputs of the polytope admissible regions (subscripts $min$ and $max$ are the minimum and maximum of the corresponding values). The following cost function is formulated:

$$\begin{array}{c}\mathcal{J}={\displaystyle \sum _{k=0}^{N_p-1}}[\Vert {\overline{x}}_{ref}-{\overline{x}}_{t+k\mid t}{\Vert }_Q^2+\Vert u_{t+k\mid t}{\Vert }_R^2]\\ +\Vert {\overline{x}}_{ref}-{\overline{x}}_{t+N\mid t}{\Vert }_P^2\end{array}$$

(56)

where ${\overline{x}}_{t+k|t}$ is the predicted state trajectory at time $t+k$ obtained by applying the control sequence $U_t^{*}={\left[u_t^T,\cdots ,u_{t+N-1}^T\right]}^T$ to the system (18), starting from an initial state of ${\xi }_{t|t}$. The parameter $N_p\in {\mathbb{N}}^{+}$ is the prediction horizon, whereas $Q\in {\mathbb{R}}^{3\times 3}$, $P\in {\mathbb{R}}^{3\times 3}$ and $R\in {\mathbb{R}}^{2\times 2}$ are weighting matrices. The performance index in (56) consists of the stage cost, input cost and the terminal cost, respectively. The desired state ${\overline{x}}_{ref}$ represents the reference state for the subject vehicle and is defined as ${\overline{x}}_{ref}={[y_{ref},v_{xref},{\psi }_{ref}]}^T$. It is noted that in this paper, $v_{xref}$ is taken as the initial value of the subject vehicle’s velocity. The following constrained optimization problem, for each sampling time, is formulated as:

$$\underset{U_t}{\min }\mathcal{J}(u_t;\overline{x}\left(t\right),{\overline{x}}_{ref})$$

(57)

subject to (18), (42), (45), (49), (50), (55)

In this framework, at every time step, the problem (57), under the constraints (18), (42), (45), (49), (50), (55) are calculated based on the current state $x\left(t\right)$, over a shifted time horizon. The optimal control input is calculated as $u^{*}\left(\overline{x}\left(t\right)\right)=U_t^{*}\left(0\right)$, which is a solution to the problem (57). As the sets $\overline{\mathcal{X}}$ and $\mathcal{U}$ are convex, then the MPC problem (57) is solved as a standard QP optimisation problem. All of the steps required to design the proposed MPC controller can be seen in Algorithm 1, and the closed-loop form is illustrated in Figure 5. The following demonstrates the numerical results to show the efficacy of the proposed control strategy.

Algorithm 1: Trajectory planning

1: initialize: 2: $v_{des}\leftarrow$ desired longitudinal velocity from user 3: procedure GENERATE TRAJECTORY 4: top: 5:       Use Equation (3) to create matrix Q 6:       $\overline{K}\leftarrow$ Ackerman pole placement formula as (4) and calculate $\overline{K}$ 7:       $P\leftarrow$ Obtained $\overline{K}$ in (28)–(37) and generate P 8:       $Q\leftarrow$ Obtained P and substitute (19) in (25)-(27) to calculate Q 9: loop: 10:     ${\overline{x}}_{ref}\leftarrow$generate target states as 11:     $\overline{x}\leftarrow$current state vector 12:     getRoadBoundaryConstraints as (38) (39)–(45) 13:     getCollisionAvoidanceConstraints as (46)–(50) 14:     $u^{*}\leftarrow$ solve MPC with generated Q from top with fixed R 15:     $x^{*}\leftarrow$ Apply optimal input $u\left(k\right)$ as (12)–(15) 16:     if user request to change $v_{des}$ then 17:            goto initialize. 18:            else 19:      goto loop.

7. Numerical Results

In this section, a simulation environment is used to investigate the ability of the proposed trajectory planning architecture to perform a collision avoidance manoeuvre. The scenarios used to perform this are: (1) an obstacle avoidance when the subject vehicle (SV) is $300 \mathrm{m}$ behind the stationary lead vehicle (LV), and travelling at $80 \mathrm{km} {\mathrm{h}}^{-1}$ on a two-lane one-way curved road with a radius of $750 \mathrm{m}$, (2) a collision avoidance manoeuvre with a moving obstacle under different road surface friction conditions. It is noted that the chosen curved road is approximately equal to the tightest corner allowed in highways [36]. The simulation results for the proposed closed-loop architecture were obtained using the MATLAB IPG CarMaker co-simulation environment.

Moreover, it was assumed that the dimension of the road, lane limits and LVs states are available to the SV, and the initialisation parameters are tabulated in

Table 1. Design Parameters.

Symbols	Value	Value
	Road Geometry
$N_{lanes}$	2	-
$w_{lane}$	5	$\mathrm{m}$
	Subject Vehicle
$l_f$	$1.144$	$\mathrm{m}$
$l_r$	$1.2060$	$\mathrm{m}$
	Lead Vehicle
$l_{Lv,l}$	$4.1$	$\mathrm{m}$
$l_{Lv,w}$	$1.7$	$\mathrm{m}$
	Controller Parameters
$t_s$	$0.1$	$\mathrm{s}$
$N_p$	14	-
closed-loop poles	($5e^{-1},0.6,0.95$)	-
	System
$\mathcal{X}$	−[500, −11.1,1.5] ≤ x ≤ [500,27.7778,1.5]	-
$\mathcal{U}$	−[0.0698, 3] ≤ u ≤ [0.0698,3]	-

. Additionally, to evaluate the efficacy of the proposed algorithm, MPC was initialized with a prediction horizon equal to 14 and low time gap ($t=0.8$) when designing the constraints for imitating more extreme manoeuvres to perform a collision avoidance scenario.

The discrete linear quadratic tracker (6) is used to obtain the proper weighting matrix Q in MPC to perform the collision avoidance manoeuvre. The value of the generated matrix Q corresponding to the proposed MPC design is $Q=diag(1.53,0.023,34.06)$. It is noted that the simulation results relating to the proposed MPC design was compared with a generic MPC controller where the values of the Q matrix have been generated through a trial and error procedure (which will be referred to as manual tuning for the rest of the paper) to demonstrate the superiority of the proposed control algorithm.

It is noted that the choice of manual tuning of $Q_{manual}$ was based on trial and error and the result of many simulations in order to select a set of suitable values to perform trajectory planning with $Q_{manual}=diag(5,1e^{-1},230)$. Moreover, in both cases the value of the weighting matrix R was equally chosen be to R = diag($10,9\times {10}^{-2}$).

7.1. Collision Avoidance with Static Obstacle

The result of the obstacle avoidance scenario with the assumption of a high friction surface, using the proposed MPC design, is demonstrated in Figure 6. This figure shows the actual trajectory of the vehicle where the SV starts to change the lane $30 \mathrm{m}$ from the lead vehicle and converges to the original lane shortly after a collision-free path is detected, while staying within the road boundaries. Moreover, it can be seen that the vehicle respects the collision avoidance constraints by maintaining the safety margins from the lead vehicle during the collision avoidance manoeuvre for all sub-manoeuvres. The result in Figure 6 indicates that the proposed structure generated a consistent trajectory with no overshoot while maintaining the safety margins to the lead vehicle during both lane changes.

For further analysis of the performance of the proposed collision avoidance system, the evolution of the states and control inputs of the SV over time is shown in Figure 7. A particular feature of the collision avoidance manoeuvre is that the vehicle starts the manoeuvre close to $11 \mathrm{s}$ where the steering wheel angle for the lateral motion shows a smooth response without having high-frequency oscillation. However, steering action after returning to the original lane is more aggressive and the activation of the input constraints can be observed in this figure. This action suggests that for a successful collision avoidance and accurate reference tracking, large steering action is required on the reverse behaviour, where the SV attempts returning to the original lane. Another key point is the two states of the SV, heading-angle and longitudinal velocity, which demonstrate smooth evolution without having any oscillation during either of the lane changes while maintaining the SV’s pose directionality. It can be seen that the velocity of the vehicle decreases rapidly while the first lane change manoeuvre is being performed. The reverse behaviour (i.e., increasing the velocity while performing the lane change) is visible right after evading the lead vehicle. This is reminiscent of a real-world collision avoidance manoeuvre, where the vehicle decelerate when an obstacle is detected to prevent a possible collision, and accelerates after evading the obstacle, which shows the efficacy of the proposed control structure.

Moreover, the longitudinal acceleration also shows a smooth profile without any oscillation during deceleration and acceleration for both of the lane changes while respecting the system constraints. Similarly, the bottom subplot in Figure 7 demonstrates the lateral acceleration and yaw-rate of the SV, which shows a smooth profile without having high oscillation for both of the lane changes. It is noted that the absolute maximum value of lateral acceleration is $2.3 \mathrm{m} {\mathrm{s}}^{-2}$, which demonstrates that the vehicle does not reach its limits of handling. This can be observed by investigating the yaw-rate profile, which the maximum absolute value reaches at about $0.18 \mathrm{rad} {\mathrm{s}}^{-1}$, indicating that the vehicle operates in the linear region of the tire.

To show the need for the proposed MPC design to perform the collision avoidance manoeuvre, we compared the performance of the proposed framework with the MPC design using manual tuning. Figure 8 demonstrates the lateral and heading angle errors for both the analytical tuning and manual tuning of the MPC. In this analysis, the lateral error $e_y$ is defined as the lateral distance of the centre of gravity of the vehicle (subject vehicle) from the desired path and $e_{\psi }$ is defined as the difference between the actual orientation of the vehicle and the desired heading angle, which are defined as [37]:

$$\begin{array}{cc}\hfill e_y& =(y_{CG}-y_{ref})cos{\psi }_{ref}-(x_{CG}-x_{ref})sin{\psi }_{ref}\hfill \end{array}$$

(58a)

$$\begin{array}{cc}\hfill e_{\psi }& =\psi -{\psi }_{ref}\hfill \end{array}$$

(58b)

The top figure shows the heading angle error, which shows similar behaviour for both the proposed MPC design and manual tuning. It can be seen that the directionality of that vehicle is maintained during the collision avoidance manoeuvre for both cases with minimum discrepancy with the maximum error of $0.15 \mathrm{rad}$. The bottom figure represents the lateral displacement error for the collision avoidance manoeuvre. The main difference can be observed after performing the collision avoidance manoeuvre, where the error in the analytical tuning strategy almost converges to zero, whereas the conventional tuning reaches about $30 \mathrm{c}\mathrm{m}$. This figure confirms that the proposed strategy is able to successfully maintain the directionality of the vehicle during the collision avoidance manoeuvre while performing better reference tracking with minimum tracking error compared to conventional trial and error tuning.

For further illustration of the effectiveness of the proposed MPC design, we compared the performance index value of the proposed technique with the MPC design using manual tuning to investigate the level of optimality between the two procedures. This can be seen in Figure 9, where the performance index ($\mathcal{J}$) indicates the minimized value of the cost function in (56). It can be inferred that the proposed MPC design gives an improvement in $\mathcal{J}$ of more than 70% over the manual tuning between $11 \mathrm{s}$ and $15 \mathrm{s}$, where the performance index is at its maximum, and for the rest of the manoeuvre the performance index for the analytical tuning is still significantly improved compared to the trial and error tuning. For the simulation purposes, the performance index for both methods was analysed for the straight driving scenario. This analysis can be seen in Figure 10, where the performance index is significantly enhanced by the proposed MPC design, which resulted in an improvement close to $80\%$ compared to manual tuning.

7.2. Collision Avoidance with Moving Obstacle and Different Surface Friction Conditions

In this section, the efficacy of the proposed algorithm is examined under different values of surface friction. Moreover, the collision avoidance manoeuvre is performed with the presence of a moving vehicle, which resembles a real world driving scenario in the simulation environment. The SV is traveling at $80 \mathrm{km} {\mathrm{h}}^{-1}$ located $300 \mathrm{m}$ behind the LV, and the LV is traveling at $40 \mathrm{km} {\mathrm{h}}^{-1}$ under different values of surface friction conditions ranging from 0.3 to 0.5. Then the system plans the trajectory at each sampling time while applying FCC and RCC constraints based on the position of the subject vehicle and the moving vehicle. Figure 11 illustrates the collision avoidance simulation with an assumption of the lowest friction surface condition ($\mu =0.3$).

It is shown that while the SV vehicle was able to smoothly overtake the moving LV vehicle, it experienced some overshooting when merging back to the original lane. This is due to the sudden and harsh steering actuation of the SV vehicle after performing the first lane change from the LV vehicle. Nevertheless, this amount of actuation is essential in order to maintain the directionality of the vehicle under low surface friction conditions. The results shown in Figure 11 also illustrate that the maximum lateral acceleration reached about $3.5 \mathrm{m} {\mathrm{s}}^{-2}$, where the value indicates that the vehicle operates in the linear region of the tire and does not reach its limit of handling. However, this simulation demonstrated some oscillations as the SV vehicle steered back to the original lane, which can be seen in the yaw-rate profile, where its value reached about $0.26 \mathrm{rad} {\mathrm{s}}^{-1}$. Overall, the simulation results indicate that the vehicle respected the collision avoidance constraints and was able to safely converge back to the original lane and follow the desired trajectory on a curved path.

Finally, a parametric analysis was carried out to verify the effectiveness of the proposed framework after performing the collision avoidance manoeuvre. In this analysis, the maximum error between lateral displacement (Figure 12) from the trajectory planner and actual vehicle response was tested in order to investigate the tracking capability of the algorithm under different road surface conditions. It is noted that in this analysis the comparison is made between the proposed analytical MPC and generic MPC controller. The lateral displacement error ($e_y$) for analytical tuning increases gradually when lowering the surface friction. This figure indicates a peak deviation of $0.05 \mathrm{m}$ at a 0.3 surface friction, which shows major superiority over manual tuning, which reaches about $0.44 \mathrm{m}$ with the same friction value. This trend is similar for the rest of the friction values, which demonstrates the effectiveness of the proposed control algorithm to improve vehicle handling while performing the collision avoidance manoeuvre.

8. Conclusions

In this paper, we have presented an MPC-based collision avoidance framework by means of an analytical tuning methodology. The main contribution of this paper lies in the derivation of the transformation of a discrete time LQR weighting matrix Q to generate the MPC weighting matrix. The importance of analytical tuning was investigated next, with results showing that it not only improves the tracking error, but can also minimize the performance index compared to manual tuning of MPC. Moreover, the analytical tuning design was then tested under various friction conditions to investigate the efficacy of the proposed algorithm. The results show that while the manual tuning of MPC is effective, the analytical tuning can achieve better performance that is close to the optimal solution while performing a collision avoidance manoeuvre. The numerical results obtained in the Simulink/IPG Carmaker co-simulation environment indicated that the proposed technique fulfills the safety considerations and generates a feasible and collision-free trajectory on a curved road. This work offers a number of suggestions for future research directions. $\left(1\right)$ The weighting matrix calculation was based on the gain matrix $\overline{K}$ in (4), which needed to be specified such that the closed-loop poles remain inside of the unit circle. However, the selection of the poles was chosen using Ackerman’s pole placement method. This approach can be extended by means of reinforcement learning in future research. In this way, the position of the poles inside of the unit circle can be adjusted depending on different operating conditions, such as variations in surface friction. $\left(2\right)$ A path tracking controller can be integrated as a lower-level controller to capture the vehicle dynamics to improve the stability and handling of the vehicle in extreme driving conditions.