**1. Introduction**

The vehicle active obstacle avoidance system is one of the core issues in the research of autonomous vehicle (AV) control [1,2]. A safe and reasonable obstacle avoidance trajectory planning in real time based on accurate obstacle information perception through multiple sensors can promote trajectory tracking technology, which can e ffectively improve the intelligent level of the autonomous system and reduce the frequency of tra ffic accidents [3–5]. As one of the key technologies of an active obstacle avoidance system for vehicles, the local trajectory replanning refers to designing a safe trajectory that enables AVs to promptly and accurately bypass obstacles based on global path planning [6]. Under the premise of satisfying multiple constraints, the designed trajectory should also comply with human drivers' driving characteristics of obstacle avoidance. Therefore, active obstacle avoidance trajectory planning and control have become a di fficulty in vehicle lateral control. Determining how to ameliorate

the human-like degree of trajectory planning and tracking is the basis for achieving obstacle avoidance control, and is also an e ffective way for improving the safety and acceptability of AVs [7].

At present, the methods of local trajectory planning mainly include the fuzzy logic (FL) method, genetic algorithm (GA), neural network (NN) method, A\* algorithm, rapidly exploring random tree (RRT), state-space trajectory-generation (ST), and artificial potential field (APF) method. The FL method is combined with the perception action of fuzzy control and replaces mathematical variables with linguistic variables [8]. Fuzzy control conditional statements describe the complex relationships between variables. Although the accuracy of the designed trajectory is improved, the FL algorithm itself lacks flexibility. The fuzzy rules and membership degree cannot change with the environment. The GA seeks the optimal solution by imitating the mechanism of selection and inheritance in nature, and then plans the trajectory through coding, crossover, mutation, and the construction of fitness function [9]. However, the programming implementation is relatively complex, and it is di fficult to ensure real-time performance. The NN method uses a biologically-inspired NN method to establish an obstacle avoidance trajectory planning model. This method treats each grid as a neuron and the motion space of the vehicle is regarded as a topological NN [10]. Through training the grid, the parameters of the NN model can be adjusted to adapt to di fferent scenarios. Therefore, the algorithm has good learning ability and stability, though the interpretability of the model is insu fficient.

The heuristic search method represented by the A\* algorithm has the characteristics of fast calculation speed and good flexibility [11]. Weight A\* and ARA\* accelerate the search direction to the target position by introducing and adjusting the weight value of the heuristic function [12]. A\*-Connect adopts a bidirectional search strategy to obtain higher search e fficiency [13]. Hybrid A\* uses the variant of the A\* search to plan trajectories that conform to kinematic constraints based on the three-dimensional motion state space, achieving local optimization through numerical nonlinear optimization [14]. By considering the vehicle motion direction information and the forward and backward movement patterns, a four-dimensional search space is constructed through this algorithm. The RRT method is an e fficient planning method in multi-dimensional space [15]. RRT\* adopts the tree node in the neighborhood with the lowest total generation value as the parent node, and reconnects the tree nodes in the neighborhood in each iteration, allowing the path on the random tree to always be progressively optimal. Informed-RRT\* establishes an ellipsoid with the initial point and the target point as the focus [16]. This ellipsoid is employed as the search domain to gradually optimize the path, and the area of the ellipsoid decreases during the search process. Finally, the ellipsoid converges into the optimal path. Reachability-guided RRT reduces the sensitivity of systems with di fferent constraints to random sampling for measurements in the extended tree, and it is only possible to select the node when the distance from the sampling point to the node is greater than the distance from the sampling point to the accessible set of the node [17]. Continuous-curvature RRT adopts a target-biased sampling strategy, a node connection mechanism based on a reasonable metric function, and a post-processing method that satisfies vehicle motion constraints to improve the speed and quality of planning [18].

The APF method was first proposed by Khatib in the study regarding the obstacle avoidance trajectory planning of robots [19]. It has the characteristics of a small amount of calculation, short planning time, and high execution e fficiency. In recent years, it has been gradually applied to the local trajectory planning of intelligent vehicles [20]. The basic theory is to establish an obstacle repulsive potential field and a target point gravitational potential field through the sensor's perception of the environment, as well as to find the descending path of the total potential field as the obstacle avoidance path in the compound potential field [21]. However, the traditional APF method has problems such as local minimum points, and the obstacle avoidance trajectory planning for the robot does not consider the robot's own size range and boundary environment. The intelligent vehicle's obstacle avoidance is also constrained by the size of the vehicle and obstacles and the road boundary [22]. In addition, the planning trajectory should also meet the actual dynamic and kinematic constraints. Therefore, the traditional APF method needs to be improved to satisfy the need for trajectory planning for AVs. Tokson et al. [23] used the gradient descent algorithm of the potential field to find an e ffective

path in the potential energy field, and added repulsive potential energy when the vehicle failed into a minimum point, to construct a modified potential field to make the controlled object continue to move towards the target point. Bounini et al. [24] proposed the concept of the steering potential field. The steering potential field is established by issuing steering commands to intelligent vehicles through remote control stations or vehicle-mounted navigation systems. Then, the obstacle repulsion potential field is established according to obstacle data. Raja et al. [25] introduced a gradient function in the traditional APF method, which was composed of gravity, repulsion, tangential force, and gradient force according to a certain weight to form a modified potential field function. The gradient force is the function of vehicle yaw and pitch angle at a specific position, which ensures that the vehicle does not drive in the direction of high gradient force, thus obtaining the expected obstacle avoidance trajectory. Zhang et al. [26] employed the elliptic distance to replace the actual distance in the traditional repulsion potential field, and comprehensively considered the influence of lane lines, obstacles, and the road boundary potential energy field on vehicles to obtain a smoother local obstacle avoidance trajectory. Kenealy et al. [27] proposed an enhanced space-based potential field model to realize through the exploration of complex environments for autonomous robots.

Trajectory tracking has been the subject of numerous empirical studies that have investigated di fferent control algorithms to establish an autonomous control model. The commonly used tracking control algorithms mainly include the proportion integration di fferentiation (PID) control algorithm, optimal preview control (OPC) algorithm, fuzzy control algorithm, sliding mode control (SMC) algorithm, linear quadratic regulator (LQR) control algorithm, and model predictive control (MPC) algorithm. The traditional PID control mainly relies on adjusting the gains of the three parts of the proportional unit, integral unit, and di fferential unit to set its characteristics [28]. This algorithm is simple and easy to operate, but has poor adaptability in trajectory tracking under complex conditions. The OPC algorithm sets a preview point on the forward road of the vehicle, and realizes the tracking control of the expected trajectory by reducing the lateral deviation between the preview point and the expected road center line [29]. Liu et al. [30] improved the preview distance based on longitudinal speed and steering angle feedback, and designed a trajectory tracking controller according to the preview error model to make the change of steering angle more stable. Park et al. [31] applied the proportion integration control in the lateral o ffset to reduce tracking error and alleviate the impact of preview distance with velocity change. The fuzzy control algorithm is mainly divided into three parts: input fuzzy, fuzzy reasoning, and defuzzification [32]. The trajectory tracking is realized by designing di fferent membership functions. The increment of the front wheel angle was taken as the output of the controller for the controller designed by Trabia et al. [33], which included a steering fuzzy module and an obstacle avoidance fuzzy module. This algorithm reduced the computation of the controller. The SMC algorithm, also known as variable structure control, can dynamically change based on the current state of the system to achieve the goal of gradually stabilizing to the equilibrium point according to the predetermined state trajectory [34]. The MPC algorithm has advantages in dealing with linear and nonlinear systems with constraints [35]. Kong et al. [36] designed MPC controllers based on vehicle kinematics and dynamics, and implemented tests to compare the prediction errors of the two controllers. The results demonstrated that the MPC controller based on kinematics had better performance and less computation under the low speed condition. Zanon et al. [37] combined the nonlinear MPC with the moving vision estimation method to solve the problem of poor trajectory tracking accuracy on the road with a low friction coe fficient.

How to improve the real-time performance and human-like degree is the di fficulty and kernel of trajectory planning research. The trajectory planning model based on machine learning and deep learning algorithm can take into account the above two key points, but the inexplicability of the model still cannot be e ffectively solved. The heuristic search method represented by the A\* algorithm has the characteristics of fast calculation speed. However, there are some di fferences between the planned trajectory and the actual driving trajectory derived from human drivers, since this type of algorithm relies more on the data processing method of computer and lacks a mechanism model similar to driver behavior. The traditional APF model could simulate the obstacle avoidance behavior of drivers, but how to prompt the human-like degree and some defects of the algorithm still need further study. In the actual driving process, the driver will control the vehicle in advance through preview behavior, and MPC algorithm can simulate the preview behavior of the driver by adjusting the prediction time domain. Existing research combines the APF trajectory planning model with the MPC algorithm to achieve obstacle avoidance. Due to the complexity of the vehicle dynamic model and considering the real-time requirements, the prediction time domain in the MPC algorithm cannot set too large. In addition, the vehicle kinematic model is frequently ignored in the control models. On the one hand, the human-like degree of the obstacle avoidance control would be weakened, and on the other hand, the comfort and smoothness of the planned trajectory would be influenced.

To address the deficiencies in the obstacle avoidance trajectory planning model based on the APF algorithm and the trajectory tracking model based on the MPC algorithm, a modified APF algorithm was proposed in the present research by establishing a road boundary repulsion potential field and an obstacle repulsion potential field with variable parameter. To make the planned obstacle avoidance trajectory meet the vehicle kinematics constraints and ameliorate the human-like degree, the APF algorithm was combined with the MPC algorithm to construct the obstacle avoidance trajectory replanning controller. Considering that there are many kinds of constraints during vehicle lateral control and for the sake of guaranteeing the real-time capability, accuracy, and robustness of the trajectory tracking control algorithm at di fferent speeds, a linear time-varying model predictive trajectory tracking controller was established based on linearizing the vehicle monorail dynamic model. The controller on the basis of MPC determined the vehicle front wheel angle as the control variable, and multiple constraints for the vehicle dynamics and kinematics were combined to design the objective function that can achieve the requirements of fast and accurate tracking of the desired trajectory. In addition, this work implemented driver obstacle avoidance experiments under di fferent speeds based on a driving simulator with six degrees of freedom to ensure that the established trajectory planning model was consistent with a human driver's obstacle avoidance characteristics; that is, the planning trajectory was similar to the driver operation trajectory. Two pivotal parameters in the APF algorithm were determined to enhance the human-like degree of planned trajectory and the trajectory characteristics derived from human drivers were extracted to provide a basis for the parameters design of the proposed trajectory planning model for AVs. Finally, the co-simulation model based on CarSim/Simulink was established for the o ff-line simulation testing of the obstacle avoidance trajectory planning controller and the trajectory tracking controller designed in this study.

The remainder of the paper is organized as follows. Section 2 details the obstacle avoidance trajectory planning model based on the APF algorithm and the MPC algorithm. Section 3 provides detailed information on the trajectory tracking model based on the linear time-varying MPC algorithm. Section 4 presents the experimental design, process, equipment, and feature analysis of the human driver's obstacle avoidance trajectory. The co-simulation results of the proposed trajectory planning controller and trajectory tracking controller are introduced in Section 5. Finally, conclusions are presented in Section 6. The main framework of this study is presented in Figure 1.

**Figure 1.** Human-like obstacle avoidance system framework for AVs.

#### **2. Obstacle Avoidance Trajectory Planning Model**

#### *2.1. Traditional Artificial Potential Field Model*

Khatib first proposed the APF algorithm in 1986. The basic idea of this algorithm is to virtualize the motion of the controlled object in the environment as a forced motion of particles in the artificial virtual force field [38]. The obstacle exerted a repulsive force on the controlled object, and the target point exerted a gravitational force on the controlled object. The controlled object moved toward the combined force of the repulsive force and the gravitational force, as shown in Figure 2. In the figure, *Frep* is the repulsive force generated by the obstacle, *Fatt* is the gravitational force generated by the target point, and *Fsum* is the resultant force. The distance between the controlled object and the obstacle and the target point mainly determines the magnitude of the repulsive force and gravitational force. The smaller the distance between the controlled object and the obstacle, the greater the repulsive force. Further, the greater the distance between the controlled object and the target point, the greater the gravity.

**Figure 2.** Model of the traditional APF algorithm.

In the traditional APF algorithm, the controlled object is reduced to a particle, and its motion space is regarded as a two-dimensional Euclidean space. Assuming that the coordinate of the controlled object *X* in space is (*x*, *y*) and the target point *Xgoal* coordinate is *xgoal*, *ygoal*, the gravitational field function of the controlled object in space is defined as a quadratic function related to the position of the controlled object and the target point:

$$\mathcal{U}\_{\rm att}(\mathbf{X}) = \frac{1}{2} k\_{\mathcal{S}} \rho\_{\mathcal{S}} \, ^2 \text{.} \tag{1}$$

where *kg* is the gain coefficient of the gravitational potential field, ρ*g* is the relative distance between the controlled vehicle and the target point, the value is the vector, and the direction is the controlled vehicle points to the target point.

The gravitational force on the controlled object can be obtained by calculating the negative gradient of the gravitational potential field:

$$F\_{\rm att}(X) = -\nabla \ell \mathcal{U}\_{\rm att}(X) = -k\_{\mathcal{J}} \overline{\boldsymbol{u}}\_{\mathcal{J}'} \tag{2}$$

where *u g* is the unit vector where the controlled object points to the target point.

Assuming that the coordinates of the obstacle *Xobs* in the space is (*xobs*, *yobs*), the repulsive force field function on the controlled object is defined as:

$$\mathcal{U}\_{\text{rep}}(X) = \begin{cases} \frac{1}{2} k\_o \left( \frac{1}{\rho\_{ob}} - \frac{1}{\rho\_o} \right)^2, \rho\_{ob} \le \rho\_o \\\ 0, \rho\_{ob} > \rho\_o \end{cases} \tag{3}$$

where *ko* is the repulsive potential field coefficient, ρ*ob* is the distance constant between the controlled object and the obstacle, and ρ*o* is the influence range of the repulsive potential field of the obstacle. When ρ*ob* > ρ*<sup>o</sup>*, the controlled object is not affected by the repulsive force of the obstacle.

The repulsive force on the controlled object can be obtained by calculating the negative gradient of the repulsive potential field:

$$F\_{\rm rep}(X) = -\nabla l L\_{\rm rep}(X) = \begin{cases} k\_o \left( \frac{1}{\rho\_{ob}} - \frac{1}{\rho\_o} \right)^2 \overline{\tilde{u}\_{ob\prime}} \rho\_{ob} \le \rho\_o \\\ 0, \rho\_{ob} > \rho\_o \end{cases} \tag{4}$$

where *uob* is the unit vector where the obstacle points to the controlled object.

Therefore, the combined force of the controlled object when moving in the force field space is:

$$F\_{\text{sum}}(X) = -\nabla l I\_{\text{sum}}(X) = F\_{\text{att}}(X) + \sum\_{i=1}^{n} F\_{\text{rep},i}(X),\tag{5}$$

where *n* is the number of obstacles.

The traditional APF algorithm has the following problems when it is used to plan the local obstacle avoidance trajectory of vehicles [39].

(1) Lack of road boundary constraints. The algorithm only considers the passability of obstacle avoidance trajectories, and does not consider the road boundary constraints during vehicle driving.

(2) The goal may be unreachable. When there is an obstacle near the target point, the repulsive force of the vehicle when approaching the target point is greater than the gravitational force, so that the controlled object cannot reach the target point.

(3) The controlled object may come to a deadlock. There may be a situation where the controlled object receives the same repulsive force and gravity at a certain point, resulting in the controlled object being unable to continue to advance.

#### *2.2. Modified Artificial Potential Field Model*

In order to solve the above deficiencies in the traditional APF algorithm, a modified APF model is proposed through establishing the road boundary repulsive potential field, ameliorating the obstacle potential field, and combining with the MPC algorithm.

#### 2.2.1. Road Boundary Repulsive Potential Field

Road boundary repulsive potential field is established on the basis of the lane boundary, which is used to limit the driving area of vehicles to ensure that vehicles continue to drive along the center line of the lane after obstacle avoidance, and the vehicle body would not exceed the road boundary during the process of turning and avoiding obstacles. The established road boundary repulsive potential field is presented in Figure 3. The repulsive potential field generates a force based on the road boundary in the direction of the vehicle, and the repulsive force only takes the lateral force component of the earth coordinate system. The value of the road boundary repulsion is inversely proportional to the relative distance between the vehicle and boundary. The smaller the relative distance, the greater the repulsion, and the larger the relative distance, the smaller the repulsion.

**Figure 3.** Schematic diagram of the road boundary repulsive potential field force.

When there is no obstacle in the lane, the vehicle travels along the center line of the right lane under the action of the repulsion potential field of the road boundary. Considering the size of the vehicle, the road boundary repulsive potential field model is established as follows:

$$\begin{cases} \llcorner \mathcal{U}\_{L\\_rep}(\mathcal{X}) = \frac{1}{2} k\_{L\\_rep} \left( \frac{1}{\rho\_{L\\_rep} - \frac{\mathcal{U}\_T}{2}} \right)^2 \\\llcorner \mathcal{U}\_{R\\_rep}(\mathcal{X}) = \frac{1}{2} k\_{R\\_rep} \left( \frac{1}{\rho\_{R\\_rep} - \frac{\mathcal{U}\_T}{2}} \right)^2 \end{cases} \tag{6}$$

where *kL* \_*rep* and *kR*\_*rep* are the repulsive potential field coefficients of the left and right road boundaries, respectively; *wv* is the lateral width of the vehicle, and ρ*<sup>L</sup>*\_*rep* and ρ*<sup>R</sup>*\_*rep* are the shortest distances between the center of mass of the vehicle and the boundary of the left and right lanes, respectively.

The repulsive force on the controlled object can be obtained by calculating the negative gradient of the road boundary repulsive potential field:

$$\begin{cases} F\_{L\text{-}rep}(X) = k\_{L\text{-}rep} \left( \frac{1}{\rho\_{L\text{-}rep} - \frac{\text{np}}{2}} \right) \left( \frac{1}{\rho\_{L\text{-}rep}} \right)^3 \overline{a}\_{L\text{-}x} \\\ F\_{R\text{-}rep}(X) = k\_{R\text{-}rep} \left( \frac{1}{\rho\_{R\text{-}rep} - \frac{\text{np}}{2}} \right) \left( \frac{1}{\rho\_{R\text{-}rep}} \right)^3 \overline{a}\_{R\text{-}x} \end{cases} \tag{7}$$

where *a Lv* and *a Rv* are the unit vector where the road boundaries point to the controlled object.

#### 2.2.2. Obstacle Repulsive Potential Field

The circular repulsion field of the traditional APF does not satisfy the requirements of the actual vehicle obstacle avoidance trajectory according to the human driver experience, and it is difficult to meet the requirements of steering smoothness in the trajectory planning, resulting in a decrease of ride comfort. Therefore, the scope of action of the potential field was modified in this work, and the longitudinal action distance of the obstacle repulsion potential field was increased, so that the vehicle can correct the direction in advance to avoid obstacles; the lateral action distance was reduced to prevent the vehicle from driving out of the lane during obstacle avoidance. Figure 4 illustrates the schematic diagram of the obstacle repulsive potential field. The longitudinal and lateral

acting distances of the repulsive potential field of an obstacle were defined as A and B, respectively. The scope of action of the repulsive field ρ*o* can be rewritten as:

> + (*y* − *yobs*)<sup>2</sup> *B*<sup>2</sup>

(8)

ρ*o* ∈ (*x* − *xobs*)<sup>2</sup> *A*<sup>2</sup>

**Figure 4.** Schematic diagram of the road boundary repulsive potential field force.

Considering that the obstacle avoidance process is similar to the lane change process, the force exerted by the obstacle repulsive potential field on the vehicle can only be retained in the lateral component under the geodetic coordinate system to avoid the vehicle coming to a deadlock. The repulsive direction of the obstacle to the vehicle is upward when the vehicle enters the obstacle repulsive potential field. At this time, the vehicle will turn to the left for avoidance. During this process, the repulsive potential energy increases with the decrease of the relative distance between the vehicle and the obstacle, thus forcing the vehicle to drive away from the obstacle.

The obstacle repulsive potential field is established with an obstacle as the center of the potential energy. Within the scope of action of the repulsive potential field of an obstacle, it exerts a repulsive force on the vehicle to keep the vehicle away from the obstacle. In the traditional APF model, the gravitational force is less than the repulsive force when the vehicle reaches the target point, which will lead to the problem of unreachable target. Therefore, an adjustment factor *Rmd* is added to the obstacle repulsive potential field. In this way, the relative distance between the vehicle and the target point *Rd* is supplemented in the modified obstacle repulsive potential field. Hence the repulsive force and the gravitation force are reduced to zero at the same time only when the vehicle reaches the target point, so that the problem of unreachable target is solved. The modified obstacle repulsion potential field function is shown as follows:

$$\mathcal{U}\_{o\\_rep}(X) = \left\{ \begin{array}{c} \eta\_{rep} \Big| \exp\Big[ -\frac{1}{2} \Big( \frac{(x - x\_{obs})^2}{A^2} + \frac{(y - y\_{obs})^2}{B^2} \Big) \Big] \Big| \Big| R\_{d\\_\prime}^m \, p\_{o\\_rep}^n \le \rho\_o \\\ 0, p\_{o\\_rep}^n > \rho\_o \end{array} \tag{9}$$

where *Rd* is the relative distance between the vehicle and the target point, *m* is constant, η*rep* is the repulsive potential field coefficient of the obstacle, *pno* \_*rep* is the distance between the vehicle and the *nth* obstacle, and ρ*o* is the range of action of the repulsive field.

In addition, the vehicle may come to a deadlock when the repulsive force from other surrounding vehicles is equal to the gravitational force. In this case, the value of *m* in the adjustment factor will gradually increase from 0 until the force balance is broken, so that the vehicle could jump out of the local minimum and then the value of *m* would return to the original value. Within the scope of the obstacle, the repulsive force on the controlled object can be obtained by calculating the negative gradient of the obstacle repulsive potential field:

$$F\_{o\\_rep}(X) = \begin{cases} -\eta\_{\Gammacp} \left( \frac{(x-x\_{\rm{sh}})^2}{A^2} + \frac{(y-y\_{\rm{sh}})^2}{B^2} \right)^2 R\_d^m \overrightarrow{a}\_{\rm{ov}} \; , \; p\_{o\\_rep}^n \le \rho\_o \\\ 0, \; p\_{o\\_rep}^n > \rho\_o \end{cases},\tag{10}$$

where *a ov* is the unit vector where the obstacle points to the controlled object.

#### *2.3. Model Prediction Algorithm With Trajectory Planning*

To ensure that the planning trajectory of the modified APF model is practical and can satisfy the kinematic constraints of the vehicle, the MPC algorithm was combined with the modified APF model and a reasonable objective function was constructed to minimize the deviation between the planning trajectory of the modified APF model and the predicted trajectory of the MPC algorithm. Due to the low real-time requirement of the planning layer, the adoption of a relatively simple point mass model can fully meet the requirements of re-planning. Therefore, as shown in Figure 5, the steering motion model was established with XOY as the geodetic coordinate system and xoy as the vehicle coordinate system.

**Figure 5.** The vehicle kinematics model.

The vehicle kinematics model can be expressed as follows:

$$\begin{cases} \dot{X} = \dot{x}\cos(\varphi) - \dot{y}\sin(\varphi) \\ \dot{Y} = \dot{x}\sin(\varphi) - \dot{y}\cos(\varphi) \\ \dot{\varphi} = \frac{\ddot{\varphi}}{\dot{x}} \\ \ddot{x} = 0 \end{cases} \tag{11}$$

where . *x* and . *y* represent the longitudinal and lateral speeds in the vehicle coordinate system, respectively; .. *x* and .. *y* correspondingly represent the longitudinal and lateral accelerations in the vehicle coordinate system; ϕ and . ϕ represent the yaw angle and yaw rate of the vehicle, respectively; . *X* and . *Y* correspondingly represent the longitudinal and lateral speeds in the geodetic coordinate system.

This article only considers the obstacle avoidance strategy of the vehicle at constant speed, so the longitudinal acceleration is set to zero. Five discrete state variables were determined as X = . *x*, . *y*,ϕ, *X*,*Y* , and the lateral acceleration was selected as the control variable *v* = .. *y* . Then the state equation can be expressed as:

$$
\dot{\lambda}(t) = f(\lambda(t), v(t)) \tag{12}
$$

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

Using Taylor expansion and first-order di fference quotient to linearize and discretize Equation (13), the linear time-varying model can be obtained as follows:

$$
\overline{\chi}(k+1) = \overline{A}\_p(k)\overline{\chi}(k) + \overline{B}\_p(k)\overline{v}(k) \tag{13}
$$

where *<sup>A</sup>p*(*k*) = ⎡ ⎢⎢⎢⎢⎢⎢⎢⎢⎣ 1 0 −.*xsin*(ϕ)*<sup>T</sup>* 0 1 .*xcos*(ϕ)*<sup>T</sup>* 00 1 ⎤ ⎥⎥⎥⎥⎥⎥⎥⎥⎦ , *Bp*(*k*) = ⎡ ⎢⎢⎢⎢⎢⎢⎢⎢⎣ *cos*(ϕ)*<sup>T</sup>* 0 *sin*(ϕ)*<sup>T</sup>* 0 *T* tan δ*f T*/*l* .*xT*/*lcos*<sup>2</sup> δ*f* ⎤ ⎥⎥⎥⎥⎥⎥⎥⎥⎦ , *T* is the sampling time,

*l* is the wheel base, and δ*f* is the front wheel angle.

The control objective in the trajectory planning layer is to minimize the deviation between the planning trajectory of the modified APF model and the predicted trajectory of the MPC algorithm under the premise of ensuring the smooth and comfortable driving of vehicles. Therefore, the objective function of trajectory planning is defined as follows:

$$J\_p(k) = \sum\_{j=1}^{N\_{\overline{\text{pr}}}} \left\| \eta\_p(k+j|t) - \eta\_{\text{ref}}(k+j|t) \right\|\_{\text{Q}\_p}^2 + \sum\_{j=1}^{N\_{\overline{\text{pr}}}-1} \left\| \Delta \overline{v}(k+j|t) \right\|\_{\text{R}\_p}^2 \tag{14}$$

where *Qp* and *Rp* are the weight matrixes, η*re f* is the planning trajectory of the modified APF model, η*p* is the predicted trajectory of the MPC algorithm, and *Npp* and *Npc* are, respectively, the prediction step size and control step size of the MPC controller.

Then the output can be expressed as:

$$
\overline{\mathcal{Y}}(k) = \psi\_p \overline{\mathcal{X}}(k) + \Theta\_p \Delta \overline{v}(k), \tag{15}
$$

$$
\eta\_p(k) = \overline{\mathbb{C}}\_p \overline{\mathbb{X}}(k), \tag{16}
$$

where, Y(*k*) = ⎡ ⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣ <sup>η</sup>*p*(*<sup>k</sup>* + 1|*k*) <sup>η</sup>*p*(*<sup>k</sup>* + 2|*k*) . . . η*p k* + *Npc k* . . . η*p k* + *Npp k* ⎤ ⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ , ψ*p* = ⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣ *C pBp C pB*<sup>2</sup> *p* . . . *C pB Npc p* . . . *C pB Npp p* ⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ , *C p* = 0 0 1 0 0 0 00 0 0 0 0 1 0 00 , <sup>Θ</sup>*p* = ⎛ ⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎝ *C pBp* 000 *C pApBp C pBp* 0 0 . . . . . . ... . . . *C pANpc p Bp C pANpc*−<sup>1</sup> *p Bp* ··· *C pApBp* . . . . . . ... . . . *C pANpp*−<sup>1</sup> *p Bp C pANpp*−<sup>2</sup> *p Bp* ··· *C pANpp*−*Npc*−<sup>1</sup> *p Bp* ⎞ ⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎠ .

It is also necessary to append obstacle avoidance constraints and limit the control variables for the sake of ensuring that the planning trajectory is practical. The obstacle avoidance constraints are divided into road constraints and obstacle constraints:

$$\begin{cases} \rho\_{L\\_rep} > \frac{\overline{w}\_v}{2} \\ \rho\_{R\\_rep} > \frac{\overline{w}\_v}{2} \\ \rho\_{obs} \, ^\wedge > 1.2w\_{obs} \end{cases} \tag{17}$$

where *wobs* is the width of the obstacle.

In addition, the lateral acceleration of the vehicle is mainly provided by the lateral force of the tire, so it must meet the limit of tire adhesion:

$$\ddot{\mathbf{y}} \in [-\mu \,\mathrm{g}, \,\mu \,\mathrm{g}] \tag{18}$$

where μ is the coefficient of road adhesion, and *g* is the acceleration of gravity.

Combining Equations (15), (18) and (19), the trajectory planning model can be expressed as:

$$\begin{cases} \min \sum\_{j=1}^{N\_{\text{pr}}} \left\lVert \eta\_{\text{p}}(k+j|k) - \eta\_{\text{ref}}(k+j|k) \right\rVert\_{Q\_{\text{p}}}^{2} + \sum\_{j=1}^{N\_{\text{pr}}-1} \left\lVert \Delta \overleftarrow{\nu}(k+j|k) \right\rVert\_{R\_{\text{p}}}^{2} \\ \text{s.t.} \quad \rho\_{\text{L-r}\text{rp}} > \frac{w\_{\text{r}}}{2} \\ \rho\_{\text{L-r}\text{rp}} > \frac{w\_{\text{r}}}{2} \\ \rho\_{\text{obs}} \text{s} > 1.2w\_{\text{obs}} \\ -\mu g \le \bar{y} \le \mu g \end{cases} \tag{19}$$

#### **3. Obstacle Avoidance Trajectory Tracking Model**

The MPC algorithm can use the dynamic prediction model to obtain the future vehicle state in a limited time domain based on the current vehicle motion state. This method has a strong ability to deal with multi-objective constraints [40]. In this work, a linear time-varying MPC controller was established to track the trajectory from the obstacle avoidance trajectory planning model.

#### *3.1. Vehicle Dynamic Model*

.

.

Considering that the longitudinal speed remains unchanged and only the front wheel angle is controlled during obstacle avoidance, the following assumptions are made in the modeling process.

(1) The lateral forces and slip angles on the left and right tires of the vehicle are symmetric and equal in the vehicle coordinate system.

(2) The test sections are all flat roads, ignoring the influence of slope and other factors on the vertical movement of vehicles.

(3) The front wheel angle is small, and the lateral force of the tire is approximately linear with the slip angle of the tire.

(4) The influence of the suspension system, transmission system, air resistance, and the longitudinal and lateral coupling force of the tire is ignored.

The monorail dynamics model is established as shown in Figure 6, and the dynamic equation of the model can be described as follows:

$$\begin{cases} \dot{X} = v\_x \cos(\varphi) - \left(\upsilon\_y + l\_f \dot{\varphi}\right) \sin(\varphi) \\ \dot{Y} = v\_y \sin(\varphi) - \left(\upsilon\_y + l\_f \dot{\varphi}\right) \cos(\varphi) \\\ I\_z \ddot{\varphi} = l\_f F\_{yf} - l\_r F\_{yr} \\\ m \dot{\upsilon}\_x = m \dot{\upsilon}\_x \dot{\varphi} + F\_{xf} + F\_{xr} \\\ m \dot{\upsilon}\_y = -m \dot{\upsilon}\_y \dot{\varphi} + F\_{yf} + F\_{yr} \end{cases} \tag{20}$$

where *X* and *Y* represent the longitudinal and lateral speed in the geodetic coordinate system, *vx*, *vy*, and ϕ represent the longitudinal speed, lateral speed, and heading angle in the vehicle coordinate system, *m* represent vehicle mass, *lf* and *lr* represent the distance from the center of mass to the front and rear axles, *Fx f* , *Fxr*, *Fy f* , and *Fyr* represents the longitudinal and lateral forces of the front and rear axles, and *Iz* represent the moment of inertia.

**Figure 6.** The vehicle monorail dynamics model.

The state variables was determined as ξ ˆ *c* = *vx*, *vy*,ϕ, .ϕ, *<sup>X</sup>*,*<sup>Y</sup><sup>T</sup>*, and the control variable was selected as *vc* = δ*f* . To satisfy the real-time requirements of the trajectory tracking controller when the vehicle is traveling at high speeds, the nonlinear dynamic model is linearized to obtain the linear time-varying equation:

$$
\dot{\xi}\_{\mathfrak{c}} = A\_{\mathfrak{c}}(t)\dot{\xi}\_{\mathfrak{c}}(t) + B\_{\mathfrak{c}}(t)v\_{\mathfrak{c}}(t), \tag{21}
$$

where *Ac*(*t*) = ∂ *<sup>f</sup>*.ξ*<sup>c</sup>*,*vc* ∂ .ξ*c* .<sup>ξ</sup>*c*(*t*),*vc*(*t*), and *Bc*(*t*) = ∂ *<sup>f</sup>*.ξ*<sup>c</sup>*,*vc* ∂*vc* .<sup>ξ</sup>*c*(*t*),*vc*(*t*).

Using the first-order difference quotient to discretize Equation (22), the discrete state space expression can be obtained:

$$
\dot{\mathcal{L}}\_{\mathfrak{c}}(k+1) = A\_{\mathfrak{c}}(k)\dot{\xi}\_{\mathfrak{c}}(k) + B\_{\mathfrak{c}}(k)\upsilon\_{\mathfrak{c}}(k) \tag{22}
$$

where *Ac*(*k*) = *Ic* + *TAc*(*t*), *Bc*(*k*) = *Ic* + *TBc*(*t*), *Cc* = 0 0 1 0 0 0 0 0 0 0 1 0 , and *Ic* is unit matrix.
