Research on Path Planning and Tracking Control of Autonomous Vehicles Based on Improved RRT* and PSO-LQR

Abstract

Path planning and tracking control are essential parts of autonomous vehicle research. Regarding path planning, the Rapid Exploration Random Tree Star (RRT*) algorithm has attracted much attention due to its completeness. However, the algorithm still suffers from slow convergence and high randomness. Regarding path tracking, the Linear Quadratic Regulator (LQR) algorithm is widely used in various control applications due to its efficient stability and ease of implementation. However, the relatively empirical selection of its weight matrix can affect the control effect. This study suggests a path planning and tracking control framework for autonomous vehicles based on an upgraded RRT* and Particle Swarm Optimization Linear Quadratic Regulator (PSO-LQR) to address the abovementioned issues. Firstly, according to the driving characteristics of autonomous vehicles, a variable sampling area is used to limit the generation of random sampling points, significantly reducing the number of iterations. At the same time, an improved Artificial Potential Field (APF) method was introduced into the RRT* algorithm, which improved the convergence speed of the algorithm. Utilizing path pruning based on the maximum steering angle constraint of the vehicle and the cubic B-spline algorithm to achieve path optimization, a continuous curvature path that conforms to the precise tracking of the vehicle was obtained. In addition, optimizing the weight matrix of LQR using POS improved path-tracking accuracy. Finally, this article’s improved RRT* algorithm was simulated and compared with the RRT*, target bias RRT*, and P-RRT*. At the same time, on the Simulink–Carsim joint simulation platform, the PSO-LQR is used to track the planned path at different vehicle speeds. The results show that the improved RRT* algorithm optimizes the path search speed by 34.40% and the iteration number by 33.97%, respectively, and the generated paths are curvature continuous. The tracking accuracy of the PSO-LQR was improved by about 59% compared to LQR, and its stability was higher. The position error and heading error were controlled within 0.06 m and 0.05 rad, respectively, verifying the effectiveness and feasibility of the proposed path planning and tracking control framework.

1. Introduction

In recent years, the rapid development of artificial intelligence has driven the technological progress of self-driving vehicles and intelligent transportation systems. Autonomous vehicles are one of the core components of intelligent transportation systems with increasing safety and reliability requirements [1,2,3,4]. Path planning and tracking control are two core issues in the autonomous driving system. The main goal of path planning is to efficiently find a collision-free and feasible path from the starting point to the target point in the workspace, while path tracking control solves the problem of how to control the vehicle to move along the pre-planned path [5,6]. The planned path’s quality directly affects the vehicle’s driving performance during autonomous driving. In contrast, the ability to accurately and stably track the planned path directly affects the vehicle’s driving safety. Therefore, path planning and tracking performance are significant for autonomous driving vehicles.

Scholars have proposed many practical algorithms and methods to address the challenges and difficulties in path planning for autonomous vehicles. Path planning algorithms can be divided into five types based on their principles: graph search methods, sampling methods, intelligent bio-inspired methods, numerical optimization methods, and artificial potential field methods [7,8]. Among them, the Rapidly-Exploring Random Tree (RRT), a sampling-based path planning method, is widely used in autonomous vehicle path planning due to its advantages, such as probability completeness, efficiency, scalability, and robustness [9]. However, the traditional RRT algorithm has limitations regarding long path search time and slow algorithm convergence speed. To address the limitations of the traditional RRT algorithm, scholars have proposed many improved RRT methods. A path planning technique based on Goal-Biased RRT [10] has been developed to increase the planning efficiency while also increasing the search efficiency of the algorithm by adding a probability element to the random sampling to create a non-random sampling extension mode. Additionally, some scholars have proposed the RRT-Connect algorithm [11], which generates random trees separately from the starting and target nodes, reducing the search space and improving search speed. To address the shortcomings of the RRT algorithm in ensuring asymptotic optimality, the RRT* algorithm changes the search mode by reselecting the parent node and rewiring, thus generating paths with the best or approximate best length [12].

In order to improve the convergence performance and feasibility of the RRT* algorithm, scholars have proposed the Informed-RRT* [13], which uses heuristic methods to reduce the planning problem to a subset of the original domain, thus improving the algorithm’s search speed. However, its performance is sensitive to the initial solution. Other researchers have attempted to introduce additional algorithmic features to improve RRT and enhance the efficiency of path planning. Qureshi et al. [14] proposed P-RRT*, which uses potential artificial fields to obtain new nodes and reduce the time required to expand to the goal. Chen et al. [15] proposed a VPF-RRT* algorithm for path planning, which introduces virtual potential fields into the RRT* algorithm to adjust the positions of path nodes and solve the problem of excessive randomness in the RRT algorithm. Fan et al. [16] proposed a trajectory planning method based on the target-biased bidirectional APF-RRT* algorithm, which integrates the Artificial Potential Field (APF) method with the Bi-RRT* algorithm that has a target-bias strategy, significantly reducing redundant points. Ayawli et al. [17] proposed an improved heuristic RRT-A* algorithm, in which the A-Star (A*) cost function is introduced into the RRT algorithm to optimize performance. Meanwhile, several metric functions are used as heuristic information functions to measure the performance of different metric functions, optimizing the planning path and reducing computational costs. However, although these algorithms have made significant improvements compared to the traditional RRT, they have yet to consider the steering constraints of the wheels, which means that they have not been directly applied to path planning for autonomous vehicles. Therefore, Ghosh et al. [18] proposed a kinematic constraint Bi-RRT (KB-RRT) algorithm, which limits the number of generated nodes without affecting accuracy and combines kinematic constraints to generate smooth trajectories. Peng et al. [19] proposed a path planning algorithm based on a kinematic constraint RRT integrated with the trajectory parameter space (TP space), introducing a new method to select candidate nodes for tree expansion and integrating RRT with TP space to meet kinematic constraints and improve performance through narrow passages. Liao et al. [20] proposed the Stack-RRT*, which can be combined with smoothing schemes based on different parameter curves to generate feasible paths with different continuity. These algorithms have significantly improved planning efficiency, reduced computational cost, and met kinematic constraints. However, further research and improvements are still needed to achieve more efficient, precise, and safe path planning for autonomous vehicles.

In addition to research on path planning techniques, tracking and control technology are also essential in autonomous vehicles. Generally, there are two types of autonomous vehicle path-tracking control methods: geometric-based methods and model-based methods. Geometric-based methods include the pure pursuit [21] and Stanley methods [22], which are widely used for handling vehicle or robot path tracking problems due to their simplicity and good performance. However, geometric methods are only suitable for vehicles that ignore their dynamic characteristics. In contrast, the model-based path-tracking method is more suitable for autonomous vehicles in real driving scenarios. Model-based tracking methods can be divided into methods based on the kinematics model and methods based on the dynamic model. The control method based on a dynamic model improves the safety and reliability of vehicles under complex working conditions [23]. There are many control methods based on dynamic models, such as Proportional–Integral–Differential (PID) control [24], Fuzzy Logic control [25], Sliding Mode control [26], Model Predictive control [27], and Linear Quadratic Regulator (LQR) control [28]. LQR controllers are widely used in autonomous vehicle path tracking due to their high-precision tracking performance, algorithmic simplicity, applicability to nonlinear systems, and ability to consider dynamic constraints. However, while LQR methods can stabilize the system, they do not consider disturbance terms and can lead to system errors. Therefore, Kapania et al. [29] proposed a feedback + feedforward steering controller to maintain vehicle stability under extreme maneuvering conditions while minimizing the path deviation. However, at high speeds, the steady-state path deviation significantly increases. Xu et al. [30] added path curvature feedforward control to LQR feedback control to reduce the steady-state error of the controller. Yang et al. [31] proposed a feedforward + predictive LQR Lateral control method, which suits intelligent vehicles’ lateral tracking control problem under complex working conditions. Although research based on the LQR control methods has been widely applied to autonomous vehicles, problems, such as input instability in solving LQR, can affect the controller’s performance. Therefore, many researchers have improved control effects by adding optimization algorithms to optimize LQR. Lu et al. [32] proposed an adaptive LQR Path-Tracking controller, which uses a genetic algorithm to optimize the parameters of LQR and applies a fitness function that considers tracking accuracy and vehicle stability. The results show the effectiveness of the controller in improving tracking accuracy and vehicle stability. Wang et al. [33] proposed a lateral path control strategy based on an improved LQR algorithm, which uses fuzzy control to adjust the weight coefficients of LQR in real time according to the vehicle state, improving tracking accuracy, steering stability, and computational efficiency. The improvements made to the LQR algorithm in these studies have shown promising results, but there is still room for further improvement in control accuracy and stability.

The literature review above provides an overview of various research achievements on path planning and tracking control strategies for autonomous vehicles, with numerous studies conducted by scholars in this field. However, the performance of path planning and tracking control for autonomous driving still needs to be improved, and a comprehensive framework is needed to cope with different driving scenarios. This paper proposes a novel framework that combines path planning and tracking control. The framework utilizes an improved RRT* algorithm for path planning, considering the actual operating environment of the vehicle and whether the planned path can meet the requirements for vehicle tracking. Additionally, a PSO-LQR controller is designed for path-tracking testing to form a closed loop, and the feasibility of the proposed planning approach is verified.

The main contributions of this paper:

(1)

For the driving intention of autonomous driving and the urban road scenario in which it is located, this study changes the sampling range of random points from the original complete state space sampling to adaptive Gaussian sampling around the nearest node, which reduces the invalid execution in the sampling process, decreases the number of algorithm iterations, and shortens the computation time.

(2)

This study introduces an improved artificial potential field method into the RRT* algorithm. For the nearest nodes, the target point and random sampling point are set to have different attractive forces on them, and the concept of road boundary repulsion is introduced to make the obstacles and road boundary repel the nearest nodes. The combined direction of gravitational and repulsive forces is used as the extension direction of the new node to control the growth of the random tree toward the target point, reduce randomness, and speed up the path search. This study also uses path pruning based on the maximum steering angle constraint of the vehicle and the thrice B-spline algorithm to optimize the sampled generated paths to obtain paths that match the actual tracking of the vehicle.

(3)

In this study, a PSO-LQR controller with feedforward control is designed to eliminate the external disturbances caused by the target path. Meanwhile, an objective function considering both tracking accuracy and vehicle stability is established to maintain vehicle stability and achieve higher tracking accuracy. The performance of the PSO-LQR controller is verified in this study by conducting simulation experiments on the LQR controller before and after optimization. Moreover, tracking experiments are conducted for planned paths at different speeds using the PSO-LQR controller to verify the feasibility of the improved RRT* algorithm for path planning and the effectiveness of the tracking control performance.

The organization of this paper is as follows: Section 2 introduces the improved RRT* algorithm, Section 3 introduces the PSO-LQR path tracking method, and Section 4 conducts experiments and analysis. Finally, in Section 5, this paper summarizes its contributions and proposes future research directions.

2. Path Planning Algorithm

2.1. Vehicle Kinematic Model

In vehicle path planning, it is necessary to consider the kinematic characteristics of the vehicle itself to ensure that the planned path meets the kinematic constraint conditions of the vehicle. This ensures that the planned path can meet the vehicle’s steering motion characteristics, thereby ensuring the stability and safety of the driving process. The vehicle kinematic model established in this paper is shown in Figure 1.

In the state space shown in Figure 1, the state of the vehicle can be represented by the variable $q=(x,y,\theta ,{\delta }_f)$, where $(x,y)$ represents the coordinate position of the rear axle of the vehicle, $\theta$ represents the angle between the longitudinal axis of the vehicle and the X-axis, and ${\delta }_f$ represents the steering angle of the front wheel. It is necessary to satisfy $\left|{\delta }_f\right|\le {\delta }_f{}_{\max }$ and $l$ as the wheelbase, and R as the turning radius.

Under the constraint of the front wheel steering angle, the vehicle cannot reach any position in the state space. Assuming that the vehicle moves at a constant speed v parallel to the ground, the vehicle can be simplified as a two-degree-of-freedom bicycle model represented by Equation (1).

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=v·\left[\begin{array}{c}\cos (\theta )\\ \sin (\theta )\\ \frac{\tan {\delta }_f}{l}\end{array}\right].$$

(1)

The relationship between the front wheel steering angle, wheelbase, turning radius, and curvature can be obtained.

$${\rho }_{\max }=\frac{1}{R_{\min }}=\frac{\tan {\delta }_f{}_{\max }}{l}$$

(2)

where $R_{\min }$ represents the minimum turning radius, ${\rho }_{\max }$ represents the maximum curvature, and ${\delta }_f{}_{\max }$ represents the maximum steering angle of the front wheels.

Based on the kinematic model and the analysis of the maximum front wheel turning angle, it is known that the curvature of the planned path should satisfy the constraint of the maximum front wheel turning angle. However, the RRT* algorithm does not consider the constraints of the vehicle kinematic model, and the planned path does not fit the kinematic characteristics of the vehicle. Therefore, the path planning of the vehicle can be constrained based on the maximum front wheel turning angle and path curvature to meet the actual driving path requirements of the autonomous vehicle.

2.2. RRT* Algorithm

Some researchers have proposed the RRT* algorithm. Before introducing the RRT* algorithm, this paper first presents the RRT algorithm proposed by other researchers as a basis. The RRT algorithm uses a random sampling method to expand the tree from the initial point to the target point. During each iteration, the RRT algorithm randomly selects a point $q_{rand}$ in the state space. If the point falls within a non-obstacle interval, it will traverse the random expansion tree to find the nearest node $q_{nearest}$ to the random point. If the line between $q_{rand}$ and $q_{nearest}$ does not collide with obstacles and the distance between them is less than the expansion step size, $q_{rand}$ is added to the random tree as a new node $q_{new}$. If the distance between $q_{rand}$ and $q_{nearest}$ is greater than the expansion step size, a point on the line between $q_{rand}$ and $q_{nearest}$, at a distance of the expansion step size from $q_{nearest}$, is intercepted as $q_{new}$ and added to the random expansion tree. The nearest node to $q_{new}$ is then considered the parent node of $q_{new}$. Repeat this iteration process until a feasible path is found from the initial point to the target point.

In contrast, the RRT* algorithm introduces two improvements: reselecting parent nodes and rewiring. Reselecting the parent nodes means that after the RRT algorithm finds a new node, the nearest node to the new node among all nodes within a predefined radius r is selected as the parent node of the new node. Rewiring refers to reorganizing all nodes in the tree near the newly selected parent node after selecting a new parent node, with the principle of minimizing the cost of all nodes to the starting point. These two processes complement each other, with reselecting the parent nodes minimizing the cost of the newly generated node’s path as much as possible, and rewiring reducing redundant paths in the random tree after generating a new node, thereby reducing path costs. These improvements significantly reduce the path cost of the RRT algorithm.

To improve the search efficiency of the RRT* algorithm, researchers have proposed a goal-biased RRT* algorithm, which, given a target-biased probability factor $p_{target}$, and a random probability value $p$ is obtained between 0 and 1. When $p>p_{target}$, a random state named $q_{rand}$ is obtained in the search space, otherwise $q_{rand}$ is equal to $q_{goal}$. This algorithm maintains the characteristics of the original algorithm and accelerates the convergence speed to the target node.

2.3. Improved RRT* Algorithm

The flow chart of the improved RRT* algorithm in this paper is shown in Figure 2. Firstly, the algorithm is initialized, and relevant parameters are set. Then the variable sampling area method is used to restrict the generation of random sampling points. After generating the random sampling point $q_{rand}$, find its nearest node $q_{nearest}$, use the improved APF method to calculate the generation direction of the new node, and finally generate a new node $q_{new}$ in that direction. If there is no collision with the obstacle, the new node will be added to the random tree and determine whether it reaches the target point. If there is a collision or the target point is not reached, the initialization is returned to continue the cycle until the target point is reached. Then the final curvature continuous path is obtained by the path optimization method. The new improvements of the algorithm are explained in detail below.

2.3.1. Variable Sampling Area

In traditional RRT* and Goal-Bias RRT* algorithms, the random sampling of nodes is based on the entire state space. However, in structured road scenarios where autonomous vehicles operate, the vehicle’s movement is constrained by traffic rules. Therefore, random sampling based on the entire state space will produce many invalid plans. The variable sampling area method adaptively samples within a range around the current nearest node using Gaussian sampling. This method dynamically adjusts the sampling interval according to the characteristics of the current state space, which helps improve the efficiency and accuracy of the algorithm.

As shown in the coordinate system in Figure 3, vehicles need to move in the direction of the lane markings while avoiding obstacles represented by the black box. Depending on the driving intent of the vehicle, the RRT* algorithm should focus more on sampling in front of the vehicle during random sampling. Therefore, the Gaussian sampling will be more consistent with actual driving behavior. Using the state space information around the current nearest node, the sampling area of the new random node is limited to an adaptive fan-shaped range along the X-axis direction of the nearest node, as shown in Equation (3):

$$\Phi =\left\{x,y|x=x_o+r\cos \eta ,y=y_o+r\sin \eta \right\}$$

(3)

$$\left\{\begin{array}{l}r={\sigma }_r{r}_{rand}+r_o\\ \eta ={\sigma }_{\eta }{\eta }_{rand}+{\eta }_o\end{array}\right.\begin{array}{cc},& \end{array}\left\{\begin{array}{l}{r}_o=\epsilon \rho (q,q_{obs})\\ {\eta }_o=\frac{\pi }{2}-\arctan \left(\frac{{\rho }_x(q,q_{goal})}{{\rho }_y(q,q_{goal})}\right)\end{array}\right.$$

(4)

where $\Phi$ is the dynamic sampling area, $q(x_o,y_o)$ is the nearest node on the random tree during the sampling process, $(x,y)$ is the coordinate of the new sampling point, $\left(r,\eta \right)$ is the Gaussian parameter obtained from Equation (4), $\left(r_o,{\eta }_o\right)$ is the mean of the Gaussian distribution relative to $(x_o,y_o)$, $({\sigma }_r,{\sigma }_{\eta })$ is the standard deviation of the Gaussian distribution in the radial and circumferential directions, $(r_{rand},{\eta }_{rand})$ is a random variable that follows a standard Gaussian distribution. $\rho (q,q_{obs})$ represents the Euclidean distance between the nearest node and the center of the obstacle, $\epsilon$ is a constant, ${\rho }_x(q,q_{goal})$ and ${\rho }_y(q,q_{goal})$ represent the Euclidean distances between the nearest node and the target point in the X and Y directions, respectively.

2.3.2. Improved APF-RRT* Algorithm

Based on the RRT* algorithm, adding an improved artificial potential field can improve the direction of new node generation and speed up the search efficiency of the random tree. The traditional APF algorithm is first introduced to pave the way for introducing the improved strategy. $q(x_o,y_o)$ is the nearest node in the random tree during the sampling process, which generates an attractive field function ${\mathrm{U}}_{\mathrm{att}}(q)$ according to Equation (5) and a repulsive field function ${\mathrm{U}}_{\mathrm{rep}}(q)$ according to Equation (6). The total potential field ${\mathrm{U}}_{\mathrm{total}}(q)$ is the sum of these two fields, as shown in Equation (7):

$${\mathrm{U}}_{\mathrm{att}}(q)=\frac{1}{2}{k}_{ag}{\rho }^2(q,q_{goal})$$

(5)

$${\mathrm{U}}_{\mathrm{rep}}(q)=\left\{\begin{array}{cc}\frac{1}{2}{k}_r\left(\frac{1}{\rho (q,q_{obs})}-\frac{1}{{\rho }_0}\right)& \rho (q,q_{obs})\le {\rho }_0\\ 0& \rho (q,q_{obs})\ge {\rho }_0\end{array}\right.$$

(6)

$${\mathrm{U}}_{\mathrm{total}}(q)={\mathrm{U}}_{\mathrm{rep}}(q)+{\mathrm{U}}_{\mathrm{att}}(q)$$

(7)

where $k_{ag}$ is the attraction gain factor of the target point to the nearest node is taken as 1.5 in this paper, $k_r$ is the repulsion gain factor of the obstacle is taken as 2 in this paper, ${\rho }_0$ is the maximum distance at which the repulsion force acts, and $\rho (q,q_{goal})$ represents the Euclidean distance between the nearest node and the target point. The magnitudes of the attraction and repulsion forces are the negative gradients of the attraction field function and the repulsion field function, respectively. Therefore, the attraction function is defined as shown in Equation (8). Similarly, the repulsion function is shown in Equation (9).

$${\mathrm{F}}_{\mathrm{att}}(q)=k_{ag}\rho (q,q_{goal})$$

(8)

$${\mathrm{F}}_{\mathrm{rep}}(q)=\left\{\begin{array}{cc}{k}_r\left(\frac{1}{\rho (q,q_{obs})}-\frac{1}{{\rho }_0}\right)& \rho (q,q_{obs})\le {\rho }_0\\ 0& \rho (q,q_{obs})\ge {\rho }_0\end{array}\right.$$

(9)

To increase the exploratory nature of the algorithm and enable better exploration of the unknown regions, this paper proposes an improvement to the attraction field as follows. Firstly, the attraction field generated by the target point and the randomly sampled point are defined as ${\mathrm{U}}_{\mathrm{attg}}(q)$ and ${\mathrm{U}}_{\mathrm{attr}}(q)$, respectively, and their sum is the new attraction field function ${\mathrm{U}}_{\mathrm{att}}^{’}(q)$, as shown in Equation (10). This produces a new attraction function ${\mathrm{F}}_{\mathrm{att}}^{’}(q)$, as shown in Equation (11).

$${\mathrm{U}}_{\mathrm{att}}^{’}(q)={\mathrm{U}}_{\mathrm{attg}}(q)+{\mathrm{U}}_{\mathrm{attr}}(q)=\frac{1}{2}(k_{ag}{\rho }^2(q,q_{goal})+k_{ar}{\rho }^2(q,q_{rand}))$$

(10)

$${\mathrm{F}}_{\mathrm{att}}^{’}(q)={\mathrm{F}}_{\mathrm{attg}}(q)+{\mathrm{F}}_{\mathrm{attr}}(q)=k_{ag}\rho (q,q_{goal})+k_{ar}\rho (q,q_{rand})$$

(11)

where $k_{ar}$ is the gain coefficient for the attraction field produced by the sampled point towards the nearest node is taken as 1.5 in this paper, $\rho (q,q_{rand})$ is the Euclidean distance between the nearest node and the sampled point, ${\mathrm{F}}_{\mathrm{attg}}(q)$ and ${\mathrm{F}}_{\mathrm{attr}}(q)$ are the attractions generated by the target point, and the random sample towards the nearest node, respectively.

In the primary APF method, when the car reaches the target point, the attractive force becomes zero while the repulsive force remains non-zero, leading to the problem of the elusive target. Moreover, when the resultant direction of all the repulsive forces from obstacles is the same as the direction of the attractive force, and the car has not yet reached the target point, it is possible to get stuck in a local optimum where the repulsive and attractive forces are equal. To solve these problems, the traditional repulsive field of the local optimal improvement method proposed by other researchers is improved in this paper by introducing a modulation factor ${\rho }^n(q,q_{goal})$ in the repulsive field function, which produces a new repulsive field function ${\mathrm{U}}_{\mathrm{rep}}^{’}(q)$ as shown in Equation (12), which ensures that the repulsive and attractive forces only reduce to zero simultaneously when the car reaches the target point, thus solving the problems of local optima and elusive targets.

$${\mathrm{U}}_{\mathrm{rep}}^{’}(q)=\left\{\begin{array}{cc}\frac{1}{2}{k}_r{\left(\frac{1}{\rho (q,q_{obs})}-\frac{1}{{\rho }_0}\right)}^2{\rho }^n(q,q_{goal})& \rho (q,q_{obs})\le {\rho }_0\\ 0& \rho (q,q_{obs})>{\rho }_0\end{array}\right.$$

(12)

$$\begin{array}{l}{\mathrm{F}}_{\mathrm{rep}}^{’}(q)=k_r\left(\frac{1}{\rho (q,q_{obs})}-\frac{1}{{\rho }_0}\right)\frac{{\rho }^n(q,q_{goal})}{{\rho }^2\left(q,q_{obs}\right)}\nabla \rho (q,q_{obs})\\ -\frac{n}{2}{k}_r{\left(\frac{1}{\rho (q,q_{obs})}-\frac{1}{{\rho }_0}\right)}^2{\rho }^{n-1}(q,q_{goal})\nabla \rho (q,q_{goal})\end{array}$$

(13)

where ${\mathrm{F}}_{\mathrm{rep}}^{’}(q)$ is the new obstacle repulsion function, $\nabla \rho (q,q_{obs})$ and $\nabla \rho (q,q_{goal})$ are the unit vectors of the nearest node and obstacle and the direction of the target point, respectively, $n$ is an arbitrary constant, and $n=2$ is taken here after many tests.

However, vehicles are different from robots when they travel on roads. They are not only constrained by obstacles but also by the road itself. According to the driving experience, the boundary area of the road is the most dangerous, followed by the centerline area, and the lane centerline area is the least dangerous. The force exerted on the vehicle by the road boundary is shown in Figure 4.

According to the distribution of road danger levels mentioned above, the potential field function of the road boundary needs to be considered in segments. When the vehicle is in the area between the centerlines of two lanes, it is in a relatively safe zone. The influence of the potential field changes slowly with position, so a function with a relatively gentle change trend is used. In other areas where the danger level is higher, the influence of the potential field changes quickly with position, so a function with a more extensive change trend is used. Taking a dual carriageway as an example, considering the above factors, the potential field function of the road boundary established is shown as Equation (14), and the boundary potential field generated by the road boundary produces the road boundary repulsive force shown as Equation (15), pushing the vehicle away from illegal areas on the road boundary and ensuring the safety of the path.

$${\mathrm{U}}_{\mathrm{road}}(q)=\left\{\begin{array}{l}{k}_{\mathrm{road}1}\left({\mathrm{e}}^{-\left|y_o-y_l\right|}-1\right),y_o\le \frac{L}{4}\hfill \\ k_{\mathrm{road}2}\sin \left(\frac{2\left(y_o-y_l\right)}{L}\right),\frac{L}{4}<y_o<\frac{3L}{4}\hfill \\ k_{\mathrm{road}3}\left({\mathrm{e}}^{|y_o-y_r|}-1\right),y_o\ge \frac{3L}{4}\hfill \end{array}\right.$$

(14)

$${\mathrm{F}}_{\mathrm{road}}(q)=\left\{\begin{array}{l}{k}_{\mathrm{road}1}{\mathrm{e}}^{-|y_o-y_l|},y_o\le \frac{L}{4}\hfill \\ k_{\mathrm{road}2}\cos \left(\frac{2\left(y_o-y_l\right)}{L}\right),\frac{L}{4}<y_o<\frac{3L}{4}\hfill \\ k_{\mathrm{road}1}{\mathrm{e}}^{\left|y_o-y_r\right|},y_o\ge \frac{3L}{4}\hfill \end{array}\right.$$

(15)

where $k_{\mathrm{road}1}$ and $k_{\mathrm{road}2}$ are the potential energy gain coefficients of the road boundary; $y_l$ and $y_r$ is the horizontal position of the centerline of the inner and outer lanes; $L$ is the width of the road.

Based on the Goal Biased RRT*, an improved APF method is combined, where both the target point and random sampling point exert attraction on the nearest node, and obstacles exert a repulsive force on it. At the same time, considering the road environment where the autonomous driving vehicle is located, the road boundary repulsive force is introduced. The force diagram of the nearest node $q(x_o,y_o)$ is shown in Figure 5, and the direction of the resultant force $F_{\mathrm{total}}^{’}{}_{ }$ is drawn using the parallelogram rule, as shown in Equation (16). Its direction serves as the extension direction for new nodes, controlling the growth of the random tree towards the target point to reduce randomness and accelerate path search speed.

$${\mathrm{F}}_{\mathrm{total}}^{’}(q)={\mathrm{F}}_{\mathrm{att}}^{’}(q)+{\mathrm{F}}_{\mathrm{rep}}^{’}(q)+{\mathrm{F}}_{\mathrm{road}}(q)$$

(16)

Then the resultant force $F_{\mathrm{total}}^{’}{}_{ }$ is decomposed into $F_x$ and $F_y$, corresponding to the components of the x-axis and y-axis, respectively, then in the case of the expansion step $\lambda$, the position of the new node is $q_{new}\left(x_{new},y_{new}\right)$, as shown in Equation (17):

$$\left\{\begin{array}{l}{x}_{new}=x_o+\lambda \frac{F_x}{F_x+F_y}\\ y_{new}=y_o+\lambda \frac{F_y}{F_x+F_y}\end{array}\right.$$

(17)

The path planning for autonomous vehicles in road scenarios was achieved by fusing the RRT* algorithm and the improved APF method. The algorithm introduces the concepts of attraction field and repulsion field to control the growth direction of the RRT* algorithm towards the target point as much as possible in the tree expansion direction. After defining the attraction field and repulsion field functions, their resultant force direction can be used as the expansion direction of new nodes for node expansion. In this way, the tree growth towards the target point can be controlled during the path planning process to reduce randomness and improve path search speed.

2.3.3. Path Optimization Algorithms

Due to the random sampling nature of the RRT* algorithm during path searching, if the path is directly generated from the goal point back to the starting point, the resulting path will inevitably be more tortuous. It may not meet the maximum curvature constraints required for vehicle steering, as well as resulting in redundant nodes, leading to an unnecessary increase in path cost. Therefore, this paper proposes a path-pruning method based on the maximum steering angle constraint of the vehicle. As shown in Figure 6, starting from the goal node $q_{goal}$, it is connected to the starting node $q_{start}$, and the intersection between the line segment and obstacles is checked. If there is no intersection, the nodes between the two path nodes are pruned, and the line connecting them is considered as the optimal path. If there is an intersection, node $q_2$ is connected to the goal node $q_{goal}$, and the intersection check is repeated while also checking whether the angle formed by the two adjacent path segments generated by nodes $q_{start}$, $q_2$, and $q_{goal}$ satisfies the maximum steering angle constraint of the vehicle. If it does not satisfy one of the judgment criteria, it will continue to connect to $q_3$ and $q_{goal}$, and repeat the above judgment. If both judgment criteria are met, it will be considered as the next path point of $q_{start}$, and the above operation is repeated until the final path that does not intersect with obstacles and meets the maximum steering angle constraint of the vehicle is obtained. This ensures that the path curvature does not exceed the maximum curvature when using B-spline curves for smooth paths and outputs the path. The path after pruning optimization is shown as a solid line in Figure 6.

After the above path clipping, the path still has the problem of discontinuous curvature. Therefore, a cubic B-spline curve is used to smooth the path and make the curvature of the planned path continuous. If there are n + 1 control points in total, the K-th order B-spline curve is defined as:

$$\mathrm{P}(t)=\left[p_0,p_1,\dots ,p_n\right]\left[\begin{array}{c}{\mathrm{B}}_{0,K}(t)\\ {\mathrm{B}}_{1,K}(t)\\ \dots \\ {\mathrm{B}}_{n,K}(t)\end{array}\right]={\displaystyle \sum _{i=0}^n{P}_i}{B}_{i,K}(t)$$

(18)

where $\mathrm{P}(t)$ are control points; ${\mathrm{B}}_{i,K}(t)$ is the basis function of cubic B-splines, recursively obtained according to the DeBoor-Cox formula:

$$\left\{\begin{array}{l}{\mathrm{B}}_{i,0}(t)\left\{\begin{array}{l}1\hspace{1em}\hspace{1em}{t}_i\le t\le t_{i+1}\\ 0\hspace{1em}\hspace{1em} Otherwise\end{array}\right.\begin{array}{cc}& K=1\end{array}\\ {\mathrm{B}}_{i,3}(t)=\frac{(t-t_i)B_{i,2}(t)}{t_{i+3}-t_i}+\frac{(t_{i+4}-t)B_{i+1,2}(t)}{t_{i+4}-t_{i+1}}\begin{array}{cc}& K\ge 2\end{array}\end{array}\right.$$

(19)

In this study, three uniform B-splines were selected to smooth the planned path, and the repeatability of the nodes at both ends was set to 3. Then the basis function can be expressed as:

$$\left\{\begin{array}{l}{\mathrm{B}}_{0,3}(t)=\frac{1}{6}{(1-t)}^3\\ {\mathrm{B}}_{1,3}(t)=\frac{1}{6}\left(3t^3-6t^2+4\right)\\ {\mathrm{B}}_{2,3}(t)=\frac{1}{6}\left(-3t^3+3t^2++3t+1\right)\\ {\mathrm{B}}_{3,3}(t)=\frac{1}{6}{t}^3\end{array}\right.$$

(20)

Here, the vector interval of parameter nodes is set to [0, 1]. By substituting Equation (20) into Equation (18), the expression of the third-order quasi-uniform B-spline curve can be obtained:

$$\mathrm{P}\left(\mathrm{t}\right)={\mathrm{P}}_0{B}_{0,3}(t)+{\mathrm{P}}_1{B}_{1,3}(t)+{\mathrm{P}}_2{B}_{2,3}(t)+{\mathrm{P}}_3{B}_{3,3}(t),\hspace{1em}t\in \left[0,1\right].$$

(21)

3. Path Tracking Controller

3.1. Establishment of Path Tracking Model

3.1.1. Vehicle Dynamic Model

The path tracking control of autonomous driving vehicles mainly studies the lateral dynamic characteristics of the vehicle, involving the vehicle’s lateral and yawing movements. To simplify the calculations, it is assumed that the vehicle’s coaxial wheels have the same lateral stiffness and turning angle, and thus, the coaxial wheels can be merged. This simplifies the vehicle’s dynamic model to a two-degree-of-freedom lateral dynamic model [34,35], as shown in Figure 7.

Assuming the vehicle is driving at a constant speed and under the condition where the front wheel steering angle ${\delta }_f$ and the front and rear wheel slip angles ${\alpha }_f$ and ${\alpha }_r$ are small, lateral load transfer can be ignored. According to the magic formula tire model [36], the vehicle’s dynamic Equation can be expressed as:

$$\left\{\begin{array}{l}m{a}_y=F_{yf}+F_{yr}=C_f{\alpha }_f+C_r{\alpha }_r=C_f\left(\frac{\dot{\phi }a+v_y}{v_x}-{\delta }_f\right)+C_r\left(\frac{v_y-\dot{\phi }b}{v_x}\right)\\ I_z\ddot{\phi }=a{F}_{yf}-b{F}_{yr}=a{C}_f{\alpha }_f-b{C}_r{\alpha }_r=a{C}_f\left(\frac{\dot{\phi }a+v_y}{v_x}-{\delta }_f\right)-b{C}_r\left(\frac{v_y-\dot{\phi }b}{v_x}\right)\end{array}\right.$$

(22)

where $m$ is the mass of the entire vehicle; $F_{yf}$ and $F_{yr}$ are the lateral forces of the front and rear wheels, respectively; $I_z$ is the rotational inertia of the vehicle around the Z-axis; $\dot{\phi }$ is the vehicle’s yaw rate; $a$ and $b$ are the distances from the center of mass of the vehicle to the front and rear axles, respectively, $C_f$ and $C_r$ are the lateral stiffness of the front and rear wheels, respectively; $v_x$ is the longitudinal speed of the vehicle; $v_y$ is the lateral velocity of the vehicle.

Define $\dot{y}=v_y$ to get the vehicle dynamics model:

$$\left(\begin{array}{l}\ddot{y}\hfill \\ \ddot{\phi }\hfill \end{array}\right)=\left(\begin{array}{ll}\frac{C_f+C_r}{m{v}_x}\hfill & \frac{a{C}_f-b{C}_r}{m{v}_x}-v_x\hfill \\ \frac{a{C}_f-b{C}_r}{I_z{v}_x}\hfill & \frac{a^2{C}_f+b^2{C}_r}{I_z{v}_x}\hfill \end{array}\right)\left(\begin{array}{l}\dot{y}\hfill \\ \dot{\phi }\hfill \end{array}\right)+\left(\begin{array}{c}-\frac{C_f}{m}\\ -\frac{a{C}_f}{I_z}\end{array}\right){\delta }_f$$

(23)

3.1.2. Path Tracking Error Model

When an autonomous vehicle is tracking a reference path, it will mainly experience lateral error and heading angle error. As shown in Figure 8, the shortest distance between the vehicle’s center of mass and the reference path projection point is defined as the lateral error $e_d$. The difference between the vehicle’s actual heading angle $\theta$ and the reference heading angle ${\theta }_r$ is defined as the heading angle error $e_{\theta }$. For convenience of calculation, it is assumed that the vehicle’s center of mass has a lateral deviation angle $\beta =0$, and the vehicle’s heading angle error is $e_{\phi }=\phi -{\theta }_r$. In actual control, the controller is required to eliminate these two errors in real time to enable the vehicle to track the planned path in real time. Based on the lateral error and heading angle error, the first-order derivatives of lateral error ${\dot{e}}_d$ and heading angle error ${\dot{e}}_{\phi }$ can be calculated.

$${\dot{e}}_d=v_x{e}_{\phi }+v_y$$

(24)

$${\dot{e}}_{\phi }=\dot{\phi }-{\dot{\phi }}_r$$

(25)

Combining Equation (23), the second-order derivative of the lateral error ${\ddot{e}}_d$ and the second-order derivative of the heading angle error ${\ddot{e}}_{\phi }$ can be obtained.

$$\begin{array}{ll}{\ddot{e}}_d& =\left(\frac{C_f+C_r}{m{v}_x}\right){\dot{e}}_d+\left(-\frac{C_f+C_r}{m}\right)e_{\phi }+\left(\frac{a{C}_f-b{C}_r}{m{v}_x}\right){\dot{e}}_{\phi }\\ & +\left(\frac{a{C}_f-b{C}_r}{m{v}_x}-v_x\right){\dot{\theta }}_r+\left(-\frac{C_f}{m}\right){\delta }_f\end{array}$$

(26)

$$\begin{array}{ll}{\ddot{e}}_{\phi }& =\left(\frac{a{C}_f-b{C}_r}{I_z{v}_x}\right){\dot{e}}_d+\left(-\frac{a{C}_f-b{C}_r}{I_z}\right)e_{\phi }+\left(\frac{a^2{C}_f+b^2{C}_r}{I_z{v}_x}\right){\dot{e}}_{\phi }\\ & +\left(\frac{a^2{C}_f+b^2{C}_r}{I_z{v}_x}\right){\dot{\theta }}_r+\left(-\frac{a{C}_f}{I_z}\right){\delta }_f\end{array}$$

(27)

Further transformations on the above formula result in the following state-space Equations for the lateral and heading errors in the steering process of autonomous driving vehicles:

$$\begin{array}{ll}\left(\begin{array}{c}{\dot{e}}_d\\ {\ddot{e}}_d\\ {\dot{e}}_{\phi }\\ {\ddot{e}}_{\phi }\end{array}\right)& =\left(\begin{array}{cccc}0& 1& 0& 0\\ 0& \frac{C_f+C_r}{m{v}_x}& -\frac{C_f+C_r}{m}& \frac{a{C}_f-b{C}_r}{m{v}_x}\\ 0& 0& 0& 1\\ 0& \frac{a{C}_f-b{C}_r}{I_z{v}_x}& -\frac{a{C}_f-b{C}_r}{I_z}& \frac{a^2{C}_f+b^2{C}_r}{I_z{v}_x}\end{array}\right)\left(\begin{array}{c}{e}_d\\ {\dot{e}}_d\\ e_p\\ {\dot{e}}_{\phi }\end{array}\right)\\ & +\left(\begin{array}{c}0\\ -\frac{C_f}{m}\\ 0\\ -\frac{a{C}_f}{I_z}\end{array}\right){\delta }_f+\left(\begin{array}{c}0\\ \frac{a{C}_f-b{C}_r}{m{v}_x}-v_x\\ 0\\ \frac{a^2{C}_f+b^2{C}_r}{I_z{v}_x}\end{array}\right){\dot{\theta }}_r\end{array}$$

(28)

Then the state space Equation about the tracking error of vehicle dynamics can be obtained:

$$\dot{X}=AX+BU+C{\dot{\theta }}_r.$$

(29)

In practical applications, we frequently work with discrete data, and the path generated by the path planning algorithm described earlier consists of a series of reference points rather than a continuous path. Hence, we opt for discrete LQR control. To design a discrete LQR controller, we discretize Equation (31) by ignoring the impact of the $C{\dot{\theta }}_r$ term, resulting in the state space Equation for discrete vehicle tracking errors.

$$X_{k+1}=\overline{A}{X}_k+\overline{B}{U}_k$$

(30)

where: $\overline{A}={\left(I-\frac{Adt}{2}\right)}^{-1}\left(I+\frac{Adt}{2}\right)$; $\overline{B}=Bdt$.

3.2. Design of LQR Path Tracking Controller Based on Particle Swarm Optimization

The overall structure of the path-tracking controller is shown in Figure 9. Firstly, based on the path tracking error model, an LQR controller is designed as the central part of the path-tracking controller so that the vehicle can travel along the reference path; on this basis, the particle swarm optimization algorithm is used to optimize the weight matrices Q and R of the LQR controller to improve its performance, and a feedforward control method is used as the steering angle compensation to eliminate the steady-state error of the system and further optimize the path tracking effect.

3.2.1. LQR Controller Design

LQR is an optimization problem that minimizes the performance objective function under given constraints and obtains the optimal feedback control by solving the corresponding algebraic Riccati Equation [37]. Its advantage in autonomous driving path tracking control is that when the system state deviates from the steady state due to obstacles or unexpected events, it can ensure that the vehicle tracking control system approaches the ideal path without causing too much computational workload. The LQR control problem of the vehicle lateral path tracking controller can be expressed as:

$$\min \mathrm{J}={\displaystyle \sum _{k=0}^{\infty }\left(X_k^{T}Q{X}_k+U_k^{T}R{U}_k\right)}$$

(31)

$$Q=\mathrm{diag}\left[q_1,q_2,q_3,q_4\right]$$

(32)

$$R=[q_5]$$

(33)

where $X$ denotes the system’s state variable, while $U$ represents its control variable. The weighted matrix $Q$ indicates the level of emphasis placed on the corresponding control target in terms of state error representation. The weight matrix of the control variable is denoted by $R$. The weight coefficients for lateral error, the lateral error rate of change, heading error, and the heading error rate of change are, respectively, represented by $q_1,q_2,q_3,q_4$. The weight coefficient of the front wheel steering angle is given by $q_5$. These weight coefficients reflect the relative importance of the variables. Larger values correspond to faster convergence of the related state variable towards the target value.

Then, the Lagrange multiplier method is used to construct the constrained optimization problem for solving extreme values. The cost function under constraint conditions is:

$$\mathrm{J}={\displaystyle \sum _{k=0}^{n-1}\left[X_k^{T}Q{X}_k+U_k^{T}R{U}_k+{\lambda }_{k+1}^T\left(\overline{A}{X}_k+\overline{B}{U}_k\right)-{\lambda }_{k+1}^T{X}_{k+1}\right]}+X_n^{T}Q{X}_n.$$

(34)

The constructed Hamiltonian function $H_k=X_k^{T}Q{X}_k+U_k^{T}R{U}_k+{\lambda }_{k+1}^T\left(\overline{A}{X}_k+\overline{B}{U}_k\right)$, Equation (34) can be simplified as:

$$\mathrm{J}={\displaystyle \sum _{k=0}^{n-1}\left(H_k-{\lambda }_k^T{X}_k\right)}+X_n^{T}Q{X}_n-{\lambda }_n^T{X}_n.$$

(35)

The control quantity of LQR obtained by solving the extreme value of Equation (35) is:

$$U_k=-{\left(R+{\overline{B}}^{T}P\overline{B}\right)}^{-1}{\overline{B}}^{T}P\overline{A}{X}_k$$

(36)

where P is obtained by calculating Riccati Equation $P_{k-1}=Q+{\overline{A}}^T{P}_k{\left(I+\overline{B}{R}^{-1}{\overline{B}}^T{P}_k\right)}^{-1}\overline{A}$.

Equation (36) can be simplified as:

$$U_k=-k{X}_k$$

(37)

where $k=[k_1,k_2,k_3,k_4]$ is the result of the LQR controller and also the feedback coefficient of the front wheel angle.

3.2.2. Feedforward Control

By bringing Equation (37) into Equation (29), it can be obtained that:

$$\dot{X}=(A-Bk)X+C{\dot{\theta }}_r$$

(38)

At this point, no matter what value $k$ takes, $\dot{X}$ cannot be zero. If only LQR feedback control is used, there will be a constant steady-state error; Therefore, we remove the influence of the $C{\dot{\theta }}_r$ term by introducing a feedforward control variable ${\delta }_{ff}$, and the system control variable after adding feedforward control is:

$$U=-kX+{\delta }_{ff}$$

(39)

When $\dot{X}=0$, the expression for the state variable of the system without steady-state error is:

$$X=-{(A-Bk)}^{-1}A\left(B{\delta }_{ff}+C{\dot{\theta }}_r\right)$$

(40)

From Equation (40), it can be concluded that in order to achieve the optimal control effect, it is necessary to calculate the appropriate ${\delta }_{ff}$, so that the steady-state error of the system is 0. Based on Equation (28), the steady-state error Equation of the system is calculated as follows:

$$\left[\begin{array}{c}{e}_d\\ {\dot{e}}_d\\ e_{\phi }\\ {\dot{e}}_{\phi }\end{array}\right]=\left[\begin{array}{c}\frac{1}{k_1}\left\{{\delta }_{ff}-\frac{{\dot{\theta }}_r}{v_x}\left[a+b-b{k}_3-\frac{m{v}_x^2}{a+b}\left(\frac{b}{C_f}+\frac{a}{C_r}{k}_3-\frac{a}{C_r}\right)\right]\right.\\ 0\\ -\frac{{\dot{\theta }}_r}{v_x}\left(b+\frac{a}{a+b}\frac{m{v}_x^2}{C_r}\right)\\ 0\end{array}\right]$$

(41)

According to Equation (41), it can be seen that:

$$e_{\phi }=-\frac{{\dot{\theta }}_r}{v_x}\left(b+\frac{a}{a+b}\frac{m{v}_x^2}{C_r}\right)=-\beta .$$

(42)

In the previous text, for the convenience of calculation, assuming that the lateral deviation angle of the vehicle’s center of mass is $\beta =0$, the heading angle error of the vehicle is $e_{\phi }=\phi -{\theta }_r$ but $e_{\theta }=\phi +\beta -{\theta }_r=0$, so the heading error can be directly eliminated. If you want the lateral error $e_d=0$, the feedforward control quantity ${\delta }_{ff}$ is:

$${\delta }_{ff}=\rho \left[a+b-b{k}_3-\frac{m{v}_x^2}{a+b}\left(\frac{b}{C_f}+\frac{a}{C_r}{k}_3-\frac{a}{C_r}\right)\right]$$

(43)

where $\rho =\frac{{\dot{\theta }}_r}{v_x}$ is the path curvature.

3.3. Optimization of LQR Controller Based on Particle Swarm Optimization Algorithm

PSO (particle swarm optimization) is widely used in parameter optimization problems because of its simplicity and fast convergence [38,39,40]. In the PSO algorithm, the optimization problem is transformed into a search problem for the optimal solution, which is easy to implement and has global search ability. The PSO algorithm finds the optimal LQR weight matrix by conducting a global search in the search space, further improving the control system’s performance. Figure 10 shows the algorithm flowchart of applying PSO to optimize the LQR weight matrix.

Firstly, the parameters required for the LQR algorithm need to be configured, followed by the initialization of the position and velocity of particles, including the initial value of the LQR weight matrix, which should be randomly generated within a feasible range. Then, the control quantity of LQR is calculated using the generated weight matrix Q and R, and the Simulink model is called to calculate relevant parameters. The fitness function of each particle is obtained by calculating the fitness function based on the parameters obtained from running the model. The fitness function is a vital component of the particle swarm algorithm. Taking into account the characteristics of autonomous driving vehicles and driving scenarios and considering the relationship between the frequency domain and time domain, this study designed a fitness function $\mathrm{F}$ based on the sum of the integral time multiplied by the absolute error (ITAE) of lateral error $e_d$, heading error $e_{\phi }$, roll angle velocity $\dot{\phi }$, and lateral acceleration $a_y$, to simultaneously consider path tracking accuracy and vehicle stability.

$$\min \mathrm{F}={\displaystyle {\int }_0^{T}t}\left|e_d(t)\right|\mathrm{d}t+{\displaystyle {\int }_0^{T}t}\left|e_{\phi }(t)\right|\mathrm{d}t+{\displaystyle {\int }_0^{T}t}\left|\dot{\phi }(t)\right|\mathrm{d}t+{\displaystyle {\int }_0^{T}t}\left|a_y(t)\right|\mathrm{d}t$$

(44)

Then, for each particle, find its best solution: the position and weight matrix with the smallest fitness value. Find the current global best solution, which is the position and weight matrix with the smallest fitness value among all particles, and update the velocity and position of each particle based on Equation (45):

$$\left\{\begin{array}{l}{V}_i(t+1)=\omega V{(t)}_i+c_1{\mathrm{r}}_1\left(p_{\mathrm{best}\_\mathrm{i} }-X_i(t)\right)+c_2{\mathrm{r}}_2\left(g_{\mathrm{best} }-X_i(t)\right)\\ X_i(t+1)=X_i(t)+V_i(t+1)\end{array}\right.$$

(45)

where $V_i(t)$ and $X_i(t)$ are the velocity and position of the $i$-th particle in the $t$-th iteration, $p_{\mathrm{best}\_\mathrm{i} }$ is the personal optimal solution of the $i$-th particle, $g_{\mathrm{best} }$ is the current global optimal solution, $\omega$ is the inertia factor, $c_1$ and $c_2$ are learning factors, ${\mathrm{r}}_1$ and ${\mathrm{r}}_2$ are random numbers between [0, 1].

Loop until the specified termination criterion is reached, and output the current global optimal solution and the optimal value of the LQR Weight Matrix, as shown in

Table 1. Optimal values of LQR weight matrix.

Longitudinal Velocity (m/s)	Q	R
10	diag [300 0.01 0.01 4.49]	6.02
15	diag [270.71 0.01 0.01 119.35]	4.91
20	diag [1.23 0.01 99.47 62.88]	1.39

.

4. Simulation Analysis

4.1. Simulation Analysis of Path Planning

Simulations were performed using Matlab 2020(b) (sourced from MathWorks, a company located in Natick, MA, United States) on a Windows 10 computer with an Intel Core i7-12700F CPU and 32 G RAM. This paper compares the simulation results of four algorithms, RRT*, Goal-Biased RRT*, P-RRT*, and Improved-RRT*, for three Map scenarios. The Improved-RRT* algorithm exhibits fast convergence, short planning time, and high security of the planned path. The simulated Maps are 2D environments, with Map 1 and 3 having dimensions of 7 m × 100 m and Map 2 having dimensions of 7 m × 120 m, representing three different scenarios. As shown in Figure 11, in the simulation, the vehicle starts at the starting point in rose red and ends at the endpoint in blue. The obstacle vehicles and road boundaries are represented in dark green, the blue line indicates the sampling process, and the red line indicates the final generated path. Each method was tried 30 times in each Map situation to remove the influence of randomness, and the average result was calculated. The results include the average path length, average path search time, average number of iterations, and average memory consumption, which are recorded and presented in

Table 2. Simulation data under Map 1.

Algorithm	RRT*	Goal-Biased-RRT*	P-RRT*	Improved-RRT*
Average path length (m)	102.4621	100.9605	100.8112	100.7826
Average running time (s)	14.7644	7.0604	1.8454	1.3079
Average iterations	480.8	310.4	219.3	146.4
Average memory consumption (MB)	44.3	33.1	26.6	22.4

,

Table 3. Simulation data under Map 2.

Algorithm	RRT*	Goal-Biased-RRT*	P-RRT*	Improved-RRT*
Average path length (m)	122.3938	121.5995	121.2425	121.2192
Average running time (s)	16.8818	11.9701	2.8668	1.0641
Average iterations	634.4	401.8	334.1	218.2
Average memory consumption (MB)	49.7	35.3	32.1	26.2

and

Table 4. Simulation data under Map 3.

Algorithm	RRT*	Goal-Biased-RRT*	P-RRT*	Improved-RRT*
Average path length (m)	102.8306	101.2469	100.7349	100.6376
Average running time (s)	12.4730	8.0140	2.1727	1.4296
Average iterations	518.5	349.7	282.5	177.1
Average memory consumption (MB)	47.4	34.1	29.3	24.5

, respectively.

Map 1 depicts a two-lane changing scenario where the vehicle must avoid three obstacles on the right lane and switch to the left lane. As shown in Figure 11a, the original RRT* algorithm employs full-state space random sampling, generating unnecessary sampling processes and resulting in a longer and more tortuous path. Figure 11b displays the simulation results of the Goal-Biased-RRT* algorithm, which significantly reduces the sampling process and improves the search efficiency by incorporating a target bias strategy. Figure 11c shows the simulation results of the P-RRT* algorithm, which enhances search speed and simplifies redundant nodes by adding attractive field constraints. However, it still generates redundant nodes around obstacles due to the lack of obstacle exclusion effect. In contrast, Figure 11d illustrates the Improved-RRT* algorithm, which restricts the variable sampling area and introduces an improved APF algorithm to enhance search efficiency. Although the path length of the Improved-RRT* algorithm does not show a significant advantage compared to the previous algorithms, the distance between the path and obstacles has a certain safety margin, thus improving the path safety.

By analyzing the data in Table 2, the path length of the proposed algorithm is 1.67% shorter than that of the original RRT* algorithm. Compared with the P-RRT* algorithm, the average running time and iteration times are reduced by 29.13% and 33.24%, respectively, and reduces memory consumption by about 15.8%. The results demonstrate that the proposed algorithm can significantly reduce the number of random points, improve the convergence speed, and ensure algorithm stability.

Map 2 depicts a passing scenario in a double-lane road, where the vehicle needs to pass a single obstacle on the right lane and also avoid two obstacles on the left lane, requiring two lane changes to complete the passing maneuver. The simulation results are shown in Figure 12. It can be seen that for such a complex environment, the advantages of the proposed algorithm are more evident. Although the reduction in path length compared to other methods is relatively small, the paths generated by other methods are not good and too close to the obstacle cars, with a low safety margin. As shown in Figure 12d, the improved algorithm proposed in this paper plans a smoother path in this scenario, with a greater distance from the obstacle cars and a higher level of safety. It also reduces the large number of redundant nodes, shortening the search time. Analyzing the data in Table 3, the new algorithm reduces the average path length by 0.96% compared to the original RRT* algorithm. Compared with the P-RRT* algorithm, the average running time is reduced by 39.67%, the average number of iterations is reduced by 34.69%, and the algorithm memory consumption is reduced by 18.4%. Therefore, the proposed algorithm can significantly reduce unnecessary sampling processes and iterations, demonstrating its stability.

The Map 3 scenario is mainly used to verify the effectiveness of the improved algorithm in this paper under different obstacle sizes and shapes, and the simulation results are shown in Figure 13. On the same road as Map 1, obstacles of different sizes and shapes appear, and the proposed improved algorithm is still practical for such a complex environment. The improved algorithm in this paper can still plan the path with a high safety factor while the redundant nodes are significantly reduced, and the path search speed is accelerated. According to the data analysis in Table 4, the average path length planned by the improved algorithm is 2.14% less than that of the RRT* algorithm. Compared with the P-RRT* algorithm, the average running time of the improved algorithm is reduced by 34.56%, the average number of iterations is reduced by 37.31%, and the memory consumption is reduced by 16.4%. The proposed algorithm still performs well in the face of obstacles of different sizes and shapes.

In the simulations of the three Map scenarios shown in Figure 11, Figure 12 and Figure 13, although the improved RRT* algorithm generates the base path, there are problems such as too many path points, too long path length, and too large path curvature, which do not meet the driving conditions of autonomous vehicles and are not the required optimal paths. Therefore, in this study, the path generated by the improved algorithm was optimized using path pruning based on the maximum steering angle of the vehicle and the third-order B-spline algorithm. The optimization results are shown in Figure 14. The red path represents the initially generated path, while the green path represents the trimmed path, and it can be seen that the redundant points in the path have been reduced. The black path represents the optimized final, whose curvature is shown in Figure 15. The path curvature after path optimization in all three Map scenarios is continuous and can meet the vehicle driving requirements.

4.2. Simulation Analysis of Path Tracking Control

The effectiveness and adaptability of the proposed path planning algorithm and path tracking control strategy were verified through joint simulation experiments using CarSim and MATLAB/Simulink. The main parameters of the vehicle are shown in

Table 5. Main parameters of the vehicles.

Parameters/Units	Value
Vehicle mass/kg	1412
Distance from the center of mass to the front axis/mm	1015
Distance from the center of mass to the rear axis/mm	1895
Moment of inertia/kg·m2	1536.7
Front-wheel cornering stiffness/N/rad	−148,970
Rear wheel cornering stiffness/N/rad	−82,204
Wheelbase of the front axle/mm	1675
Height of the center of mass/mm	540
Effective radius of wheel/mm	325

. To validate the feasibility of the planned path and the effectiveness of the tracking control strategy, the planned paths in two different Map scenarios were tested for path tracking. As the driving environment was a normal urban road with well-dried asphalt surfaces, the road adhesion coefficient was set to 0.8 in this study.

The performance of the PSO-optimized LQR path tracking controller was first verified by using the two paths planned in this paper. Simulation tests were conducted at speeds of 10 m/s and 20 m/s for both LQR and PSO-optimized LQR controllers, and representative parameters were selected for comparison. The results are shown in Figure 16 and Figure 17.

Figure 16 describes the tracking performance of the LQR controller and the PSO-optimized LQR controller under the path of Map 1 at speeds of 10 m/s and 20 m/s. According to parts (a) and (c), the maximum lateral tracking error of the LQR controller can be controlled to within 0.13 m at different speeds, and the PSO-LQR is reduced by about 54% compared to it. Parts (b) and (d) provide the analysis of the overall vehicle stability. When the longitudinal speed is 10 m/s, there is not much difference in the stability of LQR before and after optimization. However, when the speed is increased to 20 m/s, the overall vehicle stability under PSO-optimized LQR control is significantly better.

Figure 17 describes the tracking performance of the LQR controller and PSO-optimized LQR controller under two speeds, 10 m/s, and 20 m/s, on the path of Map 2. Perfect driving in the double-lane scenario is challenging, and both control algorithms have some errors in turns with large curvatures. However, the PSO-optimized LQR controller has a more minor lateral error, and the vehicle can quickly correct the current state when the error is large. By comparing parts (a) and (c), the maximum lateral tracking error of the LQR controller at different speeds in both paths can be controlled within 0.14 m. For the PSO-optimized LQR controller, it is reduced by about 64% to 0.05 m. This indicates that the improved LQR controller can more effectively reduce errors and keep the vehicle in a safe state when the tracking error is large. Similarly, at a speed of 20 m/s, parts (b) and (d) show that the whole vehicle stability under the PSO-optimized LQR controller is higher.

To verify the feasibility of the planned paths and to investigate the accuracy and stability of the improved LQR controller in terms of speed, two planned paths were tracked and tested at three different vehicle speeds. The simulation results are shown in Figure 18 and Figure 19.

A vehicle speed of 20 m/s was added to the first two simulated speeds to better validate the tracking performance of the PSO-LQR tracking controller. The simulation results are presented in Figure 18 and Figure 19. Figure 18a and Figure 19a demonstrate that the vehicle can effectively follow the planned path at all speeds, indicating the robustness of the controller. Figure 18b,c and Figure 19c show that the lateral position tracking error increases with speed; however, the lateral position errors at different paths and speeds can be controlled within 0.06 m, which is within the acceptable range and performs well. Similarly, Figure 18d and Figure 19d indicate that the yaw tracking error can be controlled within 0.05 rad at different speeds, and the differences are insignificant, suggesting that the vehicle’s tracking process is relatively stable. Figure 18b,e,f, and Figure 19b,e,f, describe that as the speed increases, the changes in lateral acceleration, front-wheel steering angle, and yaw rate become greater. Still, the front-wheel angle and lateral acceleration at different speeds do not have any step changes, and the yaw rate can be controlled within an acceptable range, indicating that the tracking process is relatively stable. Overall, the PSO-optimized LQR controller’s robustness, accuracy, and stability are acceptable at speeds that are not too fast. The planned paths meet the tracking requirements of autonomous vehicles and achieve good tracking performance at different speeds.

5. Conclusions

To address the slow convergence and randomness of the RRT* algorithm and the relatively empirical selection of the weight matrix of the LQR algorithm. This paper proposes a framework to improve the RRT* and PSO-LQR algorithms for path planning and tracking control of self-driving vehicles under ordinary urban road conditions. The framework employs a variable sampling area to limit the generation of random sampling points. Moreover, an improved artificial potential field method is integrated into the RRT* algorithm to improve its convergence speed. The path optimization is then performed using path pruning based on the vehicle’s maximum steering angle constraint and the three-time B-spline algorithm to obtain curvature-continuous paths. The simulation results of the average three Map scenarios show that the improved RRT* algorithm optimizes 34.45%, 35.08%, and 16.87% of the path search time, iteration number, and memory consumption, respectively, compared with P-RRT*, demonstrating the significant advantages of the improved algorithm. In addition, the final path optimized by the path optimization algorithm is smooth and curvature continuous, which satisfies the path tracking requirements. In the path-tracking stage, the PSO algorithm optimizes the LQR weight matrix and adds feedforward control compensation to improve the path-tracking accuracy. According to the simulation results under two different paths, the path tracking accuracy of the PSO-LQR path tracking controller is improved by 59% compared with the conventional LQR, and its robustness is also significantly improved. In addition, by comparing the tracking effects under different vehicle speeds, the path tracking errors can all be controlled within 0.06 m, which verifies the effectiveness of the proposed path planning and trajectory tracking framework.

However, further research is required to investigate and address the deployment of the proposed path planning algorithm and tracking control strategy for accurate vehicle testing. In future work, the integration of machine learning and intelligent optimization algorithms into path planning and path tracking control research will be emphasized to develop faster and safer path planners and more precise and efficient path tracking controllers.