Development of an Improved Rapidly Exploring Random Trees Algorithm for Static Obstacle Avoidance in Autonomous Vehicles

Safe path planning for obstacle avoidance in autonomous vehicles has been developed. Based on the Rapidly Exploring Random Trees (RRT) algorithm, an improved algorithm integrating path pruning, smoothing, and optimization with geometric collision detection is shown to improve planning efficiency. Path pruning, a prerequisite to path smoothing, is performed to remove the redundant points generated by the random trees for a new path, without colliding with the obstacles. Path smoothing is performed to modify the path so that it becomes continuously differentiable with curvature implementable by the vehicle. Optimization is performed to select a “near”-optimal path of the shortest distance among the feasible paths for motion efficiency. In the experimental verification, both a pure pursuit steering controller and a proportional–integral speed controller are applied to keep an autonomous vehicle tracking the planned path predicted by the improved RRT algorithm. It is shown that the vehicle can successfully track the path efficiently and reach the destination safely, with an average tracking control deviation of 5.2% of the vehicle width. The path planning is also applied to lane changes, and the average deviation from the lane during and after lane changes remains within 8.3% of the vehicle width.


Introduction
According to the World Health Organization, there are 1.35 million fatalities due to traffic accidents each year [1]. With the advances in mobile communication technology, advanced driver assistance systems and intelligent transportation systems are under development to reduce traffic accidents caused by driver negligence. The concept of the Internet of Vehicles allows vehicles to drive autonomously, reduces the operator's burden, and improves driver safety. This is making autonomous vehicles possible through high data transmission efficiency and low transmission latency [2]. To ensure safety, an autonomous vehicle must have the ability to identify and avoid obstacles.
Safe path planning is key in autonomous vehicles. An autonomous vehicle has a perception layer to detect its location, a planning layer to predict the safe path/route, and a control layer to maneuver the vehicle's direction and speed [3][4][5]. Sensors such as receivers of the Global Navigation Satellite System (GNSS), inertial measurement units (IMUs), LiDAR, cameras, and radars were all installed to investigate vehicles in an obstacle-free environment [6,7]. Simultaneous location and mapping by normal distribution transform was proposed for vehicles in deeply urbanized cities [8]. The simulation of the sensor uncertainties influencing the path planning was conducted in [9]. The methods of path planning can be summarized into four categories: the graph search method, the sampling method, the interpolating method, and the numerical optimization method. Among them, numerical optimization using deep learning neural networks (DNN) has been the recent

Rapidly Exploring Random Trees (RRT) Algorithm
The RRT algorithm is used to construct a path from the start x start to the destination x dest in a metric space X by searching the free space X f ree , X f ree ⊂ X, away from the obstacle X obs , X obs ⊂ X. Consider that x start and x dest are both points in two-dimensional space. The RRT algorithm starts from x start and randomly samples a point x rand to find the nearest neighboring point x near to construct a tree. The definition of "nearest" is the shortest distance in Euclidean distance, such that x near is in the direction toward x rand at a distance for the new point, also called the node, x new . If x new ⊂ X f ree , the tree expansion continues, and the algorithm connects x near to x new and checks if the connection collides with any obstacle X obs . If it does, the algorithm restarts; otherwise, x new is added to the tree as a new point, and the search repeats until the tree reaches the destination or the number of iterations of expanding the tree reaches the limit. Figure 1 illustrates the RRT algorithm of expanding the searching tree so as to plan a safe path from the start to the destination. the RRT algorithm of expanding the searching tree so as to plan a safe path from the start to the destination. Due to the random nature of the algorithm, there were many redundant turning points (nodes) in the path, as shown in Figure 1, and this made the path planning inefficient in reaching the destination. These redundant points were also the reason that the planned path was not continuously differentiable and hence infeasible for autonomous vehicle operation. Moreover, there was certainly more than one feasible path to the destination, but the random nature also made the search for the "optimal" path inefficient. Path smoothing and optimization are therefore needed.

Improved RRT Algorithm with Pruning
The path obtained by the RRT algorithm was plagued by the randomly generated nodes that can result in a poor connectivity path. Furthermore, the path was often not continuously differentiable and was thus infeasible for vehicle implementation. Pruning is a prerequisite for smoothing. The pruning process shown in Figure 2 was conducted to remove the redundant points in the RRT algorithm for a new path without colliding with obstacles. The process started from three consecutive path points, , , and , as illustrated in Figure 2a. If there was no collision, and were directly connected as a new path and was redundant. Conversely, if a collision was found between and , then was retained as the path point. The pruning process can remove redundant path points and obtain a more efficient path to the destination, as illustrated in Figure 2b. Due to the random nature of the algorithm, there were many redundant turning points (nodes) in the path, as shown in Figure 1, and this made the path planning inefficient in reaching the destination. These redundant points were also the reason that the planned path was not continuously differentiable and hence infeasible for autonomous vehicle operation. Moreover, there was certainly more than one feasible path to the destination, but the random nature also made the search for the "optimal" path inefficient. Path smoothing and optimization are therefore needed.

Improved RRT Algorithm with Pruning
The path obtained by the RRT algorithm was plagued by the randomly generated nodes that can result in a poor connectivity path. Furthermore, the path was often not continuously differentiable and was thus infeasible for vehicle implementation. Pruning is a prerequisite for smoothing. The pruning process shown in Figure 2 was conducted to remove the redundant points in the RRT algorithm for a new path without colliding with obstacles. The process started from three consecutive path points, P 1 , P 2 , and P 3 , as illustrated in Figure 2a. If there was no collision, P 1 and P 3 were directly connected as a new path and P 2 was redundant. Conversely, if a collision was found between P 1 and P 3 , then P 2 was retained as the path point. The pruning process can remove redundant path points and obtain a more efficient path to the destination, as illustrated in Figure 2b. In path planning, collision detection is necessary to check whether the path between the two checkpoints, say and , is in contact with any obstacle in the space. Polygon modeling for collision detection is often too complicated, time-consuming, and computationally complex, and most choose to simplify the obstacle's geometric shape for computational efficiency. Consider the obstacle as a rectangle with safety boundary , which is set at half the vehicle's width for autonomous vehicle applications, as shown in Figure  3a. In a collision check, if both checkpoints and their connection are on the same side of the obstacle, as in Figure 3b, the path after pruning is considered safe. Conversely, if the two checkpoints are on different sides of an obstacle, then one has to check: where and are the angles of the connection between the path points and the corner(s) of the obstacle, and is the angle of the connection between the two checkpoints, as shown in Figure 3c. This collision detection will ensure path safety in the pruning process. For the example of path planning in the obstacle environment shown in Figure 2b, the number of nodes is reduced significantly by 99% after pruning. Such pruning paves the way for efficient path planning, in terms of path length and number of turns, for reaching the destination. With pruning, the unnecessary turns along the path taken by the RRT algorithm will no longer affect the autonomous vehicles. In path planning, collision detection is necessary to check whether the path between the two checkpoints, say P 1 and P 3 , is in contact with any obstacle in the space. Polygon modeling for collision detection is often too complicated, time-consuming, and computationally complex, and most choose to simplify the obstacle's geometric shape for computational efficiency. Consider the obstacle as a rectangle with safety boundary d s , which is set at half the vehicle's width for autonomous vehicle applications, as shown in Figure 3a.
In a collision check, if both checkpoints and their connection are on the same side of the obstacle, as in Figure 3b, the path after pruning is considered safe. Conversely, if the two checkpoints are on different sides of an obstacle, then one has to check: where A co1 and A co3 are the angles of the connection between the path points and the corner(s) of the obstacle, and A cp is the angle of the connection between the two checkpoints, as shown in Figure 3c. This collision detection will ensure path safety in the pruning process.
For the example of path planning in the obstacle environment shown in Figure 2b, the number of nodes is reduced significantly by 99% after pruning. Such pruning paves the way for efficient path planning, in terms of path length and number of turns, for reaching the destination. With pruning, the unnecessary turns along the path taken by the RRT algorithm will no longer affect the autonomous vehicles.

Improved RRT Algorithm with Smoothing and Optimization
After the pruning process, a simplified path as shown in Figure 2b was obtained but the path was still not continuously differentiable. There remained several turning points in the path to the destination. This work applied the Bézier curve to generate a continuously differentiable path to the destination. The advantage is its simple implementation and thus comparably low computation cost to guarantee kinematic feasibility while avoiding obstacles. A Bézier curve is often adopted in computer graphics to obtain a continuously differentiable curve tangent to the two lines connecting the adjacent control points. It has also been used in the lane change [33] and path planning [34] of intelligent vehicles. The quadratic Bézier curve ( = 2) is written as where are the control points of the Bézier curve and the polynomial , ( ) is The optimal way to select the control points is to take a turning point, say , as the center point and follow the path (after pruning) forward and backward a certain distance, at a ratio set at 0.382 (Fibonacci number) from each turning point to the adjacent path points. Two sets of control points for the quadratic Bézier curves are shown in Figure 4a. By this process, a Bézier curve can be obtained at each turning point for a smooth path, as

Improved RRT Algorithm with Smoothing and Optimization
After the pruning process, a simplified path as shown in Figure 2b was obtained but the path was still not continuously differentiable. There remained several turning points in the path to the destination. This work applied the Bézier curve to generate a continuously differentiable path to the destination. The advantage is its simple implementation and thus comparably low computation cost to guarantee kinematic feasibility while avoiding obstacles. A Bézier curve is often adopted in computer graphics to obtain a continuously differentiable curve tangent to the two lines connecting the adjacent control points. It has also been used in the lane change [33] and path planning [34] of intelligent vehicles. The quadratic Bézier curve (n = 2) is written as where P i+1 are the control points of the Bézier curve and the polynomial b i,n (t) is The optimal way to select the control points is to take a turning point, say P 2 , as the center point and follow the path (after pruning) forward and backward a certain distance, at a ratio set at 0.382 (Fibonacci number) from each turning point to the adjacent path points. Two sets of control points for the quadratic Bézier curves are shown in Figure 4a. By this process, a Bézier curve can be obtained at each turning point for a smooth path, as shown in Figure 4b. The calculation in low order of a Bézier curve is simple and the results provide good performance without computational burden. shown in Figure 4b. The calculation in low order of a Bézier curve is simple and the results provide good performance without computational burden. The random nature of the RRT algorithm makes it capable of finding a path to the destination in complex space, but there definitely will be more than one path to the destination. For example, there are many feasible paths, upward or downward, as illustrated in Figure 5a. In terms of path length, the downward path is not as efficient. It is thus important to find a "near"-optimal path for efficient planning. The path planning will be repeated to yield multiple results, which are then compared to find the desired path. To ensure safety, the path points are checked to see whether they are in free space during every planning stage of the improved RRT algorithm. If all path points are in free space, the result of the smoothing process is retained as a candidate path; conversely, the path is abandoned and the planning repeats until a path is found.  Table 1 lists the success rate and average calculation time of the optimization process in one, five, and ten repeated planning steps. The success rate of one-time planning, i.e., no optimization process, is 56%, with 0.09 s of calculation time. The success rate of fivetime planning is 74% within 0.42 s, and that of ten-time planning is 96% within 0.90 s of The random nature of the RRT algorithm makes it capable of finding a path to the destination in complex space, but there definitely will be more than one path to the destination. For example, there are many feasible paths, upward or downward, as illustrated in Figure 5a. In terms of path length, the downward path is not as efficient. It is thus important to find a "near"-optimal path for efficient planning. The path planning will be repeated to yield multiple results, which are then compared to find the desired path. To ensure safety, the path points are checked to see whether they are in free space during every planning stage of the improved RRT algorithm. If all path points are in free space, the result of the smoothing process is retained as a candidate path; conversely, the path is abandoned and the planning repeats until a path is found. shown in Figure 4b. The calculation in low order of a Bézier curve is simple and the results provide good performance without computational burden. The random nature of the RRT algorithm makes it capable of finding a path to the destination in complex space, but there definitely will be more than one path to the destination. For example, there are many feasible paths, upward or downward, as illustrated in Figure 5a. In terms of path length, the downward path is not as efficient. It is thus important to find a "near"-optimal path for efficient planning. The path planning will be repeated to yield multiple results, which are then compared to find the desired path. To ensure safety, the path points are checked to see whether they are in free space during every planning stage of the improved RRT algorithm. If all path points are in free space, the result of the smoothing process is retained as a candidate path; conversely, the path is abandoned and the planning repeats until a path is found.  Table 1 lists the success rate and average calculation time of the optimization process in one, five, and ten repeated planning steps. The success rate of one-time planning, i.e., no optimization process, is 56%, with 0.09 s of calculation time. The success rate of fivetime planning is 74% within 0.42 s, and that of ten-time planning is 96% within 0.90 s of  Table 1 lists the success rate and average calculation time of the optimization process in one, five, and ten repeated planning steps. The success rate of one-time planning, i.e., no optimization process, is 56%, with 0.09 s of calculation time. The success rate of fivetime planning is 74% within 0.42 s, and that of ten-time planning is 96% within 0.90 s of calculation time. The results show that increasing the number of planning steps will increase the chances of finding a better path. The calculation time is acceptable with the advent of high-performance onboard computers. Figure 5a shows the results of 10 repeated planning steps, where the shortest is selected to be the path of the improved RRT algorithm. Table 1. The success rate and average calculation time for 1, 5, and 10 planning steps of the improved RRT algorithm in obtaining a "near"-optimal path. The results show that after the pruning and optimization process, the path length is reduced by around one half, as shown in Figure 5b, and the planning efficiency is significantly improved. It should be noted that the time needed from the start to the destination may be just as important. In this study, the path points were pruned significantly and smoothing was applied to reduce the sharp turns. The path length was considered a good indicator of optimal path planning. Table 2 shows the path length by the RRT algorithm and the improved algorithm with pruning and/or optimization processes. The length was reduced from 1043 to 776, a reduction of around 34% by pruning. The path length was further reduced to 583, around another 33% by optimization. Figure 6 shows the result of the improved RRT algorithm in different obstacle environments. The improved algorithm is an effective path planner for an efficient, continuously differentiable, safe path. Table 2. The average path length by the RRT algorithm, the improved algorithm with pruning, and the improved algorithm with pruning and optimization.

Experimental Verification by Tracking Control
In order to validate the effectiveness of the improved RRT algorithm in obstacle avoidance, an autonomous vehicle with a pure pursuit controller and a proportional-integral (PI) speed controller was applied to track the planned path. The pure pursuit controller was used to set a look-ahead point at a fixed distance in front of the vehicle from its current position. The geometric relationship between the vehicle and the look-ahead point, as illustrated in Figure 7a, can be defined to obtain the control command of the steering angle = tan ( / ), where is the vehicle wheelbase, = ( /2 + cos )/ sin is the radius of curvature with respect to the rotation center of the vehicle, is the forward drive look-ahead distance, is the distance from the rear axle (for rear wheel drive) to the forward anchor point, and is the heading of the look-ahead point (constrained on the planned path) from the forward anchor point with respect to the vehicle heading. Details of the vehicle kinematics model can be found in [35]. A PI controller, = ( − ) + ( − ) , is used for speed control, where is the speed control command, and are the proportional and integral gains, respectively, is the command velocity, and is the vehicle speed. The advantage of using the steering controller and speed controller is that they can provide good results with minimal computation load. Advanced controller design can also be implemented on the onboard computer with sufficient computation power.

Experimental Verification by Tracking Control
In order to validate the effectiveness of the improved RRT algorithm in obstacle avoidance, an autonomous vehicle with a pure pursuit controller and a proportional-integral (PI) speed controller was applied to track the planned path. The pure pursuit controller was used to set a look-ahead point at a fixed distance in front of the vehicle from its current position. The geometric relationship between the vehicle and the look-ahead point, as illustrated in Figure 7a, can be defined to obtain the control command of the steering angle ϕ = tan −1 (L/R), where L is the vehicle wheelbase, R = L f /2 + l cos ρ / sin ρ is the radius of curvature with respect to the rotation center O of the vehicle, L f is the forward drive look-ahead distance, l is the distance from the rear axle (for rear wheel drive) to the forward anchor point, and ρ is the heading of the look-ahead point (constrained on the planned path) from the forward anchor point with respect to the vehicle heading. Details of the vehicle kinematics model can be found in [35]. A PI controller, u s = K p (V cmd − V s ) + K i t 0 (V cmd − V s )dτ, is used for speed control, where u s is the speed control command, K p and K i are the proportional and integral gains, respectively, V cmd is the command velocity, and V s is the vehicle speed. The advantage of using the steering controller and speed controller is that they can provide good results with minimal compu- The experiment was conducted in a static obstacle environment, and the criterion for success was the autonomous vehicle's capability of traveling along the planned path safely, from the start to the destination, without colliding with obstacles at the speed generating l g acceleration upon turning. For the vehicle wheelbase = 26 cm, the distance from the rear axle to the forward anchor point = 6 cm, = 1 m/s, = 50 cm, and the control gains = 0.3 and = 0.04, the vehicle followed the planned path of the improved RRT algorithm to the destination on the upper right in Figure 7b. The vehicle trajectory was captured by an observer camera for verifying the improved algorithm. The deviation of the vehicle trajectory from the planned path was defined as the tracking error, as shown in Figure 8a. For the two obstacles in Figure 7b, the tracking errors shown in Figure 8b had an average tracking error of 4.9% and 4.7% of the vehicle width in the two obstacle environments, respectively. The maximum deviation of 12.6% and 20.5%, respectively, was from the vehicle's initial heading not aligning relative to the planned path, such that the vehicle had to maneuver to align with the path. The results show that the vehicle can track the planned path to the destination without colliding with obstacles. The tracking controller and the speed controller, though classical, were effective. For applications in robotics, some systems may have driving wheels with differential speeds or universal wheels. The vehicle dynamics will definitely have an influence on the controller design. Nevertheless, the path planning by the improved RRT algorithm remains applicable, for the predicted path was continuously differentiable, smooth, and efficient. The experiment was conducted in a static obstacle environment, and the criterion for success was the autonomous vehicle's capability of traveling along the planned path safely, from the start to the destination, without colliding with obstacles at the speed generating l g acceleration upon turning. For the vehicle wheelbase L = 26 cm, the distance from the rear axle to the forward anchor point l = 6 cm, V cmd = 1 m/s, L f = 50 cm, and the control gains K p = 0.3 and K i = 0.04, the vehicle followed the planned path of the improved RRT algorithm to the destination on the upper right in Figure 7b. The vehicle trajectory was captured by an observer camera for verifying the improved algorithm. The deviation of the vehicle trajectory from the planned path was defined as the tracking error, as shown in Figure 8a. For the two obstacles in Figure 7b, the tracking errors shown in Figure 8b had an average tracking error of 4.9% and 4.7% of the vehicle width in the two obstacle environments, respectively. The maximum deviation of 12.6% and 20.5%, respectively, was from the vehicle's initial heading not aligning relative to the planned path, such that the vehicle had to maneuver to align with the path. The results show that the vehicle can track the planned path to the destination without colliding with obstacles. The tracking controller and the speed controller, though classical, were effective. For applications in robotics, some systems may have driving wheels with differential speeds or universal wheels. The vehicle dynamics will definitely have an influence on the controller design. Nevertheless, the path planning by the improved RRT algorithm remains applicable, for the predicted path was continuously differentiable, smooth, and efficient. Lane change is critical to the development of autonomous vehicles, and it can be modeled as obstacle avoidance. A vehicle parked by the side of the road or a stalled vehicle in the lane ahead require similar responses to obstacle avoidance, and lane change is necessary. Determining the time to execute lane change [36] and following the lane after lane change [37] are also considered obstacle avoidance with regard to autonomous vehicles. With the advent of vehicular social networks, busy traffic spots can be modeled as obstacles. Risk assessment for collision avoidance of nearby obstacles/vehicles will be desirable in future autonomous vehicle development [38]. A decision-making algorithm of risk assessment for collision avoidance was recently proposed for vehicles with different driving style preferences [39]. In this work, the improved RRT algorithm was also verified by the experiment that combined lane change and lane keeping. Lane change is critical to the development of autonomous vehicles, and it can be modeled as obstacle avoidance. A vehicle parked by the side of the road or a stalled vehicle in the lane ahead require similar responses to obstacle avoidance, and lane change is necessary. Determining the time to execute lane change [36] and following the lane after lane change [37] are also considered obstacle avoidance with regard to autonomous vehicles. With the advent of vehicular social networks, busy traffic spots can be modeled as obstacles. Risk assessment for collision avoidance of nearby obstacles/vehicles will be desirable in future autonomous vehicle development [38]. A decision-making algorithm of risk assessment for collision avoidance was recently proposed for vehicles with different driving style preferences [39]. In this work, the improved RRT algorithm was also verified by the experiment that combined lane change and lane keeping. The result also validated that the improved RRT algorithm was efficient in path planning, and it was also effective in combined lane change and lane keeping. It should be noted that the proposed algorithm may be limited by the time needed to re-plan a safe path should an obstacle "suddenly" appear ahead. Care then has to be taken in risk assessment [38,39]. It should be noted that the proposed algorithm may be limited by the time needed to re-plan a safe path should an obstacle "suddenly" appear ahead. Care then has to be taken in risk assessment [38,39].

Conclusions
An improved RRT algorithm was developed for the path planning of autonomous vehicles in static obstacle avoidance. The algorithm integrates (a) the pruning process with geometric collision detection to reach an efficient and collision-free safe path, (b) the smoothing process by the quadratic Bézier curve to obtain a continuously differentiable

Conclusions
An improved RRT algorithm was developed for the path planning of autonomous vehicles in static obstacle avoidance. The algorithm integrates (a) the pruning process with geometric collision detection to reach an efficient and collision-free safe path, (b) the smoothing process by the quadratic Bézier curve to obtain a continuously differentiable path for vehicle implementation, and (c) the optimization process to select the relatively superior path in terms of path length as the final path. Simulation results show that the improved RRT algorithm can plan a collision-free, safe path from the start to the destination in multiple obstacle environments. It has been shown that the pruning process would substantially reduce the number of turning points in the path by 99% compared with the RRT algorithm. The smoothing process by the quadratic Bézier curves with the control points set at Fibonacci number would further avoid the "sharp" turns along the path distance.
For the path planning example in Figure 5 with the search repeated 10 times, the success rate in the optimization process of finding a desired path is 96%. The path length is reduced by 34% after pruning and by another 33% after optimization. In the experimental verification on autonomous vehicles, a pure pursuit controller and a PI controller were applied to track the desired, planned path by the improved RRT algorithm. For a vehicle speed of 1 m/s within l g acceleration upon turning, it was experimentally validated that the vehicle could track the planned path to reach the destination safely. The average tracking deviation in the two environments was 4.9% and 4.7%, respectively, of the vehicle width. Note that the deviation can be further reduced by advanced controller(s), yet for path planning through a maze of narrow corridors, the constraint of vehicle dynamics of minimum turning radius should be investigated.
Lane change is considered similar to obstacle avoidance in autonomous vehicles. The combined lane change and lane keeping was also verified by the experiment. After the lane change, the discrepancies in lane keeping remained within 8.3% of the vehicle width. The results show that the improved RRT algorithm can also be applied to combined lane change and lane keeping. It should be noted that the proposed algorithm may be limited by the time needed to re-plan a safe path should an obstacle "suddenly" appear ahead. With the advent of vehicular social networks, busy traffic spots can be modeled as obstacles. Risk assessment for collision avoidance of nearby obstacles/vehicles [39] will be desirable in future autonomous vehicle development.

Data Availability Statement:
The data presented in this study are available upon request.