BTO-RRT: A rapid, optimal, smooth and point cloud-based path planning algorithm

Zhaoliang Zheng, Thomas R. Bewley, Falko Kuester, and Jiaqi Ma Zhaoliang Zheng and Jiaqi Ma are with the Department of Electrical and Computer Engineering and Civil and Environmental Engineering, University of California, Los Angeles, CA, 90024 USA e-mail: [email protected] R. Bewley and Falko Kuester are with the University of California, San Diego.

Abstract

This paper explores a rapid, optimal smooth path-planning algorithm for robots (e.g., autonomous vehicles) in point cloud environments. Derivative maps such as dense point clouds, mesh maps, Octomaps, etc. are frequently used for path planning purposes. A bi-directional target-oriented point planning algorithm, directly using point clouds to compute the optimized and dynamically feasible trajectories, is presented in this paper. This approach searches for obstacle-free, low computational cost, smooth, and dynamically feasible paths by analyzing a point cloud of the target environment, using a modified bi-directional and RRT-connect-based path planning algorithm, with a k-d tree-based obstacle avoidance strategy and three-step optimization. This presented approach bypasses the common 3D map discretization, directly leveraging point cloud data and it can be separated into two parts: modified RRT-based algorithm core and the three-step optimization. Simulations on 8 2D maps with different configurations and characteristics are presented to show the efficiency and 2D performance of the proposed algorithm. Benchmark comparison and evaluation with other RRT-based algorithms like RRT, B-RRT, and RRT star are also shown in the paper. Finally, the proposed algorithm successfully achieved different levels of mission goals on three 3D point cloud maps with different densities. The whole simulation proves that not only can our algorithm achieves a better performance on 2D maps compared with other algorithms, but also it can handle different tasks(ground vehicles and UAV applications) on different 3D point cloud maps, which shows the high performance and robustness of the proposed algorithm. The algorithm is open-sourced at https://github.com/zhz03/BTO-RRT

I Introduction

Nowadays, robots and autonomous driving cars play a more and more important role in many areas, such as intelligent transportation systems [1, 2]. Mobile robots and robotic manipulators are widely used in unknown environment exploration, navigation, localization and mapping, etc[3]. To have a better and smarter strategy for performing these tasks, path planning is a necessary and key technique. Path planning algorithms for robots can be defined as finding an optimal and collision-free path from an initial point to the target point in the workspace while avoiding all the static obstacles or other mobile agents as well as taking into account kinematic constraints[4]. It has gained popularity among researchers around the world.

In the last decades, many sampling-based path-planning algorithms have been explored and introduced. Among them, Rapidly Exploring Random Tree (RRT) is one of the quickest and most efficient obstacle-free path-finding algorithms. RRT algorithm was first proposed in [5] as a sampling way of solving high-dimensional path planning problems. It has been proven to be probabilistically completed, and computationally efficient [6].

The advantage of RRT algorithm is that it does not need to model the system or geometrically divide the search space. It has a high coverage in the search space and a wide search scope, so it can explore the unknown space as much as possible. However, the defects of RRT algorithm are also obvious. For example, the algorithm is not deterministic, a narrow passage is also difficult to pass, and the found path is sharp-edged, which can’t be driven easily.

To improve RRT algorithm, many of variants have been proposed in the past 20 years, for example: RRT-connect [7], RRT* [8], Bidirectional RRT* [9], RRT*-Smart [10], SRRT* [11]. These variants improve the efficiency and rate of convergence and solve the problem of asymptotic optimization. However, for robot autonomous control, these pure algorithms are not enough since the dynamic constraints of vehicles are not considered. In [12], a closed-loop RRT (CL-RRT) is proposed to involve non-holonomic constraints of the four-wheel vehicle dynamics. In [13] the CL-RRT idea was improved and implemented on quadrotor UAVs. Although the UAV non-holonomic constraints and RRT-based controller design method are provided, the Simulation environment is too ideal, and the final path is not smooth enough and not cost-optimal.

Refer to caption — Figure 1: Proposed algorithm flowchart

In this paper, we proposed a bidirectional target-oriented RRT(BTO-RRT) based path planning algorithm. The core functions of this algorithm are similar to the one in our previous work [14] with some exceptions that will be pointed out later. Taking advantage of the bidirectional RRT algorithm and RRT-connect algorithm, our approach becomes more ”target-oriented.” The BTO-RRT algorithm core will first generate a zigzagged trajectory. After trajectory generation, the algorithm implements 3-step optimization to shorten further and smooth the total distance of the original path. In the optimization process, we used key point smoother optimization instead of B-Spline Curve, which combines the intermediate point interpolation and cubic-spline curve to guarantee collision-free and continuity in a clustered environment. And this is the major departure from [14]. To apply this algorithm in 3D point cloud environments and implement it on ground vehicles and UAVs applications, our algorithm first analyzes the density of the targeted environment, takes the analysis results as the inputs of the BTO-RRT algorithm, and then uses a K-d tree-based obstacle avoidance strategy to avoid obstacles points. This proposed algorithm aims to search for a collision-free, low computational cost, smooth, and dynamically feasible path. 8 different types of 2-d maps and 3 different types of point cloud maps with respective simulations are conducted in this paper to prove the efficiency and generality of this algorithm.

II Bidirectional Target-Oriented RRT-Based Algorithm

This paper proposes a bidirectional target-oriented RRT (BTO-RRT) algorithm that searches for obstacle-free, computational low-cost, smooth trajectories. The proposed algorithm contains two parts: one is the BTO-RRT core algorithm, and the other one is the optimization algorithm as you can see from the algorithm flowchart fig. 1. The core algorithm is to find a more target-oriented initial solution. If the initial path is found, the path will be further improved by the 3-step optimization which includes Down-sample optimization, up-sample optimization and key point smoother optimization. Moreover, with the modification we introduced in the section III, this algorithm can be easily applied not only in any 2D maps, but also in 3D point cloud maps.

II-A Bidirectional Target-Oriented Algorithm Core

The Bidirectional target-oriented path planning algorithm is a variant of the sample-based algorithm. It takes the advantages of bi-directional RRT(B-RRT), and RRT connect, and it improves the extension rules so it could be more ”target-oriented” and connect in a faster way.

In the bi-directional RRT algorithm, two trees that grow from the target point and the initial point are expanded with each other’s starting point as the target. If the two trees meet halfway, they will be connected to each other. As you can see from fig. 2(a), $T_{a}$ expands toward the initial point while $T_{b}$ expands toward the goal point. Since they all targeted at each other’s starting point, they may spend more tree nodes to find the connection point. In the RRT connect algorithm, Two trees that grow from the target point and the initial point respectively expand with the end position of each other as the target, and connect to the last expansion point of each other. As shown in fig. 2(b), the end nodes of both $T_{a}$ and $T_{b}$ are targeted at each other. This may spend fewer nodes on seeking connections, but it will make the path less ”target-oriented”.

Algorithm 1 BTO-RRT

(x_{init},x_{goal})

T_{a}

.init(

x_{init}

)

T_{b}

.init(

x_{goal}

)

Pathfound\leftarrow False

i\leftarrow 0

5:while

Pathfound\neq True

i<N

\{T_{a},Flag_{a}\}

\leftarrow

Extend(

T_{a},x_{goal}

)

\{T_{b},Flag_{b}\}

\leftarrow

Extend(

T_{b},T_{a}(end)

)

Pathfound

\leftarrow

Pathfound+Flag_{b}

9:end while

10:

Path_{a}\leftarrow

FindMinimumPath(

T_{a}

)

11:

Path_{b}\leftarrow

FindMinimumPath(

T_{b}

)

12:

Path\leftarrow

Concatenate(

Path_{a},Path_{b}

)

13:return

Path

Algorithm 2 Extend

(T,x)

1:Flag

\leftarrow

i\leftarrow 0

;

2:if

\textsc{CollisionDetect}=\textsc{NoCollision}

then

x_{rand}

\leftarrow

x

4:else

x_{rand}

\leftarrow

Sample(i)

6:end if

x_{nearest}

\leftarrow\textsc{NearestNode}(T,x)

x_{new}

\leftarrow

Steer

(x_{nearest},x)

9:if

\textsc{CollisionDetect}=\textsc{NoCollision}

then

10:

V\leftarrow

{

x_{new}

};

E\leftarrow\{x_{new},x_{near}\}

11: T

\leftarrow

{V, E}

12: if

|x_{new}-x|<StepSize

then

13: return Flag = 1

14: end if

15:end if

16:return

T,\textsc{Flag}

In our algorithm, we define two trees $T_{a}$ and $T_{b}$ , initial point $x_{init}$ , goal point $x_{goal}$ , and edge of the tree $E$ . Then $T_{a}:\{x_{init},E\}$ and $T_{b}:\{x_{goal},E\}$ . As can be seen from Fig. 2(c), the tree $T_{a}$ extending from the initial point always treats the goal point as its target, whereas the tree $T_{b}$ extending from the goal point always treats the most recently added node as the target. In this way, the expansion of the tree is not only more ”target-oriented”, but also it takes fewer nodes to connect two trees. The pseudocode of the BTO-RRT algorithm code is shown in algorithm 1. In the while loop, $T_{a}$ targets on the goal point and $T_{b}$ targets on the end node of $T_{b}$ , the pseudocode of the key function Extend is shown in algorithm 2.

II-B Algorithm Optimization

The original path that is generated from the RRT-based algorithm is zigzagged and to meet the requirement of optimality and dynamical feasibility, further optimization is necessary. Building on the zigzagged path, we proposed a 3-step optimization after the initial path is found. The 3-step optimization includes:

•

Down-sample optimization
•

Up-sample optimization
•

Key point smoother optimization

II-B1 Down Sample Optimization

Down-sample optimization starts from the initial point and searches along the connection tree to check the collision status between the current point and the next node point. If no collision exists between the current point and its next node point, then our algorithm moves on and keeps checking the collision status between the current point and the point that is behind its next node point. Only after a collision has been detected, the algorithm marks its previous checking node point as the current breakpoint and connects the previous checking point with the last current breakpoint. Essentially, the down-sample optimization is another variant of the greedy algorithm. The illustration of the down-sample optimization is shown in Fig. 3.

II-B2 Up Sample Optimization

After down-sample, the trajectory is locally shorter but it is still based on the zig-zag connected tree, which is not close enough to obstacles especially at the corner. Up-sample optimization is to generate more sample points that are closer to the nearest obstacles and globally shorter which will further shorten the trajectory compared with the down-sample trajectory.

This up-sample problem can be defined as follows: Consider down-sample trajectory points to be $DST$ and the up-sample trajectory points to be $UST$ . The objective is to find a $UST$ that is globally shorter, in which, $DST_{n}=\{p_{1}^{DS},p_{2}^{DS},...,p_{n}^{DS}\}$ , $UST_{m}=\{p_{1}^{US},p_{2}^{US},...,p_{m}^{US}\}$ .

We will approximate it by sampling iteration. Let $DST=UST$ for the 1st iteration. Define the cumulative sum distance of down sample trajectory points as $Cs$ , $Cs_{i}$ stands for the $i$ element of the $Cs$ . Each element will be in the form of:

Cs_{i}=\sum_{i=1}^{i}p_{i}^{DS},\forall i=1,2,...,n

(1)

Sample two random distance based on the distance between the start point and end point:

ranD_{1,2}=x\cdot Cs_{i=n},x\sim U(0,1)

(2)

where, $ranD_{1,2}$ satisfies: $ranD_{1}\leq ranD_{2}$ .

Iteratively insert random interpolation $\gamma_{1}$ and $\gamma_{2}$ into the region between $p_{i}^{US}$ and $p_{j+1}^{US}$ , where $i$ satisfies the inequality 3 and $j$ satisfies the inequality 4, and $\gamma_{1}$ , $\gamma_{2}$ are defined by Eq.5,6, 7 and 8. The connected path between $\gamma_{1}$ and $\gamma_{2}$ should be checked to be collision-free.

C_{s_{i}}\leq randD_{1}<C_{s_{i+1}}

(3)

C_{s_{j}}\leq randD_{2}<C_{s_{j+1}}

(4)

a_{1}=\frac{ranD_{1}-Cs_{i}}{Cs_{i+1}-Cs_{i}}

(5)

a_{2}=\frac{ranD_{2}-Cs_{j}}{Cs_{j+1}-Cs_{j}}

(6)

\gamma_{1}=(1-a_{1})\times p_{i}^{US}+a_{1}\times p_{i}^{US}

(7)

\gamma_{2}=(1-a_{2})\times p_{i}^{US}+a_{2}\times p_{i}^{US}

(8)

Then the new up sample points $UST_{m}^{{}^{\prime}}$ becomes:

UST_{m}^{{}^{\prime}}=\{p_{1}^{US},...,p_{i}^{US},\gamma_{1},\gamma_{2},p_{j+1}^{US},...,p_{m}^{US}\}

(9)

Let new $DST_{n}^{{}^{\prime}}=UST_{m}^{{}^{\prime}}$ and repeat the above process until it reaches the maximum iteration that we set. By doing so, the vertices from the original path that is globally further will be removed and globally shorter vertices will be added to the path. Since this is a sampling iteration method, the more iterations, the better the performance. Fig. 4 shows that higher iteration will give us better performance in terms of the distance of up sample trajectory. The sub-figures (a) and (d) are when the iteration number is 10, (b) and (e) is when the iteration number is 100, the (c) and (f) are when the iteration number is 1000. The green lines are the down-sample trajectories and the red lines are the up-sample trajectories with different iteration times.

II-B3 Key point smoother optimization

To further smooth the up-sample trajectories, we adapted cubic spline into the optimization process. A cubic spline is a spline constructed of piecewise third-order polynomials which pass through a set of n control points. According to [15], a cubic spline $S(x)$ can be defined as: $S(x)$ is a cubic polynomial $S_{j}(x)$ on [ $x_{j},x_{j+1}$ ], $\forall j=0,1,...,n-1$ .

S_{j}(x)=a_{j}+b_{j}(x-x_{j})+c_{j}(x-x_{j})^{2}+d_{j}(x-x_{j})^{3}

(10)

h_{j}=x_{j+1}-x_{j},\forall j=0,1,...,n-1

(11)

And it should satisfy the following constraints:

•

In each interval [ $x_{j},x_{j+1}$ ], $S(x)$ is given by a cubic polynomial $S_{j}(x)$ .
•

Interpolation conditions: $S(x_{j})=a_{j}$ for all $j\in{0,1,...,n-1}$
•

Twice continuously differentiable: For each $j\in{0,1,...,n-1}$ , it holds that $S_{j}^{{}^{\prime}}(x)=S_{j+1}^{{}^{\prime}}(x)$ and $S_{j}^{{}^{\prime\prime}}(x)=S_{j+1}^{{}^{\prime\prime}}(x)$
•

At its ends, the curvature of $S(x)$ vanishes: $S_{0}^{{}^{\prime\prime}}(x)=0$ , $S_{n-1}^{{}^{\prime\prime}}(x)=0$

We can Solve for coefficients $a_{j},b_{j},c_{j},d_{j}$ by using the above constraints:

•

$S_{j}(x_{j})=a_{j}$
•

$S_{j+1}(x_{j+1})=a_{j+1}=a_{j}+b_{j}h_{j}+c_{j}(h_{j})^{2}+d_{j}(h_{j})^{3}$
•

$S^{{}^{\prime}}_{j}(x_{j})=b_{j}$ , also $b_{j+1}=b_{j}+2c_{j}h_{j}+3d_{j}(h_{j})^{2}$
•

$S^{{}^{\prime\prime}}_{j}(x_{j})=2c_{j}$ , also $c_{j+1}=c_{j}+3d_{j}h_{j}$
•

Natural or clamped boundary conditions

Therefore, the coefficients $a_{j},b_{j},c_{j},d_{j}$ are:

•

$a_{j}=S_{j}(x_{j})$
•

$b_{j}=\frac{a_{j+1}-a_{j}}{h_{j}}-\frac{h_{j}(2c_{j}+c_{j+1})}{3}$
•

$c_{j}$ can be solved in $Ac=B$ format where $A\in\mathbf{R}^{n\times n},B\in\mathbf{R}^{n\times 1}$ ,
since: $h_{j-1}c_{j-1}+2(h_{j-1}+h_{j})c_{j}+h_{j}c_{j+1}=\frac{3}{h_{j}}(a_{j+1}-a_{j})-\frac{3}{h_{j-1}}(a_{j}-a_{j-1})$ for $\forall j=1,2,...,n-1$
•

$d_{j}=\frac{c_{j+1}-c_{j}}{3h_{j}}$

Algorithm 3 KPSmoothOptimization(

Kpoints,Map

)

SPoints

\leftarrow

CubicSpline(

Kpoints

)

2:while CheckCollision(

SPoints

)=Collision do

Kpoints

\leftarrow

SmoothOptimizer(

Spoints,Map

)

SPoints

\leftarrow

CubicSpline(

Kpoints

)

5:end while

6:function SmoothOptimizer(

Spoints,Map

)

MidPoints

\leftarrow

CreateMid(

Spoints

)

8:end function

In the paper [16], Christoph Sprunk proved that cubic spline is second order $C^{2}$ -continuous and it has the ability to visit all the key points (or control points), which will give us the ability to generate a final path that is dynamically feasible and to utilize Key points of interpolation to avoid obstacles. As shown in algorithm 3, Key point smoother optimization takes key points generated by up-sample optimization and maps information as inputs, generating a smooth discretized trajectory $Spoints$ . The algorithm first calculates an initial smooth trajectory and then collision checking is done assuming straight lines joining each discrete point along the smooth trajectory. If a collision occurs, insert a new key point between the key point closest to the collision point and its previous key point to avoid the collision. This collision detection process will only stop when there is no collision on the final smooth path it will be guaranteed to terminate as long as an up-sample trajectory is found. The effect of conducting a collision check is shown in Fig. 5.

III Point Cloud Based Algorithm and Analysis

To apply BTO-RRT based algorithm on maps(or environment models), it is important to understand the map environment the algorithm interacts with and adjust the algorithm accordingly. Maps are resources that enable robots to better perform their tasks [17]. Most robot maps are very important and used mainly for robots to accomplish localization and navigation [18, 19, 20]. Many robot maps representation has been proposed in the past decades, for example, grid maps, polygonal maps, occupancy maps, counter maps, and 3D mesh maps. In our case, we focus on point cloud maps.

A point cloud is a set of data points in space that contains color and position information of the real world. It is a relatively simple but very powerful and vivid way to represent the real world. In fact, generating these point clouds is now becoming more and more achievable due to cutting-edge technology like 3D reconstruction, SLAM and high-resolution equipment like 3D scanners or stereo cameras.

To implement the BTO-RRT based path planning algorithm in point cloud maps, the challenge of this problem is that different point clouds have different levels of density. Higher-density point clouds will provide more information for path-planning algorithms, making planning easier in terms of obstacle avoidance. However, the density point cloud will also increase the computational cost exponentially. To generalize this problem, we need to design an algorithm that could adapt our path-planning algorithm to different point clouds with different density levels. Moreover, to address the problem of computational expense, an efficient obstacle avoidance algorithm is also needed.

III-A Point Cloud Maps Analysis Algorithm

Point cloud density is one of the most important properties of point cloud maps. In the point cloud map analysis, point cloud density has to be maintained at an acceptable level for designing a path planning algorithm.

Since different point cloud maps have different levels of density, implementing a fixed step length RRT-based algorithm will lead to an ”Obstacle penetrating error” problem when planning a path in a very sparse point cloud map or lead to ”cannot go through the narrow tunnel” problem when planning a path in a very dense point cloud map.

As shown in Fig. 6, the red points represent the surface sampling points of the obstacle and the blue dash line represents the shape of the obstacle. If the step length is too small, then the algorithm will generate a magenta path that penetrates the obstacle, which is not what we expected. Therefore, the point cloud analysis algorithm will take the point cloud maps as inputs, analyze the average distance between red sampling points of the obstacle (distance between red dots in the fig. 6), and eventually output the step size $Stp$ (the distance between node 1 and node 2 in the fig. 6) and safe distance $S$ (radius of the circles in the fig. 6). The relationship between $S$ and $Stp$ is shown in the equation 12, where $\alpha$ can be 0.5 or 0.8 :

S=\alpha\times Stp

(12)

III-B K-D Tree Based Obstacle Avoidance Algorithm

In path planning, designing a feasible path is the first priority task. The maps environment that most path planning algorithms are dealing with is all full information geometrical maps like grid maps, occupancy maps, 3D mesh maps, and so on. Under the assumption that all the geometry information is known, it is easier to apply an obstacle avoidance strategy.

In a set of sampling data points like point clouds, designing a feasible path will become more complicated. The challenges of path planning in point cloud maps are (1) sparse point clouds will cause the planned path to easily pass through the obstacles represented by the sampling points (2) a large number of sampling points in the point cloud maps will lead to computational expenses. Researchers typically do not deal with point clouds directly. A tensor voting framework is proposed in [21] to adopt the sampling-based path-finding method to generate a flight corridor with a safety guarantee in 3-D space. Otherwise, a direct way to do global path planning on point cloud maps is to use an extremely dense point cloud as in [22]. According to the test dataset parameters table in [22], its point cloud density is $\geq$ 970 $pts/m^{2}$ . A such point cloud is not just computationally expensive, but also too difficult for SLAM to realize using current technology. Thus, a K-D tree-based collision avoidance strategy is proposed in this paper.

A Kd-Trees is a binary data structure invented in the 1970s by Jon Bentley [23]. The kd-Trees algorithm can be separated into two parts[24]: the first part is the algorithm of constructing the kd-tree; the second part is the algorithm of searching for the nearest neighbor. According to the complexity analysis in [24], the time complexity of the kd-trees nearest neighbor algorithm is $O(n^{1-1/d}+k)$ , where $d$ is the dimension and k is the k nearest neighbors to retrieve. It is far more efficient than exhaustive $O(n^{d})$ and more flexible than oct-trees.

In Fig. 6, node 4 actually “sees”(there is no obstacle blocking the way) the goal point and tries to expand a new node $5^{{}^{\prime}}$ in that direction (gray dashed line). The algorithm will calculate distances between the nearest point cloud neighbors to that node and check if it is in the ball-shape safe region, which is defined by $S$ . Eventually, the node $5^{{}^{\prime}}$ on the dashed line is discarded and node 5 is randomly selected. Repeat the previous process until the randomly selected node 5 is found which maintains the safe distance from the nearest points in the point cloud, then node 5 is added to the tree.

IV Simulation experiment and analysis

This section is devoted to an experimental study of the algorithms. Two simulations are considered to validate the performance of our algorithm with respect to their running time and the cost of the solution achieved. The validation and analysis of each step of the algorithm optimization will also be described in this section. Both simulation and analysis were implemented in MATLAB 2019a and run on a computer with a 2.4 GHz i5 processor and 16GB RAM running the Windows10 system.

We considered 8 different 2D maps with different configurations in this simulation. Each map has its own characteristics, and there are similarities between every two sets of maps, which can be compared horizontally. All 8 maps have a size of $500\times 500$ . The experiment settings for each map and their parameters are shown in table I.

In Fig. 7, for each map, two results are presented, one is the result of the BTO-RRT core algorithm, and another is the result of algorithm optimization. These two results are to illustrate the difference between the trajectories generated from the core algorithm and from the optimization. For the core algorithm results, there are three different types of lines, the blue thin line stands for the tree starting from the initial point, the red thin line stands for the tree generated from the goal point, and the red dotted line stands for the found path. For the optimization results, there are 4 different types of lines, the zig-zag orange line is the previously found path, the blue line is the trajectory of the down-sample optimization, the green line is the trajectory of the up-sample optimization, and the red-dotted line is the trajectory of the key point smoother optimization.

For these 8 maps, we divided them into four groups for horizontal comparison. Map 1 and Map 2 are one group, Map 3 and Map 4 are one group, Map 5 and Map 6 are one group, Map 7 and Map 8 are one group. The first group is to verify the performance of the algorithm when facing circular obstacles of different shapes and positions. The first group is to verify the performance of the algorithm when in irregular mazes and regular mazes. The third group is to verify the performance of the algorithm when faced with narrow pipes of different lengths and shapes. The last group is to verify the algorithm’s performance in different shapes and numbers of a clustered environment as can be seen from fig. 7(a) - 7(p), the core algorithm can find a feasible path, and the optimization steps appear to have shortened the path length on these different 2D environments.

IV-A 2D Map Experiments

TABLE I: Test Dataset Parameters

Parameter

Start point

Goal point

obstacle %

features

map 1

(10,10)

(490,490)

24.44%

Circular

map 2

(10,10)

(490,490)

27.31 %

obstacle

map 3

(350,330)

(490,490)

39.85%

Irregular

mazes

map 4

(50,170)

(430,340)

12.95%

Regular

mazes

map 5

(245,10)

(245,490)

40.52%

Single-

tunnel

map 6

(245,10)

(245,400)

39.04%

Multi-

tunnel

map 7

(10,490)

(490,10)

6.81%

Circular

scatter

map 8

(10,490)

(490,10)

25.4%

Square

scatter

IV-B Algorithm Analysis, Evaluation and Comparison

In this section, we systematically analyzed and evaluated the improvement of each step of our algorithm. The focus of the analysis and evaluation is mainly on the optimization part of the BTO-RRT algorithm. Self-analysis and evaluation are just one way of proving the progress of our proposed algorithm. To prove the efficiency of the BTO-RRT algorithm. We also compared our algorithm with other RRT-based algorithms (RRT, Bi-direction RRT, RRT star) in terms of running time, the number of nodes in the tree, and the cost of the solution achieved. The cost of the solution is defined based on the path length.

IV-B1 Down sample optimization analysis

As introduced in the section II-B1, the down-sample optimization will shorten the length of the path. To demonstrate that, we ran our algorithm 100 times on 8 maps respectively and statistically calculated the cost of the core algorithm path and the cost of the path after down-sample optimization. As shown in fig. 8, the blue bars stand for the of cost of the path generated by the BTO-RRT core, and the red bars stand for the cost of the path generated after down-sample optimization. The statistical results indicate that down-sample optimization can reduce the cost of the zig-zag core algorithm path by 6 % - 25 %.

IV-B2 Up sample optimization analysis

After the down-sample optimization, the path is locally shorter. But this is not an optimal solution. By implementing the up-sample optimization, the cost of the path will approach the asymptotic optimal value, which is the globally optimal solution in the maps. Following up illustration in section II-B2, we implemented the up-sample algorithm 100 times on the maps of figure 4 and statistically calculated the cost of the path over different iteration numbers as shown in fig. 9. As the number of iterations increases, the cost of the up-sample path decreases and it will reach an optimal value at some point. From fig. 10, you can clearly see that after up-sample optimization, the cost of paths on all 8 maps is statistically shorter.

IV-B3 Key point smoother optimization

After several iterations, the up-sample optimization algorithm converges to a globally shortest optimal solution. However, this optimal path is only trajectory continuous, and it’s not dynamically feasible. The key point smoother optimization is to generate a second-order continuous smooth trajectory. In this simulation, we choose two 2D maps (5(b) and 7(j)) as test platforms, and calculate the derivative curve of the up-sample path and key point smoother path. As can be seen from Fig. 11, the red lines and blue lines are the first-order derivative curves of the key point smoother trajectories and the up-sample trajectories. For the up-sampled path, it can be seen from the figure that the inflection point has already appeared in the first derivative. For the key point smoother path, it’s a second-order continuous curve, which means that the key-point smoother path is at least velocity continuous.

IV-B4 Algorithm comparison

In this section, we compared BTO-RRT algorithm with other RRT-based algorithms like RRT, Bi-RRT, RRT star. Running time and path cost are two very important evaluation criteria of path planning algorithms. In our simulation, we implemented the above four RRT-based algorithms on the previous 8 2d maps and statistically calculate their total running time using the tic/toc function in the MATLAB and cost of the trajectory respectively. All the experiments between different algorithms and all the algorithms themselves are implemented using the same software on the same computer. Since RRT star is an optimal convergence algorithm, and its running time will greatly increase as tree nodes and iteration time become larger and larger, we checked the average data in the literature[8],[9],[10] and chose 4000 as its maximum number of iterations to balance its cost and running time. And since our algorithm will eventually produce a smooth trajectory, which is not being considered in other three algorithms. To better compare the cost without considering the smooth effect, in our cost comparison experiment, we only compared the result from up-sample optimization with the other three algorithms.

As can be seen from Fig. 12, among all algorithms, no matter what type of map, our algorithm can always get the path that has the least cost. Even though RRT star algorithm will eventually converge to the least path cost, from fig. 13, you can clearly see that even with only 4000 maximum iteration time, the running time of RRT star algorithm is still significantly longer than our BTO-RRT algorithm.

IV-C 3D Point Cloud Map Experiment

TABLE II: 3D point cloud maps analysis results and settings for BTO-RRT algorithm

Parameter

Results from point cloud analysis

Environment

size

Average Density

Stepsize

SafeDist

map1

fig.14(a)(d)

fig.15(a)(d)

(63,65,19)

0.20

\pm

0.01

0.8

0.6

map2

fig.14(b)(e)

fig.15(b)(e)

(207,207,40)

0.34

\pm

0.01

1.5

map3

fig.14(c)(f)

fig.15(c)(f)

(270,270,52)

0.49

\pm

0.02

1.5

To verify the capability of directly utilizing the point cloud information, we consider 3 different point cloud maps with different levels of density. The first 3D map is a residential area, the second 3D map is a hospital area, and the third one is the Jacob school of engineering at UCSD. In the 3D map experiment, we will first verify the point cloud analysis algorithm since this is the first step of implementing BTO-RRT algorithm on a 3D point cloud environment. The inputs of the point cloud analysis algorithm are 3 different point cloud maps, and the outputs are their statistical density analysis shown in fig. 14 as well as the step size $Stps$ and safe distance $S$ , which are the inputs of the BTO-RRT algorithm. The results of the point cloud analysis are in table II.

We designed two different types of experiments in each 3D map: as can be seen in fig. 15, the first one is to show how our algorithm bypasses or goes through high buildings; and the second one is to show how the path calculated by the algorithm passes through a narrow corridor as shown in fig. 16.

In the first experiment, the initial and goal points we selected are all in the middle of several buildings. This selection is to test and see if our algorithm knows how to cross a building or find a path through a building. In the first 3D map (fig. 15(a) and 15(d)), the initial point is behind the tallest building of that area. In the second 3D map (fig. 15(b) and 15(e)), the goal point is in the courtyard of the hospital. In the third 3D map (fig. 15(c) and 15(f)), the initial and goal point are both behind some very tall buildings.

In the second experiment, the initial points and goal points we selected are at the end of a narrow tunnel. We want to test to see if the algorithm has the proper step size and threshold to pass these narrow tunnels. In the first 3D map (fig. 16(a) and 16(d)), the initial point and goal point are at both ends of the green shed. In the second 3D map (fig. 16(b) and 16(e)), the initial point and goal point are at both ends of the road with lots of trees on the side. In the second 3D map (fig. 16(c) and 16(f)), the initial point and goal point both are at the courtyard of two buildings.

In the above experiments, the algorithm demonstrates the ability to select a proper step size/safe distance in different point cloud maps. By doing so, in the first experiment, the generated trajectories bypass the tall buildings instead of penetrating an obstacle. And in the second experiment, the trajectories pass through some narrow corridors instead of bypassing the corridors, proving that selecting step size/safe distance works pretty well in the same point cloud maps under different scenarios. In the optimization process, the down-sample, up-sample and key point smooth optimization can be generalized from 2D to 3D by changing the collision checking algorithm from a 2D version to a 3D K-D tree-based obstacle avoidance algorithm and by changing 2D path coordinate to 3D path coordinate, which proves the generalization of our algorithm.

V Conclusion and Future work

In this paper, we proposed a general RRT-based path-planning algorithm that could rapidly search for optimal, smooth and dynamically feasible trajectories. The proposed algorithm combined the advantages between bidirectional RRT and RRT-connect to generate a more ”target-oriented” initial path, and with 3-step optimization, the initial path is further shortened and converges to a globally shortest smooth path. By utilizing the point cloud analysis and K-D tree-based obstacle avoidance strategy, our algorithm could be successfully implemented on point cloud maps. We believe that by modifying some parameters of the BTO-RRT algorithm, it can also be applied to other types of maps, for example, OctoMaps, occupancy maps, pixel maps and so on. With the capability of rapidly exploring space and generating a smooth path, it can be used in diverse platforms like quadrotors, fix-wing airplanes, and ground vehicles with fast speed.

In the future, running an experimental test with this BTO-RRT using a real quadrotor with GPS, cameras, IMU, and altimeter would be a good way to test the real-world performance of the proposed algorithm. And with the good characteristics of our proposed algorithm, it could greatly improve efficiency and enable better performance of tasks like SLAM, local navigation, and so on.

References

[1] Runsheng Xu, Hao Xiang, Zhengzhong Tu, Xin Xia, Ming-Hsuan Yang, and Jiaqi Ma. V2x-vit: Vehicle-to-everything cooperative perception with vision transformer. arXiv preprint arXiv:2203.10638, 2022.
[2] Runsheng Xu, Hao Xiang, Xin Xia, Xu Han, Jinlong Li, and Jiaqi Ma. Opv2v: An open benchmark dataset and fusion pipeline for perception with vehicle-to-vehicle communication. In 2022 International Conference on Robotics and Automation (ICRA), pages 2583–2589. IEEE, 2022.
[3] Pengcheng Cao, James Strawson, Xuebin Zhu, Everbrook Zhou, Chase Lazar, Dominique Meyer, Zhaoliang Zheng, Thomas Bewley, and Falko Kuester. Beaglerover: An open-source 3d-printable robotic platform for engineering education and research. In AIAA SCITECH 2022 Forum, page 1914, 2022.
[4] Liang Yang, Juntong Qi, Dalei Song, Jizhong Xiao, Jianda Han, and Yong Xia. Survey of robot 3d path planning algorithms. Journal of Control Science and Engineering, 2016, 2016.
[5] Steven M. Lavalle. Rapidly-exploring random trees: A new tool for path planning. Technical report, 1998.
[6] Steven M LaValle and James J Kuffner. Rapidly-exploring random trees: Progress and prospects. Algorithmic and computational robotics: new directions, (5):293–308, 2001.
[7] James J Kuffner and Steven M LaValle. Rrt-connect: An efficient approach to single-query path planning. In Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065), volume 2, pages 995–1001. IEEE, 2000.
[8] Sertac Karaman and Emilio Frazzoli. Sampling-based algorithms for optimal motion planning. The international journal of robotics research, 30(7):846–894, 2011.
[9] Matthew Jordan and Alejandro Perez. Optimal bidirectional rapidly-exploring random trees. 2013.
[10] Iram Noreen, Amna Khan, and Zulfiqar Habib. A comparison of rrt, rrt* and rrt*-smart path planning algorithms. International Journal of Computer Science and Network Security (IJCSNS), 16(10):20, 2016.
[11] Robin Hess, Roland Jerg, Tobias Lindeholz, Daniel Eck, and Klaus Schilling. Srrt*-a probabilistic optimal trajectory planner for problematic area structures. IFAC-PapersOnLine, 49(30):331–336, 2016.
[12] Yoshiaki Kuwata, Justin Teo, Gaston Fiore, Sertac Karaman, Emilio Frazzoli, and Jonathan P How. Real-time motion planning with applications to autonomous urban driving. IEEE Transactions on control systems technology, 17(5):1105–1118, 2009.
[13] Yiqun Dong, Changhong Fu, and Erdal Kayacan. Rrt-based 3d path planning for formation landing of quadrotor uavs. In 2016 14th International Conference on Control, Automation, Robotics and Vision (ICARCV), pages 1–6. IEEE, 2016.
[14] Zhaoliang Zheng, T.R. BEWLEY, and Falko Kuester. Point cloud-based target-oriented 3d path planning for uavs. In 2020 International Conference on Unmanned Aircraft Systems (ICUAS). IEEE, 2020.
[15] Richard H Bartels, John C Beatty, and Brian A Barsky. An introduction to splines for use in computer graphics and geometric modeling. Morgan Kaufmann, 1995.
[16] Christoph Sprunk. Planning motion trajectories for mobile robots using splines. 2008.
[17] Radu Bogdan Rusu, Nico Blodow, Zoltan Marton, Alina Soos, and Michael Beetz. Towards 3d object maps for autonomous household robots. In 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 3191–3198. IEEE, 2007.
[18] Shrihari Vasudevan, Stefan Gächter, Viet Nguyen, and Roland Siegwart. Cognitive maps for mobile robots—an object based approach. Robotics and Autonomous Systems, 55(5):359–371, 2007.
[19] Li Zhang, Faezeh Tafazzoli, Gunther Krehl, Runsheng Xu, Timo Rehfeld, Manuel Schier, and Arunava Seal. Hierarchical road topology learning for urban map-less driving. arXiv preprint arXiv:2104.00084, 2021.
[20] Runsheng Xu, Faezeh Tafazzoli, Li Zhang, Timo Rehfeld, Gunther Krehl, and Arunava Seal. Holistic grid fusion based stop line estimation. In 2020 25th International Conference on Pattern Recognition (ICPR), pages 8400–8407. IEEE, 2021.
[21] Fei Gao and Shaojie Shen. Online quadrotor trajectory generation and autonomous navigation on point clouds. In 2016 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), pages 139–146. IEEE, 2016.
[22] Roman Fedorenko, Aidar Gabdullin, and Anna Fedorenko. Global ugv path planning on point cloud maps created by uav. In 2018 3rd IEEE International Conference on Intelligent Transportation Engineering (ICITE), pages 253–258. IEEE, 2018.
[23] Jon Louis Bentley. Multidimensional binary search trees used for associative searching. Communications of the ACM, 18(9):509–517, 1975.
[24] Hemant M Kakde. Range searching using kd tree. from the citeseerx database on the World Wide Web: http://citeseerx. ist. psu. edu/viewdoc/summary, 2005.