Abstract
Aiming at the problems of large randomness, slow convergence speed, and deviation of Rapidly-Exploring Random Tree algorithm, a new node is generated by a cyclic alternating iteration search method and a bidirectional random tree search simultaneously. A vehicle steering model is established to increase the vehicle turning angle constraint. The Rapidly-Exploring Random Tree algorithm is improved and optimized. The problems of large randomness, slow convergence speed, and deviation of the Rapidly-Exploring Random Tree algorithm are solved. Node optimization is performed on the generated path, redundant nodes are removed, the length of the path is shortened, and the feasibility of the path is improved. The B-spline curve is used to insert the local end point, and the path is smoothed to make the generated path more in line with the driving conditions of the vehicle. The feasibility of the improved algorithm is verified in different scenarios. MATLAB/CarSim is used for joint simulation. Based on the vehicle model, virtual simulation is carried out to track the planned path, which verifies the correctness of the algorithm.
1. Introduction
In recent years, with the rapid development of smart vehicles, the advantages of smart vehicles themselves have become increasingly prominent. For example, smart vehicles can reduce driving pressure, improve the driving safety of smart vehicles, avoid traffic congestion, and reduce environmental pollution [1, 2]. Path planning is the core of intelligent driving technology and multirobot collaboration technology [3–6]. The ability to plan for unknown roads is an important criterion for measuring smart cars. Vehicle path planning refers to planning a path that does not collide with obstacles according to certain standards when the starting position, ending position of vehicles, and the distribution of obstacles in the environment are known. In recent years, scholars have done a lot of research on path-planning algorithms, and new path-planning algorithms are constantly emerging and developing. The most representative and common path-planning algorithms in the field of path planning are mainly divided into map-based path-planning algorithms, bionics-based path-planning algorithms, and sampling-based path-planning algorithms [7–9].
Sampling-based search algorithms include probability map algorithms and Rapidly-Exploring Random Tree algorithms. The Rapidly-Exploring Random Tree algorithm is a path-planning algorithm proposed by LaValle [10, 11]. Its advantages include the following four aspects: first, it does not need to model the planning space and is a random sampling algorithm; second, it considers the objective constraints of unmanned vehicles; third, it is suitable to solve the path-planning problem under dynamic and multiobstacle conditions; and fourth, it can be applied to the path-planning problem under the high-dimensional environment. Therefore, it has been widely used [12]. The basic Rapidly-Exploring Random Tree algorithm also has the following disadvantages in path planning: first, the path is randomly generated, the path is biased; second, the random tree is nonoriented in the search process; and third, the convergence speed is slow, and the search efficiency is low [13, 14].
Aiming at the shortcomings of the basic Rapidly-Exploring Random Tree algorithm, researchers at home and abroad have carried out a lot of improvements. Typical improvements include the RRT-Connect algorithm, asymptotically-optimal Rapidly-Exploring Random Tree (RRT), asymptotically-optimal bidirectional Rapidly-Exploring Random Tree (B-RRT), and intelligent bidirectional RRT (IB-RRT) [15–19]. RRT-Connect algorithm was proposed by Kuffner and LaValle in 2000 [15]. The algorithm improves the speed of path finding by generating two random trees in parallel. The Rapidly-Exploring Random Tree algorithm, which is biased to search and bidirectional expansion, improves the convergence speed and search efficiency but does not overcome the randomness when random trees generate nodes [20, 21]. The RRT ∗ algorithm was proposed by Adiyatov and Varol [22]. In 2010, the proposed algorithm solves the nonoptimal problem of the path generated by the RRT algorithm, but the efficiency of path generation is greatly reduced due to the increase in the amount of calculation during exploration. Jordan borrowed the idea of the RRT-Connect algorithm. In 2013, a two-way extended RRT algorithm (B-RRT) was proposed. At the same time, the connection function of the RRT-Connect algorithm was improved to ensure that the two trees connected by the algorithm could generate an optimal path. Qureshi proposed the IB-RRT algorithm in 2015 [19] and introduced a smart sample insertion function in the B-RRT algorithm to improve the speed of the algorithm’s convergence to the optimal path. Wang Daowei proposed the concept of dynamic step size in 2016. The rapid expansion of the dynamic step size random tree algorithm improves the uncertainty of the algorithm and improves the obstacle avoidance ability, but it is impossible to determine the step size for different obstacles [23]. Song Xiaolin proposed to introduce heuristic function into the Rapidly-Exploring Random Tree algorithm in 2017, which makes the search tree more oriented in the search process, but the algorithm is easy to fall into a dead cycle in path planning [24].
This paper proposes an improved intelligent vehicle path-planning algorithm based on a Rapidly-Exploring Random Tree, which uses a cyclic alternating iterative search method to generate new nodes. The bidirectional random tree expands simultaneously. The turning angle constraint of the vehicle is increased, the generated nodes are optimized, and the path is smoothened. The shortcomings of the Rapidly-Exploring Random Tree algorithm in path planning are improved.
2. Vehicle Steering Model
In the process of intelligent vehicle driving, ensure not only the driving safety of the vehicle but also ensure the ride comfort of the passengers on the vehicle. Therefore, when the intelligent vehicle is planning the route, it is necessary to ensure that the vehicle avoids all obstacles and the smoothness of the planned route.
According to Newton’s second law and the geometric relationship of steering, the equation of intelligent vehicle’s steady-state steering can be derived. In order to facilitate the analysis of the steering state of the intelligent vehicle, a two-wheel model, as shown in Figure 1, is used.
In Figure 1, is the wheel angle of the front wheels. When the front wheels turn, the body generates centrifugal force. The lateral reaction forces on the front and rear wheels are and , and the corresponding sideslip angles and are generated. Assuming that the center of mass and instant center of rotation of the vehicle are and , respectively, the distance between the two points is the turning radius. is the yaw angular velocity, and then the velocity at the center of mass is . is the sideslip angle at the center of mass, that is, the angle between the direction of travel of the vehicle at the center of mass and the axis.
It is assumed that the vehicle moves only in a plane direction, and there is no vertical direction and roll and pitch movement around the and axes. The speed of the car is a fixed value, and the effects of air resistance and tangential force on the vehicle are ignored. The previous wheel angle was used as the only input, ignoring the effect of the steering system on the vehicle and ignoring the changes in tire characteristics and the effect of returning torque due to load changes on the left wheel. The component of on the axis iswhere is the component of on the axis, is the velocity at the center of mass, and is the sideslip angle at the center of mass.
Vehicle’s during the turn is small, and approximately is obtained. Therefore,where is the component of on the axis, is the velocity at the center of mass, is the turning radius, and is the yaw angular velocity.
The component of on the axis iswhere is the component of on the axis, is the velocity at the center of mass, and is the sideslip angle at the center of mass.
From this, the acceleration of the center of mass iswhere is the acceleration of the center of mass, is the component of on the axis, is the component of on the axis, is the turning radius, is the yaw angular velocity, and is the velocity at the center of mass.
From the force and moment balance equations, the differential motion equation can be derived aswhere and are the lateral reaction forces on the front and rear wheels, is the mass of the entire vehicle, is the moment of inertia of the vehicle body about axis, is the velocity at the center of mass, is the component of on the axis, is the yaw angular velocity, is the distance from the front wheel to the center of mass, and is the distance from the rear wheel to the center of mass.where and are the lateral reaction forces on the front and rear wheels, and are the front and rear tire sideslip stiffness, and and are the front and rear wheel sideslip angles.
This can be obtained from the geometric relationshipwhere and are the front and rear wheel sideslip angles, is the distance from the front wheel to the center of mass, is the distance from the rear wheel to the center of mass, is the yaw angular velocity, is the velocity at the center of mass, is the sideslip angle at the center of mass, and is the wheel angle of the front wheels.
Substituting into formula (5) and formula (6), we can getwhere and are the front and rear tire sideslip stiffness, is the distance from the front wheel to the center of mass, is the distance from the rear wheel to the center of mass, is the yaw angular velocity, is the velocity at the center of mass, is the sideslip angle at the center of mass, is the wheel angle of the front wheel, is the mass of the entire vehicle, and is the moment of inertia of the vehicle body about axis.
Get from formula (3), substitute it into formulas (9) and (10), and getwhere is the mass of the entire vehicle, is the velocity at the center of mass, is the yaw angular velocity, and are the front and rear tire sideslip stiffness, is the wheel angle of the front wheel, is the component of on the axis, is the distance from the front wheel to the center of mass, is the distance from the rear wheel to the center of mass, and is the moment of inertia of the vehicle body about axis.
When the front wheel is a step angle input, its response is a constant velocity circular motion, and the yaw angular velocity is constant, and , which can be obtained by substituting into formulas (11) and (12):where is the mass of the entire vehicle, is the velocity at the center of mass, is the yaw angular velocity, and are the front and rear tire sideslip stiffness, is the wheel angle of the front wheel, is the component of on the axis, is the distance from the front wheel to the center of mass, and is the distance from the rear wheel to the center of mass.
By synchronizing and eliminating the above two types, we can getwhere is called the stability factor and is called the steady-state yaw rate gain, also called sensitivity.
From formula (15), we get
The stability factor is an important index that affects the steering stability performance. Its value is divided into three cases (neutral steering, insufficient steering, and excessive steering) to discuss.(1)Neutral Steering. At this time, . This situation is equivalent to a state of equilibrium where the lateral acceleration at the vehicle’s center of mass is equal to the sideslip angles produced by the front and rear wheels. The yaw rate gain has a linear relationship with the vehicle speed. This steering characteristic is called neutral turning.(2)Insufficient Steering. At this time, , as can be seen from formula (15), the yaw angle speed gain is smaller than that during neutral steering, is no longer linearly related to the vehicle speed, and is a negative second derivative and exists in a certain speed range The maximum value curve, the stable steering characteristic at this time, is called understeer. The amount of understeer increases as the value of increases, and the yaw rate gain decreases. In the case of understeer, the degree of sideslip of the front wheels caused by the lateral acceleration at the center of mass will be greater than that of the rear wheels. In order to keep the radius unchanged, the front wheel rotation angle needs to be increased.(3)Oversteering. At this time, , the denominator in formula (15) is less than 1, the yaw rate gain is greater than the neutral steering, and the second derivative of the gain curve is greater than zero. This steering characteristic is called oversteering.
In neutral steering, when the turning radius is constant, the steering angle does not change when the speed changes. The steering angle depends only on the turning radius and wheelbase. When the wheelbase is constant, the larger the turning radius, the smaller the steering angle of the front wheels of the vehicle, the better the stability of the vehicle, and the higher the safety factor.
According to the above analysis, the steering angle of the front wheels of the vehicle is mainly affected by the turning radius. Regardless of vehicle speed, a vehicle with a larger turning radius will have a smaller front wheel side deflection angle, the better the vehicle’s maneuverability and stability, and the safety factor will be greatly improved. In order to ensure the smooth and smooth steering, when the RRT is planning the path, the new node needs to meet certain angular constraints to make the generated path closer to the vehicle’s motion requirements.
3. Improved Rapidly-Exploring Random Tree Algorithm
3.1. Basic Rapidly-Exploring Random Tree Algorithm
The expansion diagram of the basic RRT algorithm is shown in Figure 2. The basic RRT algorithm takes the initial point as the root node of the random tree and selects the random sampling point by searching for the free space. Choose a node closest to the random sampling point on the known random tree and connect node and node . A new node from is generated with a certain step size . If there is no collision with the obstacle during the expansion from to , this new node is added to the random tree to generate a random tree. When a child node in a random tree contains a target point , a path from the initial point to the target point can be generated in the random tree. Conversely, if a collision occurs, then we discard the expansion.
3.2. Cyclic Alternating Iterative Searching
The Rapidly-Exploring Random Tree algorithm is based on sampling, which has strong randomness. Aiming at the shortcomings of the basic Rapidly-Exploring Random Tree algorithm such as low search efficiency, nonoriented search process, and slow convergence speed, the basic Rapidly-Exploring Random Tree algorithm is optimized. First, a random sampling point is generated in a random manner as the growth target point of the random tree child node, and then the target point is used as the growth target point of the random tree child node. The two search methods alternately and iteratively search until a feasible path is found or a set threshold of the number of iterations is reached, the search is exited, and the search path fails.
3.3. Bidirectional RRT Algorithm
The bidirectional RRT algorithm defines two random trees in the free space, which select the starting point and the ending point as the random root node, respectively, expanding in the opposite direction. The expansion is ended until the two trees meet. That is to say, the search path is found.
When the random tree with the starting point as the root node searches for the free space to establish the random tree, the random tree with the ending point as the root node is also established. The two random trees generate a new node by turns and detect whether the Euclidean distance between the new node and the other random tree node is less than the set threshold. When the distance between two nodes is less than the set threshold, the two nodes are connected, that is, the two random trees are merged into one random tree to generate a path.
3.4. Increase Angular Constraint
It can be known from the vehicle’s steering model that the stability factor is an important index affecting the steering stability performance. The value of is divided into three cases: neutral steering (), insufficient steering (), and excessive steering (). Ignoring other influencing factors of the vehicle, based on neutral steering, it can be obtained from formula (16) that
According to the above analysis, the steering angle of the front wheels of the vehicle is affected by the turning radius. In order to ensure the smooth and smooth steering and the driving safety of the vehicle when using the Rapidly-Exploring Random Tree algorithm for path planning, the new node needs to meet certain angular constraints to make the generated path closer to the vehicle’s motion requirements and meet the vehicle’s kinematics model.
Assume that the angular constraint of the Rapidly-Exploring Random Tree in generating new nodes is , as shown in Figure 3; when the angle between , , and is less than the constraint value , the generated new node is discarded. When is greater than the constraint value of , the new node is retained and the new node is added to the random tree.
4. Node Optimization
The Rapidly-Exploring Random Tree algorithm expands in the map with a certain step size, the generated path nodes are too many, and the path has many turning angles, which cannot meet the actual driving conditions of the vehicle. Therefore, the feasible points of the generated path are optimized, redundant nodes are deleted, and the generated path is optimized.
As shown in Figure 4, array stores the feasible points of the accessible path obtained through the Rapidly-Exploring Random Tree algorithm search. 1 represents the initial point , represents the target point , and represents the feasible point from the starting point to the target point.
The idea of optimizing nodes is as follows:(1)Add the initial points to the new array .(2)Determine whether there is an obstacle between node and node . If not, skip node and determine whether there is an obstacle between node and the next node . If there is an obstacle, add this node to the array , search backward with as the new node, and so on.(3)When the target point is found, the search is ended and the target point is added to the array .
Array is the optimized node set.
5. Path Smoothing
In this paper, the B-spline curve is selected to smooth the path generated by Rapidly-Exploring Random Tree planning.
The expression of the B-spline curve is
In the formula, , , and .
B-spline curves are defined in sections. If vertices are given, then a parametric curve of order times can be defined.
The B-spline curve has the advantages of geometric invariance, convex hull, convexity, and variation reduction, and the number of control points has little correlation with the order of the function. Therefore, it has a good effect on smoothing the path.
For the feasible path generated by the Rapidly-Exploring Random Tree search, vertices are generated when each feasible node is connected. In order to avoid collision between the path processed by the B-spline curve and the obstacle, local endpoints are generated at both ends of the vertices. As shown in Figure 5, , , and are feasible points of the path, and local endpoints and are generated at both ends of fold point , respectively.
Due to the different angles of the vertices, the local endpoints and are selected in an adaptive manner, and the linear function is determined by points and :where and are constants.
The abscissa of satisfieswhere is the adaptive coefficient.
Substituting the abscissa of into equation (19) to obtain 2, calculate the coordinates of point , and then calculate the coordinates of point . The local endpoints and are added to the feasible points to smooth the path.
6. Simulation Experiment Analysis
Whether the improved Rapidly-Exploring Random Tree algorithm meets the requirements and whether the intelligent vehicle can accurately reach the set target position and complete the path planning need to be verified and analyzed by software. Therefore, MATLAB software is used to build a simulation experiment platform to verify the correctness of the improved Rapidly-Exploring Random Tree algorithm. At the same time, it is compared and verified with the basic Rapidly-Exploring Random Tree algorithm. The simulation map is a 500500 two-dimensional grid space. The simulation experiment conditions are shown in Table 1.
6.1. Increased Angular Constraint Simulation Experiment Results
In a two-dimensional grid space with a simulation map of 500500, a point (250, 250) is used as the root node of the rapid expansion random tree to generate a random tree around the two-dimensional map. The schematic diagram of random tree expansion without increasing angle constraint is shown in Figure 6, and the schematic diagram of random tree expansion with increasing angle constraint is shown in Figure 7.
Analyzing, Figure 6, the Rapidly-Exploring Random Tree algorithm without increasing the angle constraint, the size of the angle between the feasible points generated cannot be determined, and too small an angle cannot satisfy the vehicle’s kinematic model. Analyzing, Figure 7, an angular constraint Rapidly-Exploring Random Tree is added, the angle between feasible points is smoother during the expansion process. Increasing the angle between feasible points meets the constraint requirements, and the generated path is more in line with the actual needs of the vehicle.
6.2. Simulation Results of the Rapidly-Exploring Random Tree Algorithm
In a two-dimensional grid space with a simulation map of 500500, point (10, 10) is the starting point and (490, 490) is the target point. Black squares represent obstacles in the two-dimensional map and are impassable.
The simulation results of the basic Rapidly-Exploring Random Tree algorithm are shown in Figure 8(a). The simulation results of the bidirectional Rapidly-Exploring Random Tree algorithm are shown in Figure 8(b). The simulation results of the heuristic Rapidly-Exploring Random Tree algorithm are shown in Figure 8(c).
(a)
(b)
(c)
As shown in Figure 8, red and blue indicate a generated random tree, and green indicates the Rapidly-Exploring Random Tree algorithm to search a feasible path generated in a two-dimensional space. As shown in Figure 8(a), the basic Rapidly-Exploring Random Tree algorithm is nonoriented in the search process, randomly generates new nodes, has large randomness, and generates a large number of nodes. As shown in Figures 8(b) and 8(c), although the bidirectional Rapidly-Exploring Random Tree algorithm and the heuristic Rapidly-Exploring Random Tree algorithm improve the search efficiency of the algorithm, the generated path is of poor quality, has bias, and cannot meet the driving state of the vehicle.
6.3. Simulation Results of the Improved Rapidly-Exploring Random Tree Algorithm
In a two-dimensional grid space with a simulation map of 500500, point (10, 10) is the starting point and (490, 490) is the target point. Black squares represent obstacles in the two-dimensional map and are impassable. The simulation results of the improved Rapidly-Exploring Random Tree algorithm are shown in Figure 9.
As shown in Figure 9, red and blue represent the bidirectional random tree generated with the starting point (10, 10) and the target point (490, 490) as the root node, respectively, and green is the feasible path generated by the improved Rapidly-Exploring Random Tree algorithm to search the two-dimensional space. Analyzing Figures 8 and 9, the improved Rapidly-Exploring Random Tree algorithm uses a cyclic alternating iterative search method, which greatly improves the randomness of the basic Rapidly-Exploring Random Tree algorithm. The number of nodes generated is significantly reduced, and the number of iterations of the algorithm is reduced.
Node optimization is performed on the feasible path generated in Figure 9, and the node optimization simulation experiment results are shown in Figure 10.
As shown in Figure 10, blue is the feasible path in the two-dimensional space after node optimization. In Figure 9, nodes are not optimized. Although a feasible path is generated, there are many feasible points, the path length is large, and the curvature of the path cannot meet the driving conditions of the vehicle, which is not the optimal path. In Figure 10, node optimization is added to remove redundant nodes, greatly reducing the number of feasible path nodes, reducing the length of the path, improving the feasibility of the path, and making the generated path more consistent with the actual running track of the vehicle.
The smoothness processing is performed on the path generated in Figure 10, and the result of path smoothing simulation is shown in Figure 11.
As shown in Figure 11, red is a feasible path in a two-dimensional space after path smoothing. Analyzing Figures 10 and 11, inserting local endpoints at both ends of the vertices makes the path vertices smoother, improves the accuracy of the Rapidly-Exploring Random Tree algorithm, greatly improves the quality of the path, and is more in line with the actual conditions of vehicle driving.
The improved algorithm, the basic Rapidly-Exploring Random Tree algorithm, the bidirectional Rapidly-Exploring Random Tree algorithm, and the heuristic Rapidly-Exploring Random Tree algorithm are simulated 50 times, respectively, under the premise of the same step size, and the average running time, average path length, and average number of iterations of the simulation experiment are calculated as shown in Figures 12–14 and Table 2.
Analysis of Figures 12–14 and Table 2 shows that compared with the basic Rapidly-Exploring Random Tree algorithm, the bidirectional Rapidly-Exploring Random Tree algorithm, and the heuristic Rapidly-Exploring Random Tree algorithm, the algorithm in this paper has shorter running time, shorter path length, and fewer iterations. The randomness of the Rapidly-Exploring Random Tree algorithm is reduced, the number of iterations of the algorithm is reduced, and the convergence speed of the algorithm is accelerated. All new nodes grow towards the target point, avoiding global search, improving the real-time performance of unmanned vehicle movement, and easier to obtain the optimal path, improving the bias of the basic Rapidly-Exploring Random Tree algorithm.
6.4. Analysis of Simulation Results in Different Environments
In order to further verify the advantages and disadvantages of the improved algorithm, simulation analysis is carried out in different scenarios.
6.4.1. Scene 1
No obstacles in the environment: when there is no obstacle in the environment, the simulation results of the basic Rapidly-Exploring Random Tree algorithm and the improved algorithm are shown in Figure 15.
(a)
(b)
6.4.2. Scene 2
Narrow channels in the environment: when there are narrow channels in the environment, the simulation results of the basic Rapidly-Exploring Random Tree algorithm and the improved algorithm are shown in Figure 16.
(a)
(b)
6.4.3. Scene 3
Complex obstacles in the environment: when there are complex obstacles in the environment, the simulation results of the basic Rapidly-Exploring Random Tree algorithm and the improved algorithm are shown in Figure 17.
(a)
(b)
Analysis of Figures 15–17 shows that, in different scenarios, based on the vehicle turning model, the improved algorithm addresses the problem of too many feasible nodes in the initial path and cannot meet the vehicle’s driving conditions. The redundant nodes in the feasible nodes are removed, which greatly shortens the length of the path. Finally, the vertices in the path are smoothed to make the path vertices smoother, the accuracy of the algorithm is improved, and the quality of the path is greatly improved.
6.5. Path following Simulation
In order to verify the feasibility of the path, a car model was built using CarSim simulation software, and the steering controller based on the pure tracking model was used to track the path planning results.
In the CarSim environment, the vehicle model parameters shown in Figure 18 are established, and some important parameters are shown in Figure 18.
In the Matlab/CarSim environment, a steering controller model based on a pure tracking model is built, and the software interface between CarSim and Matlab is used to integrate the vehicle model and the steering controller model, as shown in Figure 19.
In order to be able to put the planned path obtained by the improved algorithm into the joint simulation model, it is necessary to output the data of the path planning result and output the planned path in the form of two-dimensional coordinates. The output result is shown in Figure 20.
(a)
(b)
The path output by the improved algorithm is extracted and input to the controller simulation model, and the controller model is allowed to track the path and obtain the corresponding tracking path. The tracking result is shown in Figure 21.
As can be seen from Figure 21, the red path is the planned path, and the black dotted line is the tracking path. The path planned by the improved algorithm differs slightly from the tracked path, and the curve curvature changes more smoothly, so the path generated by the improved algorithm is more in line with the driving characteristics of the actual vehicle. For intelligent vehicles, the actual steering controller of the entire vehicle and the steering controller set by CarSim use the same type of controller, and then the optimized path will also be more in line with the constraints of the vehicle. So, for vehicles, the optimized path is easier to track, and for passengers, riding is more comfortable.
7. Concluding Remarks
In this paper, a new node is generated by cyclic iterative search, and a two-way random tree is used to search at the same time. The basic Rapidly-Exploring Random Tree algorithm is improved and optimized by adding the turning angle constraint of the vehicle. The problems of large randomness, slow convergence speed, and deviation of the basic Rapidly-Exploring Random Tree algorithm are solved. The node optimization of the generated path shortens the length of the path, and the smoothness of the path is processed to make the generated path more in line with the driving conditions of the vehicle. However, whether it is a Rapidly-Exploring Random Tree algorithm, particle swarm algorithm, or an improved Rapidly-Exploring Random Tree algorithm, they are model-driven and have certain limitations; further research is needed to combine data-driven path planning and obstacle avoidance for intelligent vehicles and ultimately make the technology of smart vehicles more mature.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.