Path planning is a prevalent process that helps mobile robots find the most efficient pathway from the starting position to the goal position to avoid collisions with obstacles. In this paper, we propose a novel path planning algorithm–Intermediary RRT*-PSO-by utilizing the exploring speed advantages of Rapidly exploring Random Trees and using its solution to feed to a metaheuristic-based optimizer, Particle swarm optimization (PSO), for fine-tuning and enhancement. In Phase 1, the start and goal trees are initialized at the starting and goal positions, respectively, and the intermediary tree is initialized at a random unexplored region of the search space. The trees were grown until one met the other and then merged and re-initialized in other unexplored regions. If the start and goal trees merge, the first solution is found and passed through a minimization process to reduce unnecessary nodes. Phase 2 begins by feeding the minimized solution from Phase 1 as the global best particle of PSO to optimize the path. After simulating two special benchmark configurations and six practice configurations with special cases, the results of the study concluded that the proposed method is capable of handling small to large, simple to complex continuous environments, whereas it was very tedious for the previous method to achieve.

In the era of Industry 4.0, Artificial Intelligence (AI), and Robotics including Drones (Unmanned aerial vehicles) have been identified as three typical technologies [

Path planning is an essential process that helps autonomous vehicles find the shortest path between predefined starting and goal positions to avoid collisions with obstacles along the way [

In addition to these methods, another valuable approach for global path planning is nature-inspired optimization algorithms (NIOAs), also known as metaheuristics, such as Genetic Algorithm (GA), Particle Swarm Optimization (PSO), Ant Colony Optimization (ACO), Grey Wolf Optimization (GWO), Harris Hawk Optimization (HHO), Whale Optimization (WO), Artificial Potential Field (AFP), etc. [

The main contribution of this research is the proposal of a new single-query hybrid multidirectional path planning algorithm called Intermediary RRT*-PSO. The method is a combination of the advantages of the prior algorithm, RRT*-connect, exploration phase, and uses the first solution as input for PSO for solution refinement. This study aims to create and evaluate the speed and accuracy of the proposed and previous methods for complex and tricky configurations.

The remainder of this paper is organized as follows.

We define the problem similarly to [

RRT was published in 2001 by LaValle and Kuffner as a single-query path planning method experimented on hovercraft and satellites [

The RRT strength explores the state space rapidly and is collision-free; however, its major weakness is its non-optimal solution [

In addition to RRT and RRT*, RRT-connect [

In the field of natural-inspired optimization algorithms (NIOAs), many research methods have been proposed to solve path planning problems, typically Genetic Algorithm (GA), Ant Colony Optimization (ACO), Artificial Potential Field (APF) and Particle Swarm Optimization (PSO), according to [

The main goal of the proposed algorithm is to take advantage of both the sampling-based method in the exploration phase and NIOA method in the exploitation phase. In the exploration phase, we utilized the RRT*-connect idea of expanding two trees from the start and goal positions, however, we added one more intermediary tree to a random unexplored position of the environment to increase the possibility of exploring the undiscovered region. The intermediary tree is merged with the two main trees if they can establish a safe connection. During the exploitation phase, PSO is used to refine the rough raw solution path of the first phase into a straight usable path.

The discovered set

The first phase of Intermediary RRT* is inherited from informed RRT*-connect, which has two trees that expand from both the start and target positions. However, to improve the ability to expand to an undiscovered region or narrow passage, a new intermediary tree is added to a random undiscovered area of the environment by picking one random node in the undiscovered set (Algorithm 1). Then, all trees continued to expand until the two main trees were connected. Meanwhile, if the intermediary tree can connect to any of the 2 other trees, it merges all its nodes to that tree using Algorithm 3.

The undiscovered set is updated ever iteration by Algorithm 2 if one node is added to the tree system. Let

The tree merging process is activated when the newly added node of the intermediary tree can be connected to any node of any other tree (

In Algorithm 3, let

At the end of every basic RRT* iteration, when the re-branching process is completed, the newly added nodes of the two main trees are checked to determine if they can be connected to any node of the other main tree. If there is existed 1 connection, then the trees are connected, and the exploration phase terminate condition is true.

Traditional sampling-based motion planners have proven to be highly effective in rapidly computing paths for high dimensional systems [

In this study, the path minimization process was used only once during the phase transition period. The main goal of path minimization is to reduce the number of path waypoints as much as possible because RRT can generate a huge amount of nodes while PSO takes a lot of computations if the particle dimensions are large. Therefore, minimizing the path would help remove redundant nodes and accelerate PSO speed significantly. The minimization process was performed by checking two non-consecutive nodes of the solution sequence. If the edge created by these two nodes is obstacle-free, then all nodes between them are removed (Algorithm 5).

After the first solution is obtained, PSO is deployed to refine the current solution, which uses the minimized version of the RRT* result as the global best particle.

As the solution size of RRT* can be varied as the total number of waypoints is not fixed, PSO particle size must also be dynamic. Each PSO particle contains a sequence of waypoint positions, velocities, a personal best sequence, the current path length, and the fitness values. The fitness value of each particle is its path length plus an arbitrary large number multiple with the number of obstacles crossed.

At the beginning of PSO phase, the initial population is randomly initialized based on configuration of the RRT* solution. Then, after the population initialization, the RRT* solution is injected directly into the population as one of the particles. Usually, the PSO random initial population is very chaotic and mostly crosses obstacles, thus providing one rational particle to the population, which helps other particles quickly converge to the final Pareto solution.

There are three key coefficients in PSO which conquer the behavior of exploration and exploitation-w,

Opposition-based learning (OBL) has been commonly used in metaheuristic algorithms [

To integrate OBL into PSO algorithm, we introduce a multi-restart strategy to the main loop of PSO. Let ^{th},

To verify the proposed method, we used two datasets from [

The single cube dataset (

^{th}).

The next dataset is the Multiple Narrow Passages configuration, which is proposed by [

According to expectations, Informed RRT*, RRT*-connect and Informed RRT*-connect required more effort to sample paths through the narrow gaps. In contrast, Intermediary RRT*-PSO started to sample the gap area by creating intermediary trees in those areas after the outside regions were fully sampled.

After 100 executions, Informed RRT* could not find any feasible solution with 1000 iterations; thus, their graph did not appear. RRT*-connect was able to find solutions with a gap size greater than 1/32%; however, the time required was approximately three times greater. In the other hand, Informed RRT*-connect finished finding the solution faster than Intermediary RRT*-PSO from 5% to 42% in most cases; however, Intermediary RRT*-PSO outperformed Informed RRT*-connect for configurations with a gap size smaller than 1/16% (

In the most extreme case with a gap height of 1/128%, Intermediary RRT*-PSO is four times faster than Informed RRT*-connect. For algorithm efficiency, Intermediary RRT*-PSO found the solution within 560 iterations (2% gap height), whereas Informed RRT*-connect required 827 iterations (

Zigzag is one of our proposed datasets, whose purpose is to test the superior exploration speed of Intermediary RRT*-PSO in an environment containing zigzag passages. The environment had three rectangular obstacles (

As expected, Intermediary RRT*-PSO found the first solution within 300 iterations, whereas Informed RRT*-connect and RRT*-connect struggled to find paths to the middle section of the environment. Additionally, PSO only required approximately 200 iterations to enhance the solution accuracy (

As mentioned above, RRT family methods struggle with concave walls as they force the planner to generate an indirect path. Thus, to verify the effectiveness of the intermediary tree, we propose two Concave (

As can be seen, Intermediary RRT*-PSO finds the first solution and converges faster than other algorithms in most runs. However, as the datasets have multiple possible routes, 27 percent of the time Intermediary RRT*-PSO is trapped in local minimum solutions in the Concave 1 dataset and 18% in the Concave 2 dataset. In contrast, over the long run at iteration 1000th, Informed RRT* and Informed RRT*-connect have more consistent accuracy as their ellipsoid sampling region is narrowed down considerably. However, the overall Intermediary RRT*-PSO solutions are refined rapidly within only 200 iterations and are slightly shorter than Informed RRT*-connect.

Warehouse (

In this paper, we present a novel hybrid sampling-based path planning approach based on creating a new intermediate tree between the start and end points, combined with the superiority of the available methods (RRT*-connect and PSO), to deliver a new method that is optimized for both speed and accuracy. After 100 executions for each dataset, the results showed that Intermediary RRT*-PSO outperformed the previous profound method (RRT*-connect, Informed RRT*, Informed RRT*-connect) in terms of both exploration speed and solution accuracy, with the same amount of computational effort. Although in some scenarios Intermediary RRT*-PSO takes a longer time to solve 1000 iterations (

As mentioned, the presentation of the intermediary tree helps Intermediary RRT*-PSO to create a considerable number of nodes simultaneously, which can easily approach the goal node in complex and large maps. Nevertheless, Intermediary RRT*-PSO sometimes tends to fall into local minimum states by approximately 14% in multiple route configurations.

For the computational complexity, at each iteration of phase 1, the complexity of Intermediary RRT*-PSO remains the same, O(n), as other RRT algorithms with n is the current number of nodes. In phase 2, the algorithm complexity is O(nm), where n is the number of search agents and m is the number of nodes carried by each search agent. Overall, the algorithm complexity of Intermediary RRT*-PSO is lower than the traditional informed sampling method.

In future research, we would like to improve on the following features:

Parallel computing: which means each tree is utilized on an individual processor core. Hence, there could be more than one intermediary tree, thus accelerating the algorithm’s exploration speed even more, and the exploration phase could be conducted simultaneously with the PSO phase to inject more solutions, thereby helping the planner avoid trapping in local minimum states.

Better metaheuristic algorithms: PSO is currently used to improve the feasible solution over time, as shown in the previous section. However, PSO is known to suffer from local traps and premature convergence [

Multi-objective optimization: In this study, PSO utilized path length as a metric to calculate particle fitness. However, in practice, there are many objectives that a planner should also need to consider, such as smoothness, obstacle safety distance, maximum steering angle (minimum corner radius), etc.

This research is funded by International University, VNU-HCM under Grant Number T2021-02-IEM.

The authors declare that they have no conflicts of interest to report regarding the present study.