# Improved Bidirectional RRT∗ Path Planning Method for Smart Vehicle

Authors: Qingying Ge; Aijuan Li; Shaohua Li; Haiping Du; Xin Huang; Chuanhu Niu
Journal: Mathematical Problems in Engineering
 (2021)

---

## Abstract

In this paper, an improved bidirectional RRT∗ vehicle path planning method for smart vehicle is proposed. In this method, the resultant force of the artificial potential field is used to determine the search direction to improve the search efficiency. Different kinds of constraints are considered in the method, including the vehicle constraints and the vehicle driving environment constraints. The collision detection based on separating axis theorem is used to detect the collision between the vehicle and the obstacles to improve the planning efficiency. The cubic B-spline curve is used to optimize the path to make the path’s curvature continuous. Both simulation and experiment are implemented to verify the proposed improved bidirectional RRT ∗ method. In the simulation analysis, this paper’s method can generate the smoothest path and takes the shortest time compared with the other two methods and it can be adaptive to the complicated environment. In the real vehicle experiment, we can see from the test results that this paper’s method can be applied in practice on the smart electric vehicle platform; compared with others’ algorithm, this paper’s algorithm can generate shortest and smoothest path.


---

## Body

## 1. Introduction

Smart vehicle plays an important role in the smart transportation systems. Especially, the path planning problem is an important component of the planning and control system [1]. The path planning refers to planning a path that does not collide with obstacles when the starting position, the ending position of vehicles, and the distribution of obstacles in the environment are known. The path parameters provided by the path planning method are supplied to the controller to control the vehicle drive along the planned path accurately [2].In recent years, scholars have done a lot of research on path planning algorithms, and new path planning algorithms are constantly emerging and developing [3]. The most representative and common path planning algorithms in the field are mainly divided into map-based path planning algorithms [4], sampling-based search path planning algorithms [5], interpolation method [6], and artificial potential field method [7]. Sampling-based search path planning algorithm includes probability map algorithms and rapidly-exploring random tree (RRT) algorithms [8, 9]. The RRT algorithm is widely used because of the advantage that it is suitable to solve the path planning problem under dynamic and multiobstacle conditions. However, the basic RRT algorithm has the disadvantage that the convergence speed and the search efficiency are low and the planned path is difficult to be the optimum [10, 11]. Regarding the disadvantage of the RRT algorithm, researchers have carried out a lot of improvements. The typical improvements are improving the RRT’s search style and fusing the RRT with other algorithms effectively. Thus, the generated path is optimized again and the planning effect is further improved [12]. Among them, improving the RRT’s search mode is an important improved direction. Researchers conduct target-biased search on RRT to generate an extended mode of nonrandom sampling, thus improving the planning efficiency [13]. At the same time, rapid expansion was carried out to generate PQ-RRT ∗ [14] and bidirectional expansion RRT ∗ [15], and then path optimization was carried out on the existing planning results. Based on the deviation research, the path is optimized using Bezier curves in the literature [16], and the optimized path can meet the requirements of vehicle safety. Considering the vehicle constraints, the quartic spline curve is used to provide a smooth dynamic feasible path to make the vehicle drive safely in the literature [17]. Shi et al. generated new nodes at the same time through cyclic iterative search and bidirectional search and solved the problems of slow convergence speed and large deviation by adding turning constraints and deleting redundant nodes [18]. Yang combined the RRT of biased target position with greedy RRT and then adopted the anytime strategy to improve the calculation time and path quality effectively [19]. For the dynamic driving environment full of uncertainty, Kuwata et al. mainly used the methods of biased heuristic sampling and safety detection to face the situation of uncertainty and incomplete perception conditions [20]. When faced with a large dynamic environment, Fulgenzi et al. [21] calculated the corresponding motion probability by trajectory prediction of objects moving in the driving environment, so as to carry out the dynamic motion planning of the vehicles. However, the accuracy of obstacle motion prediction needs to be improved.For the operating environment closer to the actual road scene, Bai et al. used model predictive control for partial connection and collaborative motion planning in an automated environment and used a numerical method based on dynamic programming to solve the problem. This method effectively reduces the impact caused by lane changes. Vibration phenomenon and reduced time-consuming have great potential in real-time applications [22]; Zhanbo et al. proposed a solution method based on two-layer dynamic programming based on the mixed scene where human drivers and autonomous vehicles coexist in the ramp scene, which has strong versatility and lays the foundation for traffic analysis in a mixed traffic environment [23]; Wang et al. carried out a coordinated traffic organization method in multi-intersection environment based on networked automated vehicles. In order to adapt to dynamic traffic demands, they proposed a complex path planning strategy, which could effectively improve traffic efficiency, but did not consider the influence of information delay caused by communication uncertainty [24]. Rahmati et al. considered networked autonomous vehicles and driving scenarios with high human participation and established a human-vehicle interactive decision-making framework to achieve human-vehicle coexistence, but did not combine behavior prediction and intelligent body motion planning to develop planning algorithms [25].In this paper, an improved path planning method based on the bidirectional RRT∗ is investigated. The improved path planning method has many advantages. Firstly, this method can reduce the time of planning path and improve the planning efficiency, because the artificial potential field method is applied to establish a mechanism of extension, which makes the random tree’s extension have more intentionality; secondly, the vehicle’s constraints and the obstacle environment constraints are considered so that the method can be applied to the environment with complicated dynamic obstacles; thirdly, the theory-based SAT is used to detect the collision between the vehicle and the obstacles to make the vehicle drive more safely. At last, the cubic B-spline curve is used to optimize the generated path to make the curvature of the path continuous and the vehicle can drive smoothly and steadily. The remainder of the paper is organized as follows: Section 2 presents the proposed vehicle dynamic model; Section 3 discusses the path planning method based on the improved RRT ∗; the simulation and experimental results are presented in Section 4; finally, Section 5 presents the conclusions of this research.


## 2. The Vehicle Model

RRT is an incremental path planning algorithm in which the new state (qnear, u, Δt) function can use a dynamic model to limit the vehicle’s state. In this paper, we use a dynamic vehicle model. The dynamic model of the smart vehicle is shown in Figure 1.Figure 1
Simplified vehicle dynamics model.This model is simplified by projecting the front and the rear wheels on two virtual wheels located at the middle of the vehicle. On the premise of the bicycle model assumption, the following assumptions are made: only the characteristics of the purely lateral tires are considered, and the longitudinal and lateral coupling relationships of the tire forces are ignored; the left and right shifts of the load are not considered; the transverse longitudinal aerodynamics is ignored. The state of the vehicle is defined byX = (xg, yg, θ, vy, ω)T, where xg and yg represent the coordinates of the vehicle gravity center, θ is its orientation, vy is the lateral speed, and ω is the yaw rate.As shown in Figure1, Lf and Lr are the distance from the center of mass to the front and rear axle, respectively, δf is the front wheel angle, and f are the forces in X and Y directions of the front wheel, respectively. Using the fundamental law of dynamics, we can get the dynamic formula, which is(1)mν˙x−νyω˙=−Fxfcosδf−Fyfsinδf−Fxr,mν˙y+νxω˙=Fyfcosδf−Fxfsinδf+Fyr,Izω˙=LfFyfcosδf−Fxfsinδf−LrFyr.A constant longitudinal speedvx is chosen for the vehicle model. Furthermore, as the aerodynamic resistance is neglected, the longitudinal tire force Fxf becomes zero. Considering this, we get the simplified dynamic formula, which is(2)ν˙y=Fyfmcosδf+Fyrm−νxω,ω˙=LfIzFyfcosδf−LrIzFyr.Thus, as a linear tire model is used, we can have(3)Fyf=−Cαfαf,Fyr=−Cαrαr,where Cαf and Cαr are the cornering stiffness coefficients for the front and rear tires, respectively and αf and αr are the slip angles for the front and rear wheels, respectively. These angles are assumed to be small. So, we can obtain(4)αf=νy+Lfωνx−δf,αr=νy−Lrωνx.According to this, we obtain the full nonlinear model, which is(5)ν˙yω˙=ACBDνyω+EFδf,where(6)A=−Cαfcosδf+Cαrmνx,B=−LfCαfcosδf+LrCαrmνx−vx,C=−LfCαfcosδf+LrCαrIzνx,D=−Lf2Cαfcosδf+Lr2CαrIzνx,E=Cαfcosδfm,F=LfCαfcosδfIz.By integrating, we can compute the new coordinates (xg, yg, θ) of the vehicle, which is(7)x˙g=νxcosθ−νysinθ,y˙g=νxsinθ+νycosθ,θ˙=ω.We now have nonlinear vehicle modelx˙=fx,u, starting from a given configuration qnear = x(t). We integrate our system (7) using a given input u to obtain qnew = x(t + Δt). The configuration will be added to the tree if it lies in free.


## 3. The Improved RRT∗ Path Planning Method

### 3.1. The Basic RRT∗ Algorithm

RRT∗ is a kind of advanced gradually optimal sampling method based on the RRT algorithm, in which the selected mode of the parent node is optimized. The pseudocode of the RRT ∗ is shown in Algorithm 1. The map environment M is input and the starting point qinit and target point qgoal are determined. Taking the starting point qinit as the root node of the obtained path G, the random point qrand is generated. Finding the nearest node qnear from the random point qrand on the tree, the two points qrand and qnear are connected. Taking qrand as the center and r as the radius, the nodes are searched on the tree and the potential parent node qparent is found. The cost to be the parent node qparent is calculated, and it is compared with the cost of the original path. If the cost of the parent node qparent is smaller than the original path, then the collision detection is performed. Otherwise, the next potential parent node is searched until the collision detection is passed. The previous edge is deleted from the tree and the new edge is added. Iterative selection of parent nodes is continuously performed. If a more suitable parent node is selected, the nodes on the existing tree are reconnected, and the original connection is removed, so as to ensure that the progressive optimal solution is obtained.Algorithm 1:RRT ∗ pseudocode.
Input:M, qinit, qgoalResult: a pathG from qinit to qgoal(1)
G.Init ();(2)
fori = 1 to n, do(3)
qrand ← Sample (M);(4)
qnear ← Near (qrand, G);(5)
qnew ← Steer(qrand, qnear, Stepsize);(6)
if Collision_free_(qnear, qnew), then(7)
qnear ← Near (G, qnew);(8)
qmin ← Choose qparent (qnear, qnew)(9)
G.add.Node Edge (qnear, qnew)(10)
returnG.


### 3.2. The Improvement Strategy

The improvement strategy flow chart of this paper is shown in Figure2. First, determine the vehicle model and driving environment, as well as the vehicle’s starting point and target point location, and then proceed with the vehicle path planning and design on this basis. In the sampling process, the sampling method is improved based on the idea of artificial potential field, the sampling nodes are reduced by considering the constraints of the vehicle itself and the driving environment, and the separation axis collision theory is used to detect whether the generating node collides with obstacles; finally, the whole tree is backtracked to generate the feasible path of the vehicle which is smoothed by using the cubic B-spline curve.Figure 2
Improvement strategy flow chart.


#### 3.2.1. Random Sampling Improvement Method Based on the Artificial Potential Field

According to the wide search space, the random mode is used to do the node extension when the original RRT and RRT∗ algorithms are sampled. Although the probability integrity can be guaranteed, the search efficacy of the algorithm is reduced to some extent. So, the applicability of the original RRT and RRT ∗ algorithms are challenged for the smart vehicle which has high real-time requirement. According to the narrow search space, the deviation sample reduces the probability of obtaining the feasible path as quickly as possible. So, the RRT ∗ algorithm should be improved to improve both the probability integrity and the search efficacy. The goal attraction thought of the artificial potential field is used to determine the search direction. The artificial potential field method path planning is a virtual force method proposed by Khatib. Its basic idea is to design the movement of the vehicle in the surrounding environment into an abstract artificial gravitational field. The target point produces “gravitation” to the vehicle, and the obstacle produces “repulsion” to the vehicle. Finally, the movement of vehicles is controlled by seeking the combined force. When the search range is larger than the length of the vehicle and the vehicle constraints are satisfied, a fast search method biased towards the target area is adopted to improve the efficiency of the search path and the safety performance of the vehicle. Using the idea of artificial potential field, according to the gravity function of the target position, sampling is conducted in the target direction; the formulation is defined as(8)F=Fatt+Frep.Among them,Fatt is the target gravity received by the intelligent vehicle, and Frep is the comprehensive obstacle repulsion received by the target vehicle.The target gravityFatt is specifically expressed as(9)FattM=−∇UattM=Ka⟶Gρg,where Uatt(M) = 1/2K(M − Mg)2, M = [x, y]T, Mg = [xg, yg]T, and Fatt(M) is the gravity function; it can be obtained by differentiating the negative gradient of the gravitational field function; K is the gravitational field gain, a⟶G is the forward target, ρg is the distance between the controlled vehicle and the target position, M is the current position of the vehicle, and Mg is the target position [26].Obstacle repulsion forces are based on static obstacles and dynamic obstacles to establish repulsion potential field functions and finally form a comprehensive obstacle repulsion forceFrep.(1) Static Obstacle Restraint. The static obstacle function in the driving environment is expressed as the following formula:(10)UrjM=12Kr1dob−1d02,dob≤d0,0,dob>d0.In the above formula,Kr is the static gain coefficient of the repulsive force field, dob is the distance between the controlled object and the obstacle, and d0 is the boundary distance of the repulsive force set according to the vehicle speed.Derive the negative gradient of the repulsion field function to obtain the static repulsion functionFrjM, which is the following formula:(11)FrjM=Fr1+Fr22a⟶b,dob≤d0,0,dob>d0,Among them,Fr1=Kr1/dob−1/d01/dob2Fr2=n/2Kr1/dob−1/d02 and a⟶b refers to the unit vector of the static obstacle pointing to the controlled vehicle.(2) Dynamic Obstacle Restraint. The dynamic obstacle function in the driving environment is expressed as the following formula:(12)Urmx,y=12ηsVe−Vbcosθ−β2ρx,ya⟶b,θ−βε−π2,π2,0,others.In the above formula,ηs is the speed potential field repulsion gain, Ve is the speed of the vehicle, Vb is the speed of the dynamic obstacle, θ is the angle between the direction of the controlled vehicle and the positive direction of the x-axis, and β is the positive direction of the dynamic obstacle and the x-axis. The direction is the angle; a⟶b is the unit vector of the dynamic obstacle pointing to the controlled vehicle.Whenθ−β∉−π/2,π/2, the dynamic obstacle loses its repulsive force to the controlled vehicle, so the repulsive force is 0 at this time.Derive formula (12) to obtain dynamic obstacle repulsion, as shown in the following formula:(13)Frm=ηsVe−Vbcosθ−βρx,ya⟶b,θ−βε−π2,π2,0,others.In the sampling area which is less than 0.1L, where L is the length of the car, the search mode of random sampling is switched. One reason is to ensure the vehicle can approach the target point quickly and the other reason is that it can effectively face the path generation problem in narrow environments.


#### 3.2.2. The Constraints

When the original RRT extends nodes, it is necessary to judge whether all extended nodes are obstacle positions, which reduces the computational efficiency of the algorithm to a certain extent. Therefore, firstly, the surrounding obstacles with a radius of 0.5 L are detected. If there are no obstacles within this range, the extended node directly skips the obstacle detection, or the obstacle detection is carried out, so as to improve the implementation efficiency of the algorithm.(1) The Constraints of the Vehicle. The vehicle’s executing actuator constraints should be met in the path planning algorithm so that the vehicle can drive safely in the complicate environment. The executing actuator of the vehicle includes the vehicle’s front wheel steering angle δ, the maximum driving speed vmax, and the maximum acceleration amax. These factors should be considered to do the nodes’ extension within certain limits. Therefore, in this paper, the comprehensive factor dc is used to measure the distance between the sampling point s and the parent node ti = [xi, yi], which is expressed as(14)dc=dh+ded,where, dh = 1 − h1h2/|h1||h2|, h1=[xs − xi, ys − yi], h2 = [xi − xj, yi − yj], (xs, ys) is the location of the current sampling node, (xi, yi) is the location of the current parent node, ded=xs−xi2+ys−yi2, dh is the heading change of the branch to be extended relative to the current branch, h1h2/|h1||h2| is the direction similarity, h1 is the direction vector to be extended, h2 is the brunch’s direction vector to be extended, and ded is the Euclidean distance between the node and the sampling point.Furthermore, in the cost function calculation of the traditional RRT, the size of the path length is considered usually. The energy consumption and the time consumption are also considered in this paper; the cost functionccost (ni, ne) of the extended brunch is defined as(15)Ccostni,ne=dh+ded+Nf+t,where Nf = (mae + mgμr + 0.5cdAρv2)vts, ae ≥ 0, Nf = (rbmae + mgμr + 0.5cdAρv2)vts, ae < 0, ne is the extended node, dh is the heading change of the branch to be extended relative to the current branch, ded is the Euclidean distance between the node and the sampling point, Nf is the energy consumption of the extended brunch, t is the time consumption of the extended brunch, rb is the brake feedback ratio, m is the vehicle mass, μr is the rolling resistance coefficient, cd is the wind resistance coefficient, A is the windward area, and ρ is the air density.Based on the improved method, the comprehensive distance value of peripheral nodes is calculated and the cost function is taken into consideration comprehensively. Meanwhile, it is used for subsequent node extension. The selection of candidate nodes makes the extended branch meet the vehicle motion constraints, thus reducing the amount of the subsequent optimization calculation.(2) The Constraints of the Vehicle Driving Environment. In order to make the planned path feasible, vehicles need to meet the constraints of road environment when they drive on the road. The left boundary and right boundary of the road are represented by Bl and Br, respectively, so the extended nodes need to meet the requirements of the formula:(16)B1≤ney≤Br,(17)Ts≤nex≤Tg.To sum up, firstly, artificial potential field is used to expand the bidirectional RRT∗ around the target direction. Then, in the specific expansion, the constraints of the vehicle itself and the driving environment are considered so that the planned path meets the actual driving demand of the vehicle.


### 3.3. Collision Detection Based on SAT (Separating Axis Theorem)

The main contents of SAT (separating axis theorem) are that when two objects need to be detected for collision, the two objects are supposed to be two convex polygons and the convex polygons are projected onto a vector separation axis at the same time firstly. Then, whether there is a gap between the two projections should be judged. If there is a gap, there is no collision; if there is no gap, the collision will happen. If the obstacle in the driving environment is a concave shape, connect the vertices of the two sides of the concave shape whose internal angle is greater than 180° to convert the concave shape into a convex shape. The specific conversion method is shown in Figure3. If the concave-sided shape has multiple internal angles greater than 180°, the corresponding multiple conversions are performed until it becomes a convex-sided shape.Figure 3
Concave polygon conversion diagram.
(a)(b)In order to make the planned path meet the actual driving environment of the vehicle, the actual geometry of the vehicle itself and obstacles need to be considered in the path planning method. In this paper, as to ensure the driving safety of the vehicle, the geometry of the vehicle is supposed as a circle, with the vehicle’s center of mass as the center and the length between the center and the front axle as the radius. At the same time, the geometry of the obstacles is assumed as rectangles and the separation axis theorem is used to transform the collision detection problem between vehicles and obstacles into the intersection test problem of different graphs.The schematic diagram of the SAT is shown in Figure4. Both graphs are in the Cartesian coordinate system XOY. The circle A and rectangle B represent the vehicle and obstacle, respectively. The unit vectors of the circle A in the local coordinates with Ao as the origin point are Ax and Ay; the unit vectors of the rectangle B in the local coordinates with Bo as the origin point are Bx and By; the diameter of the circle A is Da; the length and width of the rectangle B are Db and Dw; N is the distance between the center point of the circle A and the center point of rectangle B; β is the vehicle heading angle in global coordinates; α is the angle between the center line of the two graphs and the x-axis of the global coordinate system.Figure 4
The schematic diagram of the SAT.There are four separation axes, including thex axe and y axe of the circle A and the rectangle B, respectively. In order to detect the collision of the two graphics, whether the projection of two figures on all the separation axes simultaneously satisfies the following conditions should be judged; the conditions are as follows:Condition 1: the project axle is thex axle of the local coordinate system of the circle A:(18)Ncosα>Da2cosα+Da2sinβ.Condition 2: the project axle is they axle of the local coordinate system of the circle A:(19)Nsinα>Da2sinβ+Da2cosβ.Condition 3: the project axle is thex axle of the local coordinate system of the rectangle B:(20)Ncosα−β>Db2cosβ+Dw2sinβ.Condition 4: the project axle is they axle of the local coordinate system of the rectangle B:(21)Nsinα−β>Db2sinβ+Dw2cosβ.If all of the conditions are met simultaneously, then there is no collision between the vehicle and the obstacles and the path can be used, or if the collision will happen, then the path should be abandoned. The SAT is used to do the collision detection. The advantage of the SAT is the detection speed which is very quick and the detection results are accurate; this method can help find the appropriate path as quickly as possible.


### 3.4. The Path Optimization Method Based on the Cubic B-Spline Curve

Because the curvature of the path planned by the traditional bidirectional RRT∗ is not continuous, when the vehicle tracks the path, it must stop to change the driving direction such that the vehicle cannot drive smoothly. So, the planned trajectory should be optimized to make the vehicle drive smoothly and steadily [27]. The polynomial and the Bezier curve are used to optimize the path; however, different kinds of the constraints such as curvature, accelerated speed, and heading angle cannot be satisfied. Because the curvature is continuous and different kinds of the constraints are considered, the cubic B-spline curve is used to optimize the generated path.Suppose there aren + 1 control vertexes Pi(i = 0, 1, …, n), the cubic B-spline curve is expressed as(22)Cu=∑i=0nPiBi,ku,where the primary function Bi,k (i = 0, 1, …, n), the highest degree is k, the basis function is determined by the sequence of node u, and the value range of u is u0≤u1≤,⋯,≤un+k+1.The cubic B-spline curve is shown as(23)Cu=∑i=03PiBi,3u.where the basis function Bi, 3(u) is defined as(24)B0,3u=−u3+3u2−3u+13!,B1,3u=3u3−6u2+43!,B2,3u=−3u3+3u2+3u+13!,B3,3u=u33!.The B-spline fitting curve is expressed as(25)x=B0∗a1,i+B1∗a1,i+1+B2∗a1,i+2+B3∗a1,i+3,y=B0∗a2,i+B1∗a2,i+1+B2∗a2,i+2+B3∗a2,i+3,where a is the fixed coefficient.


## 3.1. The Basic RRT∗ Algorithm

RRT∗ is a kind of advanced gradually optimal sampling method based on the RRT algorithm, in which the selected mode of the parent node is optimized. The pseudocode of the RRT ∗ is shown in Algorithm 1. The map environment M is input and the starting point qinit and target point qgoal are determined. Taking the starting point qinit as the root node of the obtained path G, the random point qrand is generated. Finding the nearest node qnear from the random point qrand on the tree, the two points qrand and qnear are connected. Taking qrand as the center and r as the radius, the nodes are searched on the tree and the potential parent node qparent is found. The cost to be the parent node qparent is calculated, and it is compared with the cost of the original path. If the cost of the parent node qparent is smaller than the original path, then the collision detection is performed. Otherwise, the next potential parent node is searched until the collision detection is passed. The previous edge is deleted from the tree and the new edge is added. Iterative selection of parent nodes is continuously performed. If a more suitable parent node is selected, the nodes on the existing tree are reconnected, and the original connection is removed, so as to ensure that the progressive optimal solution is obtained.Algorithm 1:RRT ∗ pseudocode.
Input:M, qinit, qgoalResult: a pathG from qinit to qgoal(1)
G.Init ();(2)
fori = 1 to n, do(3)
qrand ← Sample (M);(4)
qnear ← Near (qrand, G);(5)
qnew ← Steer(qrand, qnear, Stepsize);(6)
if Collision_free_(qnear, qnew), then(7)
qnear ← Near (G, qnew);(8)
qmin ← Choose qparent (qnear, qnew)(9)
G.add.Node Edge (qnear, qnew)(10)
returnG.


## 3.2. The Improvement Strategy

The improvement strategy flow chart of this paper is shown in Figure2. First, determine the vehicle model and driving environment, as well as the vehicle’s starting point and target point location, and then proceed with the vehicle path planning and design on this basis. In the sampling process, the sampling method is improved based on the idea of artificial potential field, the sampling nodes are reduced by considering the constraints of the vehicle itself and the driving environment, and the separation axis collision theory is used to detect whether the generating node collides with obstacles; finally, the whole tree is backtracked to generate the feasible path of the vehicle which is smoothed by using the cubic B-spline curve.Figure 2
Improvement strategy flow chart.


### 3.2.1. Random Sampling Improvement Method Based on the Artificial Potential Field

According to the wide search space, the random mode is used to do the node extension when the original RRT and RRT∗ algorithms are sampled. Although the probability integrity can be guaranteed, the search efficacy of the algorithm is reduced to some extent. So, the applicability of the original RRT and RRT ∗ algorithms are challenged for the smart vehicle which has high real-time requirement. According to the narrow search space, the deviation sample reduces the probability of obtaining the feasible path as quickly as possible. So, the RRT ∗ algorithm should be improved to improve both the probability integrity and the search efficacy. The goal attraction thought of the artificial potential field is used to determine the search direction. The artificial potential field method path planning is a virtual force method proposed by Khatib. Its basic idea is to design the movement of the vehicle in the surrounding environment into an abstract artificial gravitational field. The target point produces “gravitation” to the vehicle, and the obstacle produces “repulsion” to the vehicle. Finally, the movement of vehicles is controlled by seeking the combined force. When the search range is larger than the length of the vehicle and the vehicle constraints are satisfied, a fast search method biased towards the target area is adopted to improve the efficiency of the search path and the safety performance of the vehicle. Using the idea of artificial potential field, according to the gravity function of the target position, sampling is conducted in the target direction; the formulation is defined as(8)F=Fatt+Frep.Among them,Fatt is the target gravity received by the intelligent vehicle, and Frep is the comprehensive obstacle repulsion received by the target vehicle.The target gravityFatt is specifically expressed as(9)FattM=−∇UattM=Ka⟶Gρg,where Uatt(M) = 1/2K(M − Mg)2, M = [x, y]T, Mg = [xg, yg]T, and Fatt(M) is the gravity function; it can be obtained by differentiating the negative gradient of the gravitational field function; K is the gravitational field gain, a⟶G is the forward target, ρg is the distance between the controlled vehicle and the target position, M is the current position of the vehicle, and Mg is the target position [26].Obstacle repulsion forces are based on static obstacles and dynamic obstacles to establish repulsion potential field functions and finally form a comprehensive obstacle repulsion forceFrep.(1) Static Obstacle Restraint. The static obstacle function in the driving environment is expressed as the following formula:(10)UrjM=12Kr1dob−1d02,dob≤d0,0,dob>d0.In the above formula,Kr is the static gain coefficient of the repulsive force field, dob is the distance between the controlled object and the obstacle, and d0 is the boundary distance of the repulsive force set according to the vehicle speed.Derive the negative gradient of the repulsion field function to obtain the static repulsion functionFrjM, which is the following formula:(11)FrjM=Fr1+Fr22a⟶b,dob≤d0,0,dob>d0,Among them,Fr1=Kr1/dob−1/d01/dob2Fr2=n/2Kr1/dob−1/d02 and a⟶b refers to the unit vector of the static obstacle pointing to the controlled vehicle.(2) Dynamic Obstacle Restraint. The dynamic obstacle function in the driving environment is expressed as the following formula:(12)Urmx,y=12ηsVe−Vbcosθ−β2ρx,ya⟶b,θ−βε−π2,π2,0,others.In the above formula,ηs is the speed potential field repulsion gain, Ve is the speed of the vehicle, Vb is the speed of the dynamic obstacle, θ is the angle between the direction of the controlled vehicle and the positive direction of the x-axis, and β is the positive direction of the dynamic obstacle and the x-axis. The direction is the angle; a⟶b is the unit vector of the dynamic obstacle pointing to the controlled vehicle.Whenθ−β∉−π/2,π/2, the dynamic obstacle loses its repulsive force to the controlled vehicle, so the repulsive force is 0 at this time.Derive formula (12) to obtain dynamic obstacle repulsion, as shown in the following formula:(13)Frm=ηsVe−Vbcosθ−βρx,ya⟶b,θ−βε−π2,π2,0,others.In the sampling area which is less than 0.1L, where L is the length of the car, the search mode of random sampling is switched. One reason is to ensure the vehicle can approach the target point quickly and the other reason is that it can effectively face the path generation problem in narrow environments.


### 3.2.2. The Constraints

When the original RRT extends nodes, it is necessary to judge whether all extended nodes are obstacle positions, which reduces the computational efficiency of the algorithm to a certain extent. Therefore, firstly, the surrounding obstacles with a radius of 0.5 L are detected. If there are no obstacles within this range, the extended node directly skips the obstacle detection, or the obstacle detection is carried out, so as to improve the implementation efficiency of the algorithm.(1) The Constraints of the Vehicle. The vehicle’s executing actuator constraints should be met in the path planning algorithm so that the vehicle can drive safely in the complicate environment. The executing actuator of the vehicle includes the vehicle’s front wheel steering angle δ, the maximum driving speed vmax, and the maximum acceleration amax. These factors should be considered to do the nodes’ extension within certain limits. Therefore, in this paper, the comprehensive factor dc is used to measure the distance between the sampling point s and the parent node ti = [xi, yi], which is expressed as(14)dc=dh+ded,where, dh = 1 − h1h2/|h1||h2|, h1=[xs − xi, ys − yi], h2 = [xi − xj, yi − yj], (xs, ys) is the location of the current sampling node, (xi, yi) is the location of the current parent node, ded=xs−xi2+ys−yi2, dh is the heading change of the branch to be extended relative to the current branch, h1h2/|h1||h2| is the direction similarity, h1 is the direction vector to be extended, h2 is the brunch’s direction vector to be extended, and ded is the Euclidean distance between the node and the sampling point.Furthermore, in the cost function calculation of the traditional RRT, the size of the path length is considered usually. The energy consumption and the time consumption are also considered in this paper; the cost functionccost (ni, ne) of the extended brunch is defined as(15)Ccostni,ne=dh+ded+Nf+t,where Nf = (mae + mgμr + 0.5cdAρv2)vts, ae ≥ 0, Nf = (rbmae + mgμr + 0.5cdAρv2)vts, ae < 0, ne is the extended node, dh is the heading change of the branch to be extended relative to the current branch, ded is the Euclidean distance between the node and the sampling point, Nf is the energy consumption of the extended brunch, t is the time consumption of the extended brunch, rb is the brake feedback ratio, m is the vehicle mass, μr is the rolling resistance coefficient, cd is the wind resistance coefficient, A is the windward area, and ρ is the air density.Based on the improved method, the comprehensive distance value of peripheral nodes is calculated and the cost function is taken into consideration comprehensively. Meanwhile, it is used for subsequent node extension. The selection of candidate nodes makes the extended branch meet the vehicle motion constraints, thus reducing the amount of the subsequent optimization calculation.(2) The Constraints of the Vehicle Driving Environment. In order to make the planned path feasible, vehicles need to meet the constraints of road environment when they drive on the road. The left boundary and right boundary of the road are represented by Bl and Br, respectively, so the extended nodes need to meet the requirements of the formula:(16)B1≤ney≤Br,(17)Ts≤nex≤Tg.To sum up, firstly, artificial potential field is used to expand the bidirectional RRT∗ around the target direction. Then, in the specific expansion, the constraints of the vehicle itself and the driving environment are considered so that the planned path meets the actual driving demand of the vehicle.


## 3.2.1. Random Sampling Improvement Method Based on the Artificial Potential Field

According to the wide search space, the random mode is used to do the node extension when the original RRT and RRT∗ algorithms are sampled. Although the probability integrity can be guaranteed, the search efficacy of the algorithm is reduced to some extent. So, the applicability of the original RRT and RRT ∗ algorithms are challenged for the smart vehicle which has high real-time requirement. According to the narrow search space, the deviation sample reduces the probability of obtaining the feasible path as quickly as possible. So, the RRT ∗ algorithm should be improved to improve both the probability integrity and the search efficacy. The goal attraction thought of the artificial potential field is used to determine the search direction. The artificial potential field method path planning is a virtual force method proposed by Khatib. Its basic idea is to design the movement of the vehicle in the surrounding environment into an abstract artificial gravitational field. The target point produces “gravitation” to the vehicle, and the obstacle produces “repulsion” to the vehicle. Finally, the movement of vehicles is controlled by seeking the combined force. When the search range is larger than the length of the vehicle and the vehicle constraints are satisfied, a fast search method biased towards the target area is adopted to improve the efficiency of the search path and the safety performance of the vehicle. Using the idea of artificial potential field, according to the gravity function of the target position, sampling is conducted in the target direction; the formulation is defined as(8)F=Fatt+Frep.Among them,Fatt is the target gravity received by the intelligent vehicle, and Frep is the comprehensive obstacle repulsion received by the target vehicle.The target gravityFatt is specifically expressed as(9)FattM=−∇UattM=Ka⟶Gρg,where Uatt(M) = 1/2K(M − Mg)2, M = [x, y]T, Mg = [xg, yg]T, and Fatt(M) is the gravity function; it can be obtained by differentiating the negative gradient of the gravitational field function; K is the gravitational field gain, a⟶G is the forward target, ρg is the distance between the controlled vehicle and the target position, M is the current position of the vehicle, and Mg is the target position [26].Obstacle repulsion forces are based on static obstacles and dynamic obstacles to establish repulsion potential field functions and finally form a comprehensive obstacle repulsion forceFrep.(1) Static Obstacle Restraint. The static obstacle function in the driving environment is expressed as the following formula:(10)UrjM=12Kr1dob−1d02,dob≤d0,0,dob>d0.In the above formula,Kr is the static gain coefficient of the repulsive force field, dob is the distance between the controlled object and the obstacle, and d0 is the boundary distance of the repulsive force set according to the vehicle speed.Derive the negative gradient of the repulsion field function to obtain the static repulsion functionFrjM, which is the following formula:(11)FrjM=Fr1+Fr22a⟶b,dob≤d0,0,dob>d0,Among them,Fr1=Kr1/dob−1/d01/dob2Fr2=n/2Kr1/dob−1/d02 and a⟶b refers to the unit vector of the static obstacle pointing to the controlled vehicle.(2) Dynamic Obstacle Restraint. The dynamic obstacle function in the driving environment is expressed as the following formula:(12)Urmx,y=12ηsVe−Vbcosθ−β2ρx,ya⟶b,θ−βε−π2,π2,0,others.In the above formula,ηs is the speed potential field repulsion gain, Ve is the speed of the vehicle, Vb is the speed of the dynamic obstacle, θ is the angle between the direction of the controlled vehicle and the positive direction of the x-axis, and β is the positive direction of the dynamic obstacle and the x-axis. The direction is the angle; a⟶b is the unit vector of the dynamic obstacle pointing to the controlled vehicle.Whenθ−β∉−π/2,π/2, the dynamic obstacle loses its repulsive force to the controlled vehicle, so the repulsive force is 0 at this time.Derive formula (12) to obtain dynamic obstacle repulsion, as shown in the following formula:(13)Frm=ηsVe−Vbcosθ−βρx,ya⟶b,θ−βε−π2,π2,0,others.In the sampling area which is less than 0.1L, where L is the length of the car, the search mode of random sampling is switched. One reason is to ensure the vehicle can approach the target point quickly and the other reason is that it can effectively face the path generation problem in narrow environments.


## 3.2.2. The Constraints

When the original RRT extends nodes, it is necessary to judge whether all extended nodes are obstacle positions, which reduces the computational efficiency of the algorithm to a certain extent. Therefore, firstly, the surrounding obstacles with a radius of 0.5 L are detected. If there are no obstacles within this range, the extended node directly skips the obstacle detection, or the obstacle detection is carried out, so as to improve the implementation efficiency of the algorithm.(1) The Constraints of the Vehicle. The vehicle’s executing actuator constraints should be met in the path planning algorithm so that the vehicle can drive safely in the complicate environment. The executing actuator of the vehicle includes the vehicle’s front wheel steering angle δ, the maximum driving speed vmax, and the maximum acceleration amax. These factors should be considered to do the nodes’ extension within certain limits. Therefore, in this paper, the comprehensive factor dc is used to measure the distance between the sampling point s and the parent node ti = [xi, yi], which is expressed as(14)dc=dh+ded,where, dh = 1 − h1h2/|h1||h2|, h1=[xs − xi, ys − yi], h2 = [xi − xj, yi − yj], (xs, ys) is the location of the current sampling node, (xi, yi) is the location of the current parent node, ded=xs−xi2+ys−yi2, dh is the heading change of the branch to be extended relative to the current branch, h1h2/|h1||h2| is the direction similarity, h1 is the direction vector to be extended, h2 is the brunch’s direction vector to be extended, and ded is the Euclidean distance between the node and the sampling point.Furthermore, in the cost function calculation of the traditional RRT, the size of the path length is considered usually. The energy consumption and the time consumption are also considered in this paper; the cost functionccost (ni, ne) of the extended brunch is defined as(15)Ccostni,ne=dh+ded+Nf+t,where Nf = (mae + mgμr + 0.5cdAρv2)vts, ae ≥ 0, Nf = (rbmae + mgμr + 0.5cdAρv2)vts, ae < 0, ne is the extended node, dh is the heading change of the branch to be extended relative to the current branch, ded is the Euclidean distance between the node and the sampling point, Nf is the energy consumption of the extended brunch, t is the time consumption of the extended brunch, rb is the brake feedback ratio, m is the vehicle mass, μr is the rolling resistance coefficient, cd is the wind resistance coefficient, A is the windward area, and ρ is the air density.Based on the improved method, the comprehensive distance value of peripheral nodes is calculated and the cost function is taken into consideration comprehensively. Meanwhile, it is used for subsequent node extension. The selection of candidate nodes makes the extended branch meet the vehicle motion constraints, thus reducing the amount of the subsequent optimization calculation.(2) The Constraints of the Vehicle Driving Environment. In order to make the planned path feasible, vehicles need to meet the constraints of road environment when they drive on the road. The left boundary and right boundary of the road are represented by Bl and Br, respectively, so the extended nodes need to meet the requirements of the formula:(16)B1≤ney≤Br,(17)Ts≤nex≤Tg.To sum up, firstly, artificial potential field is used to expand the bidirectional RRT∗ around the target direction. Then, in the specific expansion, the constraints of the vehicle itself and the driving environment are considered so that the planned path meets the actual driving demand of the vehicle.


## 3.3. Collision Detection Based on SAT (Separating Axis Theorem)

The main contents of SAT (separating axis theorem) are that when two objects need to be detected for collision, the two objects are supposed to be two convex polygons and the convex polygons are projected onto a vector separation axis at the same time firstly. Then, whether there is a gap between the two projections should be judged. If there is a gap, there is no collision; if there is no gap, the collision will happen. If the obstacle in the driving environment is a concave shape, connect the vertices of the two sides of the concave shape whose internal angle is greater than 180° to convert the concave shape into a convex shape. The specific conversion method is shown in Figure3. If the concave-sided shape has multiple internal angles greater than 180°, the corresponding multiple conversions are performed until it becomes a convex-sided shape.Figure 3
Concave polygon conversion diagram.
(a)(b)In order to make the planned path meet the actual driving environment of the vehicle, the actual geometry of the vehicle itself and obstacles need to be considered in the path planning method. In this paper, as to ensure the driving safety of the vehicle, the geometry of the vehicle is supposed as a circle, with the vehicle’s center of mass as the center and the length between the center and the front axle as the radius. At the same time, the geometry of the obstacles is assumed as rectangles and the separation axis theorem is used to transform the collision detection problem between vehicles and obstacles into the intersection test problem of different graphs.The schematic diagram of the SAT is shown in Figure4. Both graphs are in the Cartesian coordinate system XOY. The circle A and rectangle B represent the vehicle and obstacle, respectively. The unit vectors of the circle A in the local coordinates with Ao as the origin point are Ax and Ay; the unit vectors of the rectangle B in the local coordinates with Bo as the origin point are Bx and By; the diameter of the circle A is Da; the length and width of the rectangle B are Db and Dw; N is the distance between the center point of the circle A and the center point of rectangle B; β is the vehicle heading angle in global coordinates; α is the angle between the center line of the two graphs and the x-axis of the global coordinate system.Figure 4
The schematic diagram of the SAT.There are four separation axes, including thex axe and y axe of the circle A and the rectangle B, respectively. In order to detect the collision of the two graphics, whether the projection of two figures on all the separation axes simultaneously satisfies the following conditions should be judged; the conditions are as follows:Condition 1: the project axle is thex axle of the local coordinate system of the circle A:(18)Ncosα>Da2cosα+Da2sinβ.Condition 2: the project axle is they axle of the local coordinate system of the circle A:(19)Nsinα>Da2sinβ+Da2cosβ.Condition 3: the project axle is thex axle of the local coordinate system of the rectangle B:(20)Ncosα−β>Db2cosβ+Dw2sinβ.Condition 4: the project axle is they axle of the local coordinate system of the rectangle B:(21)Nsinα−β>Db2sinβ+Dw2cosβ.If all of the conditions are met simultaneously, then there is no collision between the vehicle and the obstacles and the path can be used, or if the collision will happen, then the path should be abandoned. The SAT is used to do the collision detection. The advantage of the SAT is the detection speed which is very quick and the detection results are accurate; this method can help find the appropriate path as quickly as possible.


## 3.4. The Path Optimization Method Based on the Cubic B-Spline Curve

Because the curvature of the path planned by the traditional bidirectional RRT∗ is not continuous, when the vehicle tracks the path, it must stop to change the driving direction such that the vehicle cannot drive smoothly. So, the planned trajectory should be optimized to make the vehicle drive smoothly and steadily [27]. The polynomial and the Bezier curve are used to optimize the path; however, different kinds of the constraints such as curvature, accelerated speed, and heading angle cannot be satisfied. Because the curvature is continuous and different kinds of the constraints are considered, the cubic B-spline curve is used to optimize the generated path.Suppose there aren + 1 control vertexes Pi(i = 0, 1, …, n), the cubic B-spline curve is expressed as(22)Cu=∑i=0nPiBi,ku,where the primary function Bi,k (i = 0, 1, …, n), the highest degree is k, the basis function is determined by the sequence of node u, and the value range of u is u0≤u1≤,⋯,≤un+k+1.The cubic B-spline curve is shown as(23)Cu=∑i=03PiBi,3u.where the basis function Bi, 3(u) is defined as(24)B0,3u=−u3+3u2−3u+13!,B1,3u=3u3−6u2+43!,B2,3u=−3u3+3u2+3u+13!,B3,3u=u33!.The B-spline fitting curve is expressed as(25)x=B0∗a1,i+B1∗a1,i+1+B2∗a1,i+2+B3∗a1,i+3,y=B0∗a2,i+B1∗a2,i+1+B2∗a2,i+2+B3∗a2,i+3,where a is the fixed coefficient.


## 4. Simulation Results and Analysis

### 4.1. The Simulation Test

The improved two-way RRT∗ method proposed in this paper is simulated and verified on a PC with processor Intel I7 based on MATLAB R2018a. The size of the simulation environment is equivalent to the actual driving area of 30 m ∗ 30 m. The vehicle starts from the lower left corner of the environment as the starting point, and the upper right corner is the end point of the vehicle. Set the vehicle length to 2 m, width to 1 m, wheelbase to 1.2 m, maximum front wheel turning angle to 30°, and minimum turning radius to 4 m. For the randomness of the algorithms, 100 computations are counted, including the sampling number, average path length, and average searching time.In order to verify the performance of the improved method, it was verified in multiple simulation scenarios. Based on the static obstacle environment in which concave-convex obstacles exist in Figure5(a) and the complex obstacle environment in which dynamic and static obstacles exist in Figure 5(b) and Figure 5(c), three simulation environments are established, respectively. There are concave-convex obstacles in all three simulated environments. The black area is the barrier area and the white area is the passable area. In Figure 5(b), the yellow triangle represents the dynamic obstacle in the environment, and the dotted line represents the movement process of the dynamic obstacle. In Figure 5(c), two yellow triangles represent dynamic obstacles, and the dotted line represents the movement process of dynamic obstacles. There are two dynamic obstacles in the driving environment, and a simulation scene is established. There are bumps and obstacles in these three simulated environments. The black area represents the obstacle area, and the white area represents the passable area. In Figure 5(b), the yellow triangle represents dynamic obstacles in the environment, and the dashed straight line represents the movement process of dynamic obstacles; in Figure 5(c), the two yellow triangles represent dynamic obstacles, and the dashed straight line represents the movement process of dynamic obstacles.Figure 5
Three kinds of the obstacle environments. (a) Environment a. (b) Environment b. (c) Environment c.
(a)(b)(c)In Figure6, the generated light purple path is the path formed by the original RRT, the light blue and light pink paths are the paths generated by the bidirectional RRT ∗, and the red path is the path generated by the improved bidirectional RRT ∗ article. It can be seen from Figure 6 that the path generated by RRT is relatively tortuous and relatively close to obstacles, which is not conducive to the safe driving of the vehicle. Two-way RRT ∗ is more optimized than the original RRT, but the effect is not very obvious. The improved two-way RRT ∗ method proposed in this paper takes into account the dynamic conditions of the vehicle, expands the target direction based on the idea of artificial potential field, and considers the influence factors of dynamic obstacle rejection. At the same time, it considers the separation axis theory to detect the vehicle and the collision safety between obstacles, and then the curve smoothing process is performed again according to the B-spline curve, thereby meeting the needs of vehicle driving. Tables 1–3 show the comparison results based on obstacle environments (a), (b), and (c), respectively. In order to further verify the superior performance of the improved method, the planning effect of the corresponding algorithm at a specific time is described in detail. Figure 7(a) shows a comparison effect diagram of the three methods at 0.4 s. The path generated by the improved method in this paper is represented by the red path. The red solid dots and red triangle in Figure 7(a) are the planning effect of the improved method in this paper at 0.4 s for the two-way RRT ∗. It can be seen that it successfully avoided the dynamic obstacles, the generated path is relatively smooth. Figure 7(b) shows the effect of the global path diagram generated by the three methods. It can be seen that the improved two-way RRT ∗ method successfully converges, as shown by the red five-pointed star; Figure 8 shows the verification effect in the simulation environment c. In Figure 8(a), the red route at time 0.35 s is the improved two-way RRT ∗ method in this article, and the red solid dots and red solid triangle are the planning effect of the improved method in this article when the two-way RRT ∗ is 0.35 s, and the generated path located between obstacles; there is still a certain safety distance, which effectively avoids the short generation time of dynamic obstacles; when it runs to 0.46 s, as shown in Figure 8(b), only the traditional RRT has not yet completed the planning. It proves the time advantage of the other two methods and it can be seen that the improved method proposed in this paper is better than the traditional RRT and two-way RRT ∗ methods. It can be seen from Tables 2–4 that the path generated by RRT is not smooth, and it takes the longest time and it is not easy for vehicles to follow the path. Compared with the RRT algorithm and the two-way RRT ∗ algorithm, the improved two-way RRT ∗ in this paper takes the shortest time to generate a path, and the path is the smoothest path. Obstacle environment (c) is a complex obstacle environment. It can be seen from Figure 8 and Table 3 that compared with the RRT algorithm and the two-way RRT ∗ algorithm, the path of the improved method in this paper takes the shortest time and the path is smooth. The simulation results show that the method can adapt to various obstacle environments including complex dynamic environments.Figure 6
The planned results of three algorithms based on the obstacle environment (a).Table 1
Comparison of environment simulation results based on the obstacle environment (a).
AlgorithmRRTBidirectional RRT∗Improved bidirectional RRT∗Random sampling points (piece)535045Average path length (m)26.19028.66023.071Average time (s)0.520.470.453Table 2
Comparison of environment simulation results based on the obstacle environment (b).
AlgorithmRRTBidirectional RRT∗Improved bidirectional RRT∗Random sampling points (piece)604441Average path length (m)24.19122.77321.157Average time (s)0.550.530.45Table 3
Comparison of environment simulation results based on the obstacle environment (c).
AlgorithmRRTBidirectional RRT∗Improved bidirectional RRT∗Random sampling points (piece)655746Average path length (m)27.24525.53522.412Average time (s)0.6150.5130.463Figure 7
The planned results of three algorithms based on the obstacle environment (b). (a) Local path att = 0.4 s. (b) Global build path.
(a)(b)Figure 8
The planned results of three algorithms based on the obstacle environment (c). (a) Local path att = 0.35 s. (b) Local path at t = 0.46 s. (c) Global build path.
(a)(b)(c)


### 4.2. The Real Vehicle Test

In this section, we use the real vehicle test method to verify the method employed in this paper. In order to verify the advantages of the method presented in this paper, the vehicle path planning in real obstacle environment was carried out with the method proposed in literature [28]. The smart electric vehicle is used to verify the proposed algorithm. The smart electric vehicle platform is shown in Figure 9. The vehicle has 4-wheel hub motors; the sensors include ultrasonic radar, laser radar, CCD, GPS, and minicomputer. The smart vehicle has the function of high-precision positioning, SLAM mapping, and automatic navigation tracking.Figure 9
The smart electric vehicle platform.Before the test begins, the environment map is built through SLAM (simultaneous localization and mapping). The actual driving environment of the vehicle is shown in Figure10. Figures 10(a)–10(d) show the motion states of the vehicle and the dynamic obstacle at different times. Figure 10(a) shows the original driving position and vehicle position; Figure 10(b) shows that when the vehicle moves with two obstacles at the same time and the first dynamic obstacle is detected, the vehicle makes a right turn to avoid obstacles; Figure 10(c) shows that the vehicle continues to move forward, detects the second dynamic obstacle, and avoids it safely; at this time, the first dynamic obstacle has stopped moving; Figure 10(d) shows that the vehicle successfully avoided the second obstacle and finally reached the designated driving position. The uniform velocity of dynamic obstacles is 1 m/s.Figure 10
The real obstacle environment.
(a)(b)(c)(d)The SLAM obstacle composition and the movement direction of the dynamic obstacle are shown in Figure11. The green triangle represents the first obstacle moving horizontally to the right, and the red triangle represents the second obstacle moving to the lower left corner; the black rectangle is the static obstacle in the running scene, and the white is the free driving area.Figure 11
The results of the SLAM.After the SLAM construction is completed, the starting and target points of the smart vehicle are set to observe the effect of the two kinds of the path planning methods: comparing the improved method in this paper with the method proposed in the document [28]. The average driving speed is 1.5 m/s and the safety distance is 0.5 m. The planning results of the two planning methods are shown in Figure 12. The data pairs are shown in Table 4. In Figure 12, two kinds of the lines represent different planning results of the real vehicle using two different methods. We can see from Figure 12 and Table 4 that two methods can plan the obstacle avoidance trajectory to make the vehicle reach the target point. However, the trajectory length planned by this paper’s improved RRT ∗ algorithm is shorter than the other algorithms, and the curvature of the trajectory planned by the improved RRT ∗ algorithm is the smoothest for the actual tracking of the vehicle, which improves the driving stability and safety of the vehicle.Figure 12
The planning results of the two planning methods.Table 4
Comparison of two methods.
AlgorithmMethod of the literature [28]Method of this articleRandom sampling points (piece)5647Average path length (m)14.5312.73Average time (s)9.718.47We can see from the test results that in the complex obstacle environment, the algorithm proposed in this paper can be applied in practice on the smart electric vehicle platform and can be used to plan the noncollision path of the vehicle. The length of the planned trajectory and the smoothness degree planned by this paper’s method are superior to the others’ algorithm.


## 4.1. The Simulation Test

The improved two-way RRT∗ method proposed in this paper is simulated and verified on a PC with processor Intel I7 based on MATLAB R2018a. The size of the simulation environment is equivalent to the actual driving area of 30 m ∗ 30 m. The vehicle starts from the lower left corner of the environment as the starting point, and the upper right corner is the end point of the vehicle. Set the vehicle length to 2 m, width to 1 m, wheelbase to 1.2 m, maximum front wheel turning angle to 30°, and minimum turning radius to 4 m. For the randomness of the algorithms, 100 computations are counted, including the sampling number, average path length, and average searching time.In order to verify the performance of the improved method, it was verified in multiple simulation scenarios. Based on the static obstacle environment in which concave-convex obstacles exist in Figure5(a) and the complex obstacle environment in which dynamic and static obstacles exist in Figure 5(b) and Figure 5(c), three simulation environments are established, respectively. There are concave-convex obstacles in all three simulated environments. The black area is the barrier area and the white area is the passable area. In Figure 5(b), the yellow triangle represents the dynamic obstacle in the environment, and the dotted line represents the movement process of the dynamic obstacle. In Figure 5(c), two yellow triangles represent dynamic obstacles, and the dotted line represents the movement process of dynamic obstacles. There are two dynamic obstacles in the driving environment, and a simulation scene is established. There are bumps and obstacles in these three simulated environments. The black area represents the obstacle area, and the white area represents the passable area. In Figure 5(b), the yellow triangle represents dynamic obstacles in the environment, and the dashed straight line represents the movement process of dynamic obstacles; in Figure 5(c), the two yellow triangles represent dynamic obstacles, and the dashed straight line represents the movement process of dynamic obstacles.Figure 5
Three kinds of the obstacle environments. (a) Environment a. (b) Environment b. (c) Environment c.
(a)(b)(c)In Figure6, the generated light purple path is the path formed by the original RRT, the light blue and light pink paths are the paths generated by the bidirectional RRT ∗, and the red path is the path generated by the improved bidirectional RRT ∗ article. It can be seen from Figure 6 that the path generated by RRT is relatively tortuous and relatively close to obstacles, which is not conducive to the safe driving of the vehicle. Two-way RRT ∗ is more optimized than the original RRT, but the effect is not very obvious. The improved two-way RRT ∗ method proposed in this paper takes into account the dynamic conditions of the vehicle, expands the target direction based on the idea of artificial potential field, and considers the influence factors of dynamic obstacle rejection. At the same time, it considers the separation axis theory to detect the vehicle and the collision safety between obstacles, and then the curve smoothing process is performed again according to the B-spline curve, thereby meeting the needs of vehicle driving. Tables 1–3 show the comparison results based on obstacle environments (a), (b), and (c), respectively. In order to further verify the superior performance of the improved method, the planning effect of the corresponding algorithm at a specific time is described in detail. Figure 7(a) shows a comparison effect diagram of the three methods at 0.4 s. The path generated by the improved method in this paper is represented by the red path. The red solid dots and red triangle in Figure 7(a) are the planning effect of the improved method in this paper at 0.4 s for the two-way RRT ∗. It can be seen that it successfully avoided the dynamic obstacles, the generated path is relatively smooth. Figure 7(b) shows the effect of the global path diagram generated by the three methods. It can be seen that the improved two-way RRT ∗ method successfully converges, as shown by the red five-pointed star; Figure 8 shows the verification effect in the simulation environment c. In Figure 8(a), the red route at time 0.35 s is the improved two-way RRT ∗ method in this article, and the red solid dots and red solid triangle are the planning effect of the improved method in this article when the two-way RRT ∗ is 0.35 s, and the generated path located between obstacles; there is still a certain safety distance, which effectively avoids the short generation time of dynamic obstacles; when it runs to 0.46 s, as shown in Figure 8(b), only the traditional RRT has not yet completed the planning. It proves the time advantage of the other two methods and it can be seen that the improved method proposed in this paper is better than the traditional RRT and two-way RRT ∗ methods. It can be seen from Tables 2–4 that the path generated by RRT is not smooth, and it takes the longest time and it is not easy for vehicles to follow the path. Compared with the RRT algorithm and the two-way RRT ∗ algorithm, the improved two-way RRT ∗ in this paper takes the shortest time to generate a path, and the path is the smoothest path. Obstacle environment (c) is a complex obstacle environment. It can be seen from Figure 8 and Table 3 that compared with the RRT algorithm and the two-way RRT ∗ algorithm, the path of the improved method in this paper takes the shortest time and the path is smooth. The simulation results show that the method can adapt to various obstacle environments including complex dynamic environments.Figure 6
The planned results of three algorithms based on the obstacle environment (a).Table 1
Comparison of environment simulation results based on the obstacle environment (a).
AlgorithmRRTBidirectional RRT∗Improved bidirectional RRT∗Random sampling points (piece)535045Average path length (m)26.19028.66023.071Average time (s)0.520.470.453Table 2
Comparison of environment simulation results based on the obstacle environment (b).
AlgorithmRRTBidirectional RRT∗Improved bidirectional RRT∗Random sampling points (piece)604441Average path length (m)24.19122.77321.157Average time (s)0.550.530.45Table 3
Comparison of environment simulation results based on the obstacle environment (c).
AlgorithmRRTBidirectional RRT∗Improved bidirectional RRT∗Random sampling points (piece)655746Average path length (m)27.24525.53522.412Average time (s)0.6150.5130.463Figure 7
The planned results of three algorithms based on the obstacle environment (b). (a) Local path att = 0.4 s. (b) Global build path.
(a)(b)Figure 8
The planned results of three algorithms based on the obstacle environment (c). (a) Local path att = 0.35 s. (b) Local path at t = 0.46 s. (c) Global build path.
(a)(b)(c)


## 4.2. The Real Vehicle Test

In this section, we use the real vehicle test method to verify the method employed in this paper. In order to verify the advantages of the method presented in this paper, the vehicle path planning in real obstacle environment was carried out with the method proposed in literature [28]. The smart electric vehicle is used to verify the proposed algorithm. The smart electric vehicle platform is shown in Figure 9. The vehicle has 4-wheel hub motors; the sensors include ultrasonic radar, laser radar, CCD, GPS, and minicomputer. The smart vehicle has the function of high-precision positioning, SLAM mapping, and automatic navigation tracking.Figure 9
The smart electric vehicle platform.Before the test begins, the environment map is built through SLAM (simultaneous localization and mapping). The actual driving environment of the vehicle is shown in Figure10. Figures 10(a)–10(d) show the motion states of the vehicle and the dynamic obstacle at different times. Figure 10(a) shows the original driving position and vehicle position; Figure 10(b) shows that when the vehicle moves with two obstacles at the same time and the first dynamic obstacle is detected, the vehicle makes a right turn to avoid obstacles; Figure 10(c) shows that the vehicle continues to move forward, detects the second dynamic obstacle, and avoids it safely; at this time, the first dynamic obstacle has stopped moving; Figure 10(d) shows that the vehicle successfully avoided the second obstacle and finally reached the designated driving position. The uniform velocity of dynamic obstacles is 1 m/s.Figure 10
The real obstacle environment.
(a)(b)(c)(d)The SLAM obstacle composition and the movement direction of the dynamic obstacle are shown in Figure11. The green triangle represents the first obstacle moving horizontally to the right, and the red triangle represents the second obstacle moving to the lower left corner; the black rectangle is the static obstacle in the running scene, and the white is the free driving area.Figure 11
The results of the SLAM.After the SLAM construction is completed, the starting and target points of the smart vehicle are set to observe the effect of the two kinds of the path planning methods: comparing the improved method in this paper with the method proposed in the document [28]. The average driving speed is 1.5 m/s and the safety distance is 0.5 m. The planning results of the two planning methods are shown in Figure 12. The data pairs are shown in Table 4. In Figure 12, two kinds of the lines represent different planning results of the real vehicle using two different methods. We can see from Figure 12 and Table 4 that two methods can plan the obstacle avoidance trajectory to make the vehicle reach the target point. However, the trajectory length planned by this paper’s improved RRT ∗ algorithm is shorter than the other algorithms, and the curvature of the trajectory planned by the improved RRT ∗ algorithm is the smoothest for the actual tracking of the vehicle, which improves the driving stability and safety of the vehicle.Figure 12
The planning results of the two planning methods.Table 4
Comparison of two methods.
AlgorithmMethod of the literature [28]Method of this articleRandom sampling points (piece)5647Average path length (m)14.5312.73Average time (s)9.718.47We can see from the test results that in the complex obstacle environment, the algorithm proposed in this paper can be applied in practice on the smart electric vehicle platform and can be used to plan the noncollision path of the vehicle. The length of the planned trajectory and the smoothness degree planned by this paper’s method are superior to the others’ algorithm.


## 5. Conclusions

In this paper, an improved bidirectional RRT∗ path planning method has been proposed and tested. This improved bidirectional RRT ∗ method can generate the planning path with high efficiency and accuracy; the vehicle can track the path smoothly and safely. The contents are summarized as follows:(1)
The proposed path planning algorithm for the smart vehicle applied the artificial potential field to establish a mechanism of extension, which makes the random tree’s extension have more intentionality. Based on the speed factor, the dynamic obstacle repulsion is constructed, and the collision detection is determined according to the distance between the obstacle and the node, thereby improving the search efficiency of the method.(2)
The vehicle’s own constraints and driving environment constraints are considered together to make the vehicle avoid obstacles accurately. First, the concave polygon obstacles are converted to convex polygon obstacles, and then collision detection is performed based on SAT to make the generated path more reasonable and the car can drive safely. The cubic B-spline curve is used to optimize the generated path to make the curvature of the path continuous. Thus, the vehicle can track the path smoothly.(3)
The simulation and experiment were implemented and both validated the proposed improved bidirectional RRT∗ algorithm. In the simulation analysis, this paper’s method can generate the smoothest path and takes the shortest time compared with the other two methods and it can adapt to different kinds of obstacle environments including the complicated environment. In the real vehicle experiment, we can see from the test results that in the complex dynamic obstacle environment, compared with others’ algorithm, this paper’s method can generate the shortest and smoothest path. The improved bidirectional RRT ∗ method can be applied in practice on the smart electric vehicle platform and it has a practical value in engineering application.



---

*Source: 6669728-2021-05-27.xml*