**4. Proposed approach**

In this work, a new sampling-based motion planner is introduced for autonomous vehicle navigation. The proposed method is based on the optimality concept of the RRT\* algorithm [18] and the low-dispersion concept of the LD-RRT algorithm [19]. Furthermore, this planner utilizes a novel procedure that divides the sampling domain based on the updates received from the vehicle's sensory system.


**Algorithm 1.** RRT\* Planner

The first component of the proposed planner is the RRT\* algorithm that can be described in **Algorithm 1**.

For instance, there is no proper method for takeover which is one of the most common behaviours in driving. Takeover is a complicated and critical task that sometimes is not avoidable and the motion planner should be able to plan takeovers while considering the safety issues. Furthermore, the running time of the planner is usually too high which makes the method less practical. Finally, the quality of the generated solutions, i.e. paths, is a critical issue. The quality of the resulted paths from sampling-based methods is normally low and finding

In this work, a new sampling-based motion planner is introduced for autonomous vehicle navigation. The proposed method is based on the optimality concept of the RRT\* algorithm [18] and the low-dispersion concept of the LD-RRT algorithm [19]. Furthermore, this planner utilizes a novel procedure that divides the sampling domain based on the updates received

The first component of the proposed planner is the RRT\* algorithm that can be described in

the optimal path is usually a challenging task in randomized algorithms.

**4. Proposed approach**

28 Autonomous Vehicle

from the vehicle's sensory system.

**Algorithm 1.** RRT\*

**Algorithm 1**.

Planner

**Figure 2.** The implementation of the boundary sampling in a simple 2D environment with three obstacles. The new sample (orange circle) will be generates on the boundary of the union of forbidden region after the occurrence of the first rejection.

In the RRT\* algorithm, the important recoveries can be seen in Algorithm 1, where an edge is added to the tree only if it can be connected to the latest generated point through a set of path segments with minimal cost. Then, if other nodes exist in the vicinity of the current node with better costs, these nodes will take the position of the parent for the current node. These improvements facilitate the RRT\* algorithm with the unique capability of finding the optimal solution. **Figure 2** shows the optimality behaviour of the RRT\* algorithm in an empty envi‐ ronment.

The second component of the proposed method is the low-dispersion properties of the LD-RRT planner. This component utilizes the poison disk sampling strategy to reduce the number of samples required to capture the connectivity of the sampling domain. The proposed method performance is similar to the PRM\*/RRT\* algorithms with a basic different in the sampling technique. Unlike the original planners that would let any collision-free samples to be included in the tree, the proposed method contains an extra checking procedure that makes sure that the samples possess the Poisson-disk distribution property.

There are various efficient techniques for creating fast Poisson-disk structures. In this research, the boundary sampling technique [31] is selected for generating the Poisson-disk samples for its simplicity and implementation facility. In this method, the existing neighbourhood of all samples reduces to a set of spheres located at the sample's position with the radius of *rs* . The first result of such arrangement is that the existing neighbourhood can be represented by a set of spherical ranges at which a point can be placed on the boundary. **Figure 3** shows the sampling phase as proposed in [31] where the Poisson disks and the forbidden sampling areas are illustrated by grey and green circles, respectively. After the first rejection, the boundary of the union of the forbidden areas will be selected and a random sample is generated accordingly as shown by the orange circle. The sampling radius is defined as follows:

**Figure 3.** The optimal behaviour of RRT\* algorithm in a planar empty environment.


Application of Sampling-Based Motion Planning Algorithms in Autonomous Vehicle Navigation http://dx.doi.org/10.5772/64730 31

$$r^\*\left(n\right) \le \pi \sqrt{Q\_{\text{free}}/n} \tag{1}$$

where *τ* is a scaling coefficient ranging within (0, 1]. The main idea behind this radius is that the volume of the space should be approximately equal to n(rs ) 2 in order to fill an obstacle-free square space with n disks with radius rs . Compelling the samples to follow the Poisson-disk distribution usually decreases the cardinality of the resulted graph. In other words, it is almost impossible to find *n* samples with τ Qfree / n distance from one another with a randomized sample generator. Upon the generation of the Poisson-disk samples, the algorithm will follow normal steps in the original PRM\*/RRT\*. The pseudo code of the proposed algorithms can be seen in **Algorithm 2**.

the union of the forbidden areas will be selected and a random sample is generated accordingly

as shown by the orange circle. The sampling radius is defined as follows:

30 Autonomous Vehicle

**Figure 3.** The optimal behaviour of RRT\* algorithm in a planar empty environment.

**Algorithm 2.** The proposed asingle–uery algorithm

As can be seen in Algorithm 2, the Poisson-disk sampling takes place at line 4 by forcing the generated samples to satisfy the sampling radius rule. The rest of both algorithms are same as the originals. The relevance between the neighbourhood and sampling radii is an important index about the performance of the proposed algorithm. It is essential for the sampling radius to be smaller than the connection radius. Otherwise, it will not be possible to connect any two samples and the resulted structure will be a set of separate configurations. **Figure 4** shows the relation between these two radii along with the ratio of *rs* (*n*) over *r*\*(*n*).

**Figure 4.** Different values for neighbourhood and sampling radii and the corresponding ratio for *μ*(*Q*free) = 1 and *τ* = 1. For *n* ≥ 4, the neighbourhood radius is always greater than the sampling radius.

According to the definitions of neighbourhood and sampling radii, the cardinality of the graph/ tree should follow the following rule:

$$m \ge 10^{n^2/6} \tag{2}$$

This requirement ensures that there will be at least one eligible sample within the neighbour‐ hood region of the current sample. Considering the acceptable range of *τ*, i.e. (0, 1], the sampling radius in a 2D configuration space is smaller than the neighbourhood radius if and only if the number of samples exceeds four.

Another important property of the proposed planner takes place in the RRT\* algorithm where the steering factor in the original RRT\*, which is 'step', will be automatically replaced by the sampling radius. As stated before, the samples will be created randomly from the perimeter of the current Poisson disks. As a result, the highest distance between any two samples exactly equals the sampling radius rs (n). As stated before, the cost of the final optimum solution can be calculated as the total Euclidean distance between all members (nodes) of the optimum path which can be calculated as follows:

$$\mathcal{L}\left(\boldsymbol{\alpha}^\*\right) = \sum\_{i=1}^{n^\*} \parallel \boldsymbol{\alpha}\_{i+1}^\* - \boldsymbol{\alpha}\_i^\* \parallel \tag{3}$$

where *n*\* is the number of nodes in the optimal path resulted from the algorithm. Considering the fact that *ωi*+1 \* - *<sup>ω</sup><sup>i</sup>* \* <*r <sup>s</sup>*(*n*), now it is possible to find an upper bound for the path of the optimal solution:

$$\mathcal{L}\left(o\boldsymbol{o}^\*\right) = \sum\_{i=1}^{n^\*} \|\boldsymbol{\bot}\boldsymbol{o}\boldsymbol{o}^\*\_{i+1} - o^\*\_i\|\tag{4}$$

This upper bound merely depends on the size of the final graph/tree structure. On the other hand, reducing the total number of samples (*n*), will reduce the number of samples in any solution. Therefore, it can be concluded that using a Poisson-disk distribution as the sampling domain will improve the cost of the final solution and maintains the asymptotic optimality property of the proposed algorithm. **Figure 5** illustrates the graph construction phase for the PRM\* planner.

**Figure 5.** The connection strategy in the proposed algorithm.

The next component of the proposed method is a novel procedure that divides the sampling domain into different sampling segments based on the information received online from the sensory system of the vehicle. The proposed approach divides the sampling domain into two regions namely tabu region and valid region. The sampling procedure takes place only in the valid region and the tabu region is forbidden to be included in the sampling. **Figure 6** shows different possibilities of tabu and valid segments in a sampling domain.

**Figure 6.** The performance of the segmentation procedure. The sampling domain is divided into different valid or tabu segments.

Three different scenarios have been considered in the simulation studies. In the first one, there is no vehicle in front of the autonomous car and the sampling domain is only the current lane. In the second situation, there is a vehicle in front but takeover is possible and the valid sampling region is expanded to include proper space for the takeover. Finally, when takeover is not possible, the valid sampling domain is restricted to the available space between the two vehicles.
