Fast Path Planning of Autonomous Vehicles in 3D Environments

Abstract

Three dimensional path planner is crucial for the safe navigation of autonomous vehicles (AV), such as unmanned aerial vehicles or unmanned underwater vehicles, which operate in three dimensions. In this paper, we develop a novel 3D path planner, which is fast in generating a near-optimal solution path. The planner generates the 3D path considering the size of an AV so that as the AV traverses the constructed path, it does not collide with an obstacle. This paper introduces a 3D path planner with novel concepts, such as a virtual agent and virtual sensors. In order to generate a 3D path to the goal as fast as possible, we let the virtual agent deploy virtual sensors iteratively, such that the connected sensor network can be formed. The constructed sensor network serves as a topological map for the AV, and we find a shortest path from the start to the goal utilizing the network. The virtual agent’s maneuver is biased towards the goal, in order to find a path to the goal as fast as possible. Moreover, the size of the agent is set considering the safety margin of the generated path. Through MATLAB simulations, we demonstrate the outperformance (low computational load and short path length) of our 3D path planner by comparing it with the 3D RRT-star algorithm.

1. Introduction

Recently, various autonomous vehicles (AV), such as unmanned aerial vehicles (UAV) or unmanned underwater vehicles (UUV), perform many missions, such as environment monitoring, target tracking, and UAV photogrammetry [1,2,3]. Three dimensional path planning is important for safe navigation of an AV, which operates in three dimensions [2,4,5,6].

Once the 3D path is planned, the AV follows the path utilizing path following controls [7,8,9]. However, how to make the AV follow the planned path is not within the scope of our paper.

Simple 2D path planning algorithms cannot handle complex 3D environments, where there exist a lot of structures constraints and uncertainties [10]. Therefore, 3D path planners for an AV are urgently required, especially in complicated 3D environments such as urban environments.

It is desirable that a path planner searches for a safe path as fast as possible. Therefore, this paper proposes a novel 3D path planner that is fast in generating a sub-optimal path. In other words, our path planner considers both the computational load and the generated path length. Since our path planner runs fast, our planner is suitable for on-line path planning of an autonomous vehicle with cheap embedded systems.

There are many papers on 3D path planner for an AV [10]. A-star planner, Theta-star planner, Dijkstra planner, and Voronoi diagram have been implemented as path planners of various AVs [11,12,13,14,15]. A-star planner achieves better performance than Dijkstra planner by utilizing heuristics for guiding its search towards its goal. A-star, Theta-star, or Djkstra planners [11,12,13,14,15] require that the entire workspace is already covered by multiple node points (grid cells) completely. A-star, Theta-star, or Djkstra planners then work to find a cost minimum path to the goal [10]. As one increases the volume of the entire workspace, the number of node points required to cover the workspace also increases. Therefore, calculating a path utilizing A-star, Theta-star, or Djkstra planners in a large 3D environment may be infeasible due to large computational load.

Our paper considers a 3D workspace without node points. Considering a 2D workspace without node points, a Voronoi diagram has been utilized as a safe path for an AV [16,17,18,19,20,21]. However, it is not trivial to generate a 3D path utilizing 3D Voronoi diagrams. Two 3D obstacles generate a 3D Voronoi surface that is equidistant from the two obstacles. However, it is not clear how to generate a 3D path on the 3D Voronoi surface.

Considering a 3D workspace without node points, ref. [22] addressed a potential field based 3D path planner in cluttered environments. The path planners in [22] describes obstacles by simulated annealing neural networks. The computational load of the path planner in [22] increases as one increases the total number of planar surfaces of the obstacle. However, adding spheres and cylinders, that are described with second order equations, increases the computational load of the path planner in [22]. The 3D path planner in our paper does not depend on obstacle shapes, which distinguishes our paper from [22]. In other words, our path planner works for obstacles with arbitrary shapes.

Considering a workspace without node points, RRT planners in [23,24,25] utilized random sampling to generate a feasible path. RRT planners works for obstacles with arbitrary shapes. RRT planners have been utilized in complex environments due to their ability to search high-dimensional input spaces, in order to navigate among static and dynamic obstacles [26]. In RRT planners, sample points are randomly generated in the obstacle-free space. The authors of [24] showed that the path generated by RRT-star converges to the optimum path asymptotically when infinite time is consumed. But, one cannot wait for unspecified time in practice. In other words, it may take too much time until randomly generated sample points cover the entire obstacle-free space. This inspired us to develop a path planner that is fast in generating a near-optimal path considering a 3D workspace without node points.

This paper considers the problem of generating a safe path to the goal in a 3D cluttered workspace without node points. This paper introduces a 3D path planner with novel concepts, such as a virtual agent and virtual sensors. In order to generate a 3D path to the goal as fast as possible, we let the virtual agent deploy virtual sensors iteratively, such that the connected sensor network can be formed. If there is no obstacle between a generated virtual sensor, say $n_g$, and the goal, then our path planner ends. The constructed sensor network serves as a topological map for the AV, and we find a shortest path from the start to $n_g$ utilizing the network. As the AV reaches $n_g$, it moves directly towards the goal, since there is no obstacle along the line segment connecting $n_g$ and the goal. This direct maneuver is utilized for decreasing the computational load of generating a path to the goal. As far as we know, this direct maneuver towards the goal was not utilized in other path planners, such as RRT-star or A-star planners.

The proposed path planner is fast in generating a near-optimal path considering a cluttered 3D workspace without node points. The virtual agent’s maneuver is biased towards the goal, in order to find a path to the goal as fast as possible. Moreover, the size of the agent is set considering the safety margin of the generated path.

This paper proves that our path planner is assured to find a near-optimal path to the goal in finite time. Through MATLAB simulations, we demonstrate the outperformance (low computational load and short path length) of our 3D path planner by comparing it with the 3D RRT-star planner in [24].

The organization of this paper is as follows: Section 2 introduces assumptions and definitions. Section 3 addresses a fast path planner in three dimensions. Section 4 shows simulation results of our path planner. Section 5 presents the discussion of our paper. Section 6 provides conclusions.

2. Assumptions and Definitions

Let goal define a 3D point in an open area. We assume that the location of the goal is known to the AV. The problem solved in this paper is as follows: generate a safe 3D path to the goal so that the AV can reach the goal as fast as possible while avoiding collision.

Consider the case where we generate a path for the AV, that can be approximated as a sphere with radius r. We utilize one virtual agent R to plan the path of the real AV. R is a sphere with radius r centered at $\mathbf{R}$. Here, $\mathbf{R}\in {\mathcal{R}}^3$ is the 3D position of R.

We plan the AV’s path so that the virtual agent R with radius r does not collide with obstacles as it moves along the generated path. As we increase the radius r, we increase the safety margin of the agent. This shows that as we increase r, we plan the path while increasing the AV’s safety.

Note that the virtual agent R is utilized to generate a real AV’s path under simulations. Since R moves in simulated (virtual) environments, we can make it move with unbounded speed in simulations. The term “virtual” is utilized to indicate that the agent exists in simulated environments. Note that as a real AV moves along the constructed path, it cannot move with infinite speed due to hardware limits.

We say that R collides with an obstacle if the sphere centered at $\mathbf{R}$ meets an obstacle boundary. We plan the path of R so that R with radius r does not collide with an obstacle while traversing the constructed path.

For convenience, let $L({\mathbf{c}}_1,{\mathbf{c}}_2)$ define a straight line segment connecting two 3D coordinates given by ${\mathbf{c}}_1\in {\mathcal{R}}^3$ and ${\mathbf{c}}_2\in {\mathcal{R}}^3$ respectively. We say that $L({\mathbf{c}}_1,{\mathbf{c}}_2)$ is a collision-free path once the following condition is met: consider the case where R with radius r is located at ${\mathbf{c}}_1$ initially. As R moves along $L({\mathbf{c}}_1,{\mathbf{c}}_2)$, R does not collide with an obstacle.

In order to generate a 3D path to the goal as fast as possible, R deploys virtual sensors iteratively, such that the connected sensor network can be formed. The constructed sensor network serves as a topological map for the AV, and we find a shortest path from the start to the goal utilizing the network.

Each virtual sensor has spherical sensing coverage with radius $r_s$. Let footprint of a sensor n define the sphere whose center corresponds to n and the radius of which is $r_s$. Let footprint boundary $S_n$ define the boundary of the footprint of n. Let $\mathbf{n}\in {\mathcal{R}}^3$ indicates the 3D coordinates of sensor n.

Recall that R deploys virtual sensors iteratively, such that the connected sensor network can be formed. Let the connectivity graph I be defined by a set $I=\left(V\right(I),E(I\left)\right)$, where $V\left(I\right)$ is the vertex set and $E\left(I\right)$ is the edge set. Every vertex in $V\left(I\right)$ represents a deployed virtual sensor. Every edge, say $\{n_i,n_j\}\in E\left(I\right)$, indicates that $L({\mathbf{n}}_i,{\mathbf{n}}_j)$ is a collision-free path and that $\parallel {\mathbf{n}}_i-{\mathbf{n}}_j\parallel <r_c$ such that $r_c>r_s$. Here, ${\mathbf{n}}_i\in {\mathcal{R}}^3$ indicates the 3D coordinates of sensor $n_i$. In MATLAB simulations (Section 4), we utilize $r_c=5\ast r_s$. In other words, the connection distance is 5 times longer than the footprint radius $r_s$.

For instance, Figure 1 shows an illustration of I. I has five vertices $n_1,n_2,n_3,n_4,n_5$. There is a rectangular obstacle in this figure. Also, the length of $r_c$ is depicted at the bottom of this figure. Each edge in I is plotted with dashed line segments in Figure 1. See that each edge length is shorter than $r_c$ and that each edge is a collision-free path.

A virtual sensor $n_i$ is adjacent to another sensor, say $n_j$, in the case where $L({\mathbf{n}}_i,{\mathbf{n}}_j)$ is a collision-free path and $\parallel {\mathbf{n}}_i-{\mathbf{n}}_j\parallel <r_c$. Each edge, say $e\in E\left(I\right)$, is weighted by $w\left(e\right):e\to {\mathrm{Z}}^{+}$. The weight of $\{n_i,n_j\}$ is set as $\parallel {\mathbf{n}}_i-{\mathbf{n}}_j\parallel$.

In simulations, R can move along any edge in $E\left(I\right)$ infinitely fast. Therefore, we say that R jumps from one sensor in I to another sensor in I instantly.

Utilizing the definition of $E\left(I\right)$, R with radius r does not collide with an obstacle as it moves along an edge in $E\left(I\right)$. Hence, I is a collision-free topological map for the agent R. By accessing the weight for every edge in I, R can find the shortest path from one sensor to every other sensor.

Let $\mathbf{g}$ define the 3D coordinates of the goal. $S_n$, the footprint boundary of a sensor n, meets $L(\mathbf{n},\mathbf{g})$ at one point. This point is called the goal-closest point, since it is closest to the goal among all points on $S_n$. See Figure 2 for an illustration.

An open surface of a sensor n defines the set of points on $S_n$, such that each point in this set is not inside an obstacle. A frontier of n is the subset of an open surface, such that every point on the frontier is not inside the footprint of any other sensor.

We next introduce how to generate frontier points that are utilized to discretize a frontier with discrete points. We first generate q points on the footprint boundary of a sensor n so that they are evenly spaced on the footprint boundary. As q increases, we obtain densely spaced points on a footprint boundary.

For instance, we address how to generate $q=36\ast 36$ points on a footprint boundary. We rotate a vector ${[r_s,0,0]}^T$ by $\varphi \in \{\pi /18,2\ast \pi /18,\dots ,2\ast \pi \}$ centered at the z-axis. The matrix representing this rotation is

$$\begin{array}{c}\hfill \mathbf{r}\left(\varphi \right)=\left(\begin{array}{ccc}cos\left(\varphi \right)& -sin\left(\varphi \right)& 0\\ sin\left(\varphi \right)& cos\left(\varphi \right)& 0\\ 0& 0& 1\end{array}\right).\end{array}$$

(1)

Thereafter, we rotate the vector $\mathbf{r}\left(\varphi \right)\ast {[r_s,0,0]}^T$ by $\theta \in \{\pi /18,2\ast \pi /18,\dots ,2\ast \pi \}$ centered at the y-axis. The matrix representing this rotation is

$$\begin{array}{c}\hfill \mathbf{r}\left(\theta \right)=\left(\begin{array}{ccc}cos\left(\theta \right)& 0& sin\left(\theta \right)\\ 0& 1& 0\\ -sin\left(\theta \right)& 0& cos\left(\theta \right)\end{array}\right).\end{array}$$

(2)

The resulting vector after two rotations is $fp(\varphi ,\theta )=\mathbf{r}\left(\theta \right)\ast \mathbf{r}\left(\varphi \right)\ast {[r_s,0,0]}^T$. The vector $fp(\varphi ,\theta )$ represents the relative position of a points on a footprint boundary with respect to $\mathbf{n}$ (3D coordinates of sensor n). Since $\varphi$ and $\theta$ are chosen from 36 values respectively, $q=36\ast 36$ points exist on a footprint boundary. By adding $\mathbf{n}$ to $fp(\varphi ,\theta )$, we generate the 3D coordinates of the j-th frontier point ($j\in \{1,2,\dots ,q\}$) of n.

The goal-closest point and these q points are termed the candidate frontier points of n. Among these $q+1$ candidate frontier points, we select a set of frontier points, $f\left(n\right)$, on a frontier satisfying the following conditions:

• A point in $f\left(n\right)$ is not inside the footprint of any other sensor.
• The line segment connecting n and a point in $f\left(n\right)$ is a collision-free path for the agent R.
• The distance between every point in $f\left(n\right)$ and an obstacle boundary is larger than the agent radius r.

In our path plan strategy, R utilizes a frontier point as a “way-point” for reaching the goal. The first condition shows that a frontier point corresponds to the border between a space covered by footprints and a space covered by no footprints. If every sensor has no frontier point, then footprints of all sensors cover the entire workspace. The second condition indicates that as R moves from n to any point in $f\left(n\right)$, R does not collide with an obstacle boundary. The third condition is required to avoid the case where R collides with an obstacle when R arrives at a frontier point. The second and third conditions assure that a frontier point is not inside an obstacle. See Algorithm 1 for generation of both candidate frontier points and frontier points.

Figure 3 illustrates R deploying a new virtual sensor. In this figure, R is depicted as a sphere. R moves inside a tunnel in this figure. The tunnel boundaries are depicted with red curves, and the trajectory of R is depicted with a yellow curve. The large dots on the trajectory of R indicate the virtual sensors deployed by R. The footprint boundary of each virtual sensor is depicted as a dotted sphere. Frontier points are marked as large dots on the footprint boundary of one sensor.

Algorithm 1 Path Plan Strategy

1: $initPhase=TRUE$;

2: Generate a virtual agent R at the position of a real AV;

3: repeat

4:    R deploys a new virtual sensor, say n, at its position;

5:    if $initPhase==TRUE$ then

6:      Let $n_{init}$ define a virtual sensor deployed at the real AV position;

7:      $initPhase=FALSE$;

8:    end if

9:    Update I utilizing the newly deployed sensor n;

10:   Enable the sensing capability of n.

11:   $FrontierGenerate\left(n\right)$ in Algorithm 2;

12:   Every frontier point within $r_s$ distance from n is removed;

13:   R.$FrontierJump$ in Algorithm 3;

14: until $L(\mathbf{n},\mathbf{g})$ is a collision-free path;

15: Set n as $n_g$, representing the sensor nearest to the goal;

16: Generate a shortest path from $n_{init}$ to $n_g$ utilizing I;

17: The real AV moves along the generated shortest path;

18: The real AV moves along $L({\mathbf{n}}_g,\mathbf{g})$ until reaching $\mathbf{g}$;

3. Fast Path Planner

A time-efficient 3D path planner utilizing one virtual agent R is addressed in Algorithm 1. We address Algorithm 1 in detail. Algorithms 2 and 3 are sub-algorithms of Algorithm 1.

At the initial phase of Algorithm 1, we generate a virtual agent R at the position of a real AV. R deploys a virtual sensor at its position. Let $n_{init}$ define a virtual sensor deployed at the position of a real AV.

Whenever R deploys a new virtual sensor, the sensor is connected to the network and we update I. Furthermore, sensing capability of the sensor is enabled. Recall that each virtual sensor has footprint radius $r_s$.

Algorithm 2 is utilized to generate candidate frontier points. Furthermore, Algorithm 3 is utilized to make R jump to a frontierSensor, say $n^c$, which is closest to the goal $\mathbf{g}$. Here, we say that a sensor is a frontierSensor if the sensor has a frontier point. Here, R jumps to a frontierSensor, which is closest to $\mathbf{g}$, since we want to find a shortest path to $\mathbf{g}$.

Algorithm 2 $FrontierGenerate\left(n\right)$

1: Generate the goal-closest point of n and set the goal-closest point as the first candidate frontier point;

2: Generate q candidate frontier points on $S_n$;

3: for i = 1:1:q + 1 do

4:    if i-th candidate frontier point satisfies the conditions for a frontier point then

5:      Store the candidate frontier point as a frontier point of n;

6:    end if

7: end for

Algorithm 3 $R.FrontierJump$

1: R finds a frontierSensor, say $n^c$, that is closest to the goal $\mathbf{g}$;

2: R jumps to $n^c$;

3: if the virtual sensor $n^c$ has the goal-closest point as a frontier point then

4:    R sets the goal-closest point as $f_R$;

5: else

6:    R randomly selects one frontier point, which is not the goal-closest point, as $f_R$;

7: end if

8: R jumps to $f_R$;

In Algorithm 3, $n^c$ is a frontierSensor, that is closest to the goal $\mathbf{g}$. In Algorithm 3, $f_R$ is a frontier point of $n^c$, that will be visited by R after R jumps to $n^c$. If $n^c$ has the goal-closest point as a frontier point, then R sets the goal-closest point as $f_R$. Otherwise, R randomly selects one frontier point, which is not the goal-closest point, as $f_R$. Due to this random selection, our approach is based on random sampling. Thereafter, R jumps to $f_R$.

In this way, R visits the goal-closest point of $n^c$ before visiting other frontier points of $n^c$. This makes R head towards the goal if possible. This movement is utilized for minimizing the path length to the goal. In this way, the virtual agent’s maneuver is biased towards the goal, in order to find a path to the goal as fast as possible. Our approach is similar to A-star planners [12], since A-star planners utilize heuristics for guiding its search towards the goal.

Once R reaches $f_R$, it deploys a new sensor at $f_R$. Whenever a new sensor is deployed, we enable the sensing capability of the sensor. Thereafter, every frontier point within $r_s$ distance from the sensor is removed.

Algorithm 1 iterates until the newly deployed sensor n satisfies that $L(\mathbf{n},\mathbf{g})$ is a collision-free path. The sensor n is set as $n_g$, representing the sensor nearest to the goal. We then generate a shortest path from $n_{init}$ to $n_g$ utilizing I. Then, the real AV moves along the generated shortest path. After the AV moves along the generated path, it moves along $L({\mathbf{n}}_g,\mathbf{g})$ until reaching $\mathbf{g}$. As far as we know, this direct maneuver towards the goal was not utilized in other path planners, such as RRT-star or A-star planners.

Algorithm 1 cannot proceed if R cannot find any frontierSensor. The following theorem proves that if R cannot find any frontierSensor, then the entire open space is covered by footprints.

Theorem 1.

If R cannot find any frontierSensor, then the entire open space is covered by footprints.

Proof. 

Under the transposition rule, we prove the following statement: if an open space, which has not been covered by footprints, exists, then R can find a frontierSensor.

Suppose that an open space, which has not been covered by footprints, exists. Let O indicate this uncovered open space. Utilizing the definition of a frontier, at least one sensor has a frontier on the boundary of O. Therefore, R can find this frontierSensor. □

Theorem 1 shows that if R cannot find any frontierSensor, then the goal must be covered by a footprint of a sensor. Therefore, the path to the goal is constructed before R cannot find any frontierSensor. In this way, Algorithm 1 proceeds until a path to the goal is found.

Theorem 2 proves that the real AV does not collide with an obstacle while moving along the path constructed utilizing Algorithm 1. This shows that Algorithm 1 generates a safe path for the real AV.

Theorem 2.

As the real AV moves along the path constructed utilizing Algorithm 1, it does not collide with an obstacle.

Proof. 

The path to $n_g$ is constructed utilizing I. Under the definition of I, every edge, say $\{n_1,n_2\}\in E\left(I\right)$, indicates that $L({\mathbf{n}}_1,{\mathbf{n}}_2)$ is a collision-free path. Since the real AV moves along edges of I until meeting $n_g$, the real AV does not collide with an obstacle during the maneuver. Moreover, utilizing the definition of $n_g$, $L({\mathbf{n}}_g,\mathbf{g})$ is a collision-free path. Thus, as the real AV moves along $L({\mathbf{n}}_g,\mathbf{g})$, it does not collide with an obstacle. We proved this theorem. □

Theorem 1 shows that if R cannot find any frontierSensor, then the goal must be covered by a footprint of a sensor. Hence, Algorithm 1 continues until a path to the goal is found.

Handle the Case Where a Narrow Passage Exists

In practice, frontier points are generated to discretize a frontier with discrete points. At each sensor n, we generate q points on the footprint boundary of n, so that they are evenly spaced on the footprint boundary. As q increases, we obtain densely spaced points on a footprint boundary.

If q is not sufficiently large, then we may have a case where a frontier point is not generated on the boundary of O (uncovered open space). This case may happen when R is at the entrance of a narrow passage (tunnel) in the workspace.

For instance, let S define the entrance cross area of a narrow tunnel. The area of a footprint boundary is $4\pi r_s^2$. Since q points are evenly generated on a footprint boundary, each point has its associated area as $\frac{4\pi r_s^2}{q}$. In order to generate a frontier point on S, it is required that $\frac{4\pi r_s^2}{q}<S$. Thus, we require that

$$\begin{array}{c}\hfill q>\frac{4\pi r_s^2}{S}\end{array}$$

(3)

Equation (3) implies that as the footprint radius $r_s$ decreases, we have more probability to make the AV move through a narrow tunnel. (3) further shows that as q increases, we have more probability to make the AV move through a narrow tunnel. If q is sufficiently large, then we can assure of finding a path to the goal utilizing Theorem 1.

If we can access the minimum entrance cross area S in the environment, then we can select q that satisfies (3). In this way, our path planner can handle narrow tunnels.

4. Simulation Results

In this section, we demonstrate the outperformance of our path planner (Algorithm 1) through MATLAB simulations. We utilize $q=36\ast 36$, $r_s=20$, and $r_c=5\ast r_s$. The radius of the AV is $r=2$ distance units. The goal position is [170, 170, 170]. The initial position of the AV is [30, 30, 30].

To demonstrate the performance of Algorithm 1, we compare Algorithm 1 with the 3D RRT-star planner in [24]. We implemented 20 Monte-Carlo (MC) simulations to demonstrate the performance of our planner method rigorously. Let $L^t$ where $t\le 20$ define the constructed path length utilizing the t-th MC simulation.

4.1. Cluttered Environments

We present the MATLAB simulation results of the proposed path planner (Algorithm 1). Using MATLAB 2016, we spend 61 s to run 20 MC simulations under Algorithm 1. The mean of $[L^1,L^2,\dots ,L^{20}]$ is 268. Furthermore, the variance of $[L^1,L^2,\dots ,L^{20}]$ is 95.

Figure 4 depicts the path constructed utilizing one MC simulation of Algorithm 1. The obstacles are shown as spheres, and the generated path is depicted with red line segments. In Algorithm 1, the AV moves toward the goal directly if possible. This direct movement is depicted in Figure 4. Since the radius of the virtual agent is r distance units in the simulations, the constructed path is a safe path for the AV that can be approximated as a sphere with radius r distance units.

Comparison with the 3D RRT-Star Planner

We compare the performance of the proposed planner with the 3D RRT-star planner in [24]. The 3D RRT-star planner ends when the distance between a sample point and the goal position is shorter than 10 distance units. Two sample points are neighbors if the relative distance between them is shorter than 10 distance units. The step size (expansion of the tree within one time step) in the 3D RRT-star planner is set as 10 distance units.

Once a path is generated, we can further improve the generated path by smoothing it or removing vertices on the path. The Open Motion Planning Library (OMPL) library has a path simplifier and a B-Spline smoother for these tasks. See the following website for the OMPL library: https://ompl.kavrakilab.org/index.html, accessed on 10 April 2022.

For fair comparison with the 3D RRT-star planner, we do not apply a path simplifier or a B-Spline smoother.

Using MATLAB 2016, one spends 644 s to run 20 MC simulations under the 3D RRT-star planner. Under the 3D RRT-star planner, the mean of $[L^1,L^2,\dots ,L^{20}]$ is 384. Furthermore, the variance of $[L^1,L^2,\dots ,L^{20}]$ is 1099.

See that Algorithm 1 runs much faster than the RRT-star planner. Also, Algorithm 1 generates a shorter path than the 3D RRT-star planner. This shows that Algorithm 1 outperforms the 3D RRT-star planner.

Figure 5 depicts the path constructed utilizing one MC simulation of the 3D RRT-star planner. The obstacles are shown as spheres. The generated path is depicted with red line segments.

4.2. Highly Cluttered Environments

We next consider highly cluttered environments. We present the simulation results of the proposed path planner (Algorithm 1). Using MATLAB 2016, we spend 113 s to run 20 MC simulations under Algorithm 1. The mean of $[L^1,L^2,\dots ,L^{20}]$ is 280. Furthermore, the variance of $[L^1,L^2,\dots ,L^{20}]$ is 727.

Considering highly cluttered environments, Figure 6 depicts the path constructed utilizing one MC simulation of Algorithm 1. The obstacles are shown as spheres, and the generated path is depicted with red line segments. In Algorithm 1, the AV moves toward the goal directly if possible. This direct movement is depicted in Figure 6.

Comparison with the 3D RRT-Star Planner

We next show the simulation results of the 3D RRT-star planner in highly cluttered environments. Using MATLAB 2016, we spend 627 s to run 20 MC simulations under the 3D RRT-star planner. See that Algorithm 1 runs much faster than the RRT-star planner.

Under the 3D RRT-star planner, the results are as follows. The mean of $[L^1,L^2,\dots ,L^{20}]$ is 377. Furthermore, the variance of $[L^1,L^2,\dots ,L^{20}]$ is 842. See that Algorithm 1 generates a shorter path than the 3D RRT-star planner. This shows that Algorithm 1 outperforms the 3D RRT-star planner.

Figure 7 depicts the path constructed utilizing one MC simulation of the 3D RRT-star planner. The obstacles are shown as spheres. The generated path is depicted with red line segments.

4.3. The Effect of Changing the Footprint Radius

Next,

Table 1. The effect of changing the footprint radius $r_s$.

${\mathit{r}}_{\mathit{s}}$	$\mathit{simT}$	$\mathit{MeanL}$	$\mathit{VarL}$
40	53	295	1536
30	86	292	956
20	113	280	727

shows the effect of changing the footprint radius $r_s$. We run the proposed path planner (Algorithm 1) in highly cluttered environments (Figure 6).

In Table 1, $simT$ (sec) indicates the simulation time of running 20 MC simulations utilizing MATLAB. $MeanL$ defines the mean of $[L^1,L^2,\dots ,L^{20}]$. Also, $VarL$ defines the variance of $[L^1,L^2,\dots ,L^{20}]$. As $r_s$ decreases, the computational load $simT$ increases. As $r_s$ decreases, the variance in the generated path length decreases. However, decreasing $r_s$ slightly reduces the average path length. Table 1 shows that our path planner outperforms the 3D RRT-star, regardless of $r_s$.

5. Discussion

The proposed path planner is fast in generating a near-optimal path considering a cluttered 3D workspace. We prove that our fast path planner is assured to find a near-optimal path to the goal in finite time. Since our path planner runs fast, our planner is suitable for on-line path planning of an autonomous vehicle with cheap embedded systems.

Once the path is set, the AV follows the path utilizing path following controls. Many papers [7,8,9] addressed 3D path following controls for AVs. How to make the AV follow the given path is not within the scope of our paper.

We acknowledge that the proposed planner does not generate an optimal path, considering path length. However, once a safe and near-optimal path is generated utilizing the proposed fast planner, we can further improve the generated path by smoothing it or removing vertices on the path. The Open Motion Planning Library (OMPL) library has a path simplifier and a B-Spline smoother for these tasks. See the following website for the OMPL library: https://ompl.kavrakilab.org/index.html, accessed on 10 April 2022. The authors of [27] reviewed various path smoothing techniques for navigation of an AV. How to smooth the generated path is not within the scope of our paper.

6. Conclusions

This paper introduces a 3D path planner utilizing a virtual agent and virtual sensors. The proposed planner generates a near-optimal and safe path in a time efficient manner. We prove that our planner is guaranteed to find a safe path to the goal in finite time. Through MATLAB simulations, we demonstrate the outperformance (low computational load and short path length) of our 3D path planner by comparing it with the 3D RRT-star planner.

In our future works, we will demonstrate the outperformance of our 3D path planner utilizing experiments with real AVs. Furthermore, we will extend our path planner to a path planner utilizing multiple virtual agents. Since path planning is done in a parallel manner utilizing multiple virtual agents, we can improve the time efficiency considerably compared to the case where only one virtual agent is utilized.