Abstract

In recent years, the path planning of robot has been a hot research direction, and multirobot formation has practical application prospect in our life. This article proposes a hybrid path planning algorithm applied to robot formation. The improved Rapidly Exploring Random Trees algorithm PQ-RRT with new distance evaluation function is used as a global planning algorithm to generate the initial global path. The determined parent nodes and child nodes are used as the starting points and target points of the local planning algorithm, respectively. The dynamic window approach is used as the local planning algorithm to avoid dynamic obstacles. At the same time, the algorithm restricts the movement of robots inside the formation to avoid internal collisions. The local optimal path is selected by the evaluation function containing the possibility of formation collision. Therefore, multiple mobile robots can quickly and safely reach the global target point in a complex environment with dynamic and static obstacles through the hybrid path planning algorithm. Numerical simulations are given to verify the effectiveness and superiority of the proposed hybrid path planning algorithm.

1. Introduction

With the continuous deepening of network applications, especially the rapid development of the Internet of Things, big data, cloud computing, and edge computing, the integration of information and physical systems has become increasingly close. Also, the connection between the network and human society has become much more closer. Cyber-physical-social system (CPSS) includes system engineering such as embedded environment perception, dynamic analysis of human organization behavior, network communication, and network control. Such CPSSs have functions of computing, communication, precise control, remote collaboration, and autonomy. Technologies such as artificial intelligence, multiagent, and machine learning have been more widely used in the CPSS area [1–4]. As a typical representative of agents, multiple autonomous robots, including unmanned aerial vehicles (UAVs), automatic ground vehicles (AGVs), and unmanned underwater vehicles (UUVs), play an important role in military and civil fields [5–13]. To ensure robots carry out related work in our life efficiently and safely, path planning, which means finding a feasible path without collisions from the starting state to the target state, has been a hot research point in the field of mobile robot applications [14–18]. At present, path planning algorithms can be divided into global path planning algorithms and local path planning algorithms, which mainly include geometric algorithms, artificial potential field algorithms, grid-based search algorithms, and sampling-based algorithms. Among them, the sampling-based algorithm has received extensive attention because of its superior performance in high-dimensional state space. Furthermore, the probability of finding a feasible path in space approaches 1 as the sampling number approaches infinity.

Rapidly Exploring Random Tree (RRT) [19], as a representative of sampling-based path planning, has attracted wide attention of the research community. A large number of improved algorithms for RRT have emerged in the past decade. RRT-connect [20] is a dual-tree RRT algorithm. Two random trees are generated from the start point and the end point, respectively. However, neither RRT nor RRT-connect considers the path cost; therefore, the optimality of the algorithm cannot be guaranteed. Based on the previous algorithms, the RRT algorithm [21] was proposed, in which the cost of the path is covered. Also, the steps of selection and rewiring are added. This algorithm obtains progressive optimality, which has become a breakthrough in the development of the Rapidly Exploring Random Trees algorithm. However, the convergence speed of the algorithm has become a new problem. One of the fundamental reasons for the slow convergence speed of RRT is its global exploration, which does not have a specific direction. To solve this problem, P-RRT was proposed in [22]. This algorithm incorporates APF into RRT, and the addition of APF [23] provides a direction for exploration, making P-RRT converge faster than RRT. In addition, another algorithm named Quick-RRT [24] was proposed which uses the triangle inequality to optimize the process of selecting the parent node and connection. Compared with RRT, it has a faster convergence speed. PQ-RRT [25] combines P-RRT and Quick-RRT, which makes the algorithm generate a better initial solution and can quickly converge to obtain a relatively optimal solution. However, the dynamic obstacles are not considered in PQ-RRT. Therefore, it can be only used for static path planning and still has some limitations.

In the local path planning algorithm part, the dynamic window approach (DWA) [26] and other algorithms plan the path of the mobile robot through the surrounding information collected by the sensor. However, these algorithms usually do not consider the global map information. Tang et al. proposed a high-speed USV local response obstacle avoidance based on the DWA method [27]. However, it fails to consider the global map information. It is difficult to find the optimal path in the global range using only this kind of algorithm. Based on this kind of situation, various hybrid planning algorithms have been proposed [28–30].

Motivated by the above discussions, we improve the traditional PQ-RRT algorithm and propose a hybrid planning algorithm—New Potential Quick-RRT (NPQ-RRT), which takes the attitude adjustment angle of the robot into consideration and adds the DWA local planning algorithm. Moreover, the algorithm is extended and applied to the path planning problem of multirobot.

The remainder of the paper is organized as follows. The problem description is addressed in Section 2, and the traditional PQ-RRT algorithm is explained in Section 3. Then, the hybrid planning algorithm NPQ-RRT is presented in Section 4. In Section 5, simulation results are provided to show the effectiveness of the proposed approach. Finally, the paper is concluded in Section 6.

2. Problem Description

Throughout the paper, denotes the set of real numbers, denotes the set of natural numbers, and denotes the space of real -vectors.

We consider an -robot system, where each robot moves in the region. Let be the obstacle area and the unobstructed area . The start position and the target position of the robot are and , respectively.

A trajectory of robot is defined as follows: , where is the duration of the trajectory. In addition, and . The trajectory is obstacle-free if for all . The cost function finds the path length in terms of Euclidean distance function.

Trajectory is said to be conflict-free if it is obstacle-free and also keeps robot at a safety distance from all other robots. , where and represent the positions of robots and , , , , and . The total cost of the trajectories is defined as .

Our goal is to find the trajectory set in which is conflict-free.

The Rapidly Exploring Random Trees is a sampling-based planning method that builds an undirected graph on a known map through sampling and then finds a relatively optimal path through a search method. The PQ-RRT is an improved version after adding the target attraction function RGD and the deep parent node search function Ancestry, which can generate a better initial solution and quickly converge to obtain the optimal solution. The pseudocode of the specific algorithm flow is shown in Algorithm 1 [25], where represents the generated graph.

 Input: ;
 Output:
(1)for i = 1 to n do
(2) ;
(3) ;
(4) ;
(5) ;
(6) if CollisionFree() then
(7)   ;
(8)  ;
(9)  () ChooseParent();
(10)  ;
(11)  ;
(12)  ;
(13) end
(14)end
Algorithm 1 
PQ-RRT [25].

The related functions are defined as follows: SampleFree: randomly pick points in the global map. Here, the return value is random point . RGD: the adjustment function that adjusts the random point under the gravitational force of the target point. Here, the return value is the improved sample . NearestObstacle function calculates the distance from to the obstacle space . The parameter represents the number of iterations. represents the safety distance, and represents the step size. The specific pseudocode is shown in Algorithm 2. Nearest: distance evaluation function. The Euclidean distance function is selected in this situation, which returns the node closest to in the graph and defines the node as . Steer: this function connects two given points. The return value of the function is the segment between the two points. CollisionFree: this function detects whether there is a collision with a static obstacle. Near: given a graph , it returns a set , which contains the nodes in the range with as the center and as the radius. Ancestry: this function deeply searches the parent node of each point in . And it returns the parent node set of . The specific process is as follows: in a given graph , for a node , a natural number , if the depth , it returns and otherwise returns the th parent of . ChooseParent: compare the cost of each path and then determine the parent node and the path . Rewire-PQ-RRT: a function to generate the final path diagram.

 Input: random point , target point
 Output: improved point
(1)for n = 1 to z do
(2) 
(3) ;
(4) ifthen
(5)  return;
(6) else
(7)  ;
(8) end
(9)end
Algorithm 2 
RGD().

4. An Improved Algorithm NPQ-RRT for Multirobot

4.1. Overall Ideas

For the robot formation, we divide it into a leader and several followers. When planning the formation path, we first select the leader as the research object and generate a global path through the global planning algorithm. The path is taken as the target path of the robot formation. Each node in the global path is taken as the local starting point and the local target point. Subsequently, for the movement between the local starting point and the local target point of the robot formation, a local path is generated by the local planning algorithm. Compared with the traditional RRT, RRT, and other algorithms, the global planning algorithm PQ-RRT has excellent global search capabilities, but it still has limitations: all these algorithms mentioned above discuss fast random expansion search while ignoring the characteristics of the robot to find a feasible path in the global state. However, when applying this algorithm to practical path planning, the attitude adjustment angle of the robot will have an impact on the operation of the algorithm, as shown in Figure 1.


Figure 1 
The influence of attitude adjustment angle on the selection of the nearest node.

According to the steps in the traditional RRT series algorithms, the closest point to Position 1 is evaluated according to the Euclidean distance. It is concluded that Position 2 is the closest point to Position 1. However, for a mobile robot, there is an attitude adjustment angle to Position 2. First, the robot needs to adjust the direction that it faces and then goes to Position 2. For Position 3, it only needs to travel along a straight line. Therefore, the attitude adjustment angle needs to be incorporated into the process of calculating the closest point to Position 1 so as to balance the problems of algorithm convergence time and path smoothness caused by the cumulative rotation angle in the practical application.

At the same time, there are dynamic obstacles in the real environment, which increases the safety risk of mobile robots that perform path planning. The local planning algorithms can better solve these problems and optimize the local path when taking into account dynamic obstacle avoidance. Based on the abovementioned requirements, this paper proposes a hybrid path planning algorithm that considers the attitude adjustment angle.

The local path planner generates a local path for each robot in the formation to follow the local target and avoid obstacles on the local map. By using local targets, the local path planner and the global path planner are combined. The global planner plans the path to the target point in a relatively long period of time for the leader, and the followers follow the leader’s path. The local planner updates the trajectory in real time to avoid dynamic and static obstacles. The main idea is shown in Figure 2.


Figure 2 
The intersection of global path planner and local path planner.
4.2. Specific Steps
4.2.1. The Generation of Global Target Path

The hybrid planning algorithm NPQ-RRT proposed in this paper improves the problems of the attitude adjustment and dynamic obstacle avoidance. The leader generates the formation target path through the global planning part of the algorithm.

After applying the new distance evaluation function and local programming algorithm to the PQ-RRT algorithm, the pseudocode of NPQ-RRT is shown in Algorithm 3. In the pseudocode, most of the function operations are consistent with the operations of related functions in Algorithm 1. The specific operations of the newly proposed distance evaluation function NewNearest are given as follows.

 Input:
 Output:
(1)for i = 1 to n do
(2) ;
(3) ;
(4) ;
(5) ;
(6) if CollisionFree(k) then
(7)  ;
(8)  ;
(9)  
(10)  
(11)  ;
(12)  ;
(13)  ;
(14) end
(15)end
Algorithm 3 
NPQ-RRT.

The function incorporates the attitude adjustment angle as a new influencing factor into the distance consideration range:where and are the evaluation weights of velocity and angular velocity, respectively. Since the local starting point is close to the local target point, is the Euclidean distance between the th node and the sample point . is the attitude adjustment angle, as shown in Figure 3.


Figure 3 
The attitude adjustment of the mobile robot.

and are defined as follows:where and are the coordinates of the node and the modified sample . is the angle between the orientation of the mobile robot and the -axis of the global coordinate system, as shown in Figure 3.

Since and have different units in the new distance evaluation function, it is of no practical significance to directly add and subtract numerically. The selection of the evaluation weights and becomes the core of the evaluation function. Under the circumstances, the time to reach the destination can be used to measure the length of the distance. When measuring the distance between two points, we assume that the forward speed and the attitude adjustment angular velocity are constant. Therefore, the new distance evaluation function will select the time as the evaluation index. The smaller the value of the function, the shorter the distance between the two points. The evaluation weights and are numerically equal to the reciprocal of the robot’s speed and angular velocity at the current moment. Therefore, the new distance evaluation function returns the node that minimizes the function value in the graph and then defines this node as .

After the leader completes the steps in the pseudocode, the graph is obtained which is regarded as the global goal path of the formation.

4.2.2. The Generation of Local Path

For each robot in the formation, take the selected and as the starting point and target point of the local planning, respectively. Using the function Local to obtain the , the specific operations are given as follows: Step 1: generate velocity space [26]. Define the th robot’s velocity set as follows: where and represent the minimum and maximum velocities that the robot can reach, respectively. and represent the minimum and maximum angular velocities that the robot can reach, respectively. Due to the limited torque of the motor, there are maximum acceleration and deceleration limits. The achievable velocity set affected by the motor performance is defined as where and are the current velocity and angular velocity of the th mobile robot, respectively. and correspond to the maximum acceleration during deceleration and the maximum acceleration during acceleration. and correspond to the maximum angular acceleration during deceleration and the maximum angular acceleration during acceleration. The opposite direction of the original movement direction is defined as the deceleration direction. When the robot decelerates with the maximum acceleration at the current velocity, it can be guaranteed to stop before encountering an obstacle, then the velocity is safe. The safe velocity set is defined as follows: where the function represents the distance between the th robot and the nearest obstacle on the current trajectory. The final definition of the th robot’s feasible velocity space set is Step 2: obtain the predicted trajectory corresponding to each velocity and avoid collisions within the formation. First of all, we need to build a model of the robot. Assume that the robot only has two movement modes: forward and rotating. At the current moment, the robot has velocities and . Consider two adjacent moments, as shown in Figure 4. Since the robot’s adjacent moment (usually measured by the code disc sampling period in ms) is relatively short, the motion at the two adjacent moments can be regarded as uniform motion. The trajectory of the motion can be regarded as a straight line. Within , the robot moves in the current direction. is the angle between the direction of the mobile robot and the -axis of the global coordinate system. In the real coordinate system, the displacement of the robot moving in the -axis direction of the global coordinate system and the displacement of the -axis movement in the global coordinate system are defined as follows: As for the trajectory of the robot, represents the previous moment and represents the current moment. , , and represent the robot’s position information and orientation angle information, respectively, which are defined as Substituting the velocity space of the robot obtained in the first step into equations (8)–(10), we can obtain the corresponding trajectory expression. In addition, due to the possibility of collisions between the robots in the robot formation, we make further constraints. For the obtained local predicted trajectories of two adjacent robots, when the predicted trajectories of the two robots have no intersection, they will not collide. On the contrary, when the two predicted trajectories have intersections, they may collide, as shown in Figure 5. Define the positions of the two robots as and , respectively. The meeting point is and the velocities of the two robots are and . The distances and are defined as According to the distance, we can constrain the velocities of the two robots: if , then Else, if , then where is a constant that can be set according to the relationship between the velocity and the maximum acceleration. Step 3: select the locally optimal path through the evaluation function. Define the evaluation function of the th robot’s local path as where and the variables and are the initial weights of the function. is the total number of all trajectories sampled. The function is used to evaluate the heading score, which is defined as follows: The function is defined as where is the angle between the current direction that the mobile robot is facing and the direction when it reaches the local target point. It can be defined as where is the coordinate of the local target point in the global map and is the coordinate of the predicted position of the th robot in the global map. The function is used to evaluate the safety distance score, which is defined as follows: The function can be defined as where is a random point on the trajectory, represents the corresponding obstacle, and dist is a function to calculate Euclidean distance. If there are no obstacles on this trajectory, set as a constant. The function is used to evaluate the speed score of the current trajectory. The function corresponds to the velocity value of the current trajectory. is defined as follows: The function is used to evaluate the score of internal collision probability. The function corresponds to the number of intersections with the predicted trajectories of other robots in the formation. is defined as follows:


Figure 4 
The model of the mobile robot.

Figure 5 
The model of multirobots.

The path with the highest overall score obtained by the evaluation function evaluation is the optimal path. This path is regarded as the local path and is combined with the global planning path to obtain the final path.

5. Simulation

In this section, we discuss the practical effects of the proposed hybrid path planning algorithm NPQ-RRT in complex environments with dynamic and static obstacles. We perform a comparative simulation experiment through MATLAB on an Intel Core i5 4-core, 8 GB RAM computer.

5.1. Build a Simulation Environment and Set Relevant Parameters

We first construct a 1000  1000 map environment. We set (0,0) as the starting point for global planning and (1000,1000) as the target point for global planning. There are 5 static obstacles, which are represented by green rectangles. The obstacles are distributed in this environment, as shown in Figure 6.


Figure 6 
The global environment of simulation.

After the command to start path planning is issued, additional obstacles (not overlapping with the current mobile robot position) will be randomly generated as dynamic obstacles in the environment at any given time, as shown in Figure 7.


Figure 7 
The intersection of global path planner and local path planner.

In this simulation, the relevant parameters of the NPQ-RRT hybrid path planning algorithm are set as follows: in the RGD function,  =  = 1 and  = 80. In the ChooseParentfunction, . In the Local function, the initial weights , and of the evaluation function are set to 0.05, 0.2, 0.1, and 0.1, respectively. The maximum speed of the mobile robot is 1 m/s, the maximum angular speed is 0.5 rad/s, and the acceleration range is . The angular acceleration range is , the velocity resolution is 5 m/s, and the angular velocity resolution is 6 rad/s.

5.2. The Simulation Design and Results
5.2.1. Group 1

To verify the performance of using the improved distance evaluation function in the path planning process of mobile robots, we conduct 10 sets of comparative tests without considering dynamic obstacles. The original PQ-RRT algorithm and our proposed NPQ-RRT perform the same path planning missions. The sum of the changes in the heading angle between the nodes makes up the entire adjustment angles. The adjustment angular velocity  = 0.5 rad/s is used for obtaining the posture adjustment time. After conducting 10 groups of calculations, the average results in Table 1 are obtained.

Table 1 
Comparing the time of attitude adjustment.

Figures 8 and 9 show the results of path planning for NPQ-RRT and PQ-RRT, respectively. It can be seen that the improved algorithm’s robot attitude adjustment time is shortened, the algorithm’s running speed is sharply accelerated, and the resulting path is smoother than the original PQ-RRT.


Figure 8 
The path planned by NPQ-RRT.

Figure 9 
The path planned by PQ-RRT.
5.2.2. Group 2

To verify that the hybrid path planning algorithm NPQ-RRT after adding local planning has better dynamic obstacle avoidance performance than the original PQ-RRT algorithm, we carry out the following comparative test in consideration of obstacle dynamic obstacles, as shown in Figure 7.

The confirmed and are used as the target point and starting point of local planning. The original algorithm PQ-RRT lacks a local path planning algorithm; that is, it moves along the line of and (represented by the red line in Figure 7). It is unable to reach the target due to the existence of dynamic obstacles, causing path planning to fail. After using NPQ-RRT with local planning algorithm, the mobile robot successfully avoids obstacles and reaches the local target point (the blue line in Figure 7).

5.2.3. Group 3

To compare the performance of the common RRT mixed planning algorithm and NPQ-RRT, we perform experiments in the same obstacle environment. The results are obtained, as shown in Figures 10 and 11.


Figure 10 
The path planned by hybrid RRT.

Figure 11 
The path planned by NPQ-RRT.

It can be seen that NPQ-RRT can obtain a smoother path with a shorter overall length and better overall performance compared to the ordinary RRT mixed planning algorithm.

5.2.4. Group 4

To test the performance of the algorithm when applied to multirobot path planning, we take three robots as an example to perform the following test. Among them, the small black circle represents the robot. The green area represents the detection range of the robotic lidar. The big red circle represents obstacles randomly generated on the map. The blue lines represent the local paths generated by each robot. We use confirmed and as the target point and starting point of the formation planning. Obstacles are randomly generated on the map, and the robot formation moves from the starting point to the target point at the same time. For randomly generated obstacles, the three robots use local planning algorithms to plan their paths to generate obstacle-free paths. At the same time, each robot in the robot formation adopts an avoidance strategy to maintain a safe distance to avoid collisions in the formation. The result is shown in Figures 12–15.


Figure 12 
The initial states of the robots.

Figure 13 
The positions of the robots:  = 10s.

Figure 14 
The positions of the robots:  = 15s.

Figure 15 
The positions of the robots:  = 20s.

Through the simulation results, we can find that the robot formation completes the obstacle avoidance to the target point, which verifies the feasibility of the proposed algorithm applied to the robot formation.

Based on the above simulation results, it can be found that NPQ-RRT has better dynamic obstacle avoidance ability and can effectively shorten the attitude adjustment time and get a smoother path. In addition, this algorithm can also obtain ideal results when it is applied to the robot formation path planning.

6. Conclusions

This paper proposes a hybrid path planning algorithm NPQ-RRT, which studies the path planning of multirobot in an environment with dynamic and static obstacles. NPQ-RRT chooses the improved version of the Rapidly Exploring Random Trees algorithm PQ-RRT as the global planning algorithm. Combined with the attitude adjustment angle of the mobile robot, we propose a new distance evaluation function, which optimizes the selection of the nearest node. After the parent node and the child node are identified, the local planning step is added. The parent node and child node are used as the local starting point and the local target point to generate a local path avoiding dynamic obstacles. The global path is obtained by tracking the local target point. At the same time, the algorithm optimizes the potential collisions within the robot formation to ensure the safety of the robots. Also, the potential collision possibility in the formation is added as a new evaluation index into the evaluation function to select the optimal path. The simulation results show that compared with PQ-RRT, the hybrid path planning algorithm NPQ-RRT has better dynamic obstacle avoidance ability. Furthermore, it can get a relatively better path compared with the ordinary RRT hybrid planning algorithm. When applied to the path planning of robot formation, it can effectively shorten the attitude adjustment time and obtain a smoother path.

Data Availability

No data were used to support this study.

Conflicts of Interest

The authors declare that they have no conflicts of interest.

Acknowledgments

This work was supported in part by the National Natural Science Foundation of China under grant no. 61973064, the Natural Science Foundation of Hebei Province of China under grant no. F2019501126, the Natural Science Foundation of Liaoning Province of China under grant no. 2020-KF-11-03, and the Fundamental Research Funds for the Central Universities under grant no. N182304013.