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

Authors: Zihan Yu; Linying Xiang
Journal: Complexity
 (2021)

---

## 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.


---

## Body

## 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 Section2, 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,R denotes the set of real numbers, N denotes the set of natural numbers, and Rd denotes the space of real d-vectors.We consider ann-robot system, where each robot moves in the region. Let Xobs⊂X be the obstacle area and the unobstructed area Xfree=X/Xobs. The start position and the target position of the robot i are xiniti and xgoali, respectively.A trajectory of robotii=1,2,…,n is defined as follows: ki:0,τi⟶X, where τi is the duration of the trajectory. In addition, ki0=xiniti and kiτi=xgoali. The trajectory is obstacle-free if kit∈Xfree for all t∈0,τi. The cost function c· finds the path length in terms of Euclidean distance function.Trajectoryki is said to be conflict-free if it is obstacle-free and also keeps robot i at a safety distance ds>0 from all other robots. xit−xjt>ds, where xit and xjt represent the positions of robots i and j, t∈0,τ, i,j=1,2,…,n, i≠j, and τ=minτi,τj. The total cost of the trajectories is defined as ∑i=1ncki.Our goal is to find the trajectory setK=k1,k2,…,kn in which ki is conflict-free.


## 3. Related Work

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 G=V,E represents the generated graph.Algorithm 1:PQ-RRT∗ [25].
Input: V⟵xinit; E⟵∅Output: G=V,E(1)
fori = 1 to ndo(2)
xrand⟵SampleFreei;(3)
xprand⟵RGDxrand;(4)
xnearest⟵NearestV,xprand;(5)
k⟵steerxnearest,xprand;(6)
if CollisionFree(k) then(7)
Xnear⟵NearV,xprand,r ;(8)
Xsparent⟵AncestryG,Xnear;(9)
(xparent,kparent) ⟵ChooseParent(Xnear∪Xsparent,xnearest,k);(10)
V⟵V∪xprand;(11)
E⟵E∪xparent,xprand;(12)
G⟵Rewire−PQ−RRT∗G,xprand,Xnear;(13)
end(14)
endThe related functions are defined as follows:SampleFree: randomly pick points in the global map. Here, the return value is random point xrand.RGD: the adjustment function that adjusts the random point xrand under the gravitational force of the target point. Here, the return value is the improved sample xprand. NearestObstacle function calculates the distance from xprand to the obstacle space Xobs. The parameter z represents the number of iterations. dobs 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 xprand in the graph G=V,E and defines the node as xnearest.Steer: this function connects two given points. The 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 Xnear, which contains the nodes in the range with xprand as the center and r as the radius.Ancestry: this function deeply searches the parent node of each point in Xnear. And it returns the parent node set Xsparent of Xnear. The specific process is as follows: in a given graph G=V,E, for a node V1, a natural number p∈N, if the depth p=0, it returns ∅ and otherwise returns the pth parent of V1.ChooseParent: compare the cost of each path and then determine the parent node xparent and the path kparent.Rewire-PQ-RRT∗: a function to generate the final path diagram.Algorithm 2:RGD(xrand).
Input: random point xrand, target point xgoalOutput: improved point xprand(1)
forn = 1 to zdo(2)
F⟶=xgoal−xrand(3)
dmin⟵NearestObstacleXobs,xrand;(4)
ifdmin≤dobsthen(5)
returnxprand;(6)
else(7)
xprand⟵xrand+λF⟶/F⟶;(8)
end(9)
end


## 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 Figure2.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.Algorithm 3:NPQ-RRT∗.
Input:V⟵xinit;E⟵∅Output:G=V,E(1)
fori = 1 to ndo(2)
xrand⟵SampleFreei;(3)
xprand⟵RGDxrand;(4)
xnearest⟵NewNearestV,xprand;(5)
k⟵steerxnew_nearest,xprand;(6)
ifCollisionFree(k)then(7)
Xnear⟵NearV,xprand,r;(8)
Xsparent⟵AncestryG,Xnear;(9)
xparent,kparent⟵ChooseParentXnear∪Xsparent,xnearest,k(10)
LocalPath⟵Localxparent,xprand(11)
V⟵V∪xprand;(12)
E⟵E∪xparent,xprand;(13)
G⟵Rewire−NPQ−RRT∗G,xprand,Xnear;(14)
end(15)
endThe function incorporates the attitude adjustment angle as a new influencing factor into the distance consideration range:(1)q=εvl+ζwφ,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 xprand. φ is the attitude adjustment angle, as shown in Figure 3.Figure 3
The attitude adjustment of the mobile robot.l and φ are defined as follows:(2)l=xv−xr2+yv−yr2,φ=ϕ−180∘·arctanyr−yv/xr−xvπ,where xv,yv and xr,yr are the coordinates of the node and the modified sample xprand. ϕ is the angle between the orientation of the mobile robot and the X-axis of the global coordinate system, as shown in Figure 3.Sincel 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 ε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. 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 εv and ζw 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 G=V,E and then defines this node as xnew_nearest.After the leader completes the steps in the pseudocode, the graphG=V,E 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 selectedxparent and xprand 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 Vmi as follows:(3)Vmi=vi∈vmin,vmax,ωi∈ωmin,ωmax,wherevmin and vmax 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.Due to the limited torque of the motor, there are maximum acceleration and deceleration limits. The achievable velocity setVdi affected by the motor performance is defined as(4)Vdi=vi,ωi|vi∈vci−abΔt,vci+amΔt∩ωi∈ωci−αbΔt,ωci+αmΔt,wherevci and ωci are the current velocity and angular velocity of the ith mobile robot, respectively. ab and am 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. 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 setVsi is defined as follows:(5)Vsi=vi,ωi|vi≤2·dvi,ωi·ab∩ωi≤2·dvi,ωi·αb,where the functiondvi,ωi represents the distance between the ith robot and the nearest obstacle on the current trajectory.The final definition of theith robot’s feasible velocity space set is(6)Vai=Vmi∪Vdi∪Vsi.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 velocitiesvt 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. The trajectory of the motion can be regarded as a straight line. Within Δt, the robot moves vtΔ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:(7)Δx=vtΔtcosϕt,Δy=vtΔtsinϕt.As for the trajectory of the robot,t represents the previous moment and t+1 represents the current moment. xt+1, yt+1, and ϕt+1 represent the robot’s position information and orientation angle information, respectively, which are defined as(8)xt+1=xt+vtΔtcosϕt,(9)yt+1=yt+vtΔtsinϕt,(10)ϕt+1=ϕt+ωtΔt.Substituting the velocity spaceVai 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 Figure5.Define the positions of the two robots aspit and pjt, respectively. The meeting point is mt and the velocities of the two robots are vit and vjt. The distances Δl1 and Δl2 are defined as(11)Δl1=pit−mt,Δl2=pjt−mt.According to the distance, we can constrain the velocities of the two robots: ifΔl1≥Δl2, then(12)vit+1=vit−ab·ϵ,vjt+1=vjt+am·ϵ.Else, ifΔl1≤Δl2, then(13)vit+1=vit+am·ϵ,vjt+1=vjt−ab·ϵ,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 theith robot’s local path as(14)Gvi,ωi=α·m_headvi,ωi+δ·m_dvi,ωi+γ·m_velvi,ωi−ρ·m_meetvi,ωi,wherevi,ωi∈Vai and the variables α,δ,γ and ρ are the initial weights of the function. m is the total number of all trajectories sampled. The function m_headvi,ωi is used to evaluate the heading score, which is defined as follows:(15)m_headvi,ωi=headvi,ωi∑c=1mheadvi,ωi.The functionheadvi,ωi is defined as(16)headvi,ωi=180∘−θi,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(17)θi=ϕ−180∘·arctanyb−yp/xb−xpπ,wherexb,yb is the coordinate of the local target point in the global map and xp,yp is the coordinate of the predicted position of the ith robot in the global map.The functionm_dvi,ωi is used to evaluate the safety distance score, which is defined as follows:(18)m_dvi,ωi=dvi,ωi∑c=1mdvi,ωi.The functiondvi,ωi can be defined as(19)dvi,ωi=mindistxi,sj,wherexi is a random point on the trajectory, sj represents the corresponding obstacle, and dist is a function to calculate Euclidean distance. If there are no obstacles on this trajectory, set dvi,ωi as a constant.The functionm_velvi,ωi is used to evaluate the speed score of the current trajectory. The function velvi,ωi corresponds to the velocity value of the current trajectory. m_velvi,ωi is defined as follows:(20)m_velvi,ωi=velvi,ωi∑c=1mvelvi,ωi.The functionm_meetvi,ωi is used to evaluate the score of internal collision probability. The function meetvi,ωi corresponds to the number of intersections with the predicted trajectories of other robots in the formation. m_meetvi,ωi is defined as follows:(21)m_meetvi,ωi=meetvi,ωi∑c=1mmeetvi,ωi.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.


## 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 Figure2.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.Algorithm 3:NPQ-RRT∗.
Input:V⟵xinit;E⟵∅Output:G=V,E(1)
fori = 1 to ndo(2)
xrand⟵SampleFreei;(3)
xprand⟵RGDxrand;(4)
xnearest⟵NewNearestV,xprand;(5)
k⟵steerxnew_nearest,xprand;(6)
ifCollisionFree(k)then(7)
Xnear⟵NearV,xprand,r;(8)
Xsparent⟵AncestryG,Xnear;(9)
xparent,kparent⟵ChooseParentXnear∪Xsparent,xnearest,k(10)
LocalPath⟵Localxparent,xprand(11)
V⟵V∪xprand;(12)
E⟵E∪xparent,xprand;(13)
G⟵Rewire−NPQ−RRT∗G,xprand,Xnear;(14)
end(15)
endThe function incorporates the attitude adjustment angle as a new influencing factor into the distance consideration range:(1)q=εvl+ζwφ,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 xprand. φ is the attitude adjustment angle, as shown in Figure 3.Figure 3
The attitude adjustment of the mobile robot.l and φ are defined as follows:(2)l=xv−xr2+yv−yr2,φ=ϕ−180∘·arctanyr−yv/xr−xvπ,where xv,yv and xr,yr are the coordinates of the node and the modified sample xprand. ϕ is the angle between the orientation of the mobile robot and the X-axis of the global coordinate system, as shown in Figure 3.Sincel 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 ε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. 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 εv and ζw 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 G=V,E and then defines this node as xnew_nearest.After the leader completes the steps in the pseudocode, the graphG=V,E 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 selectedxparent and xprand 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 Vmi as follows:(3)Vmi=vi∈vmin,vmax,ωi∈ωmin,ωmax,wherevmin and vmax 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.Due to the limited torque of the motor, there are maximum acceleration and deceleration limits. The achievable velocity setVdi affected by the motor performance is defined as(4)Vdi=vi,ωi|vi∈vci−abΔt,vci+amΔt∩ωi∈ωci−αbΔt,ωci+αmΔt,wherevci and ωci are the current velocity and angular velocity of the ith mobile robot, respectively. ab and am 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. 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 setVsi is defined as follows:(5)Vsi=vi,ωi|vi≤2·dvi,ωi·ab∩ωi≤2·dvi,ωi·αb,where the functiondvi,ωi represents the distance between the ith robot and the nearest obstacle on the current trajectory.The final definition of theith robot’s feasible velocity space set is(6)Vai=Vmi∪Vdi∪Vsi.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 velocitiesvt 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. The trajectory of the motion can be regarded as a straight line. Within Δt, the robot moves vtΔ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:(7)Δx=vtΔtcosϕt,Δy=vtΔtsinϕt.As for the trajectory of the robot,t represents the previous moment and t+1 represents the current moment. xt+1, yt+1, and ϕt+1 represent the robot’s position information and orientation angle information, respectively, which are defined as(8)xt+1=xt+vtΔtcosϕt,(9)yt+1=yt+vtΔtsinϕt,(10)ϕt+1=ϕt+ωtΔt.Substituting the velocity spaceVai 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 Figure5.Define the positions of the two robots aspit and pjt, respectively. The meeting point is mt and the velocities of the two robots are vit and vjt. The distances Δl1 and Δl2 are defined as(11)Δl1=pit−mt,Δl2=pjt−mt.According to the distance, we can constrain the velocities of the two robots: ifΔl1≥Δl2, then(12)vit+1=vit−ab·ϵ,vjt+1=vjt+am·ϵ.Else, ifΔl1≤Δl2, then(13)vit+1=vit+am·ϵ,vjt+1=vjt−ab·ϵ,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 theith robot’s local path as(14)Gvi,ωi=α·m_headvi,ωi+δ·m_dvi,ωi+γ·m_velvi,ωi−ρ·m_meetvi,ωi,wherevi,ωi∈Vai and the variables α,δ,γ and ρ are the initial weights of the function. m is the total number of all trajectories sampled. The function m_headvi,ωi is used to evaluate the heading score, which is defined as follows:(15)m_headvi,ωi=headvi,ωi∑c=1mheadvi,ωi.The functionheadvi,ωi is defined as(16)headvi,ωi=180∘−θi,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(17)θi=ϕ−180∘·arctanyb−yp/xb−xpπ,wherexb,yb is the coordinate of the local target point in the global map and xp,yp is the coordinate of the predicted position of the ith robot in the global map.The functionm_dvi,ωi is used to evaluate the safety distance score, which is defined as follows:(18)m_dvi,ωi=dvi,ωi∑c=1mdvi,ωi.The functiondvi,ωi can be defined as(19)dvi,ωi=mindistxi,sj,wherexi is a random point on the trajectory, sj represents the corresponding obstacle, and dist is a function to calculate Euclidean distance. If there are no obstacles on this trajectory, set dvi,ωi as a constant.The functionm_velvi,ωi is used to evaluate the speed score of the current trajectory. The function velvi,ωi corresponds to the velocity value of the current trajectory. m_velvi,ωi is defined as follows:(20)m_velvi,ωi=velvi,ωi∑c=1mvelvi,ωi.The functionm_meetvi,ωi is used to evaluate the score of internal collision probability. The function meetvi,ωi corresponds to the number of intersections with the predicted trajectories of other robots in the formation. m_meetvi,ωi is defined as follows:(21)m_meetvi,ωi=meetvi,ωi∑c=1mmeetvi,ωi.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.


## 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.Algorithm 3:NPQ-RRT∗.
Input:V⟵xinit;E⟵∅Output:G=V,E(1)
fori = 1 to ndo(2)
xrand⟵SampleFreei;(3)
xprand⟵RGDxrand;(4)
xnearest⟵NewNearestV,xprand;(5)
k⟵steerxnew_nearest,xprand;(6)
ifCollisionFree(k)then(7)
Xnear⟵NearV,xprand,r;(8)
Xsparent⟵AncestryG,Xnear;(9)
xparent,kparent⟵ChooseParentXnear∪Xsparent,xnearest,k(10)
LocalPath⟵Localxparent,xprand(11)
V⟵V∪xprand;(12)
E⟵E∪xparent,xprand;(13)
G⟵Rewire−NPQ−RRT∗G,xprand,Xnear;(14)
end(15)
endThe function incorporates the attitude adjustment angle as a new influencing factor into the distance consideration range:(1)q=εvl+ζwφ,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 xprand. φ is the attitude adjustment angle, as shown in Figure 3.Figure 3
The attitude adjustment of the mobile robot.l and φ are defined as follows:(2)l=xv−xr2+yv−yr2,φ=ϕ−180∘·arctanyr−yv/xr−xvπ,where xv,yv and xr,yr are the coordinates of the node and the modified sample xprand. ϕ is the angle between the orientation of the mobile robot and the X-axis of the global coordinate system, as shown in Figure 3.Sincel 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 ε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. 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 εv and ζw 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 G=V,E and then defines this node as xnew_nearest.After the leader completes the steps in the pseudocode, the graphG=V,E 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 selectedxparent and xprand 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 Vmi as follows:(3)Vmi=vi∈vmin,vmax,ωi∈ωmin,ωmax,wherevmin and vmax 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.Due to the limited torque of the motor, there are maximum acceleration and deceleration limits. The achievable velocity setVdi affected by the motor performance is defined as(4)Vdi=vi,ωi|vi∈vci−abΔt,vci+amΔt∩ωi∈ωci−αbΔt,ωci+αmΔt,wherevci and ωci are the current velocity and angular velocity of the ith mobile robot, respectively. ab and am 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. 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 setVsi is defined as follows:(5)Vsi=vi,ωi|vi≤2·dvi,ωi·ab∩ωi≤2·dvi,ωi·αb,where the functiondvi,ωi represents the distance between the ith robot and the nearest obstacle on the current trajectory.The final definition of theith robot’s feasible velocity space set is(6)Vai=Vmi∪Vdi∪Vsi.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 velocitiesvt 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. The trajectory of the motion can be regarded as a straight line. Within Δt, the robot moves vtΔ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:(7)Δx=vtΔtcosϕt,Δy=vtΔtsinϕt.As for the trajectory of the robot,t represents the previous moment and t+1 represents the current moment. xt+1, yt+1, and ϕt+1 represent the robot’s position information and orientation angle information, respectively, which are defined as(8)xt+1=xt+vtΔtcosϕt,(9)yt+1=yt+vtΔtsinϕt,(10)ϕt+1=ϕt+ωtΔt.Substituting the velocity spaceVai 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 Figure5.Define the positions of the two robots aspit and pjt, respectively. The meeting point is mt and the velocities of the two robots are vit and vjt. The distances Δl1 and Δl2 are defined as(11)Δl1=pit−mt,Δl2=pjt−mt.According to the distance, we can constrain the velocities of the two robots: ifΔl1≥Δl2, then(12)vit+1=vit−ab·ϵ,vjt+1=vjt+am·ϵ.Else, ifΔl1≤Δl2, then(13)vit+1=vit+am·ϵ,vjt+1=vjt−ab·ϵ,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 theith robot’s local path as(14)Gvi,ωi=α·m_headvi,ωi+δ·m_dvi,ωi+γ·m_velvi,ωi−ρ·m_meetvi,ωi,wherevi,ωi∈Vai and the variables α,δ,γ and ρ are the initial weights of the function. m is the total number of all trajectories sampled. The function m_headvi,ωi is used to evaluate the heading score, which is defined as follows:(15)m_headvi,ωi=headvi,ωi∑c=1mheadvi,ωi.The functionheadvi,ωi is defined as(16)headvi,ωi=180∘−θi,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(17)θi=ϕ−180∘·arctanyb−yp/xb−xpπ,wherexb,yb is the coordinate of the local target point in the global map and xp,yp is the coordinate of the predicted position of the ith robot in the global map.The functionm_dvi,ωi is used to evaluate the safety distance score, which is defined as follows:(18)m_dvi,ωi=dvi,ωi∑c=1mdvi,ωi.The functiondvi,ωi can be defined as(19)dvi,ωi=mindistxi,sj,wherexi is a random point on the trajectory, sj represents the corresponding obstacle, and dist is a function to calculate Euclidean distance. If there are no obstacles on this trajectory, set dvi,ωi as a constant.The functionm_velvi,ωi is used to evaluate the speed score of the current trajectory. The function velvi,ωi corresponds to the velocity value of the current trajectory. m_velvi,ωi is defined as follows:(20)m_velvi,ωi=velvi,ωi∑c=1mvelvi,ωi.The functionm_meetvi,ωi is used to evaluate the score of internal collision probability. The function meetvi,ωi corresponds to the number of intersections with the predicted trajectories of other robots in the formation. m_meetvi,ωi is defined as follows:(21)m_meetvi,ωi=meetvi,ωi∑c=1mmeetvi,ωi.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 Figure7.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, λ = dobs = 1 and z = 80. In the ChooseParentfunction, d=2. 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 −1,1. The angular acceleration range is −0.5,0.5, 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.
NumberPQ-RRT∗NPQ-RRT∗165.97159.311256.56750.889357.28845.499463.75547.139573.20962.714667.42341.650759.42264.298864.47852.669971.49249.1841068.42451.770Average64.70352.512Figures8 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 confirmedxprand and xparent 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 xchild and xparent (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 confirmedxprand and xparent 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:t = 10s.Figure 14
The positions of the robots:t = 15s.Figure 15
The positions of the robots:t = 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.


## 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 Figure7.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, λ = dobs = 1 and z = 80. In the ChooseParentfunction, d=2. 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 −1,1. The angular acceleration range is −0.5,0.5, 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.
NumberPQ-RRT∗NPQ-RRT∗165.97159.311256.56750.889357.28845.499463.75547.139573.20962.714667.42341.650759.42264.298864.47852.669971.49249.1841068.42451.770Average64.70352.512Figures8 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 confirmedxprand and xparent 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 xchild and xparent (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 confirmedxprand and xparent 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:t = 10s.Figure 14
The positions of the robots:t = 15s.Figure 15
The positions of the robots:t = 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.


## 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.
NumberPQ-RRT∗NPQ-RRT∗165.97159.311256.56750.889357.28845.499463.75547.139573.20962.714667.42341.650759.42264.298864.47852.669971.49249.1841068.42451.770Average64.70352.512Figures8 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 confirmedxprand and xparent 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 xchild and xparent (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 confirmedxprand and xparent 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:t = 10s.Figure 14
The positions of the robots:t = 15s.Figure 15
The positions of the robots:t = 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.



---

*Source: 6633878-2021-02-17.xml*