# Path Planning for Autonomous Vehicle Based on a Two-Layered Planning Model in Complex Environment

Authors: Jiajia Chen; Rui Zhang; Wei Han; Wuhua Jiang; Jinfang Hu; Xiaoshan Lu; Xingtao Liu; Pan Zhao
Journal: Journal of Advanced Transportation
 (2020)

---

## Abstract

The autonomous vehicle consists of perception, decision-making, and control system. The study of path planning method has always been a core and difficult problem, especially in complex environment, due to the effect of dynamic environment, the safety, smoothness, and real-time requirement, and the nonholonomic constraints of vehicle. To address the problem of travelling in complex environments which consists of lots of obstacles, a two-layered path planning model is presented in this paper. This method includes a high-level model that produces a rough path and a low-level model that provides precise navigation. In the high-level model, the improved Bidirectional Rapidly-exploring Random Tree (Bi-RRT) based on the steering constraint is used to generate an obstacle-free path while satisfying the nonholonomic constraints of vehicle. In low-level model, a Vector Field Histogram- (VFH-) guided polynomial planning algorithm in Frenet coordinates is introduced. Based on the result of VFH, the aim point chosen from improved Bi-RRT path is moved to the most suitable location on the basis of evaluation function. By applying quintic polynomial in Frenet coordinates, a real-time local path that is safe and smooth is generated based on the improved Bi-RRT path. To verify the effectiveness of the proposed planning model, the real autonomous vehicle has been placed in several driving scenarios with different amounts of obstacles. The two-layered real-time planning model produces flexible, smooth, and safe paths that enable the vehicle to travel in complex environment.


---

## Body

## 1. Introduction

Nowadays, mobile robots represented by autonomous vehicles have been widely used in transportation, agriculture, and industry [1]. The autonomous vehicle consists of perception, decision-making, and control system, and the study of robot planning system has always been a core problem. One of the key issues is how to generate an appropriate path to reach a goal position without any collision for autonomous vehicle [2–4].In the previous studies, two categories of searching methods can be found: geometric methods and graph search methods [5]: the calculation of geometry method is very simple, reasonable, and practical, such as spline curve, and it is adaptive to the simple environments. Although the geometric methods have some advantages, these methods cannot cover up their disadvantages. In the situations of complex dynamic environments, the intelligence and adaption of these path planning methods need to be enhanced [6].When searching path based on graph, the ability of the search algorithms that are widely used has been shown in mobile robot path planning. RRT is to perform forward search in continuous coordinates [7]. This algorithm can search rapidly, but when the environment is complex, this algorithm cannot be widely used in the narrow passages. A∗ algorithm is effective to find a shortest obstacle-free path on basis of a certain decision criteria [8], but the generated path is always composed of straight lines that are hard to execute. D∗ algorithm has been proposed with the aim of navigating autonomous vehicles in the environment of 2D, and the main advantage of D∗ method is that it can find an optimal path when navigating in dynamic environment [9]. This method is often constrained with vehicle kinematic.At Defense Advanced Research Projects Agency (DARPA) urban challenge 2007, the path planning ability of many research units has been demonstrated. The design scheme and theoretical analysis set up a base for further research. MIT developed a “Talos” vehicle and a path planning method based on a closed-loop RRT [10] is used. Dolgov et al. [11] developed a well-known hybrid A∗ searching method. In their approach, the 3D kinematic state space of the vehicle and nonlinear optimization for local planning are all used, which lead to a local optimum. In complex dynamic environment, these path planning methods implemented in urban road do not prove their practicability and effectiveness.In recent years, in order to avoid multiple dynamic obstacles, FSVM has been used by Suresh et al. [12] to secure a collision-free path. The results draw a conclusion that this method is effective. This method is proved in simulations by constructing the fuzzy rules using simple evaluation data. A real-time path planning algorithm has been put into application by Chu et al. [13]. After generating some paths on the basis of predefined waypoints, the optimal function deals with the selection of final safe and smooth path. Makarem and Gillet [14] have developed a method that contains a navigation function suitable for autonomous vehicle but does not consider the influence of obstacles. “Tentacle method” [15] is another way that has been proposed by Chebly et al. By considering the vehicle as the origin, the method develops a set of virtual tentacles that indicate possible paths for vehicle. Then, the best path is chosen according to the evaluation function. In complex dynamic environment, Moreau et al. [16] have investigated an improved curve planning method. All sensors required for obstacle detection are considered in an autonomous vehicle. Taking equality constraints into account, the planning problem is transferred to an optimal problem. Then, Lagrangian and gradient-based methods are used to cover it. In [17], Tazir et al. make a real-time planning based on the combination of two methods. The robot uses genetic algorithms and Dijkstra algorithm to avoid static obstacles. The wait/accelerate concept leads to travel in local dynamic environment. This method is test-efficient but lacks consideration of robot kinematic constraints. In order to let autonomous vehicles drive along a basic path and at the same time avoid obstacles and meet the vehicle kinematic constraints, researchers have proposed an original integrated local trajectory planning and tracking control (ILTPTC) framework [18]. In this framework, MPC based planning method is used, which could satisfy the requirement of vehicle kinematic but cannot meet the need of real time.In simulations or simple environments, the path planning methods developed by scholars are easy to handle the navigation problem. Despite the fact that doable paths can be found by using these traditional methods, it is difficult to meet the requirement of the involved vehicle dynamic and fluxional constraints. In complex dynamic environment, when carrying out the path planning method, the following must be considered: (1) The path is often planned once. The path is not suitable when change occurs in dynamic environment, and the changed environment cannot influence the performance of vehicle. (2) To ensure safety, a secure clearance barrier must be generated and verified. (3) There is a real-time performance of planning method. (4) The maximum curvature of generated smooth path must be less than the steering curvature of vehicle. (5) The execution error of control leads to the failure of planning, and the path must be trackable [19].To address these problems, this paper proposed a two-layered path planning model. This method includes a high-level model that produces a rough path and a low-level model that provides precise navigation according to the rough route computed previously. In the high-level model, the improved Bi-RRT [20, 21] based on steering constraints is used to generate an obstacle-free path while satisfying the nonholonomic constraints of vehicle; the result path guides the planning of low level. When executing improved Bi-RRT, the searching speed and vehicle constraints can be easily considered. The vehicle steering constraints are introduced when the nodes of Bi-RRT are extended. The Bi-RRT with two trees searching in the same time can speed up the planning obviously. In the low-level model, the planning is refining, and the obstacle avoidance, dynamic scene, smoothness, and real-time performance all need to be considered. With these constraints, a Vector Field Histogram (VFH) guided polynomial planning method based on Frenet coordinates is introduced. Borenstein and Koren have developed the VFH path planning algorithm in order to guide the robot CARMEL [22]. The candidate directions around the vehicle have been taken into consideration by VFH. After calculating the effect of surround obstacles, the appropriate direction is chosen. This method can handle the cluttered obstacle scenarios well [23].In this paper, the Frenet coordinates system is applied to generate local paths [24]. At any point along the curve, the Frenet frame coordinates can be computed analytically. In the low-level model, the vehicle position is transformed to the Frenet coordinates based on the improved Bi-RRT path. According to the result of VFH, the aim point chosen from the improved Bi-RRT path is moved to the most suitable location. Then, a local path is generated from the vehicle to the moved aim point using the polynomial planning method in Frenet coordinates. The method presented in this paper is insensitive when environmental change occurs; also the safety and smoothness of path are guaranteed.The rest of this paper is organized as follows: Section2 briefly introduces the system architecture, especially the two-layered planning model framework for autonomous vehicle. Section 3 proposes the improved Bi-RRT planning method in high-level model. Section 4 describes the planning method in low-level model; in this section, a VFH guided polynomial planning method based on Frenet coordinates is detailed. The simulation of two-layered path planning model is shown in Section 5. The description of experiments, results, and future work are provided in Section 6. Finally, the conclusions are given in Section 7.


## 2. Two-Layered Path Planning Model Structure

Figure1(a) shows the experimental autonomous vehicle with a suite of sensors. There are one camera, one VLP-16 lidar, and a GPS positioning receiver. These sensors consist of the perception system.Figure 1
(a) The sensor configuration of the autonomous vehicle. (b) The software architecture of planning system.
(a)(b)The perception system generates a grid map and passes the map to the decision-making system. According to the cooperation of global path, maneuver, and path planning subsystem in decision-making system, the vehicle is able to deal with different scenarios. Finally, the decision-making system produces a path and transfers the path to the control system of vehicle.Figure2 illustrates the model structure of the proposed method. The input of high-level model is a global grid map and GPS positioning information. The effective lidar scanning range is about 50 meters; all detected objects are filled to the global grid map, although there may be some inaccuracy caused by the sensor noise. The high-level model based on steering constraint quickly generates a collision-free path and transmits the path to the low-level model.Figure 2
The two-layered model architecture.In low-level model, besides the improved Bi-RRT path, the local grid map which is 20 m × 20 m and GPS information are all treated as the input. The local grid map is changing in real time and this close-range map is accurate. According to the preview follower theory [25, 26], the aim point that is treated as the planning end point is chosen dynamically from the improved Bi-RRT path with different speed of vehicle. Due to the noise of global grid map or the drawback of Bi-RRT method, the aim point may not be suitable, and perhaps it is too close to the barrier. Then, AFH method is introduced to find out the most suitable direction. After obtaining the best aim point, Frenet coordinates are set up on the grounds of base line generated by improved Bi-RRT. In Frenet space, the final local path is obtained through the polynomial planning method.


## 3. The High-Level Path Planning Method Based on Improved Bi-RRT

### 3.1. Basic Bi-RRT Algorithm

Construct the search tree simultaneously in the initial state and the target state is a principle of Bi-RRT. The initiative procedure of Bi-RRT is not different from the basic RRT algorithm, as shown in Figures3 and 4, and the tree is expanded after generating random points. The search process of Bi-RRT is shown in Figure 3. Figure 5 shows the searching schematic diagram of basic Bi-RRT algorithm. The basic Bi-RRT algorithm that is more effective is shown in Figure 4.Figure 3
Basic RRT algorithm.Figure 4
Basic Bi-RRT algorithm.Figure 5
Schematic diagram of basic Bi-RRT algorithm.


### 3.2. Vehicle Kinematical Model

The nonholonomic constraint of autonomous vehicle must be considered in complex environment, owing to the fact that there are always some tortuous lines in final path. Once the curvature of path exceeds the maximum curvature of vehicle, the control system is hard to deal with the path tracking problem. Observing Figure6, it is a simplified model of vehicle. The kinematics equation can be described as follows and Table 1 shows the parameters of vehicle.(1)x.y.z.=cosθsinθkv.Figure 6
Vehicle kinematics model.Table 1
The parameters of vehicle.
ParameterDefinitionsLWheelbaseθThe orientation of vehicleφThe front wheel steering wheelRThe vehicle curvature radiusvThe vehicle linear velocityωThe vehicle yaw rateAccording to the kinematic equation mentioned above, the radius and the radius change rate can be explained as(2)k≤kmax=1Ltanϕmax,σ≤σmax=1Lωmax≤1Lcos2ϕωmax.In the expansion of Bi-RRT nodes, the steering radius of vehicle should be considered.


### 3.3. Improved Bi-RRT Algorithm

Based on the above analysis of vehicle path planning problem, this paper proposes a set of path planning algorithms suitable for autonomous vehicles based on the RRT framework. The steps of the algorithm are shown as the improved Bi-RRT algorithm. There are two main improvements in the algorithm, as Figure7 shows. First, the sampling space is limited by the vehicle steering constraint, which can avoid searching in the entire sampling space, thus speeding up the convergence speed of the algorithm. The second point is to restrict the generation of nodes according to the steering constraints, so that the generated paths are more in line with the dynamic constraints of the vehicle.Figure 7
Improved Bi-RRT algorithm.


#### 3.3.1. A Probabilistic Target Bias Sampling Strategy Based on Steering Constraints

Rapidly-exploring Random Tree planning method or its expansion is a random search in the global scope during the random sampling process, which will lead to a lot of unnecessary searches and waste computing resources. To solve this problem, in order to speed up the random tree to the target point, the improved method used in this paper is as follows: in each growth process of the random tree, the sampling space is limited based on the steering constraint, thereby avoiding a lot of unnecessary searches. As shown in Figure8, for the entire T-tree, when looking for the nearest neighbour, it is not necessary to search the entire tree. The permissible steering area for the vehicle is determined in the generated subtree, and then the nearest point is found from the corresponding area.Figure 8
A search based on the steering constraint.To enable the initial state and the target state to meet faster, this paper uses the target bias strategy in the initial expansion stage of the random number. The random probability is used to determine whetherqrand is the target point or the random point. Set a target bias threshold qprob, and then get a stochastic number from zero to one when expanding. When 0 < p < qprob, the random tree grows toward the target point; when qprob < p < 1, the random tree grows in a random direction. This strategy can explore the space faster and more effectively.


#### 3.3.2. The Extension Strategy with Steering Constraints

Considering the dynamic constraints of vehicles, this paper presents that, when searching for the nearest point, firstly, reduce the sampling space based on the steering constraint of vehicle, and then K-D tree is applied to search the nearest points using the Euclidean distance. Finally, the closest point can meet the maximum curvature constraint. As shown in Figure9, although the Euclidean distance between qi and qrand is closer, because θi > θj, qj is selected as the nearest neighbour node. In this way, the requirements for nonholonomic constraints of the vehicle will be satisfied by the obtained path. The metric function between random sampling point and adjacent point is defined as follows:(3)Dist=w1D+w2A,where D and A are the normalization values of Euclidean distance and angle, respectively. Based on the experimental data, w1 = 0.55 and w2 = 0.45 were set in this paper. D and A, respectively, are(4)D=dmax−didmax;A=θmax−θiθmax.Figure 9
Schematic diagram for selecting the nearest node.


#### 3.3.3. Postprocessing Method

Due to the random sampling of RRT algorithm, the generated paths are usually very tortuous and extremely unsmooth, especially in complex obstacle environments. It is difficult to effectively track them. Meanwhile, too many fold points will affect the ride comfort of the vehicle; it is unacceptable for path planning. Therefore, pruning and smoothing of the improved Bi-RRT path are required.According to the maximum curvature constraint and the continuity of curvature, the safe nodes in the whole T-tree are deleted to achieve the pruning effect. As shown in Figure10, q1 to q8 and q10 to q15 are safe, so q1 to q8 and q10 to q15 can be connected directly. If the pruned curve does not satisfy the maximum curvature constraint, the necessary nodes are inserted. The contrast effect before and after pruning is shown in Figure 11.Figure 10
Pruning principle.Figure 11
Schematic diagram of algorithm (a) before and (b) after pruning.


### 3.4. Simulations of Improved Bi-RRT

To test the effectiveness and adaptability of improved Bi-RRT planning method in high-level model, the simulations are applied. Assume that the grey and black points mean the start and end point separately. The black line refers to the planned path.Figures12 and 13 indicate the Bi-RRT and improved Bi-RRT planning results in simple or complex obstacle environments. It can be observed that the improved Bi-RRT method reduces expanding nodes successfully, so the calculation time is reduced. The final path not only satisfies the requirements of vehicle kinematic constraints but also is very smooth.Figure 12
The basic and improved Bi-RRT algorithm in simple environment. (a) The basic Bi-RRT method. (b) The improved Bi-RRT method.
(a)(b)Figure 13
The basic and improved Bi-RRT algorithm in complex environment. (a) The basic Bi-RRT method. (b) The improved Bi-RRT method.
(a)(b)


## 3.1. Basic Bi-RRT Algorithm

Construct the search tree simultaneously in the initial state and the target state is a principle of Bi-RRT. The initiative procedure of Bi-RRT is not different from the basic RRT algorithm, as shown in Figures3 and 4, and the tree is expanded after generating random points. The search process of Bi-RRT is shown in Figure 3. Figure 5 shows the searching schematic diagram of basic Bi-RRT algorithm. The basic Bi-RRT algorithm that is more effective is shown in Figure 4.Figure 3
Basic RRT algorithm.Figure 4
Basic Bi-RRT algorithm.Figure 5
Schematic diagram of basic Bi-RRT algorithm.


## 3.2. Vehicle Kinematical Model

The nonholonomic constraint of autonomous vehicle must be considered in complex environment, owing to the fact that there are always some tortuous lines in final path. Once the curvature of path exceeds the maximum curvature of vehicle, the control system is hard to deal with the path tracking problem. Observing Figure6, it is a simplified model of vehicle. The kinematics equation can be described as follows and Table 1 shows the parameters of vehicle.(1)x.y.z.=cosθsinθkv.Figure 6
Vehicle kinematics model.Table 1
The parameters of vehicle.
ParameterDefinitionsLWheelbaseθThe orientation of vehicleφThe front wheel steering wheelRThe vehicle curvature radiusvThe vehicle linear velocityωThe vehicle yaw rateAccording to the kinematic equation mentioned above, the radius and the radius change rate can be explained as(2)k≤kmax=1Ltanϕmax,σ≤σmax=1Lωmax≤1Lcos2ϕωmax.In the expansion of Bi-RRT nodes, the steering radius of vehicle should be considered.


## 3.3. Improved Bi-RRT Algorithm

Based on the above analysis of vehicle path planning problem, this paper proposes a set of path planning algorithms suitable for autonomous vehicles based on the RRT framework. The steps of the algorithm are shown as the improved Bi-RRT algorithm. There are two main improvements in the algorithm, as Figure7 shows. First, the sampling space is limited by the vehicle steering constraint, which can avoid searching in the entire sampling space, thus speeding up the convergence speed of the algorithm. The second point is to restrict the generation of nodes according to the steering constraints, so that the generated paths are more in line with the dynamic constraints of the vehicle.Figure 7
Improved Bi-RRT algorithm.


### 3.3.1. A Probabilistic Target Bias Sampling Strategy Based on Steering Constraints

Rapidly-exploring Random Tree planning method or its expansion is a random search in the global scope during the random sampling process, which will lead to a lot of unnecessary searches and waste computing resources. To solve this problem, in order to speed up the random tree to the target point, the improved method used in this paper is as follows: in each growth process of the random tree, the sampling space is limited based on the steering constraint, thereby avoiding a lot of unnecessary searches. As shown in Figure8, for the entire T-tree, when looking for the nearest neighbour, it is not necessary to search the entire tree. The permissible steering area for the vehicle is determined in the generated subtree, and then the nearest point is found from the corresponding area.Figure 8
A search based on the steering constraint.To enable the initial state and the target state to meet faster, this paper uses the target bias strategy in the initial expansion stage of the random number. The random probability is used to determine whetherqrand is the target point or the random point. Set a target bias threshold qprob, and then get a stochastic number from zero to one when expanding. When 0 < p < qprob, the random tree grows toward the target point; when qprob < p < 1, the random tree grows in a random direction. This strategy can explore the space faster and more effectively.


### 3.3.2. The Extension Strategy with Steering Constraints

Considering the dynamic constraints of vehicles, this paper presents that, when searching for the nearest point, firstly, reduce the sampling space based on the steering constraint of vehicle, and then K-D tree is applied to search the nearest points using the Euclidean distance. Finally, the closest point can meet the maximum curvature constraint. As shown in Figure9, although the Euclidean distance between qi and qrand is closer, because θi > θj, qj is selected as the nearest neighbour node. In this way, the requirements for nonholonomic constraints of the vehicle will be satisfied by the obtained path. The metric function between random sampling point and adjacent point is defined as follows:(3)Dist=w1D+w2A,where D and A are the normalization values of Euclidean distance and angle, respectively. Based on the experimental data, w1 = 0.55 and w2 = 0.45 were set in this paper. D and A, respectively, are(4)D=dmax−didmax;A=θmax−θiθmax.Figure 9
Schematic diagram for selecting the nearest node.


### 3.3.3. Postprocessing Method

Due to the random sampling of RRT algorithm, the generated paths are usually very tortuous and extremely unsmooth, especially in complex obstacle environments. It is difficult to effectively track them. Meanwhile, too many fold points will affect the ride comfort of the vehicle; it is unacceptable for path planning. Therefore, pruning and smoothing of the improved Bi-RRT path are required.According to the maximum curvature constraint and the continuity of curvature, the safe nodes in the whole T-tree are deleted to achieve the pruning effect. As shown in Figure10, q1 to q8 and q10 to q15 are safe, so q1 to q8 and q10 to q15 can be connected directly. If the pruned curve does not satisfy the maximum curvature constraint, the necessary nodes are inserted. The contrast effect before and after pruning is shown in Figure 11.Figure 10
Pruning principle.Figure 11
Schematic diagram of algorithm (a) before and (b) after pruning.


## 3.3.1. A Probabilistic Target Bias Sampling Strategy Based on Steering Constraints

Rapidly-exploring Random Tree planning method or its expansion is a random search in the global scope during the random sampling process, which will lead to a lot of unnecessary searches and waste computing resources. To solve this problem, in order to speed up the random tree to the target point, the improved method used in this paper is as follows: in each growth process of the random tree, the sampling space is limited based on the steering constraint, thereby avoiding a lot of unnecessary searches. As shown in Figure8, for the entire T-tree, when looking for the nearest neighbour, it is not necessary to search the entire tree. The permissible steering area for the vehicle is determined in the generated subtree, and then the nearest point is found from the corresponding area.Figure 8
A search based on the steering constraint.To enable the initial state and the target state to meet faster, this paper uses the target bias strategy in the initial expansion stage of the random number. The random probability is used to determine whetherqrand is the target point or the random point. Set a target bias threshold qprob, and then get a stochastic number from zero to one when expanding. When 0 < p < qprob, the random tree grows toward the target point; when qprob < p < 1, the random tree grows in a random direction. This strategy can explore the space faster and more effectively.


## 3.3.2. The Extension Strategy with Steering Constraints

Considering the dynamic constraints of vehicles, this paper presents that, when searching for the nearest point, firstly, reduce the sampling space based on the steering constraint of vehicle, and then K-D tree is applied to search the nearest points using the Euclidean distance. Finally, the closest point can meet the maximum curvature constraint. As shown in Figure9, although the Euclidean distance between qi and qrand is closer, because θi > θj, qj is selected as the nearest neighbour node. In this way, the requirements for nonholonomic constraints of the vehicle will be satisfied by the obtained path. The metric function between random sampling point and adjacent point is defined as follows:(3)Dist=w1D+w2A,where D and A are the normalization values of Euclidean distance and angle, respectively. Based on the experimental data, w1 = 0.55 and w2 = 0.45 were set in this paper. D and A, respectively, are(4)D=dmax−didmax;A=θmax−θiθmax.Figure 9
Schematic diagram for selecting the nearest node.


## 3.3.3. Postprocessing Method

Due to the random sampling of RRT algorithm, the generated paths are usually very tortuous and extremely unsmooth, especially in complex obstacle environments. It is difficult to effectively track them. Meanwhile, too many fold points will affect the ride comfort of the vehicle; it is unacceptable for path planning. Therefore, pruning and smoothing of the improved Bi-RRT path are required.According to the maximum curvature constraint and the continuity of curvature, the safe nodes in the whole T-tree are deleted to achieve the pruning effect. As shown in Figure10, q1 to q8 and q10 to q15 are safe, so q1 to q8 and q10 to q15 can be connected directly. If the pruned curve does not satisfy the maximum curvature constraint, the necessary nodes are inserted. The contrast effect before and after pruning is shown in Figure 11.Figure 10
Pruning principle.Figure 11
Schematic diagram of algorithm (a) before and (b) after pruning.


## 3.4. Simulations of Improved Bi-RRT

To test the effectiveness and adaptability of improved Bi-RRT planning method in high-level model, the simulations are applied. Assume that the grey and black points mean the start and end point separately. The black line refers to the planned path.Figures12 and 13 indicate the Bi-RRT and improved Bi-RRT planning results in simple or complex obstacle environments. It can be observed that the improved Bi-RRT method reduces expanding nodes successfully, so the calculation time is reduced. The final path not only satisfies the requirements of vehicle kinematic constraints but also is very smooth.Figure 12
The basic and improved Bi-RRT algorithm in simple environment. (a) The basic Bi-RRT method. (b) The improved Bi-RRT method.
(a)(b)Figure 13
The basic and improved Bi-RRT algorithm in complex environment. (a) The basic Bi-RRT method. (b) The improved Bi-RRT method.
(a)(b)


## 4. The Low-Level Path Planning Method Based on VFH-Guided Polynomial Planning in Frenet Coordinates

### 4.1. Local Aim Point Selection Based on VFH

In local path planning, the point that is chosen from the improved Bi-RRT path is treated as the local aim point. In dynamic environment, it is hard to make a precise description about the scenario in a big range, so the aim point must be adjusted in real time.VFH algorithm will divide the environment around the vehicle into many small pieces, called angular sectors. In each angular sector, the risk magnitude can be calculated and then polar histogram is obtained. The best sector is chosen according to the polar histogram and distance to origin aim point finally. This method is calculated fast.Instead of using the entire grid map of sensor range, only a small circle around the vehicle can be used to determine the new direction of movement. This space is called the active window. The vehicle is located in the center of the window, the window can move with the vehicle, and its shape is a square or circle. In this section, the circle is introduced for VFH.


#### 4.1.1. Active Region

Observing Figure14, a moving circle window of Ld × Ld called active region is introduced. The sectors around the vehicle are not all approachable for vehicle, due to the kinematic constraints. When these constraints are considered, the new active regions are built [27]. In Figure 15, rmin is the turning radius and s represents the searching step. The effective angle of VFH is defined as follows:(5)ϕ∈−arcsins2rmin,arcsins2rmin,where φ denotes the sector angle. Figure 16 shows the occupancy grid map used in AFH method. All the perception data including the static and moving obstacles are rendered in the grid map. The red grids mean there are obstacles located in the region and the white grids indicate that these regions are obstacle-free. The sectors that consisted of the dark blue grids represent that it is more dangerous for vehicle to travel than other sectors, and the sectors including the light blue grids indicate that these regions are safe for autonomous driving.Figure 14
Original active regions.Figure 15
The active region of autonomous vehicle.Figure 16
The local searching grid map.


#### 4.1.2. Polar Histogram

The scanning radius of lidar isdmax, and the angular resolution of each sector is chosen as 10° in this paper. When the nearest distance between vehicle and obstacles d < dmax, the risk magnitude of each region is defined as(6)Hi=cv2a−bd2,where cv, a, and b are constant; a=bdmax2.Polar histogram consists of risk magnitude with each sector. The direction of vehicle should be chosen from the sectors whose risk magnitudes are lower. According to the result of polar histogram and distance from the sector to the origin aim point, new aim point is moved to the most suitable region around the vehicle according to the following principle.(7)Si=w1Hi+w2Ai,where Ai means the distance between sectors and the origin aim point.


### 4.2. Local Path Planning Based on Polynomial Planning Method in Frenet Coordinates

The positions of the autonomous vehicle in the global coordinates system and Frenet coordinates system are shown in Figure17. The Frenet coordinates system is established based on a given reference line (denoted as Tref). Tref can be any curve; in this paper, it is the improved Bi-RRT path. Suppose that the coordinates of the autonomous vehicle in the global coordinates system are (x, y), and the projection point is F from the vehicle position (x, y) to reference line. The distance between point F and vehicle position (x, y) is the lateral displacement d. The curve distance from the start point of the reference line to the projection point F is the longitudinal displacement s. (s, d) is used to describe the coordinates values of autonomous vehicle in Frenet coordinates system, and the mapping relationship is constructed as follows:(8)x,yTref⇔s,d.Figure 17
Frenet coordinates system.Based on Frenet coordinates system, the position state of autonomous vehicles is decomposed into the direction ofs and d to describe the motion state of the vehicle. For each original coordinates point (xv,yv), the corresponding mapping point (xp, yp) on the reference trajectory is determined, and the Frenet coordinates (sv,lv) are obtained by(9)lv=xp−xv2+yp−yv2,sv=Sxp,yp.Local path generation problem is divided into the direction ofs (longitudinal) and d (lateral); in this paper, only the d direction is taken into consideration; thus the vehicle is assumed at a constant speed.A polynomial of degree 5 generated in Frenet coordinates is used as the path planning method in low-level. The vehicle position is treated as the start point and the local aim point is chosen from the improved Bi-RRT path [28]. The polynomial path is calculated fast and smooth:(10)pt=α0+α1t+α2t2+α3t3+α4t4+α5t5.Given the initial configurationD0 = {d0,d0⋅,d0⋅⋅}, target configuration D1 = {d1,d1⋅,d1⋅⋅}, and braking time T, the coefficient of quintic polynomial in d direction with respect to time t can be calculated. d0,d0⋅,d0⋅⋅ represent lateral offset, lateral velocity, and lateral acceleration, respectively.


## 4.1. Local Aim Point Selection Based on VFH

In local path planning, the point that is chosen from the improved Bi-RRT path is treated as the local aim point. In dynamic environment, it is hard to make a precise description about the scenario in a big range, so the aim point must be adjusted in real time.VFH algorithm will divide the environment around the vehicle into many small pieces, called angular sectors. In each angular sector, the risk magnitude can be calculated and then polar histogram is obtained. The best sector is chosen according to the polar histogram and distance to origin aim point finally. This method is calculated fast.Instead of using the entire grid map of sensor range, only a small circle around the vehicle can be used to determine the new direction of movement. This space is called the active window. The vehicle is located in the center of the window, the window can move with the vehicle, and its shape is a square or circle. In this section, the circle is introduced for VFH.


### 4.1.1. Active Region

Observing Figure14, a moving circle window of Ld × Ld called active region is introduced. The sectors around the vehicle are not all approachable for vehicle, due to the kinematic constraints. When these constraints are considered, the new active regions are built [27]. In Figure 15, rmin is the turning radius and s represents the searching step. The effective angle of VFH is defined as follows:(5)ϕ∈−arcsins2rmin,arcsins2rmin,where φ denotes the sector angle. Figure 16 shows the occupancy grid map used in AFH method. All the perception data including the static and moving obstacles are rendered in the grid map. The red grids mean there are obstacles located in the region and the white grids indicate that these regions are obstacle-free. The sectors that consisted of the dark blue grids represent that it is more dangerous for vehicle to travel than other sectors, and the sectors including the light blue grids indicate that these regions are safe for autonomous driving.Figure 14
Original active regions.Figure 15
The active region of autonomous vehicle.Figure 16
The local searching grid map.


### 4.1.2. Polar Histogram

The scanning radius of lidar isdmax, and the angular resolution of each sector is chosen as 10° in this paper. When the nearest distance between vehicle and obstacles d < dmax, the risk magnitude of each region is defined as(6)Hi=cv2a−bd2,where cv, a, and b are constant; a=bdmax2.Polar histogram consists of risk magnitude with each sector. The direction of vehicle should be chosen from the sectors whose risk magnitudes are lower. According to the result of polar histogram and distance from the sector to the origin aim point, new aim point is moved to the most suitable region around the vehicle according to the following principle.(7)Si=w1Hi+w2Ai,where Ai means the distance between sectors and the origin aim point.


## 4.1.1. Active Region

Observing Figure14, a moving circle window of Ld × Ld called active region is introduced. The sectors around the vehicle are not all approachable for vehicle, due to the kinematic constraints. When these constraints are considered, the new active regions are built [27]. In Figure 15, rmin is the turning radius and s represents the searching step. The effective angle of VFH is defined as follows:(5)ϕ∈−arcsins2rmin,arcsins2rmin,where φ denotes the sector angle. Figure 16 shows the occupancy grid map used in AFH method. All the perception data including the static and moving obstacles are rendered in the grid map. The red grids mean there are obstacles located in the region and the white grids indicate that these regions are obstacle-free. The sectors that consisted of the dark blue grids represent that it is more dangerous for vehicle to travel than other sectors, and the sectors including the light blue grids indicate that these regions are safe for autonomous driving.Figure 14
Original active regions.Figure 15
The active region of autonomous vehicle.Figure 16
The local searching grid map.


## 4.1.2. Polar Histogram

The scanning radius of lidar isdmax, and the angular resolution of each sector is chosen as 10° in this paper. When the nearest distance between vehicle and obstacles d < dmax, the risk magnitude of each region is defined as(6)Hi=cv2a−bd2,where cv, a, and b are constant; a=bdmax2.Polar histogram consists of risk magnitude with each sector. The direction of vehicle should be chosen from the sectors whose risk magnitudes are lower. According to the result of polar histogram and distance from the sector to the origin aim point, new aim point is moved to the most suitable region around the vehicle according to the following principle.(7)Si=w1Hi+w2Ai,where Ai means the distance between sectors and the origin aim point.


## 4.2. Local Path Planning Based on Polynomial Planning Method in Frenet Coordinates

The positions of the autonomous vehicle in the global coordinates system and Frenet coordinates system are shown in Figure17. The Frenet coordinates system is established based on a given reference line (denoted as Tref). Tref can be any curve; in this paper, it is the improved Bi-RRT path. Suppose that the coordinates of the autonomous vehicle in the global coordinates system are (x, y), and the projection point is F from the vehicle position (x, y) to reference line. The distance between point F and vehicle position (x, y) is the lateral displacement d. The curve distance from the start point of the reference line to the projection point F is the longitudinal displacement s. (s, d) is used to describe the coordinates values of autonomous vehicle in Frenet coordinates system, and the mapping relationship is constructed as follows:(8)x,yTref⇔s,d.Figure 17
Frenet coordinates system.Based on Frenet coordinates system, the position state of autonomous vehicles is decomposed into the direction ofs and d to describe the motion state of the vehicle. For each original coordinates point (xv,yv), the corresponding mapping point (xp, yp) on the reference trajectory is determined, and the Frenet coordinates (sv,lv) are obtained by(9)lv=xp−xv2+yp−yv2,sv=Sxp,yp.Local path generation problem is divided into the direction ofs (longitudinal) and d (lateral); in this paper, only the d direction is taken into consideration; thus the vehicle is assumed at a constant speed.A polynomial of degree 5 generated in Frenet coordinates is used as the path planning method in low-level. The vehicle position is treated as the start point and the local aim point is chosen from the improved Bi-RRT path [28]. The polynomial path is calculated fast and smooth:(10)pt=α0+α1t+α2t2+α3t3+α4t4+α5t5.Given the initial configurationD0 = {d0,d0⋅,d0⋅⋅}, target configuration D1 = {d1,d1⋅,d1⋅⋅}, and braking time T, the coefficient of quintic polynomial in d direction with respect to time t can be calculated. d0,d0⋅,d0⋅⋅ represent lateral offset, lateral velocity, and lateral acceleration, respectively.


## 5. Simulations of Two-Layered Path Planning Model

A normal scenario is designed to show the effect of the proposed method mentioned above. In this scenario, there is one obstacle in the way; the vehicles must avoid the obstacle. In this simulation, the expansion step of improved Bi-RRT is set to 0.5 m and the angular resolution of VFH is set to 10°. The main parameters of polar histogram are shown in Table2.Table 2
The parameters of risk magnitude.
ParameterValueCv2a400b1In Figure18, the grey and black points mean the start and end point separately. The black line refers to improved Bi-RRT path. Although this method avoids the obstacle successfully, there may be no guarantees about the usage in dynamic environment.Figure 18
The Bi-RRT planning result in high-level model.Observing Figure19(a), in local planning based on polynomial, the blue and green points indicate the origin and moved aim point. The red line represents the local path generated in Frenet coordinates. In normal situations, there is no need to shift aim point because the current sector is the best direction. In Figure 19(a), the distance from origin aim point chosen from Bi-RRT path to obstacle is too near. According to the risk magnitude calculation results in different sectors in Figure 20, the suitable sector that is safe and keeps a short distance from origin aim point is chosen. The aim point is moved and the clearance between vehicle and obstacles is bigger, as shown in Figure 19(b).Figure 19
The main steps of low-level path planning methods.
(a)(b)Figure 20
The polar histogram based on risk magnitude.


## 6. Experimental Results and Discussion

We conduct a real experiment on the autonomous vehicle for the purpose of verifying the utilities of the path planning method. Figure21 shows a typical unstructured environment, which consists of lots of obstacles. Three typical scenarios including different amounts of obstacles are used in the experiments. In these scenarios, the speed of vehicle has been set at 15 miles per hour. The low-level planning subsystem does a real-time mission when perception information is updated. Under these conditions, the global grid map is 50 m × 50 m and the local grid map is 20 m × 20 m.Figure 21
A typical unstructured environment.The generated grid maps are plotted according to program. The obstacles are marked with black circles and the red-dotted line indicates the improved Bi-RRT path. The blue and green points are showing the original and moved aim points when necessary. The local path of vehicle which is executed finally is shown as a red solid line.When following the path generated in high-level model in Figures22(b), 23(b), and 24(b), the vehicle may be unsafe due to the incorrect perception results or tracking error. The vehicle must do a real-time planning based on the planning method in low-level model.Figure 22
The results of two-layered path planning method in scenario 1. (a) The experiment scenario. (b) The improved Bi-RRT planning result in high-level model. (c) The planning result of low-level model at timet0. (d) The planning result of low-level model at time t1.
(a)(b)(c)(d)Figure 23
The results of two-layered path planning method in scenario 2. (a) The experiment scenario. (b) The improved Bi-RRT planning result in high-level model. (c) The planning result of low-level model at timet1. (d) The planning result of low-level model at time t2.
(a)(b)(c)(d)Figure 24
The results of two-layered path planning method in scenario 3. (a) The experiment scenario. (b) The improved Bi-RRT planning result in high-level model. (c) The planning result of low-level model at timet1. (d) The planning result of low-level model at time t2. (e) The planning result of low-level model at time t3.
(a)(b)(c)(d)(e)In scenario 1, assume thatt0 is the start time. At time t0, the local planning result is shown in Figure 22(c). The chosen aim point from Bi-RRT path is not moved, because the risk magnitude of original travelling direction is less than others. At time t1, the vehicle is close to the obstacles, so the aim point is moved to the most suitable region based on the evaluation function. Figure 22(d) shows the planning result in local map.In scenario 2, at timet1 and time t2, Figures 23(c) and 23(d) indicate the local planning results. When approaching the obstacles, the risk magnitude is increased; the aim points are shifted to the safe location in local grid map.Scenario 3 is more complex than other scenarios. This region is small, while one person is around the end point, which makes the outlet narrower. At timet1, time t2, and time t3, the aim points based on improved Bi-RRT are both moved, as Figures 24(c)–24(e) show.The main evaluation indicators of path planning results are listed in Tables3 and 4. Table 3 shows the improved Bi-RRT planning indicators in high-level model and Table 4 shows the polynomial planning indicators in low-level model.Table 3
The main evaluation indicators of path planning results in high-level model.
ScenarioIndicatorPlanning time (ms)Minimum distance to obstacle (m)Max curvature (m−1)123.161.180.09225.761.090.1328.320.950.11Table 4
The main evaluation indicators of path planning results in low-level model.
MomentIndicatorPlanning time (ms)Minimum distance to obstacle (m)Max curvature (m−1)Scenario 1,t038.7450.056Scenario 1,t140.121.460.092Scenario 2,t141.561.380.094Scenario 2,t241.381.260.12Scenario 3,t140.791.400.096Scenario 3,t241.671.320.099Scenario 3,t342.681.220.13It can be observed that, in these scenarios of Table3, the max curvatures of these paths are 0.09 m−1, 0.1 m−1, and 0.11 m−1 separately, and the maximum steering curvature of our autonomous vehicle is 0.22 m−1. These results all meet the nonholonomic requirement of vehicle. Although the generated Bi-RRT paths avoid the obstacles successfully, the distances to the obstacle in Table 3 are not big enough to ensure the safety of vehicle.Observing Table4, the max curvature, minimum distance to the obstacles, and planning time in local planning are shown. At different times in these scenarios, the maximum curvatures of the generated paths are also satisfying the vehicle nonholonomic constraint. Due to the fact of planning in polynomial, the curvature is continuous so that the local path is smooth. Compared to Table 3, the minimum distance to obstacles is bigger due to the moved aim point; all the planning results have a safe clearance to obstacles. The local planning results are smooth and safe. According to Tables 3 and 4, the planning time of two-layered model is less than 100 ms, which conforms to the requirement of real-time planning.For the sake of verifying the effectiveness of the proposed method mentioned above, a variation of RRT method, RRT∗, has been implemented for comparison. Scenario 3 which is most complex is used. Figure 25 shows the planning result of RRT∗. Three main indicators of two methods are shown in Table 5. Observing the planning speed, two-layered planning method is slower than RRT∗ method, and this may be caused by the fact that the two-layered planning model has to plan twice. Because the RRT∗ method is not considering the steering radius when expanding new nodes, this method does not satisfy the kinematic constraints of vehicle. At last, considering the distance between paths and obstacles, the two-layered model is much safer.Figure 25
The planning result of RRT∗.Table 5
The main evaluation indicators of two contrastive path planning results.
ScenarioIndicatorPlanning time (ms)Minimum distance to obstacle (m)Max curvature (m−1)RRT∗32.561.080.30Two-layered planning71.321.220.13


## 7. Conclusions

For solving the path planning problem of vehicle when navigating in complex dynamic environments, this paper proposed a two-layered planning model. The high-level model produces a base path and the low-level model provides precise navigation according to the base route computed previously.The planning results show that this method can realize the navigation of autonomous vehicle with safe and smooth path under the condition of satisfying the nonholonomic constraint of vehicle in complex environment. The vehicle navigates successfully and properly in environments with different amounts of obstacles. Through the experiments, the planning time is not fast enough in a high speed; future works will focus on improving the planning speed and applying more scenarios for testing.



---

*Source: 6649867-2020-11-27.xml*