NPQ-RRT ∗ : An Improved RRT ∗ Approach to Hybrid Path Planning

. 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 eﬀectiveness and superiority of the proposed hybrid path planning algorithm.


Introduction
With the continuous deepening of network applications, especially the rapid development of the Internet of ings, 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. Cyberphysical-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][2][3][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][6][7][8][9][10][11][12][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][15][16][17][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 samplingbased 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.
is 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].
is 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 * . erefore, 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][29][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. e remainder of the paper is organized as follows. e problem description is addressed in Section 2, and the traditional PQ-RRT * algorithm is explained in Section 3. en, 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.

Problem Description
roughout the paper, R denotes the set of real numbers, N denotes the set of natural numbers, and R d denotes the space of real d-vectors.
We consider an n-robot system, where each robot moves in the region. Let X obs ⊂ X be the obstacle area and the unobstructed area X free � (X/X obs ). e start position and the target position of the robot i are x i init and x i goal , respectively.
A trajectory of robot i (i � 1, 2, . . . , n) is defined as follows: k i : [0, τ i ] ⟶ X, where τ i is the duration of the trajectory. In addition, e cost function c(·) finds the path length in terms of Euclidean distance function.
Trajectory k i is said to be conflict-free if it is obstacle-free and also keeps robot i at a safety distance d s > 0 from all other robots. ‖x i (t) − x j (t)‖ > d s , where x i (t) and x j (t) represent the positions of robots i and j, t ∈ [0, τ], i, j � 1, 2, . . . , n, i ≠ j, and τ � min(τ i , τ j ). e total cost of the trajectories is defined as n i�1 c(k i ). Our goal is to find the trajectory set K � k 1 , k 2 , . . . , k n in which k i is conflict-free.

Related Work
e 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. e 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. e pseudocode of the specific algorithm flow is shown in Algorithm 1 [25], where G � (V, E) represents the generated graph.
e related functions are defined as follows: SampleFree: randomly pick points in the global map.
Here, the return value is random point x rand . RGD: the adjustment function that adjusts the random point x rand under the gravitational force of the target point. Here, the return value is the improved sample x prand . NearestObstacle function calculates the distance from x prand to the obstacle space X obs . e parameter z represents the number of iterations. d obs represents the safety distance, and λ represents the step size. e specific pseudocode is shown in Algorithm 2.
Nearest: distance evaluation function.
e Euclidean distance function is selected in this situation, which returns the node closest to x prand in the graph G � (V, E) and defines the node as x nearest . Steer: this function connects two given points. e return value of the function is the segment k between the two points. CollisionFree: this function detects whether there is a collision with a static obstacle. Near: given a graph G � (V, E), it returns a set X near , which contains the nodes in the range with x prand as the center and r as the radius. Ancestry: this function deeply searches the parent node of each point in X near . And it returns the parent node set X sparent of X near . e specific process is as follows: in 2 Complexity a given graph G � (V, E), for a node V 1 , a natural number p ∈ N, if the depth p � 0, it returns ∅ and otherwise returns the pth parent of V 1 . ChooseParent: compare the cost of each path and then determine the parent node x parent and the path k parent .
Rewire-PQ-RRT * : a function to generate the final path diagram.

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. e 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.
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. erefore, 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. e 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. e 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. e 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. e local planner updates the trajectory in real time to avoid dynamic and static obstacles. e main idea is shown in Figure 2.

e Generation of Global Target
Path. e hybrid planning algorithm NPQ-RRT * proposed in this paper improves the problems of the attitude adjustment and dynamic obstacle avoidance. e 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.
e specific operations of the newly proposed distance evaluation function NewNearest are given as follows.
e function incorporates the attitude adjustment angle as a new influencing factor into the distance consideration range: where ε v and ζ w are the evaluation weights of velocity and angular velocity, respectively. Since the local starting point is close to the local target point, l is the Euclidean distance between the nth node and the sample point x prand . φ is the attitude adjustment angle, as shown in Figure 3. l and φ are defined as follows: where (x v , y v ) and (x r , y r ) are the coordinates of the node and the modified sample x prand . ϕ is the angle between the orientation of the mobile robot and the X-axis of the global coordinate system, as shown in Figure 3. Since l and ϕ have different units in the new distance evaluation function, it is of no practical significance to directly add and subtract numerically. e selection of the evaluation weights ε v and ζ w 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. erefore, the new distance evaluation function will select the time as the evaluation index. e smaller the value of the function, the shorter the distance between the two points. e evaluation weights ε v and ζ w are numerically equal to the reciprocal of the robot's speed and angular velocity at the current moment. erefore, the new distance evaluation function returns the node that minimizes the function value in the graph G � (V, E) and then defines this node as x new nearest .
After the leader completes the steps in the pseudocode, the graph G � (V, E) is obtained which is regarded as the global goal path of the formation.

e Generation of Local Path.
For each robot in the formation, take the selected x parent and x prand as the starting point and target point of the local planning, respectively. Using the function Local to obtain the LocalPath, the specific operations are given as follows: Step 1: generate velocity space [26]. Define the ith robot's velocity set V mi as follows: where v min and v max represent the minimum and maximum velocities that the robot can reach, respectively. ω min and ω max represent the minimum and maximum angular velocities that the robot can reach, respectively.

Complexity
Due to the limited torque of the motor, there are maximum acceleration and deceleration limits. e achievable velocity set V di affected by the motor performance is defined as where v ci and ω ci are the current velocity and angular velocity of the ith mobile robot, respectively. a b and a m correspond to the maximum acceleration during deceleration and the maximum acceleration during acceleration. α b and α m correspond to the maximum angular acceleration during deceleration and the maximum angular acceleration during acceleration. e 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. e safe velocity set V si is defined as follows: where the function d(v i , ω i ) represents the distance between the ith robot and the nearest obstacle on the current trajectory. e final definition of the ith 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 v(t) and ω(t). Consider two adjacent moments, as shown in Figure 4. Since the robot's adjacent moment Δt (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. e trajectory of the motion can be regarded as a straight line. Within Δt, the robot moves v(t)Δt in the current direction. ϕ(t) is the angle between the direction of the mobile robot and the X-axis of the global coordinate system. In the real coordinate system, the displacement Δx of the robot moving in the X-axis direction of the global coordinate system and the displacement Δy of the Y-axis movement in the global coordinate system are defined as follows: Δy � v(t)Δtsinϕ(t).
As for the trajectory of the robot, t represents the previous moment and t + 1 represents the current moment. x(t + 1), y(t + 1), and ϕ(t + 1) represent the robot's position information and orientation angle information, respectively, which are defined as Complexity 5 Substituting the velocity space V ai 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 p i (t) and p j (t), respectively. e meeting point is m(t) and the velocities of the two robots are v i (t) and v j (t). e distances Δl 1 and Δl 2 are defined as According to the distance, we can constrain the velocities of the two robots: if Δl 1 ≥ Δl 2 , then Else, if Δl 1 ≤ Δl 2 , 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 ith robot's local path as where (v i , ω i ) ∈ V ai and the variables α, δ, c and ρ are the initial weights of the function. m is the total number of all trajectories sampled. e function m head(v i , ω i ) is used to evaluate the heading score, which is defined as follows: e function head(v i , ω i ) is defined as where θ i 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 (x b , y b ) is the coordinate of the local target point in the global map and (x p , y p ) is the coordinate of the predicted position of the ith robot in the global map. e function m d(v i , ω i ) is used to evaluate the safety distance score, which is defined as follows: e function d(v i , ω i ) can be defined as where  6 Complexity corresponds to the velocity value of the current trajectory. m vel(v i , ω i ) is defined as follows: e function m meet(v i , ω i ) is used to evaluate the score of internal collision probability. e function meet(v i , ω i ) corresponds to the number of intersections with the predicted trajectories of other robots in the formation. m meet(v i , ω i ) is defined as follows: e path with the highest overall score obtained by the evaluation function evaluation is the optimal path. is path is regarded as the local path and is combined with the global planning path to obtain the final path.

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.

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. ere are 5 static obstacles, which are represented by green rectangles. e obstacles are distributed in this environment, as shown in Figure 6.
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.
In this simulation, the relevant parameters of the NPQ-RRT * hybrid path planning algorithm are set as follows: in the RGD function, λ � d obs � 1 and z � 80. In the ChooseParentfunction, d � 2. In the Local function, the initial weights α, δ, c, and ρ of the evaluation function are set to 0.05, 0.2, 0.1, and 0.1, respectively. e maximum speed of the mobile robot is 1 m/s, the maximum angular speed is 0.5 rad/s, and the acceleration range is [−1, 1]. e angular acceleration range is [−0.5, 0.5], the velocity resolution is 5 m/s, and the angular velocity resolution is 6 rad/s.

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. e original PQ-RRT * algorithm and our proposed NPQ-RRT * perform the same path planning missions. e sum of the changes in the heading angle between the nodes makes up the entire adjustment angles. e 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. 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 * .

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.
e confirmed x prand and x parent are used as the target point and starting point of local planning. e original algorithm PQ-RRT * lacks a local path planning algorithm; that is, it moves along the line of x child and x parent   (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).

Group 3.
To compare the performance of the common RRT mixed planning algorithm and NPQ-RRT * , we perform experiments in the same obstacle environment. e results are obtained, as shown in Figures 10 and 11. 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.

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. e green area represents the detection range of the robotic lidar. e big red circle represents obstacles randomly generated on the map. e blue lines represent the local paths generated by each robot. We use confirmed x prand and x parent 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     Complexity 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. e result is shown in Figures 12-15. rough 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.

Conclusions
is 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. e 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. e 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. e 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 e authors declare that they have no conflicts of interest.