Local Path Planning of Autonomous Vehicle Based on an Improved Heuristic Bi-RRT Algorithm in Dynamic Obstacle Avoidance Environment

The existing variants of the rapidly exploring random tree (RRT) cannot be effectively applied in local path planning of the autonomous vehicle and solve the coherence problem of paths between the front and back frames. Thus, an improved heuristic Bi-RRT algorithm is proposed, which is suitable for obstacle avoidance of the vehicle in an unknown dynamic environment. The vehicle constraint considering the driver’s driving habit and the obstacle-free direct connection mode of two random trees are introduced. Multi-sampling biased towards the target state reduces invalid searches, and parent node selection with the comprehensive measurement index accelerates the algorithm’s execution while making the initial path gentle. The adaptive greedy step size, introducing the target direction, expands the node more effectively. Moreover, path reorganization minimizes redundant path points and makes the path’s curvature continuous, and path coherence makes paths between the frames connect smoothly. Simulation analysis clarifies the efficient performance of the proposed algorithm, which can generate the smoothest path within the shortest time compared with the other four algorithms. Furthermore, the experiments on dynamic environments further show that the proposed algorithm can generate a differentiable coherence path, ensuring the ride comfort and stability of the vehicle.


Introduction
The intelligent transportation system is a real-time, accurate, efficient, and comprehensive transportation management system that plays a role in various directions [1]. It can effectively improve road capacity, reduce traffic accidents, improve transportation efficiency, and alleviate traffic congestion [2,3]. Meanwhile, it can also reduce energy consumption and improve environmental pollution [4,5]. Therefore, it has become the future development direction of the transportation system and has attracted more and more attention from all countries. As a component, the autonomous vehicle plays an essential role in the intelligent transportation system. It consists of an environmental perception layer, a path planning layer, and a path tracking control layer, and the study of path planning has always been a core problem. Commonly, path planning refers to efficiently finding a collision-free and feasible path from a starting point to a target point in a workspace [6][7][8]. In practical usage, the quality of the planned path will directly affect the vehicle's driving performance, so how to plan a passable path that can be tracked is very important for autonomous vehicles.
Scholars have carried out much research on path planning, and new path-planning algorithms are constantly emerging and developing. In the previous studies, five common categories of path planning algorithms can be found: geometric algorithms [9,10], graph search algorithms [11,12], intelligent bionic algorithms [13,14], the artificial potential field algorithm [15], and sampling-based search algorithms. Sampling-based search algorithms, 1.
The multiple-sampling states plus a guided method biased towards the target point are designed to reduce the blind growth of the random trees, and the node extension mechanism integrating the greedy algorithm, namely the adaptive greedy step size considering the target direction, can effectively accelerate the growth of two random trees.

2.
The nearest node selection mechanism considering the kinematic constraint of the vehicle and the target state is put forward to reduce the effect of random sampling on path smoothness and speed up the growth of the random trees. 3.
An amplifying vehicle constraint considering the driver's driving habit is introduced to make the vehicle move more safely, and the obstacle-free direct connection mode of two trees is introduced to further accelerate the execution of the algorithm.

4.
A path reorganization process is designed to optimize the initial path to decrease the length of the final planned path to the maximum extent while ensuring path smoothness.

5.
A novel path coherence method considering the inter-frame correlation of paths is used to ensure the curvature continuity of the path and make the vehicle be controlled easily and move more steadily.
The article is organized as follows: Section 2 discusses the Bi-RRT path planning algorithm, the simplified vehicle model, and the differences in path planning between front and back frames. The improved heuristic Bi-RRT algorithm is presented in Section 3. Section 4 presents simulation experiments to demonstrate the effectiveness and practicability of the proposed algorithm. Section 5 presents the discussion about its performance, and conclusions are provided thereafter.

Problem Statements
The basic bi-RRT algorithm is briefly described in this section, and its shortcomings are pointed out. A brief introduction of the vehicle model points out that the turning radius constraint of the vehicle needs to be considered in the path planning process. Furthermore, the influence of the difference between the path planning results of the front and rear frames in the dynamic path planning process on vehicle driving is described.

Basic Bi-RRT
The Bi-RRT is a variant of the basic RRT, which changes the expansion mode of the algorithm. That is, two random trees are constructed from the initial state and the target state, respectively. In each cycle, a random tree is first expanded to generate a new tree node, and then another random tree also starts to generate a new tree node, making two random trees expand towards each other. The two random trees expand alternately until the nodes of the two random trees meet. The searching schematic diagram of the basic Bi-RRT is shown in Figure 1.

Problem Statements
The basic bi-RRT algorithm is briefly described in this section, and its shortcomings are pointed out. A brief introduction of the vehicle model points out that the turning radius constraint of the vehicle needs to be considered in the path planning process. Furthermore, the influence of the difference between the path planning results of the front and rear frames in the dynamic path planning process on vehicle driving is described.

Basic Bi-RRT
The Bi-RRT is a variant of the basic RRT, which changes the expansion mode of the algorithm. That is, two random trees are constructed from the initial state and the target state, respectively. In each cycle, a random tree is first expanded to generate a new tree node, and then another random tree also starts to generate a new tree node, making two random trees expand towards each other. The two random trees expand alternately until the nodes of the two random trees meet. The searching schematic diagram of the basic Bi-RRT is shown in Figure 1. and nearest P is less than threshold l (Lines 9-11). Otherwise, a T and b T are swapped, and the procedures mentioned above are executed on the random tree b T again (Line 12).
Additionally, then, a path is generated by the function _ ( ) Get Path T (Line 16).  Algorithm 1 shows the basic Bi-RRT algorithm. Once initialized, the basic Bi-RRT algorithm conducts its iterative circle by selecting a random point P rand from the configuration space using the sampling function Random_State ( ) (Line 3). The algorithm then determines a near tree node P near by the function Nearest_Neighbor ( ) and obtains a new tree node P new by the function Extend ( ) (Lines [4][5]. If there are no obstacles between P near and P new , the new tree node P new is added to the random tree T a , and the nearest tree node P nearest from the random tree T b is found by the function Nearest_Neighbor ( ) (Lines [6][7][8]. The iterative circle terminates if the distance between P new and P nearest is less than l threshold (Lines 9-11). Otherwise, T a and T b are swapped, and the procedures mentioned above are executed on the random tree T b again (Line 12). Additionally, then, a path is generated by the function Get_Path T ( ) (Line 16). Algorithm 1: Build Bi − RRT (P init ,P goal ) 1: T a (P init ); T b (P goal ); 2: while 1 do 3: P rand ← Random_State ( ) ; 4: P near ← Nearest_Neighbor (P rand , T a ) ; 5: P new ← Extend (P near , P rand ) ; 6: if Collision_Free (P near , P new ) then 7: T a .Add (P new ), T a .Add (P near , P new ) 8: if Dis tan ce ( P new , P nearest ) < l threshold then 10: Return T (T a , T b ) 11: break 12: else Swap ( T a , T b ) 13: end if 14: end if 15: end while 16: Path ← Get_Path T ( T a , T b ) ; Algorithm 2 outlines the implementation procedure of the function Get_Path T ( ). Once the basic Bi-RRT algorithm completes the construction of two random trees, T a and T b , two path point sets, path_a and path_b, are defined, and the last added tree nodes of the two random trees are put into two sets, path_a and path_b, respectively (Lines 1-3). Then, the two random trees are searched reversely according to indexes of parent nodes until their initial points are put into the path point sets, path_a and path_b, respectively (Lines 4-17). Finally, the path point set path_a is reversed and then combined with the path point set path_b to obtain a final path point set path (Lines [18][19]. 1: Var path_a, path_b; 2: path_a.Add_Node (T a .node n ); 3: path_b.Add_Node (T b .node n ); 4: while 1 do 5: i ← Index pre_node (T a ) ; 6: path_a.Back_Add_Node (T a .node i ); 7: if i = 1 then 8: break 9: end if 10: end while 11. while 1 do 12: j ← Index pre_node (T b ) ; 13: path_b.Back_Add_Node (T b .node j ); 14 The basic Bi-RRT algorithm simultaneously generates two random trees from the starting and target points and expands them in opposite directions, accelerating the convergence speed of the algorithm. However, the expansion mode of tree nodes still lacks directivity, the connective mode of two trees can be further improved, and the generated path is difficult to be directly tracked by the vehicle.

Vehicle Kinematical Model
Since the Bi-RRT is an incremental path planning algorithm, the kinematic vehicle model can be used to limit the expansion process of tree nodes to ensure the feasibility of the path. That is, the nonholonomic constraint of the vehicle should be considered when increasing new tree nodes. Because the sophisticated kinematic model is seldom available, a simplified theoretical motion model is provided, as shown in Figure 2. Assuming that the vehicle does not slip laterally, and the rear wheels do not steer, the vehicle kinematic model is expressed by Equation (1). Furthermore, the steering radius of the vehicle can be expressed by Equation (2).
where (x, y) represents the coordinate of the vehicle gravity center in the coordinate reference frame, v is the longitudinal speed of the vehicle, ϕ is the included angle between the vehicle main axis and the X axis, δ f is the steering angle of the front wheels and |δ f | ≤ δ f max , L is the wheelbase of the vehicle, k and R are the turning curvature and steering radius of the vehicle, respectively. In order to consider the feasibility of newly generated path segments, the minimum steering radius constraint should be taken into account during the node extension procedure of Bi-RRT.

Path Planning Difference between Previous and Subsequent Frames
Because the length and smoothness of the path are calculated based on the environmental information collected in a certain frame, only considering the length and smoothness of the path cannot guarantee the steady driving of the autonomous vehicle. Suppose the path planned in the current frame deviates too far from the previous one. In that case, the driving stability of the autonomous vehicle will decline and even collide with the obstacle vehicle. It can be seen from Figure 3 that the path of the previous frame of the autonomous vehicle is on the left side of the obstacle, whereas the path of the current frame is on the right side of the obstacle. Because of the inconsistency of the path of the previous and subsequent frames, the autonomous vehicle may not avoid the obstacle and has the risk of collision with the obstacle. The dotted arrow may represent the actual driving direction of the autonomous vehicle.
Consequently, to prevent the difference in the paths generated by the previous and current frames of two adjacent planning cycles from influencing vehicle driving stability, it is necessary to consider the path information of the previous frame when planning the path in the current frame. In order to consider the feasibility of newly generated path segments, the minimum steering radius constraint should be taken into account during the node extension procedure of Bi-RRT.

Path Planning Difference between Previous and Subsequent Frames
Because the length and smoothness of the path are calculated based on the environmental information collected in a certain frame, only considering the length and smoothness of the path cannot guarantee the steady driving of the autonomous vehicle. Suppose the path planned in the current frame deviates too far from the previous one. In that case, the driving stability of the autonomous vehicle will decline and even collide with the obstacle vehicle. It can be seen from Figure 3 that the path of the previous frame of the autonomous vehicle is on the left side of the obstacle, whereas the path of the current frame is on the right side of the obstacle. Because of the inconsistency of the path of the previous and subsequent frames, the autonomous vehicle may not avoid the obstacle and has the risk of collision with the obstacle. The dotted arrow may represent the actual driving direction of the autonomous vehicle.

Improved Heuristic Bi-RRT Algorithm
Based on the above analysis, this section proposes an improved heuristic Bi-RRT algorithm for path planning in a dynamic obstacle avoidance environment. Figure 4 illustrates the model structure of the improved heuristic Bi-RRT. The input of the proposed model is a local driving environment and positioning information. The proposed algo-  Consequently, to prevent the difference in the paths generated by the previous and current frames of two adjacent planning cycles from influencing vehicle driving stability, it is necessary to consider the path information of the previous frame when planning the path in the current frame.

Improved Heuristic Bi-RRT Algorithm
Based on the above analysis, this section proposes an improved heuristic Bi-RRT algorithm for path planning in a dynamic obstacle avoidance environment. Figure 4 illustrates the model structure of the improved heuristic Bi-RRT. The input of the proposed model is a local driving environment and positioning information. The proposed algorithm model based on heuristic random sampling, heuristic nearest neighbor, heuristic extension, collision detection, direct connection detection, and path organization quickly generates a differentiable and collision-free path. Algorithm 3 shows the specific steps of the improved heuristic Bi-RRT algorithm.    The improved heuristic Bi-RRT algorithm obtains an initial point by the function Current_Root ( ) and initializes two random trees T a and T b in the same manner as they are in the basic Bi-RRT (Lines 1-2). Once the two random trees cannot be directly interconnected, the improved heuristic Bi-RRT begins its iterative processing by picking a random point in the feasible domain space through a sampling function Heuristic_Random_Sampling ( ) (Lines 4-5). Then, the parent node P near from tree T a is found by the function Heuristic_ Nearest_Neighbor ( ), and the new tree node P new is generated by the function Heuristic_ Extend ( ) (Lines 6-7). If there are no obstacles between P near and P new , the new tree node P new is added to the random tree T a , and the nearest node P nearest from tree T b is found (Lines 8-11). Subsequently, the iterative processing ends if there are no obstacles between P nearest and P new , namely P judge2 and respectively (Lines 13-15). Otherwise, random trees T b and T b are swapped, and the procedures mentioned above are executed on the other random tree T b again (Line 16). Afterward, an initial path is generated by the function Get_Path T ( ) (Line 20). Path organization, including path node reconnection and path smoothing, processes the initial path to obtain a feasible path (Lines 21-22).

Algorithm 3:
Build Improved Heuristic Bi-RRT (P init ,P goal ) 1: P init ← Current_Root ( ) ; 2: T a (P init );T b (P goal ); 3: P judge1 ← P init ; P judge2 ← P goal ; 4: while Obstacle_Collision (P judge1 , P judge2 ) do 5: P rand ← Heuristic_Random_Sampling ( ) ; 6: P near ← Heuristic_Nearest_Neighbor(P rand , T a ) ; 7: P new ← Heuristic_Extend ( P near , P rand ) ; 8: if Collision_Free ( P near , P new ) then 9: T a .Add(P new ), T a .Add(P near , P new ) 10: P judge1 ← P new ; 11: P nearest ← Nearest_Neighbor (P new , T b ) ; 12: P judge2 ← P nearest ; 13: if Collision_Free (P judge1 , P judge2 ) then 14: Return T (T a , T b ) 15: The improved heuristic Bi-RRT algorithm contains a set of heuristic methods to the benefit of path planning of the autonomous vehicle. The improvements in the connective mode of two random trees, node sampling, node selection, and node expansion based on the Bi-RRT framework are used to generate an initial path quickly. Additionally, then, path reorganization, including path reconnection and path smoothing, is employed to improve the quality of the initial path, making it suitable for tracking by the autonomous vehicle. The improved constraints containing the improved road environment and the improved vehicle constraint are conducive to generating a feasible path complying with the driver's driving habit. In addition, a novel path coherence method is introduced to make the generated path smoothly connected when planning a dynamic obstacle avoidance path. Specific methods of the algorithm are described as follows in detail.

Connective Mode of Two Random Trees
In order to further accelerate the running speed of the algorithm, the connective mode of two random trees can change from how the distance between two random trees is less than a certain distance threshold to the way of obstacle-free direct connection, as shown in Figure 5. After expanding the random tree T a to obtain a new tree node P new−a , the nearest node P nearest on the random tree T b closest to the node P new−a is calculated, and whether the area between P new−a and P nearest is passable is checked. If there are no obstacles between P new−a and P nearest , P new−a and P nearest are directly connected, and the initial path planning is complete. Otherwise, the algorithm continues to execute until the two random trees are connected successfully. shown in Figure 5. After expanding the random tree a to obtain a new tree node new a − , the nearest node nearest P on the random tree b T closest to the node new a P − is calculated, and whether the area between new a P − and nearest P is passable is checked. If there are no obstacles between new a P − and nearest P , new a P − and nearest P are directly connected, and the initial path planning is complete. Otherwise, the algorithm continues to execute until the two random trees are connected successfully.

Pgoal Tb Ta
Pinit Pnear-a Pnew-a Pnearest Figure 5. Obstacle-free direct connection of two random trees.

Heuristic Target Bias Sampling Method
The basic Bi-RRT usually adopts random searches in the global scope during the random sampling process, which will cause the generation of random points without guidance and too much unnecessary computation. As to this problem, a heuristic bias sampling strategy composed of a multiple-sampling method and a target bias method is adopted to make the random tree grow in a biased direction, enabling the initial state and

Heuristic Target Bias Sampling Method
The basic Bi-RRT usually adopts random searches in the global scope during the random sampling process, which will cause the generation of random points without guidance and too much unnecessary computation. As to this problem, a heuristic bias sampling strategy composed of a multiple-sampling method and a target bias method is adopted to make the random tree grow in a biased direction, enabling the initial state and the goal state to meet more effectively and faster. Equations (3) and (4) demonstrate how to calculate the random sampling state P rand .
where P randm is the random sampling state generated by the multiple-sampling method, P target is the target point defined in each sampling process, and χ is the biased step size. d ob and d threshold are the distance from the random point P randm to the obstacle and the distance threshold from the obstacle, respectively.

Heuristic Random Sampling
With the knowledge of the initial and goal state, to make the sampling random point close to the target state, the Multiple_Random_State function generates several random points instead of one random point generated by the basic Bi-RRT algorithm in the free region using the Random_State function. The multiple-sampling point function does not include the probability of bias to the target, hence avoiding the local minimum problem. Additionally, then, the introduction of the Nearest_To_Target function is used to select the random state closer to the goal from several candidate sampling points. The random point out of these candidate sampling points closest to the target becomes the chosen random state P randm , causing the Bi-RRT to search toward the target point. The number of random states to be selected was set to two after simulation results showed the best performance when the number of random states was limited to two.

Heuristic Target Bias
After obtaining the chosen random state P randm , the target bias method is introduced. It can make the random point expand a step size χ along the direction from it to the target state to generate the random sampling point further closer to the target state, resulting in making the random tree grow more directionally and improving computational efficiency. When d ob is less than d threshold , if the generated random sampling point is still closer to the target state, it will make the newly generated tree node easy to collide with the obstacle, resulting in sampling failure and directly affecting the solution speed. Therefore, the target bias method introduces the distance threshold d threshold , which is the projection distance of the semi-major axis of the expanded safety ellipse on the x axis of the coordinate reference frame. When d ob ≤ d threshold , that is, the chosen random state P randm is close to the obstacle, P randm is regarded as the generated random sampling point P rand , when d ob > d threshold , that is, the chosen random state P randm is far from the obstacle, P rand is generated from the target bias function. This strategy can maintain the balance between exploration and search speed.
Based on the basic Bi-RRT sampling function framework, the novel target bias sampling method changes the generation mode of the sampling point to make the generated sampling points more directional and effective, improving search efficiency and accelerating the algorithm's convergence. Algorithm 4 outlines the working of the function Heuristic_Random_Sampling ( ). The improved heuristic Bi-RRT obtains candidate sampling points through the sampling function Random_State ( ) and selects the sampling point with the minimum distance cost as the current sampling point P randm (Lines 1-3). Then, according to the distance between the current sampling point P randm and the obstacle, the current sampling point P randm is processed by the heuristic target bias method to obtain the final sampling point P rand (Lines 4-7). This procedure is performed in each sampling process.

Heuristic Parent Node Selection Method
In the basic Bi-RRT algorithm, the nearest tree node is measured by the Euclidean distance from the random point to the tree node, which may generate a polyline path with sharp included angles between connecting line segments of path nodes. Even if the polyline line path is smoothed, it cannot be successfully followed by vehicles. Because the vehicle has a minimum steering radius in actual motion, the Euclidean distance should not be the only factor to consider during each node selection process. As shown in Figure 6, the current vehicle state is a solid rectangle containing a yellow vehicle with three alternative driving states shown as dashed rectangles. In order to find the nearest driving state for the current vehicle, if the Euclidean distance is only considered, there is no doubt that the first driving state is the closest with a Euclidean distance of zero and the second driving state takes second place. However, the first and second driving states, also named in situ steering and lateral translation, are not possible for the vehicle due to the minimum steering radius. Hence, the third driving state is more reasonable.
changes the growing characteristic of the random tree. The introduction of the comprehensive measurement index hinders the pure expansion of the random tree growth towards the configuration region. On the contrary, it drives the tree growth towards the direction associated with the target state and the gentle trend. As a result, it improves the running speed of the algorithm to some extent, and there is a natural trade-off between quick tree growth and better path quality. Given the above analysis, the steering radius of the vehicle needs to be considered to choose the near tree node, namely the parent tree node. Therefore, a heuristic selection method of the parent tree node, namely a comprehensive measurement index considering the distance factor and angle factor, is introduced to make the generated path gentler and easily tracked by the vehicle. Furthermore, it can speed up the algorithm convergence and reduce the calculation time. Equations (5)- (11) show the calculation process when selecting the near tree node. The near tree node P near is determined as the tree node corresponding to the maximum comprehensive measurement index.
where ω 1 and ω 2 are weighted coefficients of the distance index C 1 dis tan ce and the angle index C 1 angle , respectively. C 1 dis tan ce and C 1 angle are the normalized values of the distance C dis tan ce and the angle C angle , respectively. The diagrammatic presentation of the angle C angle is shown in Figure 7. C dis tan ce is the weighted sum of the distance C dis tan ce1 and the distance C dis tan ce2 , ξ 1 and ξ 2 are the weighted coefficients of C dis tan ce1 and C dis tan ce2 , respectively. C dis tan ce1 represents the distance from each tree node to the random sampling node, and C dis tan ce2 represents the distance from each tree node to the target state. The introduce of the distance C dis tan ce2 can make the selected near tree node have a trend close to the target state to increase the computational efficient. The transition from Euclidean distance to the comprehensive measurement index changes the growing characteristic of the random tree. The introduction of the comprehensive measurement index hinders the pure expansion of the random tree growth towards the configuration region. On the contrary, it drives the tree growth towards the direction associated with the target state and the gentle trend. As a result, it improves the running speed of the algorithm to some extent, and there is a natural trade-off between quick tree growth and better path quality.

Heuristic Node Extension Method
Expanding the near tree node to the random point usually adopts a fixed step size in the basic Bi-RRT. The large fixed step size tends to make the new node difficult to extend in the surrounding area of the obstacle, especially dense obstacle areas, resulting in extension failure and reducing the growth rate of the random tree. The small fixed step size could lead to a slow convergence speed of the algorithm. In other words, the fixed step size has the disadvantages of low flexibility and low security. In addition, the expansion direction of the tree node is along the vector direction from P near to P rand . When utilizing the large step size to extend the parent tree node along the expansion direction of the tree node deviating from the target state, an invalid and unnecessary node could be generated, resulting in the low quality of the generated path, and affecting the running speed of the algorithm. Thus, to solve these two problems, a heuristic node extension method, namely the adaptive greedy step size, is introduced. 'Adaptive' is reflected in the pattern that the step size is dynamically and gradually changing rather than fixed, and 'greedy' is reflected in the pattern that the greater the extent of the node expansion direction approaching the direction of the target state, the larger the step size. Equations (12)- (16) show the calculation of the adaptive greedy step size, and its simple legend is shown in Figure 8.
where λ is the basic lower bound of the step size, − −−− → V unit_ t and − −−− → V unit_ r are unit vectors from P near to P target and from P near to P rand , respectively. ϕ is the included angle between − −−− → V unit_ t and − −−− → V unit_ r , η cos is the cotangent of ϕ, s is a constant as a regulating coefficient, and d near_ob is the distance from P near to the obstacle. Far away from the obstacle means d nearest_ob ≥ d threshold . Compared with the traditional fixed step size, the adaptive greedy step size is no longer a constant. Its size is not only related to the distance from the obstacle but also to the included angle of the vector near rand PP towards the target state. In this way, when near an obstacle, a small step size can be used for the basic extension, making the search path more detailed and safer. When far from an obstacle, the step size can be greedily adjusted as the included angle changes, making the random tree adopt the larger step size to expand rapidly along the extension direction closer to the target state and reduce some blind expansions. As a result, the greedy mode of the step size accelerates the growth of the random tree and makes the growth of the path tend to approach the target state to a certain extent. It is exactly because of its dynamic adjustability that the adaptive greedy step size extension can often pass the obstacle detection when approaching the obstacle, unlike the fixed step size extension. Using the adaptive greedy step size can significantly improve the success rate of new node generation and accelerate overall search efficiency effectively.

Improved Constraints
Suppose a path can be successfully and effectively tracked by an autonomous vehicle. In that case, the path should not only meet the obstacle constraints but also comply with the driving characteristics of the actual driver, resulting in avoiding causing excessive tension and discomfort to the driver and passengers. The obstacle constraints include the road environment and the environmental constraint formed by the vehicle being regarded as an obstacle, namely, the vehicle constraint. Therefore, the road environment and the vehicle constraint are improved to obtain a feasible path meeting the driving habits of the actual driver.

Improved Road Environment
In order to generate a practical path, the path nodes need to meet the constraints of road environments when planning. The road environment restricts the planned path more effectively by considering the vehicle width to avoid collision between the vehicle and the Compared with the traditional fixed step size, the adaptive greedy step size is no longer a constant. Its size is not only related to the distance from the obstacle but also to the included angle of the vector − −−−−− → P near P rand towards the target state. In this way, when near an obstacle, a small step size can be used for the basic extension, making the search path more detailed and safer. When far from an obstacle, the step size can be greedily adjusted as the included angle changes, making the random tree adopt the larger step size to expand rapidly along the extension direction closer to the target state and reduce some blind expansions. As a result, the greedy mode of the step size accelerates the growth of the random tree and makes the growth of the path tend to approach the target state to a certain extent. It is exactly because of its dynamic adjustability that the adaptive greedy step size extension can often pass the obstacle detection when approaching the obstacle, unlike the fixed step size extension. Using the adaptive greedy step size can significantly improve the success rate of new node generation and accelerate overall search efficiency effectively.

Improved Constraints
Suppose a path can be successfully and effectively tracked by an autonomous vehicle. In that case, the path should not only meet the obstacle constraints but also comply with the driving characteristics of the actual driver, resulting in avoiding causing excessive tension and discomfort to the driver and passengers. The obstacle constraints include the road environment and the environmental constraint formed by the vehicle being regarded as an obstacle, namely, the vehicle constraint. Therefore, the road environment and the vehicle constraint are improved to obtain a feasible path meeting the driving habits of the actual driver.

Improved Road Environment
In order to generate a practical path, the path nodes need to meet the constraints of road environments when planning. The road environment restricts the planned path more effectively by considering the vehicle width to avoid collision between the vehicle and the road edge. Thus, the newly extended nodes need to meet the requirements of the following Equation (17), and the schematic diagram of the road environment is shown in Figure 9.
where P initial_x and P goal_x are the x coordinates of the initial point and the goal point in the reference coordinate frame, respectively. B l and B r are the left and right boundaries of the road, and W h is the width of the host vehicle.

Improved Vehicle Constraint
In order to express the constraint generated by the obstacle vehicle conveniently, a safety ellipse is introduced to envelop the obstacle vehicle. When considering the subjective comfort of the driver and passengers, a planned path needs to be able to avoid the obstacle vehicle in advance, that is, avoiding the generation of excessive path curvature when approaching the obstacle vehicle. In this case, the planned path can avoid causing discomfort to the driver and passengers and be easily tracked by the vehicle. Therefore, on the basis that half of the vehicle length is used as the semi-major axis of the safety ellipse, the advanced obstacle avoidance distance is added to the semi-major axis to ensure that the vehicle obstacle avoidance occurs at a long distance. Equation (18) expresses the condition that the newly expanded node needs to meet, and the schematic diagram of the vehicle constraint is shown in Figure 10.
where ( , ) xy is the point on the connecting segment between the newly extended node new P and its parent node near P ,

Improved Vehicle Constraint
In order to express the constraint generated by the obstacle vehicle conveniently, a safety ellipse is introduced to envelop the obstacle vehicle. When considering the subjective comfort of the driver and passengers, a planned path needs to be able to avoid the obstacle vehicle in advance, that is, avoiding the generation of excessive path curvature when approaching the obstacle vehicle. In this case, the planned path can avoid causing discomfort to the driver and passengers and be easily tracked by the vehicle. Therefore, on the basis that half of the vehicle length is used as the semi-major axis of the safety ellipse, the advanced obstacle avoidance distance is added to the semi-major axis to ensure that the vehicle obstacle avoidance occurs at a long distance. Equation (18) expresses the condition that the newly expanded node needs to meet, and the schematic diagram of the vehicle constraint is shown in Figure 10.
where (x, y) is the point on the connecting segment between the newly extended node P new and its parent node P near , (x ob , y ob ) is the position of the obstacle vehicle, L o and W o are the length and width of the obstacle vehicle, respectively. s f 1 and s f 2 are the expansion coefficients of the semi-major axis and the semi-minor axis of the safety ellipse, respectively, v is the speed of the host vehicle, η is the friction coefficient, g is the acceleration of gravity, and d sa f e is the advanced obstacle avoidance distance.  To sum up, during the process of the specific node extension, the improved constraints, including the improved road environment and the improved vehicle constraint, can make the planned path meet the actual driving requirement of the vehicle, namely, the path feasibility and the characteristics of avoiding the obstacle in advance.

Path Reorganization Method
Because the basic Bi-RRT algorithm adopts random sampling, the obtained path, also named the initial path, often has poor quality, mainly reflecting in containing too many unnecessary turn points and the discontinuity of path curvature. When tracking the initial path, the autonomous vehicle has to stop and change its driving direction so that it cannot drive smoothly and has unnecessary mechanical wear. Thus, a path reorganization method is needed to optimize the initial path, that is, to remove unnecessary turn points and smooth the path.
After obtaining an initial path by the function _ ( -) Get Path T Bi T , as shown in Algorithm 2, the path reorganization method containing path node reconnection and path smoothing is used to process it. The path node reconnection is utilized to remove unnecessary turn nodes and insert path nodes to replace some necessary path nodes. As a result, it can make the included angles of the connecting line between path points meet the vehicle steering requirement, decreasing the control difficulty of the autonomous vehicle while reducing the path distance to the maximum extent. Additionally, then, the path obtained by the path node reconnection is a broken line composed of discrete path nodes; thus, it needs a further smoothing process to make the vehicle drive smoothly and steadily. Algorithm 5 describes the pseudocode of the path reorganization method. To sum up, during the process of the specific node extension, the improved constraints, including the improved road environment and the improved vehicle constraint, can make the planned path meet the actual driving requirement of the vehicle, namely, the path feasibility and the characteristics of avoiding the obstacle in advance.

Path Reorganization Method
Because the basic Bi-RRT algorithm adopts random sampling, the obtained path, also named the initial path, often has poor quality, mainly reflecting in containing too many unnecessary turn points and the discontinuity of path curvature. When tracking the initial path, the autonomous vehicle has to stop and change its driving direction so that it cannot drive smoothly and has unnecessary mechanical wear. Thus, a path reorganization method is needed to optimize the initial path, that is, to remove unnecessary turn points and smooth the path.
After obtaining an initial path by the function Get_Path T (Bi − T), as shown in Algorithm 2, the path reorganization method containing path node reconnection and path smoothing is used to process it. The path node reconnection is utilized to remove unnecessary turn nodes and insert path nodes to replace some necessary path nodes. As a result, it can make the included angles of the connecting line between path points meet the vehicle steering requirement, decreasing the control difficulty of the autonomous vehicle while reducing the path distance to the maximum extent. Additionally, then, the path obtained by the path node reconnection is a broken line composed of discrete path nodes; thus, it needs a further smoothing process to make the vehicle drive smoothly and steadily. Algorithm 5 describes the pseudocode of the path reorganization method.

Path Node Reconnection
A path obtained by the function Get_Path T (Bi − T) usually has poor connectivity due to the random attribute of the algorithm. Furthermore, there are many redundant turning segments in the path. As a result, it is often not continuously differentiable and infeasible. Thus, the path needs the path node reconnection to meet the prerequisite of path smoothing. Path node reconnection shown in lines 1-43 of Algorithm 5 is conducted to remove redundant path nodes and insert path nodes to replace some existing path nodes for obtaining a new path with maximum length reduction and no collision with obstacles. Moreover, it can ensure that the complementary angles of the included angles of line segments between path nodes are less than the steering angle of the front wheel. The function Get_Path T (Bi − T) is used to obtain a path node set S 0 of the initial path from the forward node of the goal node to the root node of the initial node (Line 2). The first path node is defined as the root node p root , the second path node is defined as the parent node p parent , and the third path node is defined as the current node p current (Line 3). A line segment connects the current node p current and the first subsequent path node from the set S 2 . Additionally, then, connecting this current node p current and one of all subsequent path nodes from the set S 2 through a line segment successively in sequence is conducted until a collision occurs between the line segment and the obstacle (Lines 7-10). In this case, the path nodes between the line segment are removed, and the parent node of the path node that results in the collision with the obstacle is defined as the previous forward node p f orward_last When the complementary angle is less than the steering angle constraint δ f , p root is redefined as the previous root node p root_last , p parent is redefined as the new root node p root , p current is redefined as the new parent node p parent , and p f orward_last is redefined as the new current node p current (Lines 12-13). On the contrary, a path node p insert is inserted between the parent node p parent and the previous forward node p f orward_last to replace the current node p current and meet the conditions that the  (Lines 17-18). After that, p root is redefined as the previous root node p root_last , p parent is redefined as the new root node p root , p insert is redefined as the new parent node p parent , and p f orward_last is redefined as the new current node p current (Line 19). The above operations are applied to the remaining path nodes in the set S 2 in sequence until the path node p n−1 is found. Finally, suppose the complementary angle of the included angle between the line segment connecting the finally defined p current and the finally defined p parent and the line segment connecting the finally defined p current and p n is less than the steering angle constraint δ f . In that case, p n is added to the set S 1 (Lines 29-30). Otherwise, the final path node p n is added to the set S 1 after meeting the conditions that the complementary angle of the included angle between the line segment connecting the finally defined p root and the finally defined p root_last and the line segment connecting the finally defined p root and the inserted node p insert , the complementary angle of the included angle between the line segment connecting the inserted node p insert and the finally defined p root and the line segment connecting the inserted node p insert and the finally defined p current , and the complementary angle of the included angle between the line segment connecting the finally defined p current and the inserted node p insert and the line segment connecting the finally defined p current and the final node p n are less than the steering angle constraint δ f , and the line segment connecting the finally defined p root and the inserted point p insert and the line segment connecting the inserted point p insert and the finally defined p current do not collide with the obstacle (Lines 31-43).
The path node reconnection process is illustrated in Figure 11 specifically. The solid black line is the initial path obtained by the function Get_Path T (Bi − T). The connecting line segments between the black nodes P 1 , P 2 , P 3 , P 4 , P 5 , and P 6 , which are obtained by the process of the path node reconnection, constitute a path to be smoothed in the next step, which is represented as the red solid line. The connecting line segments between the black nodes P 2 , P 3 , P 4 , and P 5 do not intersect with the obstacle, removing the redundant nodes between them, namely, the green path nodes. P 3 , a path node from the initial path, can directly connect with P 2 and P 4 . However, the complementary angle of the included angle between the line segment connecting P 3 and P 2 , and the line segment connecting P 3 and P 4 is more than the steering angle constraint δ f . As a result, a node, namely, node P 3 , is inserted between P 2 and P 4 based on the constraint δ f to replace the node P 3 for obtaining a relatively gentle path with the maximum length reduction, namely the red path. Furthermore, the complementary angles of the included angles of line segments consisting of these black nodes are less than δ f , that is, β 1 , β 2 , β 3 , and β 4 are less than δ f . Thus, the path node reconnection method can reduce the length of the initial path to the greatest extent and obtain a path convenient for further smoothing.
Algorithm 5: Function path_reorganization ( ) 1: Var S 0 , S 1 , S 2 : path 2: S 0 (p n . . . , p 2 , p 1 , p 0 ) ← Get_path T (Bi − T) ; 3: p root ← p 0 ; p parent ← p 1 ; p current ← p 2 ; 4: S 1 ← (p root , p parent , p current ) ; 5: S 2 ← (p 3 . . . p n ) ; 6: while p current ! = p n−1 do 7: for each node p i ∈ S 2 do 8: if Collision_Free (p current , p i ) then 9: p f orward ← p i ; 10: else 11:    P for obtaining a relatively gentle path with the maximum length reduction, namely the red path. Furthermore, the complementary angles of the included angles of line segments consisting of these black nodes are less than f  , that is, 1  , 2  , 3  , and 4  are less than f  . Thus, the path node reconnection method can reduce the length of the initial path to the greatest extent and obtain a path convenient for further smoothing. Figure 11. Path node reconnection processing.

Path Smoothing
After the path node reconnection process, a simplified path, the solid red line in Figure 11, is obtained, but the path contains some necessary turning nodes. As a result, the path is still not continuously differentiable such that it cannot be directly tracked by the vehicle. Thus, the obtained path should be further smoothed to make it be successfully tracked by the vehicle [40]. The cubic B-spline curve can move a control point to make the local modification without affecting the overall shape of the path and have a simple implementation and relatively low computational cost to ensure kinematic feasibility while avoiding an obstacle [41]. In addition, it is also often adopted to obtain a continuously differentiable cure [42]. Therefore, the cubic B-spline curve can be used to optimize the obtained path.  (19) where the basic function ,3 () i Gt is defined as Figure 11. Path node reconnection processing.

Path Smoothing
After the path node reconnection process, a simplified path, the solid red line in Figure 11, is obtained, but the path contains some necessary turning nodes. As a result, the path is still not continuously differentiable such that it cannot be directly tracked by the vehicle. Thus, the obtained path should be further smoothed to make it be successfully tracked by the vehicle [40]. The cubic B-spline curve can move a control point to make the local modification without affecting the overall shape of the path and have a simple implementation and relatively low computational cost to ensure kinematic feasibility while avoiding an obstacle [41]. In addition, it is also often adopted to obtain a continuously differentiable cure [42]. Therefore, the cubic B-spline curve can be used to optimize the obtained path.
Supposing there are m + 1 control points P i (i = 0, 1, · · ·, m), the cubic B-spline curve is expressed as where the basic function G i,3 (t) is defined as In order to make the cubic B-spline curve start from P 0 , tangent to the vector − − → P 0 P 1 , end at P m , and tangent to the vector − −−− → P m P m-1 , the control points P −1 and P m+1 are added to meet the conditions P −1 + P 1 = 2P 0 and P m−1 + P m+1 = 2P m .
The path node organization method can make the generated path continuously differentiable while reducing path length to the maximum extent. Meanwhile, the generated path does not have unnecessary steering and meets the tracking requirement of the vehicle.

Path Coherence Method
Due to the randomness of the algorithm, the difference in the planned paths between two adjacent frames could easily cause the vehicle to shake during driving, If the difference is too large, it may even cause the vehicle to collide with an obstacle. Hence, to solve this path planning problem in dynamic environments, the interframe path coherence method is introduced to guarantee the smooth connection of planned paths between frames. The interframe path coherence method refers to the need to consider the information of the planned path of the previous frame when planning a path in the current frame. Its main idea is that at the beginning of each new planning cycle, the root node P root_newly of the new planning cycle is the position whose distance from the root node in the trajectory planned in the previous planning cycle is equal to R. At the same time, a point along the tangent line of the newly generated root node is selected as the new search starting node P initial_newly . Equations (21) and (22) express the calculation of the root node P root_newly . Equation (23) shows how to calculate P initial_newly .
where P root is the root node of the previous planning cycle, R is the offset distance of the root node P root , and ρ is the coefficient of proportionality. R is more than the preview distance of the vehicle to ensure that the preview point is still on the trajectory of the previous frame. As a result, there is no need to make more tracking control adjustments. P root_newly is the right intersection point of the cubic B-spline curve and the designed circle. B slope is the slope of the cubic B-spline curve at the intersection point, ϑ is the included angle between the tangent line and the x axis at the intersection point, and L skew is the offset distance along the direction of the tangent line. P root_newly and P initial_newly are introduced to ensure that the trajectory generated in the current frame is tangent to the tangent line at the intersection point P root_newly and passes through the intersection point P root_newly . Thereby, there are smooth connections in the interframe paths. As seen in Figure 12, The path coherence method can significantly eliminate the difference between interframe trajectories and make the planned trajectory smooth and continuous, resulting in maintaining the vehicle's overall stability during driving.

Simulation Environment
In order to verify the performance of the improved heuristic Bi-RRT algorithm, two typical road scenarios, including a straight road scenario and a curved road scenario, as shown in Figure 13a,b, are separately taken into account. Road information and other parameters in the whole simulation are shown in Tables 1 and 2. The performance comparison of different RRT variants in the static straight road and curved road scenarios is conducted to verify the superiority of the improved heuristic Bi-RRT method. Meanwhile, interframe path planning and tracking are performed in dynamic scenarios to verify the The path coherence method can significantly eliminate the difference between interframe trajectories and make the planned trajectory smooth and continuous, resulting in maintaining the vehicle's overall stability during driving.

Simulation Environment
In order to verify the performance of the improved heuristic Bi-RRT algorithm, two typical road scenarios, including a straight road scenario and a curved road scenario, as shown in Figure 13a,b, are separately taken into account. Road information and other pa-  Step size  (m) 10 Expansion coefficient (curve)

Performance Measure of Path Planning
The improved heuristic Bi-RRT algorithm is compared with the basic RRT, the biased RRT, the Bi-RRT, and the RRT* in the straight and curved road scenarios, and the results are shown in Figure 14a,b, respectively. The blue dotted line represents the path planned by the basic RRT, the red dashed line represents the path planned by the biased RRT, and the black dash-dotted line represents the path planned by the RRT*. The solid green and black lines represent the paths planned by the Bi-RRT and the improved heuristic Bi-RRT, respectively. It can be seen in Figure 14a,b that the paths generated by the basic RRT, the Bi-RRT, and the biased RRT contain a large number of corners, and there are frequent large curvature changes. In contrast, the path generated by the RRT* is relatively gentle, but it is still a broken line, which does not meet the steering requirement of the vehicle. However, the path generated by the improved heuristic Bi-RRT is a continuous and smooth curve, and the path curvature is also continuous, as shown in Figure 15a,b, respectively, resulting in the convenience being well followed by the vehicle. Observing Figure 14a,b, all paths planned by the basic RRT, the biased RRT, the Bi-RRT, the RRT*, and the improved heuristic Bi-RRT can successfully avoid the obstacle vehicle. However, there are differences in obstacle avoidance modes. The paths planned by the basic RRT, the biased RRT, the Bi-RRT, and the RRT* always start emergency obstacle avoidance when approaching the obstacle vehicle. The path planned by the proposed algorithm can start obstacle avoidance in advance by a safe distance from the obstacle vehicle, ensuring safe driving, conforming to the driver's behavior habit, and not bringing excessive tension to passengers. The proposed algorithm can realize obstacle avoidance in advance because the obstacle avoidance distance is embedded when constructing the obstacle vehicle constraint.  A set of benchmarking parameters is defined to objectively compare the performance of the improved heuristic Bi-RRT and some RRT variants. The number of nodes on the mature random tree is denoted by "tree nodes." The length of the generated path is denoted by "path length." The searching time of path planning is denoted by "time." In addition, the number of segments comprising the generated path is donated by "path segments"; in particular, the path segments of the improved heuristic RRT refer to the number of segments of the path after being processed by path reconnection [30,43,44].
Thirty independent simulation experiments were implemented on each algorithm to offset the random deviation of a single experiment, and the results are shown in Tables 3 and 4. The average path length generated by the RRT* algorithm is much lower than those of the basic RRT, the biased RRT, and the Bi-RRT because the RRT * embedded with the reconnection mechanism can approach the approximate shortest path. However, the average path length generated by the improved heuristic Bi-RRT algorithm is smaller than that of the RRT* algorithm, and the average tree nodes and the average path segments of the proposed algorithm are also minimal compared with those of the other four algorithms. The decrease in path length and number of path segments is mainly due to the introduction of the path node reconnection method. The relatively small number of path segments can reduce the control difficulty of the vehicle and make the further smoothed path not contain unnecessary turns. In terms of the average planned time, the performance of the improved heuristic Bi-RRT algorithm is optimal compared with those of the other four algorithms, and especially in the straight road scenario, the planning time of the improved heuristic Bi-RRT algorithm can reduce significantly. As a result of the performance measure, the improved heuristic Bi-RRT algorithm shows superior performance regarding path quality and planning time compared with the basic RRT, the biased RRT, the Bi-RRT, and the RRT*.

Path Coherence Validation
The dynamic environment can usually be treated as a static environment in a dynamic path planning process. That is, the re-planning of paths needs to be conducted in each frame environment region obtained by the perception module. Thus, the running speed of the path planning algorithm should not be only considered, but also the interframe connection of the planned paths should be concerned. In order to further verify the superior performance of the improved heuristic Bi-RRT algorithm, the planning effect of the corresponding algorithm at a specific frame is described in detail.
In order to express the coherence between the front and back frame paths, the realtime re-planning is carried out in two dynamic driving scenarios, where the host vehicle and the obstacle vehicle move in opposite and the same directions, respectively. The red triangle represents the position of the obstacle vehicle in the current frame, the yellow arrow represents the driving direction of the obstacle vehicle, and the green solid point and the black solid point represent the start point and end point of the current frame, respectively. Figure 16a-c show the dynamic path planning process of the improved heuristic Bi-RRT algorithm for three consecutive frames in the dynamic scenario with driving in opposite directions. Observing Figure 16a-c, the paths obtained in the first, second, and third frames are smooth and successfully bypass the moving obstacle vehicle. It can be seen from Figure 16d that the paths, magenta lines, obtained in the first frame, the second frame, and the third frame can be smoothly connected to form a smooth coherence path, as shown in Figure 16e. Moreover, the curvature of the coherence path is continuous and varies in a small range, which is convenient for it to be tracked by the host vehicle, as shown in Figure 16f. Figure 17a-f show the dynamic path planning process of the improved heuristic Bi-RRT algorithm for six consecutive frames in the dynamic scenario with driving in the same direction. It can be seen from Figure 17a-f that the paths obtained in the first, second, third, fourth, fifth, and sixth frames are smooth and also successfully bypass the moving obstacle vehicle. As seen in Figure 17g, the paths, magenta lines, obtained in the first frame, the second frame, the third frame, the fourth frame, the fifth frame, and the sixth frame can be smoothly connected to generate a smooth coherence path, as shown in Figure 17h. In addition, the curvature of the obtained coherence path is continuous and within the range of 0.02 1/m, as shown in Figure 17i, resulting in the condition that the host vehicle can easily track the coherence path.  For further verifying the tracking performance of the obtained coherence paths shown in Figures 16e and 17h, the path following experiments can be carried out in the Carsim simulation platform. The appropriate vehicle and driver models are selected to simulate the driving state of the real vehicle. The solid black and red dashed lines represent the target and followed paths, respectively.
The path following experiment in the dynamic scenario with driving in opposite directions is conducted. The result shown in Figure 18a represents the target path and the followed path. The following error between the followed path and the target path is For further verifying the tracking performance of the obtained coherence paths shown in Figures 16e and 17h, the path following experiments can be carried out in the Carsim simulation platform. The appropriate vehicle and driver models are selected to simulate the driving state of the real vehicle. The solid black and red dashed lines represent the target and followed paths, respectively.
The path following experiment in the dynamic scenario with driving in opposite directions is conducted. The result shown in Figure 18a represents the target path and the followed path. The following error between the followed path and the target path is relatively small and within the range of 0.06 m, as shown in Figure 18b, resulting in making the following effect acceptable. The yaw rate is within the range of 8 deg/s, as shown in Figure 18c, and the lateral acceleration is within the range of 0.25 g and less than the usual value of 0.8 g of the maximum lateral acceleration of steady driving, as shown in Figure 18d. These two indicators prove that the dynamic performance of the host vehicle is stable in the simulation experiment. Based on these facts, the obtained coherence path is satisfactory and effective.
rection is conducted. The target path and the followed path are shown in Figure 19a. The following error between the followed path and the target path is within the range of 0.07 m, as shown in Figure 19b, and is relatively small, which shows that the coherence path has a satisfactory tracking effect. The yaw rate and the later acceleration of the host vehicle when following the trajectory are shown in Figure 19c,d, respectively. The yaw rate is within the range of 7 deg/s, and the lateral acceleration is within the range of 0.2 g and less than the usual value of 0.8 g of the maximum lateral acceleration of steady driving, which shows the excellent dynamic performance of the host vehicle in the simulation process. Thus, the obtained coherence path is feasible and acceptable.
It can be seen from the test results that the algorithm proposed in this article can plan a path with smooth transition connections and continuous curvature. Furthermore, it is applied successfully in the dynamic driving scenarios of opposite driving and traveling in the same direction, which are the most common in vehicle driving. The path following experiment in the dynamic scenario with driving in the same direction is conducted. The target path and the followed path are shown in Figure 19a. The following error between the followed path and the target path is within the range of 0.07 m, as shown in Figure 19b, and is relatively small, which shows that the coherence path has a satisfactory tracking effect. The yaw rate and the later acceleration of the host vehicle when following the trajectory are shown in Figure 19c,d, respectively. The yaw rate is within the range of 7 deg/s, and the lateral acceleration is within the range of 0.2 g and less than the usual value of 0.8 g of the maximum lateral acceleration of steady driving, which shows the excellent dynamic performance of the host vehicle in the simulation process. Thus, the obtained coherence path is feasible and acceptable.

Discussion
The path planning of the vehicle in dynamic scenarios often pays attention not only to the quality of each frame path but also to the difference in paths between frames. In addition, the Bi-RRT algorithm, a variant of the basic RRT algorithm, is often used for path planning because of its probability completeness and rapidity. However, its planned path is not differentiable and relatively poor in length. Based on those facts, some improved heuristic methods are introduced to the Bi-RRT algorithm to make it suitable for the dynamic path planning of the vehicle. The multi-sampling method biased towards the target state makes the growth of the random tree directional and reasonable. The adaptive greedy step size considering the target direction can increase the success rate of node expansion and make the newly extended node tend to the target state direction to a certain extent. The two random trees are directly interconnected when there are no obstacles between them, as a result, which further reduces the running time of the algorithm. Changing from only considering the Euclidean distance to considering the distance from the target state and the included angles between the connection lines of tree nodes, the parent node selection method improves the running speed of the algorithm to a certain extent while making the generated path tend to be gentle. The path reorganization method, including path node reconnection and path smoothing, can remove necessary turning points, significantly reduce the path length, and plan a path with continuous curvature. As for the problem of path connection between frames, the path coherence method can make paths between different frames smoothly connect to form a differentiable path. By way of the simulation experiment study, the improved heuristic Bi-RRT algorithm has a real-time performance, especially in a straight road scenario, and guarantees the shortest path while obeying the road constraints and the vehicle constraint and considering the It can be seen from the test results that the algorithm proposed in this article can plan a path with smooth transition connections and continuous curvature. Furthermore, it is applied successfully in the dynamic driving scenarios of opposite driving and traveling in the same direction, which are the most common in vehicle driving.

Discussion
The path planning of the vehicle in dynamic scenarios often pays attention not only to the quality of each frame path but also to the difference in paths between frames. In addition, the Bi-RRT algorithm, a variant of the basic RRT algorithm, is often used for path planning because of its probability completeness and rapidity. However, its planned path is not differentiable and relatively poor in length. Based on those facts, some improved heuristic methods are introduced to the Bi-RRT algorithm to make it suitable for the dynamic path planning of the vehicle. The multi-sampling method biased towards the target state makes the growth of the random tree directional and reasonable. The adaptive greedy step size considering the target direction can increase the success rate of node expansion and make the newly extended node tend to the target state direction to a certain extent. The two random trees are directly interconnected when there are no obstacles between them, as a result, which further reduces the running time of the algorithm. Changing from only considering the Euclidean distance to considering the distance from the target state and the included angles between the connection lines of tree nodes, the parent node selection method improves the running speed of the algorithm to a certain extent while making the generated path tend to be gentle. The path reorganization method, including path node reconnection and path smoothing, can remove necessary turning points, significantly reduce the path length, and plan a path with continuous curvature. As for the problem of path connection between frames, the path coherence method can make paths between different frames smoothly connect to form a differentiable path. By way of the simulation experiment study, the improved heuristic Bi-RRT algorithm has a real-time performance, especially in a straight road scenario, and guarantees the shortest path while obeying the road constraints and the vehicle constraint and considering the driving habit of the driver. On the contrary, the vehicle constraint and the driver's driving habits are not considered when applying the basic Bi-RRT algorithm.
However, it must be admitted that the running time may not be fast enough when the proposed algorithm is applied in a high-speed and dynamic curved road scene, which may be due to a large number of numerical calculations based on graphic geometry in obstacle detection. In future research, obstacle detection of gray value comparison and parallel computing are introduced to further reduce the proposed algorithm's running time to meet the path planning requirement in a high-speed curved road scenario. After the preparation of the perception module, changing from the numerical calculation mode of graphics geometry to the comparison of gray values mode on binarized images, the obstacle detection can greatly reduce the calculation amount of the algorithm, thus speeding up the search speed of the algorithm. On this basis, the improved heuristic Bi-RRT algorithm can be applied to more complex driving scenarios, such as unstructured road scenes with a mixture of static and moving obstacles with grooves, to explore its adaptability. Furthermore, compared with other related algorithms, finding out the algorithm's shortcomings and the scenes where the algorithm cannot be applied are made to improve the algorithm and enable it to be applied to more driving scenes.

Conclusions
This paper is concerned with the path planning of autonomous vehicles in a dynamic environment. An improved heuristic Bi-RRT algorithm has been proposed and tested. The proposed algorithm can solve the path query problem of the basic Bi-RRT algorithm and the interconnection problem of paths between various frames in a dynamic scenario to obtain a smooth and asymptotically optimal path with continuous curvature with high efficiency and accuracy. The proposed path planning algorithm consists of the obstacle-free direct connection of two trees, the heuristic target bias sampling, the heuristic parent node selection, the heuristic node extension, the improved constraints, path reorganization, and path coherence. The obstacle-free direct connection mode can further accelerate the interconnection of the two random trees. The heuristic target bias sampling can reduce blind sampling, and the heuristic node extension can decrease invalid expansion, thereby accelerating the running speed and improving the searching efficiency of the algorithm. The heuristic parent node selection speeds up the algorithm's calculation and improves path quality to some extent. The improved environmental constraint and the improved vehicle constraint integrated with the advanced obstacle avoidance distance are considered together to make the vehicle avoid the obstacle in advance and accurately and make the vehicle drive safely. Path reorganization aims to post-process the initial path to obtain a reasonable and differentiable path with the approximate optimal length, which can be tracked by the vehicle smoothly and successfully. In addition, path coherence solves the problem of the smooth connection of paths between different frames, enabling the vehicle to run smoothly and steadily at the connection point. Through the simulation experiments, the improved heuristic Bi-RRT algorithm can generate the smoothest path and takes the shortest time compared with the other four algorithms. As a result, it is an effective local path planning algorithm for the autonomous vehicle and has practical value in the application of the wheeled robot.
In future works, the research focuses on increasing the solution speed, further reducing the calculation time, especially in the curved road scenario. The proposed algorithm will be applied in more complex driving scenarios, such as the parking scene and the drift scene with moving obstacles, to test its adaptiveness. Moreover, after the preparation of the test platform of the autonomous vehicle, an on-site experiment is conducted to test its effectiveness in practical applications.