*3.2. Objective Function*

To ensure that the trajectory tracking controller can promptly and smoothly track the expected trajectory, the following form of objective function is adopted:

$$J\_{\varepsilon}(k) = \sum\_{i=1}^{N\_{\rm cp}} \left\| \eta\_{\varepsilon}(k + i|t) - \eta\_{\rm prcf}(k + i|t) \right\|\_{\mathcal{Q}\_{\varepsilon}}^2 + \sum\_{i=1}^{N\_{\rm cr}-1} \left\| \Delta \mathcal{U}(k + i|t) \right\|\_{\mathcal{R}\_{\varepsilon}}^2 + \rho \varepsilon^2,\tag{23}$$

where *Ncp* and *Ncc* are the prediction step size and control step size of the controller respectively; *Qc* and *Rc* are the weight coefficients, ε is the relaxation factor, ρ is the relaxation coefficient, and η*pre f* is the expected trajectory from the trajectory planning controller.

In Equation (24), the first item on the right side of the equal sign reflects the degree of tracking accuracy of the system; the second item is the constraint on the change of control quantity and increment of control quantity, reflecting the vehicle's ability to maintain stability; the third item is the relaxation factor, which prevents the objective function from having no solution in the real-time calculation process.

In the objective function, it is necessary to calculate the output of the vehicle in the predictive time domain based on the linear error model, and Equation (23) was converted into:

$$\begin{cases} \ \widetilde{\xi\_{\varepsilon}}(k+1|t) = \widetilde{A}\_{\varepsilon}(k)\dot{\xi}\_{\varepsilon}(k|t) + \widetilde{B}\_{\varepsilon}(k)\Delta v\_{\varepsilon}(k|t) \\\ \eta\_{\varepsilon}(k|t) = \widetilde{C}\_{\varepsilon}\dot{\xi}\_{\varepsilon}(k|t) \end{cases} \tag{24}$$

*Sensors* **2020**, *20*, 4821

where *A c*(*k*) = *Ac*(*k*) *Bc*(*k*) 0*m*×*n Im* , *Bc*(*k*) = *Bc*(*k*) *Im* , *Cc* = 0 0 1 0 0 0 00 0 0 0 0 1 0 00 , *m* is the dimension of state quantity, and *n* is the dimension of control quantity.

To simplify the calculation, assume *k* = 1, ... ,*t* + *N* − 1, and the predicted output expression of the system can be deduced as follows:

$$\mathcal{Y}\_{\mathfrak{c}}(t) = \psi\_{\mathfrak{c}} \widetilde{\mathfrak{z}\_{\mathfrak{c}}}(t \Big| t) + \Theta\_{\mathfrak{c}} \Delta l I\_{\mathfrak{c}}(t \Big| t) \tag{25}$$

$$\begin{aligned} \text{where} \quad \boldsymbol{Y}(t) &= \begin{bmatrix} \eta\_{c}(t+1|t) \\ \eta\_{c}(t+2|t) \\ \vdots \\ \eta\_{c}(t+N\_{lc}|t) \\ \end{bmatrix}, \quad \boldsymbol{\psi}\_{c} &= \begin{bmatrix} \widetilde{\mathbf{C}}\_{c}\widetilde{B}\_{c} \\ \widetilde{\mathbf{C}}\_{c}\widetilde{B}\_{c}^{2} \\ \vdots \\ \widetilde{\mathbf{C}}\_{c}\widetilde{B}\_{c}^{N\_{c}} \\ \end{bmatrix}, \quad \boldsymbol{\psi}\_{c} &= \begin{bmatrix} \widetilde{\mathbf{C}}\_{c}\widetilde{B}\_{c} \\ \widetilde{\mathbf{C}}\_{c}\widetilde{B}\_{c}^{2} \\ \vdots \\ \widetilde{\mathbf{C}}\_{c}\widetilde{B}\_{c}^{N\_{c}} \\ \widetilde{\mathbf{C}}\_{c}\widetilde{B}\_{c}^{N\_{c}} \\ \end{bmatrix}, \quad \boldsymbol{\Delta}\boldsymbol{L}\_{c}(t) &= \begin{bmatrix} \Delta\boldsymbol{u}(t|t) \\ \Delta\boldsymbol{u}(t+1|t) \\ \vdots \\ \Delta\boldsymbol{u}(t+N\_{c}|t) \\ \end{bmatrix}, \quad \text{and} \quad \boldsymbol{\Theta}\_{c} &= \begin{bmatrix} \Delta\boldsymbol{u}(t|t) \\ \Delta\boldsymbol{u}(t+1|t) \\ \vdots \\ \Delta\boldsymbol{u}(t+N\_{c}|t) \\ \end{bmatrix}. \end{aligned}$$

By substituting Equation (26) into Equation (24), the complete objective function can be obtained.

## *3.3. Constraint Condition*

One of the advantages of the MPC controller is its ability to handle multiple target constraints. On the one hand, the design of the constraints in the optimization solution should match the mechanical design constraints of the vehicle steering mechanism. On the other hand, it should also satisfy the needs of vehicle smooth control. The vehicle dynamic constraints need to be considered in the actual trajectory tracking control process, and the specific constraints include the centroid slip angle constraint, tire slip angle constraint, and road adhesion condition.

During the obstacle avoidance process, the front wheel angle and the increment of front wheel angle should satisfy the following constraints:

$$\begin{cases} \begin{array}{c} -25^{\circ} \le \delta\_f \le 25^{\circ} \\ -0.47^{\circ} \le \Delta \delta\_f \le 0.47^{\circ} \end{array} \end{cases} \tag{26}$$

The centroid slip angle directly affects the vehicle's driving stability and is an important reference index in vehicle stability control. The empirical formula of the centroid slip angle constraint is expressed as follows:

$$-\arctan(0.196\mu) \le \beta \le \arctan(0.196\mu),\tag{27}$$

where μ is the coefficient of road adhesion.

According to the relationship between the centroid slip angle and the front wheel angle, the tire slip angle can be expressed as:

$$\begin{cases} \alpha\_f = \frac{v\_y + l\_f \dot{q}}{v\_x} - \delta\_f\\ \alpha\_r = \frac{v\_y + l\_r \dot{q}}{v\_x} \end{cases} . \tag{28}$$

There is a linear relationship between the slip angle and the corresponding lateral force of tire when the tire slip angle is relatively small. Hence, the front tire slip angle constraint can be expressed as:

$$-6^{\circ} < \alpha\_f < 6^{\circ} \tag{29}$$

The road adhesion condition determines the range of vehicle lateral force that can be provided, and it also affects vehicle control stability. The following constraints should be met between the vehicle lateral acceleration and the road adhesion condition:

$$-\mu \,\mathrm{g} \le a\_{\mathcal{Y}} \le \mu \,\mathrm{g} \tag{30}$$

Therefore, the specific optimization problem can be equivalent to the multi-constraint quadratic programming problem, which can be expressed as:

$$\begin{aligned} \min & \sum\_{i=1}^{N\_{\mathcal{O}}} \left\| \eta\_c(k+i|t) - \eta\_{\mathit{pref}}(k+i|t) \right\|\_{Q\_c}^2 + \sum\_{i=1}^{N\_{\mathcal{C}}-1} \left\| \Delta \mathcal{U}(k+i|t) \right\|\_{R\_c}^2 + \rho \varepsilon^2\\ & \text{s.t.} \quad -25^\circ \le \delta\_f \le 25^\circ\\ & -0.47^\circ \le \Delta \delta\_f \le 0.47^\circ\\ & -6^\circ < a\_f < 6^\circ\\ & -\mu g \le a\_y \le \mu g\\ & \varepsilon > 0 \end{aligned} \tag{31}$$

By solving Equation (32), the increment sequence of the control quantity can be expressed as:

$$
\Delta \mathcal{U}(t) = \begin{bmatrix}
\Delta u(t|t) \\
\Delta u(t+1|t) \\
\vdots \\
\Delta u(t+N\_c - 1|t)
\end{bmatrix} \tag{32}
$$

On this basis, the first increment of control quantity in Equation (33) is taken as the actual output and is superimposed with the actual output control quantity in the previous period to obtain the actual control output quantity in the current period:

$$
\mu(t) = \mu(t-1) + \Delta\mu(t|t) \tag{33}
$$

The actual output control quantity was implemented on the system, and the objective function was resolved based on the feedback state quantity in the next control cycle. Therefore, the incremental sequence of the control quantity was constantly updated to achieve the purpose of rolling optimization. Finally, the above optimization solution process was repeated to complete the vehicle trajectory tracking control.

#### **4. Driving Simulator Experiments**

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

To make the trajectory planned by the obstacle avoidance trajectory planning controller satisfy the safety requirements and be more human-like, it is necessary to extract the human driver's obstacle avoidance trajectory and perform statistical analysis on the trajectory characteristics, which provides a basis for the parameters design of the trajectory planning controller. Therefore, in this study, the obstacle avoidance experiments based on a driving simulator with six degrees of freedom were implemented, and the obstacle avoidance trajectories from different drivers were extracted for further analysis.
