Path Planning for Multiple Unmanned Vehicles (MUVs) Formation Shape Generation Based on Dual RRT Optimization

Abstract

In this paper, dual RRT optimization is proposed to solve the formation shape generation problem for a large number of MUVs. Since large numbers of MUVs are prone to collision during formation shape generation, this paper considers the use of path planning algorithms to solve the collision avoidance problem. Additionally, RRT as a commonly used path planning algorithm has non-optimal solutions and strong randomness. Therefore, this paper proposes a dual RRT optimization to improve the drawbacks of RRT, which is applicable to the formation shape generation of MUVs. First, an initial global path can be obtained quickly by taking advantage of RRT-connect. After that, RRT* is used to optimize the initial global path locally. After finding the section that needs to be optimized, RRT* performs a new path search on the section and replaces the original path. Due to its asymptotic optimality, the path obtained by RRT* is shorter and smoother than the initial path. Finally, the algorithm can further optimize the path results by introducing a path evaluation function to determine the results of multiple runs. The experimental results show that the dual RRT operation optimization can greatly reduce the running time while avoiding obstacles and obtaining better path results than the RRT* algorithm. Moreover, multiple runs still ensure stable path results. The formation shape generation of MUVs can be completed in the shortest time using dual RRT optimization.

1. Introduction

Unmanned ground vehicles (UGVs) are vehicles that can drive and perform certain tasks on a variety of roads or in the field, but with no operator inside the vehicle. The rapid development of advanced mechatronic, computing, and communication technologies in the last two decades enables a new robotic system that can interact with both humans and other UGVs in a cooperative manner. This technology is called multiple unmanned vehicles (MUVs) [1]. Compared with single vehicles, MUVs have many advantages, such as faster completion of multiple tasks, stronger system fault tolerance, better overall flexibility, and so on. It is due to these advantages that MUVs are increasingly used in the military and social field to perform reconnaissance and surveillance, alert patrol, positioning guidance, high-risk operations, special operations, and material transportation, showing a broad application prospect in the military field [2].

For all practical purposes, MUVs are usually a collection of large numbers of vehicles. These vehicles are required to form predetermined formations to travel to a location to perform support or combat missions. For ease of management and subsequent formation control, each vehicle has its own group and number in the formation and needs to form a defined formation as required prior to the mission. Therefore, the formation and coordination control of MUVs has become a hot issue at present, because a group of robots can accomplish the mission more effectively by maintaining a predefined formation.

The formation control problems can be mainly summarized as: (1) formation shape generation; (2) formation tracking; (3) formation reconfiguration and selection; (4) task assignment in formation [3].

Among the above problems, the most widely studied is formation shape generation and tracking. Formation shape generation refers to a certain design method to guide multiple unmanned vehicles to certain designated target locations and form a predefined formation from here; while formation tracking is to design a formation controller to maintain the predefined formation shape stably after the formation is completed. In this regard, various formation control strategies have been proposed. The most common approaches are virtual structure, the behavior-based approach, leader–follower approach, graph-based approach, and artificial potential approach.

In L Yang, Y Jia [4], a distributed D-type iterative learning scheme is developed for multi-agent systems with unknown nonlinear dynamics. This presents an efficient solution to the multi-agent formation control problem. The switching time and order of this learning scheme vary according to the actual trajectory of the agents to ensure that a predefined formation is always formed after some iterations.

In Giroung and Dongkyoung [5], a decentralized behavior-based formation control algorithm is designed to accomplish the formation task using only the relative position information between robots and obstacles and between neighboring robots.

Sida et al. [6] present a hybrid formation control of wheeled mobile robots (WMRs), which is based on model predictive control (MPC) and adaptive terminal sliding mode control (ATSMC). The MPC is used to ensure the stability of the formation, while the ATSMC is used to compensate for external interference. Sarrafan and Shojaei [7] propose a formation tracking control for non-autonomous wheeled mobile robots (WMRs) based on a leader–follower strategy using an inverse dynamics control technique that combines a high-gain observer and a radial basis function (RBF) network to achieve the desired formation.

Dongyu et al. [8] propose a hierarchically distributed finite-time estimator to allow agents in each layer to obtain their target positions and velocities based on the information of the agents in their previous layer. A model-based control method is then utilized in order to achieve multi-layer formations.

In Liu et al. [9], a full potential field and a local potential field are combined to design a formation potential field for multi-agent formation control. The goal is to control a set of agents to automatically generate and maintain a specific formation while avoiding internal collisions and collisions with spatial constraints.

These traditional and commonly used methods all have some capability of formation shape generation that actually comes from the formation tracking control in Figure 1. When performing formation, it is only necessary to give the geometric configuration of the desired formation, and by having the vehicles track the nodes of each geometric configuration, the desired formation target is eventually formed. When the number of vehicles in the MUVs is small, these methods are fast and effective and facilitate further formation tracking control. When each vehicle is not required to have a fixed position within the formation, the proximity principle can also be used to make the target point as close as possible to the initial position of each vehicle while allowing the vehicle to track the nearest node, which can effectively reduce collisions between vehicles and speed up the formation assembly.

When the number of vehicles in MUVs increases, the density of vehicles in the spatial plane increases, and the distribution area of vehicles increases, which makes it possible for the vehicles to intersect in the process of tracking the expected formation geometry nodes using the traditional UGV formation control method, and therefore it is very easy to lead to the occurrence of collision among vehicles, and the more the number of vehicles, the greater the risk of collision. At this point, the formation shape generation of MUVs is transformed from a target point tracking problem to an obstacle avoidance problem in the process of moving to the target point, i.e., the path planning problem of vehicles. Since the traditional formation control algorithm cannot effectively solve the obstacle avoidance problem, the unmanned vehicle path planning algorithm, which has better results in the obstacle avoidance problem, is considered to complete the formation shape generation of MUVs. Collision avoidance algorithms can be categorized into the following major methods: (1) geometric methods, (2) force-field methods, and (3) optimization-based methods [10,11]. Among these methods, optimization-based methods include path planning algorithms. There are many unmanned vehicle path planning algorithms, such as the multi-objective genetic algorithm proposed by Luis et al. [12], the EBS-A* algorithm proposed by Wang et al. [13], the ant colony algorithm improved by M Miao C et al. [14], and so on. Among these algorithms, the rapidly exploring random tree (RRT) has been widely used in the field of UGV path planning because of the advantages of rapid path search and significant collision avoidance, but there are also two problems brought about by the algorithm itself that need improvement.

• Non-optimal path. RRT returns the path result as soon as the target point is reached, without considering whether the path is the optimal solution;
• Strong randomness. The nature of random exploring in space makes the path results obtained from repeated runs inconsistent.

To improve the adverse effects of the RRT, researchers have explored a number of different optimization approaches. Jingu et al. [15] proposed a method based on triangular inequality rewiring for the RRT-connect robot path planning algorithm to both guarantee the planning time and make its path results closer to optimal. Jung J W [16] proposed a post-processing method called bidirectional interpolation to improve the RRT-connect created paths. Feng et al. [17] proposed an RRT path planning algorithm based on the A* bootstrap domain, which effectively combines A* with RRT and uses A* to improve the sampling efficiency of RRT. J Wang et al. [18] proposed a convolutional neural network (CNN)-based optimal path planning algorithm, the neural RRT* (NRRT*), using the non-uniform sampling distribution generated from the CNN model. It can be seen that RRT and its improved algorithms are usually used in existing studies for path planning of a single vehicle. Additionally, there is little mention of the research on multiple vehicle path planning using RRT.

RRT has its unique advantages, but there are also some discordant and unavoidable disadvantages. RRT is suitable for path planning of a single vehicle under a known map where the obstacles are stationary. That is, RRT cannot handle dynamic obstacle avoidance in the case of multiple vehicles moving simultaneously. In addition, when the number of vehicles increases, more vehicles are seen as obstacles during the movement. The surrounding environment becomes more complex, and the path results obtained from multiple runs of the algorithm are more likely to expose the problem of non-optimal routes and excessive randomness of RRT. Therefore, in response to the various problems that occur in the formation shape generation of a large number of UGVs, this paper proposes a method for the formation of a large number of UGVs based on dual RRT optimization. By improving the traditional RRT algorithm, the algorithm path result is shortened and the problem of strong randomness of RRT is reduced. In this way, the path planning of obstacle avoidance is performed for a large number of UGVs, so that they can be arranged into the expected formation according to their respective routes and avoid collisions between vehicles.

2. Methods

In order to solve the problem of collision avoidance during formation shape generation for a large number of MUVs, and reduce the adverse effects of RRT itself defects on the path planning results, this paper proposes dual RRT optimization, applicable to a large number of UGVs in formation shape generation. The algorithm as a whole can be divided into two subparts:

• Dual RRT optimization: firstly, the RRT-connect is used for global planning, and then the RRT* is used to locally optimize the unsmooth part of the initial path after obtaining the initial path, so as to achieve a better path result by ensuring the efficiency of the algorithm while shortening the path as much as possible;
• Introduction of the evaluation function: dual RRT optimization is run several times simultaneously at the starting point of the path to obtaining multiple path results, and then the path evaluation function is set to select the best path as the output of the algorithm.

Firstly, we introduce the advantages and disadvantages of RRT and the commonly used RRT improvement algorithms.

2.1. RRT

As a common path search algorithm, RRT takes the starting point of the path as the root node and the target node as the leaf node and expands based on the starting point to grow in all directions, generating a search tree with branches growing randomly. When a route linking the starting point and the target point similar to the red line in Figure 2 is obtained during the search, i.e., a solution is found in the space, at which point the expansion of each point stops. In the process of search, once a new expansion node encounters an obstacle, the node will no longer expand and select the previous node of the current node to continue to expand, which ensures that the final search result obtained will not collide with the obstacle.

Since RRT has the characteristic of probabilistic completeness, the algorithm can definitely find the path when the solution exists in that space, that is, when there exists the path to avoid obstacles required for path planning. Moreover, RRT is highly adaptable and still has excellent obstacle avoidance in the face of different environments. However, RRT lacks an optimization mechanism and only pursues the result of “arrival”, so the obtained path is not optimal and the path is often not smooth. In addition, when the number of vehicles increases, the run number of the algorithm increases accordingly due to the strong randomness in the expansion process of the algorithm itself. Even if the algorithm is run repeatedly for the same start and end points, it is difficult to get the same path results, and the difference in results is usually large. Those are the two drawbacks that RRT needs to address.

To address the above disadvantages of RRT, the following RRT improvement algorithms are commonly used.

• Extend RRT, which introduces a collection of path points to speed up the convergence and improve the stability of the paths, like in [19].
• RRT-connect, which generates two trees from the initial point and the target point, and when the two trees are connected together, the algorithm converges to generate the path, and this improved type increases the efficiency of the algorithm, like in [20,21].
• RRT*, which improves the parent node selection on the original RRT, like in [22,23]. The RRT* will have a reconnection (rewire) process when selecting the parent node, using a cost function to select the node with the smallest cost in the domain of the expanded node as the parent node, and at the same time, the nodes on the existing tree are reconnected after each iteration, thus ensuring computational complexity and asymptotically optimal solutions.

These improved algorithms have their unique advantages and also reduce the impact of the two main drawbacks of RRT to some extent, but do not eliminate them. For example, the RRT-connect algorithm has a very fast algorithm running efficiency and can obtain the planning path of multiple vehicles in a very short time, but this algorithm still has the problems of non-optimal solutions and excessive randomness, and even the pursuit of efficiency makes the defects more obvious; while the RRT* algorithm can obtain more excellent path results, compared with other algorithms, the results tend to be more asymptotically optimal, at the cost of longer running time, while still not guaranteeing the randomness problem. For a large number of fleets, the complex operating environment caused by the increase in the number of vehicles, the increased chance of collisions between vehicles, and the multiple runs of the algorithm further amplify the shortcomings of the RRT algorithm, which seriously affects the path planning results of the vehicles and thus has a negative effect on the overall queueing arrangement of the fleet. Therefore, this paper addresses these two drawbacks separately to obtain an improved RRT algorithm suitable for the formation of a large number of fleets.

2.2. Dual RRT Optimization

For the path planning task, the fundamental goal is to obtain a path that avoids obstacles and allows the vehicle to reach the target point safely, while the length of this path should be as short as possible, i.e., the optimal solution of the path. RRT is widely used in the field of UGV path planning because of its excellent obstacle avoidance ability. While ensuring the obstacle avoidance ability, how to get the optimal solution of the path needs to be considered. Among the commonly used RRT improvement algorithms, the path result of RRT* is asymptotically optimal, but the running time is also significantly longer than other algorithms. On the contrary, RRT-connect has excellent running efficiency, and the path results are often unsatisfactory. Therefore, in this paper, we combine the advantages of each of the two algorithms and propose dual RRT optimization to ensure the efficiency of the algorithm while shortening the path as much as possible to get better path results.

The dual RRT optimization algorithm divides the path planning process into two phases: (1) global path planning for the RRT-connect algorithm, and (2) local path optimization for the RRT* algorithm.

The vehicle path planning is generally expected to reach the target point with the shortest possible route, and RRT-connect often leads to redundant paths in the path results for efficiency, which are generally manifested by large or frequent corners, and these corners also increase the vehicle energy consumption and wear on mechanical parts. First of all, define $\phi$ as the path corner threshold, when the corner amplitude of a section of the path is larger than $\phi$, it is considered that the section of the path has a large corner.

Since RRT itself is a tree-like structure, the final path result is obtained as nodes connected to nodes. Let a node in the path result be $l_n$, then $l\left(l_n-l_{n-1}\right)$ denotes the path between node $l_{n-1}$ to node $l_n$. After giving the starting point $x_{start}$ and the target point $x_{end}$, the initial path result $L_0$ obtained using RRT-connect can be expressed as:

$$L_0=RRT\_connect\left(x_{start},x_{end}\right)={\displaystyle \sum }_{n=2}^{m}l\left(l_n-l_{n-1}\right)$$

(1)

Because of the characteristics of the RRT-connect, $L_0$ is not smooth at this time, and there are a number of nodes with large corner turns, as shown in Figure 3a. Define the corner judgment function $S\left(·\right)$, and use $S\left(·\right)$ to make corner judgment for each node of $L_0$, then the corner judgment result of the $k$-th node $l_n$ on $L_0$ is:

$$S\left(l_k\right)=\{\begin{array}{c}0, \left|{\theta }_{k+1}-{\theta }_k\right|>\phi \\ 1, \left|{\theta }_{k+1}-{\theta }_k\right|\le \phi \end{array}$$

(2)

in which ${\theta }_{k+1}$ and ${\theta }_k$ are the forward direction angles of node $l_{k+1}$ and node $l_k$, respectively. When the corner judgment result $S\left(l_k\right)$ of a node is equal to 0, it is considered that a large corner occurs at this node $l_k$ and the path near it needs to be locally optimized. Then, $a$. nodes are taken forward to node l_k−a and $b$ nodes are taken backward to node $l_{k+b}$ for node $l_k$. The path $l\left(l_{k+b}-l_{k-a}\right)$ between the two nodes is the path to be locally optimized, as shown in Figure 3b. Take node $l_{k-a}$ and node $l_{k+b}$ as the starting point and end point of the locally optimized path, respectively, and use the RRT* algorithm to generate a new path, i.e.,

$$L_k^{*}={\mathrm{RRT}}^{*}\left(l_{k-a}, l_{k+b}\right)={\displaystyle \sum }_{i=k-a+1}^{k+b}l\left(l_i-l_{i-1}\right)$$

(3)

Due to the asymptotically optimal nature of the RRT*, the new path $L_k^{*}$ generated is flatter than the original path, so the original path $l\left(l_{k+b}-l_{k-a}\right)$ is replaced by the path $L_k^{*}$ to obtain the new path ${\left(L_0\right)}^{*}$, as shown in Figure 3c.

$${\left(L_0\right)}^{*}=S\left(L_0\right)+L_k^{*}={\displaystyle \sum }_{n=2}^{m}S\left(l_n\right)\times l\left(l_n-l_{n-1}\right)+{\displaystyle \sum }_{i=k-a+1}^{k+b}l\left(l_i-l_{i-1}\right)$$

(4)

Continue to make corner judgments on the remaining nodes of $L_0$. If large corners are encountered again, continue to optimize them, and finally obtain a more excellent path result ${\left(L_0\right)}^{*n}$ compared to the initial path $L_0$. The algorithm framework of dual RRT optimization is described in Algorithm 1.

Algorithm 1: Dual RRT Optimization
1	Input: $x_{start}$, $x_{end}$
2	Output: ${\left(L_0\right)}^{*}$
3	$L_0$←$RRT\_connect\left(x_{start},x_{end}\right)$
4	for I = 1→m
5	if $S\left(l_i\right)$=0
6	$L_0$←$L_0$- $l\left(l_i-l_{i-1}\right)$
7	$L_i^{*}$←$RR{T}^{*}\left(l_{\mathrm{i}-a}, l_{\mathrm{i}+b}\right)$
8	else
9	$L_i^{*}$ = 0
10	end
11	$L_0$←$L_0$+ $L_i^{*}$
12	end
13	${\left(L_0\right)}^{*}$←$L_0$
14	return ${\left(L_0\right)}^{*}$

2.3. Dual RRT Optimization with Evaluation Function

The dual RRT optimization proposed in the previous subsection combines two commonly used RRT improvement algorithms, which enables the new algorithm to achieve faster running efficiency as well as path results that tend to be asymptotically optimal. This solves the first defect of the RRT algorithm effectively. However, the dual RRT optimization still suffers from the strong randomness of the path results when the number of vehicles increases and the algorithm needs to be run repeatedly several times. This requires further improvement of the algorithm.

In order to improve the strong randomness of the algorithm, this paper introduces a path result evaluation function to further improve the stability of the dual RRT optimization. As Figure 4 shows, that the dual RRT optimization algorithm is run multiple times at the starting point to obtain multiple path results, and the path evaluation function is set to select the best path as the output of the algorithm. The evaluation function needs to consider the path length, the number of corners, and the corner amplitudes, so that the final path has a shorter length and fewer and smaller turns than other paths as much as possible. Assuming that $m$ expected paths are repeatedly generated by the dual RRT optimization, where the $i$-th expected path $L_i$ contains $n_i$ nodes, the evaluation function of the path is as follows:

$$E\left(L_i\right)=ale{n}_i+b{\displaystyle \sum }_{k=2}^{n_i}\left({\theta }_k^i-{\theta }_E\right)+c{N}_i$$

(5)

In Equation (5), $E_i$ is the evaluation index of the $i$-th expected path; $le{n}_i$ is the total path length of the $i$-th expected path; ${\theta }_k^i$ is the angle between the forward direction of the current $k$. -th node of the $i$-th expected path and the forward direction of the previous node; ${\theta }_E$ is the corner threshold when $\theta$ is greater than ${\theta }_s$, it is considered that a corner will occur when the vehicle travels along this expected path; $N_i$ denotes the number of corners; $a$, $b$, and $c$ indicate the weights of each index, respectively. By comparing the path evaluation index of each expected path, the path with the smallest evaluation index is selected as the final output of the algorithm. The specific algorithm framework is shown in Algorithm 2.

Algorithm 2: Evaluation Function
1	Input: $x_{start}$,$x_{end}$
2	Output: $P_{min}$
4	while i = 1 < n
5	$L_i$←Dual RRT Optimization ($x_{start}$, $x_{end}$)
6	$S\left(i\right)$←$E\left(L_i\right)$
7	end
8	$S\left(k\right)$ = $min\left\{S\left(1\right), S\left(2\right), S\left(3\right), \dots , S\left(n\right)\right\}$
9	$P_{min}$←$L_k$
10	return $P_{min}$

Based on the dual RRT optimization, the algorithm further shortens the path length and smoothes the curve by introducing the path evaluation function. Repeatedly running the algorithm many times can still get approximate results, effectively reducing the impact of strong randomness.

2.4. MUV Formation Shape Generation Based on Dual RRT Optimization

For a large number of MUVs with randomly distributed initial positions in a finite spatial plane, it is very easy to cause collisions among vehicles when the traditional formation control method is used to control the vehicles to form the expected formation at a specified location, and the increase in the number of vehicles makes the surrounding environment of each vehicle become very complex during the driving process. As a common path regularization algorithm, RRT has the problems of non-optimal routes and excessive randomness, which also makes the path results obtained using RRT full of more uncertainties when the number of vehicles increases.

To solve the above problems, this paper uses the dual RRT optimization algorithm for path planning for each vehicle within the MUVs, driving sequentially to the target point according to the vehicle markers. While ensuring collision avoidance among vehicles and completing the formation arrangement, the algorithm shortens and smoothes the path results, while improving the stability of the path results.

For vehicles on the move, the obstacles that need to be avoided are not only those originally existing on the map but also the rest of the vehicles within the MUVs, which makes the vehicle’s own environment more complex during the driving process. In order to ensure that a safe distance is maintained during the driving process to avoid collision, this paper adopts the way of expanding the radius to set the obstacle avoidance area of the vehicle, as shown in Figure 5a, which is regarded as maintaining the safe distance when the vehicle is driving outside the area. In the whole experimental process, each vehicle and its obstacle avoidance area can be simplified as shown in Figure 5b, the red circle center represents the unmanned vehicle, and the black dashed part represents the obstacle avoidance area formed by this unmanned vehicle based on the obstacle avoidance radius, where $r$ indicates the vehicle length and $R$ indicates the obstacle avoidance radius

When MUVs move towards an area on the map and form a formation, the density of vehicles in the area increases, and the more the number of vehicles, the more obvious this phenomenon is. In addition, RRT and its improved algorithms essentially obtain paths based on the known surroundings, and the RRT algorithms are unable to update the path results if the obstacles move during the vehicle’s progress. Therefore, RRT is suitable for path planning with static obstacle maps rather than dynamic path planning. Considering the above two factors, this paper chooses to plan the path for each vehicle in the MUVs in order of vehicle number after the target position is obtained, and while a vehicle is moving, the other vehicles remain stationary. This has the advantage of avoiding overly complex surroundings caused by many vehicles moving towards the target points at the same time, and making full use of the characteristics of RRT to reduce the possibility of collisions between vehicles. As more and more vehicles complete planning, there are fewer and fewer obstacles on the map, which makes the planning task easier for subsequent vehicles and improves the efficiency of the algorithm.

First, on the premise of ensuring good communication among vehicles, the position of each vehicle in the current coordinate system is obtained, which is the starting point of each vehicle. Then, the position point coordinates are generated in the target area according to the geometric configuration of the expected formation, and according to the label assigned to each vehicle in the MUVs, as the respective target point. Subsequently, based on the starting and target points, the path of the first vehicle is obtained using dual RRT optimization, and the vehicles will follow this path to reach the desired target location. Completing path regularization is considered that the vehicle has moved to the target point, and the vehicle that has completed planning no longer exists on the map for subsequent vehicle planning. The above process is repeated until all vehicles have completed the path regularization. At this point, all vehicles reach their respective target positions and form the MUVs’ desired formation shape.

3. Simulation and Experiment

In this section, the algorithm simulation experiments will be given. Firstly, it is given that the performance of the traditional UGV formation control on the formation problem decreases, when the number of vehicles increases. After that, the results of the dual RRT optimization are compared with the commonly used RRT improvement algorithm with different numbers of vehicles under the obstacle-free map. The comparison of path length and running time of different algorithms is given, as well as the stability comparison of each algorithm under multiple runs. Finally, the paper gives a comparison of the results of the dual RRT optimization and other algorithms under the map with obstacles. The advantages of this algorithm compared with other algorithms are proved through sufficient experiments.

3.1. The Traditional UGV Formation Control Method

The traditional UGV formation control method is to directly give the geometric configuration of the expected formation. By having the vehicles track each node of geometric configuration, the expected formation target is eventually formed. This method is fast and effective when the number of vehicles is small. When each vehicle is not required to have a fixed position within the formation, the proximity principle can also be used to make the target point as close as possible to the initial position of each vehicle while allowing the vehicle to track the nearest node, which can effectively reduce collisions among vehicles and speed up the formation assembly.

However, in most practical applications, including military, civilian, etc., the vehicles in the convoy are not unmarked and need to be arranged in formation according to a defined numbering sequence in the expected formation. In this case, the vehicle formation cannot use the proximity principle to plan target points for vehicles, which makes the vehicles in the process of tracking their respective target points produce the possibility of path intersection, and therefore it is very easy to lead to the occurrence of collisions between vehicles. Figure 6 gives the vehicle trajectories derived by using the traditional UGV formation control method when the number of vehicles under the 500 × 500 m² obstacle-free map is 5, 15, and 30, vehicles respectively.

Since the traditional UGV formation control method does not have obstacle avoidance, this method is suitable for a small number of MUVs to form up in a small area. When the number of vehicles increases and MUVs are needed to form up to a specified area, the traditional UGV formation control method will lead to intersecting vehicle trajectories and increase the chance of inter-vehicle collisions during driving. In order to demonstrate this more intuitively, Figure 7 gives the change in the interval distances between vehicles of the traditional UGV formation control method under different numbers of vehicles. It can be seen that there are several situations where the vehicle spacing is below the red safety range in the moving process.

3.2. Dual RRT Optimization

From the experimental results in the previous section, we can see that the traditional UGV formation control method cannot effectively cope with the formation problem of a large number of MUVs. Next, the simulation experimental results of the dual RRT optimization are given. To verify the superiority of the algorithms, the final path results of RRT-connect, RRT*, and dual RRT optimization are compared under the maps without and with obstacles, respectively, and the corresponding analysis is given in this paper.

3.2.1. Under Obstacle-Free Maps

Under the obstacle-free maps, there are no obstacles in the spatial plane, and for the vehicles within the MUVs, the collision avoidance objects to be considered are the rest of the vehicles except themselves. Since there is no influence of other obstacles, the complexity of the environment when performing path planning is only determined by the number of vehicles in the fleet. Therefore, in this paper, simulation comparison experiments are conducted for 5, 15, and 30 vehicles, respectively, under the same obstacle-free map using the RRT-connect algorithm, the RRT* algorithm, and the dual RRT optimization, and the respective results are presented.

Table 1. Experimental environmental parameters under obstacle-free maps.

Number of Vehicles	Map Size/m2	Radius of Obstacle Avoidance/m	Expected Formation	Distance between Vehicles in Formation/m
5	500 × 500	15	5 × 1 square	15
15	500 × 500	15	5 × 3 square	15
30	500 × 500	15	5 × 6 square	15

lists the relevant experimental index parameters such as map size, vehicle-related attributes, expected formation, etc.

To visualize the comparison of the path results of each algorithm, it is assumed that the unmanned vehicles in the experiment are all similar, the vehicles all move at the same uniform speed and satisfy the vehicle non-completeness constraint. The initial positions of the vehicles are randomly distributed on the map, and the specific initial positions of different numbers of MUVs under the map are shown in Figure 8, where the vehicles have been simplified in the way of Figure 5b, i.e., the red circle center represents the UGV, and the black dashed part represents the obstacle avoidance area formed by this vehicle based on its radius of obstacle avoidance. For easy identification, specific vehicle markers have been shown near each vehicle, and the expected formation is generated in the northeast corner of the map. It can be seen that the higher the number of vehicles in the MUVs, the higher the density of vehicle distribution within the map, the more likely collisions among vehicles will occur, and the planning of obstacle avoidance paths will be more challenging.

The path results of each algorithm for the same vehicle with different numbers of vehicles are shown in Figure 9, Figure 10 and Figure 11. The green dot indicates the initial position of the vehicle, the red dot at the top right indicates the target position of the vehicle, and the red dashed dots indicate the vehicle that has completed path regularization.

As can be seen from the figure, under the condition of no obstacles, the obstacles on the map are only the vehicles themselves. When the number of vehicles increases, the distribution density of vehicles on the map becomes larger, the surrounding environment becomes more crowded, and the requirements for path planning will be more demanding. The paths obtained by these RRT algorithms avoid the obstacle avoidance areas of other vehicles and keep a safe distance between vehicles during the travel, which effectively avoids the collision problem caused by the increase in the number of vehicles, yet the path results of different algorithms differ significantly. RRT-connect produces many unnecessary redundant routes and large corners in the path results, which makes the overall results not smooth enough. RRT* has better results in path smoothness and path length compared with the former. Additionally, dual RRT optimization proposed in this paper further shortens the path length while retaining the advantages of RRT*.

Figure 12a gives the total path length of the path results of each algorithm after the MUVs complete the formation arrangement for different numbers of vehicles. It can be seen that the total path length under the dual RRT optimization is the smallest among all the algorithm results and has better path results than the other algorithms. Figure 12b represents the algorithm running time for different numbers of vehicles. The graph shows that RRT-connect always takes the shortest time regardless of the number of vehicles in the MUVs. In contrast, RRT* consistently takes longer to complete the algorithm, and dual RRT optimization takes somewhere in between, but significantly less than RRT*. Combined with the previous results on path length, dual RRT optimization can obtain similar or even better path results than RRT* using far less time than the RRT* algorithm takes to run.

To better show the advantages of dual RRT optimization, we consider the overall time used from the time the algorithm starts running to the time the convoy travels to complete the desired formation based on the path results, assuming that the vehicle maintains a uniform speed of 10 m/s during the movement, the final result is shown in Figure 13. This also shows that, from a practical point of view, dual RRT optimization can complete the task of the formation of a large number of MUVs in a much shorter time, effectively improving the overall speed of the process.

In order to verify the improvement of the algorithm on randomness, this paper conducts 10 repetitions of the experiment in the above environment, and the total length change of each algorithm path result is shown in Figure 14.

As can be seen from the above figure, with the increase in the number of experiments and the number of vehicles, the path results of RRT-connect and RRT* will produce large fluctuations due to the randomness of RRT; in contrast, the path results of dual RRT optimization are more stable, and the total path length under the obstacle-free map is always smaller than the path results of the RRT*, which effectively reduces the impact of the randomness of the RRT itself on the algorithm results and improves the stability of the algorithm.

3.2.2. Under Obstacle Maps

When there are obstacles in the spatial plane, the vehicles need to consider more complex collision avoidance objects, and the paths obtained at this time are more tortuous, which also puts forward higher requirements on the optimization ability of the algorithm. Therefore, this paper sets up two different obstacle maps and conducts simulation comparison experiments using the RRT-connect algorithm, RRT*, and dual RRT optimization proposed in this paper, so as to verify the adaptability of the algorithm to the complex environment.

Table 2. Experimental environmental parameters under obstacle maps.

Map Size/m2	Number of Vehicles	Radius of Obstacle Avoidance/m	Distance between Vehicles in Formation/m
500 × 500	12	15	15

lists the corresponding experimental index parameters

The initial locations of the vehicles are randomly distributed on the two maps, and the specific locations of the vehicles on each map are shown in Figure 15, where the vehicles have been simplified in a diagrammatic way, i.e., the red circle center represents the unmanned vehicle and the black dashed area represents the obstacle avoidance area formed by this unmanned vehicle based on the obstacle avoidance radius. Affected by the map size and obstacles, the number of vehicles for this experiment is 12, and the specific vehicle markings have been shown near each vehicle. The expected formation is a 3 × 4 square generated in the northeast corner of the map.

In order to see the optimization effect of dual RRT optimization on the path more intuitively, the comparison of the results before and after the path optimization is given in this paper. Taking vehicle No. 4, vehicle No. 6, and vehicle No. 11 as examples, Figure 16 and Figure 17 give the results of the obstacle avoidance paths obtained by each vehicle under dual RRT optimization under different maps, respectively. The dashed line in the figure indicates the initial path obtained in the first stage of the algorithm; the solid line indicates the final path results obtained after the optimization in the second stage of the algorithm.

As can be seen from the above figure, the overall path result is more complex than that without obstacles. While ensuring good obstacle avoidance, the second stage of the optimization algorithm effectively reduces the initial path curvature and cuts out unnecessary large corners to make the route smoother. To further demonstrate the advantages of dual RRT optimization over the traditional RRT improvement algorithms, this paper conducts multi-unmanned vehicle formation assembly experiments using RRT-connect, RRT*, and dual RRT optimization for the same map, respectively. Taking vehicle No. 1 as an example, the path results of each algorithm for two maps are shown in Figure 18, Figure 19 and Figure 20 indicating the average total length of path results and average running time after 10 runs of each algorithm.

It can be seen that even under different obstacle environments, dual RRT optimization achieves better results in terms of overall capability. In terms of the average total path length, the total length of dual RRT optimization results is significantly smaller than RRT-connect results, reaching the level of RRT* results; while in terms of the algorithm running time, dual RRT optimization retains the efficient running efficiency of RRT-connect, and obtains the approximate RRT* path results in the shortest possible time, reflecting the algorithm’s dual advantages.

4. Conclusions

In this paper, we propose dual RRT optimization to solve the problem of formation shape generation for a large number of MUVs. Due to the increase in the number of vehicles, which leads to the inability of traditional formation control methods to deal with it effectively, a multi-vehicle path planning perspective is used. Considering the shortcomings of RRT as a commonly used path planning algorithms such as non-optimal solution and strong randomness, we use dual RRT optimization to decouple the path planning into global path search and local path optimization. First, the results of the global path search are obtained by RRT-connect. Using the advantage of RRT-connect can quickly get an initial path, but the path is not smooth and has too many redundant paths at this time. After that, we use RRT* to optimize the initial path locally. After finding the section that needs to be optimized, RRT* performs another path search on the section and replaces the original path. Due to its asymptotic optimality, the path obtained again is shorter and smoother than the initial path. Finally, the algorithm can further optimize the path results by introducing the path evaluation function to optimize the results of multiple runs. This paper also compares the effectiveness of the dual RRT optimization algorithm with other algorithms for path planning of MUVs with different numbers of vehicles under different map environments. The results show that dual RRT optimization can significantly shorten the run time while avoiding obstacles and achieve even better path results than RRT*, and multiple runs can still ensure stable path results and complete the task of formation shape generation in the shortest time. In addition, dynamic obstacle avoidance is also very important for unmanned vehicles. We will also include the exploration of dynamic collision avoidance for a large number of MUVs in our future research.