# Parallel Swarm Intelligent Motion Planning with Energy-Balanced for Multirobot in Obstacle Environment

Authors: Shoubao Su; Wei Zhao; Chishe Wang
Journal: Wireless Communications and Mobile Computing
 (2021)

---

## Abstract

Multirobot motion planning is always one of the critical techniques in edge intelligent systems, which involve a variety of algorithms, such as map modeling, path search, and trajectory optimization and smoothing. To overcome the slow running speed and imbalance of energy consumption, a swarm intelligence solution based on parallel computing is proposed to plan motion paths for multirobot with many task nodes in a complex scene that have multiple irregularly-shaped obstacles, which objective is to find a smooth trajectory under the constraints of the shortest total distance and the energy-balanced consumption for all robots to travel between nodes. In a practical scenario, the imbalance of task allocation will inevitably lead to some robots stopping on the way. Thus, we firstly model a gridded scene as a weighted MTSP (multitraveling salesman problem) in which the weights are the energies of obstacle constraints and path length. Then, a hybridization of particle swarm and ant colony optimization (GPSO-AC) based on a platform of Compute Unified Device Architecture (CUDA) is presented to find the optimal path for the weighted MTSPs. Next, we improve the A∗ algorithm to generate a weighted obstacle avoidance path on the gridded map, but there are still many sharp turns on it. Therefore, an improved smooth grid path algorithm is proposed by integrating the dynamic constraints in this paper to optimize the trajectory smoothly, to be more in line with the law of robot motion, which can more realistically simulate the multirobot in a real scene. Finally, experimental comparisons with other methods on the designed platform of GPUs demonstrate the applicability of the proposed algorithm in different scenarios, and our method strikes a good balance between energy consumption and optimality, with significantly faster and better performance than other considered approaches, and the effects of the adjustment coefficient q on the performance of the algorithm are also discussed in the experiments.


---

## Body

## 1. Introduction

Robot technology is always a remarkably interdisciplinary topic of study, one that can be applied to various engineering practices as well as emerging industrial fields. The edge intelligence, on the other hand, has been known as one of the most difficult research areas to apply multirobot systems (MRSs), in which the key technologies such as path planning, robustness, fault tolerance, and stability cooperation of MRS need to be studied. Motion planning is an extension of path planning, and there exist few differences between them. Generally, path planning is to find the path between starting node and target node in specific scenes by objectives like the shortest distance or the shortest time, while motion planning is to generate interactive trajectories in the situation when robots interact with their surroundings, where there are usually many factors such as velocities, obstacles, and energies that need be considered as constraints [1]. Path planning has always been one of the important challenges, which has been widely used in engineering practices, such as underwater robot detection, UAV cruise, vehicle tracking, warehousing, robot delivery, smart edge systems, and various navigation scenarios [2]. Multirobot path planning (MRPP) is to find the shortest way that allows each robot to follow a safe and effective path to reach a goal from an initial node, optimize the motion path and avoid obstacles, and generate a smooth motion path between nodes that conforms to the objectives.With decades of year development, there are various studies of motion planner including traditional interpolating and heuristics approaches employed effectively in multirobot navigation over different environmental conditions so far. Even though, it is still an open challenging for multirobot regarding uncertain constraints, especially dynamic adaptive, scalable, and online replanning are still open and challenge problems for practical applications in real-world with large-scale nodes [3–5], which are far from being completely solved, and many practical engineering problems are still tackled today using learning-based heuristics algorithms, for the purpose of robustness, safety, speed, and efficiency. The main advantages of these methods lie in their ease of implementation, effectiveness for specific applications, and feasibility for small-scale and low dimensional workspace. However, some of them fails to complicated time-sequential motion planning, or limited robustness, or computational expensive, or unknown environments, especially the large-scale problem still have to be faced by the exponential growth of the search space and the disaster of dimensionality.The early graphics processing unit (GPU) did not design a programming architecture for parallel computing. Programmers usually utilize special skills, such as Shader, to realize algorithm parallelism, which result in limitations and complexity of programming. With the development of GPU, especially edge intelligence, in the field of scientific computing, NVIDIA and AMD successively launched GPU-based parallel software and hardware architecture CUDA (Compute Unified Device Architecture) and ROCm (radeon open computing platform). Similarly, ROCm stack and provided a path for the parallel implementation of large-scale evolutionary computing [6]. Modern GPUs use multistream processor (SM) design and single instruction multithreading (SIMT) instruction architecture, which provides a good hardware platform for parallel algorithms. In reality, speed is essential in the applied scenes of MRSs, when the energy consumption of multirobots is not balanced, some robots may be stopped on the way, which will affect the stability and persistence of the whole multirobot system and even affect the completion of the task.Therefore, this work is focused on motion planning for multirobot in a complex scene that has different surrounding obstacles. The goal is to design fast approaches that enable a real-time calculation of trajectories for multirobot systems which have balanced energies to finish their tasks. After the problem is to be solved modeled as a weighted MTSP, an improved RRT search combining slam construction method is proposed, which can help robots locate and map unknown areas more quickly. Then, we use particle swarm clustering and ant colony optimization methods to find the point-to-point direct path satisfying the basic constraints in the low-dimensional space and then restore the path to the actual scene through the trajectory generation algorithm. At the same time, combined with the characteristics of easy parallelization of swarm intelligence algorithm, aiming at the problems of high accuracy but long execution time of hybrid iterative algorithm, considering the energy consumption balance of multiple robots, by developing a GPU based on CUDA platform to accelerate the running speed of the algorithm, a smooth grid path algorithm combined with the minimum snap algorithm is proposed to make the final path globally optimal, safe, and fast, collision-free, energy-balanced and meet the dynamic constraints.


## 2. Related Works

### 2.1. Map Modeling

Multirobot motion planning mainly involves three stages which are building the global map model, generating a search path, and optimizing the motion trajectories. The first stage is to model a map of the given environment that is the input data for algorithms. There are algorithms commonly used to model maps that cover grid method, topology method, line of sight diagram method, tangent diagram method, and Voronoi diagram method [7, 8]. The former two are mostly utilized for global path planning, while the latter three are mostly used for local search. Among them, the grid method is simple and clear, which is recognized as an algorithm with high safety [9]. In the algorithm, the environment can be discretely gridded on a mesh map as fine as possible when the computing resources permit, to reduce the time complexity of finding both obstacles and target-nodes to O1. As shown in Figure 1, we assume that some irregular polygons are obstacles, and triangles are target nodes on the original map. After offline processing using the grid method, an original environment, the gridded mesh, and the discretized matrix are as shown in Figures 1(a)–1(c).Figure 1
Grid modeling.
(a)
Original environment(b)
Gridded mesh(c)
Discretized matrix


### 2.2. Solving Algorithms

Due to the characteristics and complexity of MRPP, it is a difficult and complex NP hard problem, which is essentially an optimization problem with many constraints. The shortest path planning with balanced-energy consumption for each robot is often modeled as the optimization objective. Researchers, so far, use heuristic algorithms to solve the problems, mainly including hybrid iterative method, divide and conquer method, and coevolution strategy. The A∗ algorithm is one of the most typical graph-based in path search, and it inherits the idea of the Dijkstra and makes it different improved variants [10, 11], which can generate not only the shortest path but also the path more in line with the preference of various models, for example, it can be abstracted a variety of energies into the problems to find the optimal solution [12]. But the disadvantage is its failure to the problems of large-scale nodes, so, deep neural network (DNN) was employed to optimize the evaluation functions gn and Hn of the A∗ algorithm [13]. The sampling-based algorithms mainly include the probability spectrum method and RRT algorithm. The nearest sampling nodes on the map for obstacle collision detection are connected by trees, which can quickly find a reachable path in a limited time. If there are a large number of obstacles or difficult areas in the environment, the algorithm will run fast, but the algorithm has no perception of the environment. These algorithms are mostly used in local search by combining with Slam (simultaneous localization and mapping), so that enhance the capabilities of local perception [14].In recent years, there are numerous researches focused on this issue concerning intelligent methods. In many fields related to artificial intelligence, motion planning for multirobot systems (MRS) is undoubtedly one of the crucial topics that cover all kinds of applications based on swarm optimizers [15–17], including ant colony optimization (ACO) [16] and particle swarm optimization (PSO) [17], because of their effective ways to take the advantages of population information to enhance the overall solution quality and accelerate the convergence speed [15]. For example, ACO clustering with crowding mechanism and GA-based multitask scheduling methods were developed for drones safely flight in a specific airspace [18], two phases heuristic algorithm (TPHA) [19], an improved shuffled frog leaping algorithm (SFLA) [20] was embedded into the trajectory smooth path to determine an optimal subsequent position for each robot, and so on. Neural networks and reinforcement learning (RL) are never absent from routing problems, an optimal navigation strategy with energy-aware coverage designed by using RL for self-reconfigurable robots [21], while deep learning-based warm-starting optimizing motion planner [22] was employed to reduce motion time for robots picking an e-commerce warehouse. To make full use of the advantages of various methods, hybrid intelligent methods have been paid more and more attention recently, and a hybridization planning method is demonstrated [23] by integrating 0-1 optimization and EDA and GA for UAVs and UGVs to cooperative complete the coverages with minimum travel time in urban environments. When PSO planner applied to an intricate unknown environment, Shukla et al. [24] assessed its parameters including move (number of visited nodes), coverage (area explored), energy (distance traveled), and time (time elapsed). By making the best use of limited time, Salah et al. [25] proposed a path-planning approach for multidrones to conduct multiple photographic aerial wildlife surveys.Some theoretical methodologies are introduced to plan the shortest path in static and dynamic environments. A framework regarding Satisfiability Modulo Theory (SMT) was proposed [26], which models the path as a connected network of mass-spring-damper systems and leverages the properties of Voronoi diagrams to handle complex constraints. To plan a path for the robot on the graph with edge energies, a nonmyopic path planner based on a game-theoretic framework was presented [27] by employing an infinite-horizon Markov decision process. When facing complex and changeable factors of traffic networks, the shortest path with the maximum passing probabilities based on the mobile trajectory can be effectively obtained by calculating the Markov chain and probabilistic symbolic model [28]. To minimize the path length as well as to reduce the number of handovers while sustaining the wireless connectivity of the robots, a multirobot access node association planner was presented [29] in millimeter-wave industrial scenarios. When the motion control for multirobot navigation is used in large-scale dynamic scenarios, a personalized route algorithm was presented by introducing the Polychromatic Sets (PS) for users to obtain real-time route that meet their travel preferences [4], all obstacle features can be integrated into a scalar potential field to make decisions [30], an online adaptive replanning strategy for multiple drones flying in an urban environment with a number of dynamic changes [5], and a reliable routing optimization scheme based on the Manhattan mobility model is presented [31] for UAV real-time planning in a vehicular ad hoc networks (VANETs). While a real-time tracking method was designed [32] that combines A∗ algorithm with dynamic window to guarantee the ability of obstacle avoidance and path smoothness. Thus, it has been observed that the heuristics are more robust and conduct well in scenarios when compared to other algorithms.


## 2.1. Map Modeling

Multirobot motion planning mainly involves three stages which are building the global map model, generating a search path, and optimizing the motion trajectories. The first stage is to model a map of the given environment that is the input data for algorithms. There are algorithms commonly used to model maps that cover grid method, topology method, line of sight diagram method, tangent diagram method, and Voronoi diagram method [7, 8]. The former two are mostly utilized for global path planning, while the latter three are mostly used for local search. Among them, the grid method is simple and clear, which is recognized as an algorithm with high safety [9]. In the algorithm, the environment can be discretely gridded on a mesh map as fine as possible when the computing resources permit, to reduce the time complexity of finding both obstacles and target-nodes to O1. As shown in Figure 1, we assume that some irregular polygons are obstacles, and triangles are target nodes on the original map. After offline processing using the grid method, an original environment, the gridded mesh, and the discretized matrix are as shown in Figures 1(a)–1(c).Figure 1
Grid modeling.
(a)
Original environment(b)
Gridded mesh(c)
Discretized matrix


## 2.2. Solving Algorithms

Due to the characteristics and complexity of MRPP, it is a difficult and complex NP hard problem, which is essentially an optimization problem with many constraints. The shortest path planning with balanced-energy consumption for each robot is often modeled as the optimization objective. Researchers, so far, use heuristic algorithms to solve the problems, mainly including hybrid iterative method, divide and conquer method, and coevolution strategy. The A∗ algorithm is one of the most typical graph-based in path search, and it inherits the idea of the Dijkstra and makes it different improved variants [10, 11], which can generate not only the shortest path but also the path more in line with the preference of various models, for example, it can be abstracted a variety of energies into the problems to find the optimal solution [12]. But the disadvantage is its failure to the problems of large-scale nodes, so, deep neural network (DNN) was employed to optimize the evaluation functions gn and Hn of the A∗ algorithm [13]. The sampling-based algorithms mainly include the probability spectrum method and RRT algorithm. The nearest sampling nodes on the map for obstacle collision detection are connected by trees, which can quickly find a reachable path in a limited time. If there are a large number of obstacles or difficult areas in the environment, the algorithm will run fast, but the algorithm has no perception of the environment. These algorithms are mostly used in local search by combining with Slam (simultaneous localization and mapping), so that enhance the capabilities of local perception [14].In recent years, there are numerous researches focused on this issue concerning intelligent methods. In many fields related to artificial intelligence, motion planning for multirobot systems (MRS) is undoubtedly one of the crucial topics that cover all kinds of applications based on swarm optimizers [15–17], including ant colony optimization (ACO) [16] and particle swarm optimization (PSO) [17], because of their effective ways to take the advantages of population information to enhance the overall solution quality and accelerate the convergence speed [15]. For example, ACO clustering with crowding mechanism and GA-based multitask scheduling methods were developed for drones safely flight in a specific airspace [18], two phases heuristic algorithm (TPHA) [19], an improved shuffled frog leaping algorithm (SFLA) [20] was embedded into the trajectory smooth path to determine an optimal subsequent position for each robot, and so on. Neural networks and reinforcement learning (RL) are never absent from routing problems, an optimal navigation strategy with energy-aware coverage designed by using RL for self-reconfigurable robots [21], while deep learning-based warm-starting optimizing motion planner [22] was employed to reduce motion time for robots picking an e-commerce warehouse. To make full use of the advantages of various methods, hybrid intelligent methods have been paid more and more attention recently, and a hybridization planning method is demonstrated [23] by integrating 0-1 optimization and EDA and GA for UAVs and UGVs to cooperative complete the coverages with minimum travel time in urban environments. When PSO planner applied to an intricate unknown environment, Shukla et al. [24] assessed its parameters including move (number of visited nodes), coverage (area explored), energy (distance traveled), and time (time elapsed). By making the best use of limited time, Salah et al. [25] proposed a path-planning approach for multidrones to conduct multiple photographic aerial wildlife surveys.Some theoretical methodologies are introduced to plan the shortest path in static and dynamic environments. A framework regarding Satisfiability Modulo Theory (SMT) was proposed [26], which models the path as a connected network of mass-spring-damper systems and leverages the properties of Voronoi diagrams to handle complex constraints. To plan a path for the robot on the graph with edge energies, a nonmyopic path planner based on a game-theoretic framework was presented [27] by employing an infinite-horizon Markov decision process. When facing complex and changeable factors of traffic networks, the shortest path with the maximum passing probabilities based on the mobile trajectory can be effectively obtained by calculating the Markov chain and probabilistic symbolic model [28]. To minimize the path length as well as to reduce the number of handovers while sustaining the wireless connectivity of the robots, a multirobot access node association planner was presented [29] in millimeter-wave industrial scenarios. When the motion control for multirobot navigation is used in large-scale dynamic scenarios, a personalized route algorithm was presented by introducing the Polychromatic Sets (PS) for users to obtain real-time route that meet their travel preferences [4], all obstacle features can be integrated into a scalar potential field to make decisions [30], an online adaptive replanning strategy for multiple drones flying in an urban environment with a number of dynamic changes [5], and a reliable routing optimization scheme based on the Manhattan mobility model is presented [31] for UAV real-time planning in a vehicular ad hoc networks (VANETs). While a real-time tracking method was designed [32] that combines A∗ algorithm with dynamic window to guarantee the ability of obstacle avoidance and path smoothness. Thus, it has been observed that the heuristics are more robust and conduct well in scenarios when compared to other algorithms.


## 3. Energy-Balanced Motion Trajectory Planning Algorithm

### 3.1. Problem Description and Modeling

This paper focuses on the problem of multirobot motion planning, the first stage of which can be abstracted as a single-point MTSP problem. Assume thatm traveling salesmen (denoted as b1,b2⋯bm) will visit n cities (denoted as T0,T1⋯Tn) to sell their goods, and all traveling salesmen start from the same city T1 and finally meet in city T1; it is required that there must be only one salesman to visit each city, and the total energy and minimum route scheme of all salesmen are obtained [12]. The objective function is
(1)minZ=∑k=1m∑i=1n∑j=1ndijpijk,where pijk is denoted as equation (2) that is the direction marking variable between cities i and j of the traveling salesman with serial number k.
(2)pijk=1,travelingsalesmanktravelsfromcityitoj,2,else.Then, we assume that a given environment as shown in Figure2, in which there are a large number of obstacles (irregular polygons), m target nodes (triangles) Qii=1,⋯,, a given starting node P (diamond at the bottom left). Now, n mobile robots will visit to m target nodes to perform tasks, each node is only visited by one robot, and all robots must return to the starting node after finishing their tasks.Figure 2
A schematic map of the environment with obstacles.


### 3.2. Proposed Method

In this section, let us first explain what is energy balance and why we should balance the energy of multirobots. When each robot carries the same energy at the starting node, how to ensure that the total traveling distance of all robots is the shortest, and how to avoid the imbalance of energy consumption of each robot, that is, some robots stop on the way due to insufficient energy, while the others return to the starting node with a large amount of energy remaining. Thus, we present our motion planning scheme in this paper that has two optimization objectives, one is to minimize the total length of the multirobot traveling path, and another is to minimize the deviation of the path length of the multirobot to ensure the energy consumption balance. For these purposes, in our scheme, the environment information is simplified and merged according to the target to be optimized, the quantitative data and the model are established to solve the initial planning path in line with the optimization target, and then the initial planning path is added with obstacles constraints and dynamic constraints so that the planned path can be truly used in the actual environment. The scheme for multirobot motion planning in obstacles is first proposed as Algorithm1, and its methodology stages of pathfinding, trajectory optimization, and smoothing can be described in details next sections.Algorithm 1: Parallel swarm intelligent motion planning scheme.
Input: Structured 2D plan map of the environmentOutput:m motion trajectories1 define the starting node, target node, and obstacles2 parameter settings, swarm size, the adjustment coefficientq3 for the structured map of the environment4 discretize for using griding method in section2.15 create a gridded mesh and discretized matrix6 until all target nodes and obstacles are discretized on the mesh7 initialize the open set and closed set of nodes by calling A∗ algorithm8 while the open set is not empty9 generate all direct path between nodes10 end while//all nodes are reachable on the fully connected path graph11 while two nodes is not on a direct path12 calculate the unit energies for all not direct path13 Endwhile modelling the problem of MTSP with energies14 for each robot ofm, using GPSO-AC (args)15 generatem route trajectories that met the objectives16 Endfor17 form route trajectories using minimum snap algorithm18 trajectories optimization to reduce unnecessary turning points19 Endfor20 form route trajectories using improve A∗ and RRT algorithm21 solving trajectory polynomial and remove all breakpoints22 Endfor generate m smoothed motion trajectories


#### 3.2.1. ImprovedA∗ Algorithm for Path Search

Based on the gridded environment, the A∗ algorithm can be selected for pathfinding with efficiency. The core evaluation function of the A∗ algorithm is as follows:
(3)fn=gn+hn,where fn is the total evaluation value of the current node n, gn is the evaluation generation value from the starting node to the current node in the map, and hn is the evaluation generation value from the current node to the endpoint. In the algorithm, Manhattan distance between nodes is usually used as the evaluation basis:
(4)Dn=xd−xn+yd−yn.The A∗ algorithm also needs two sets to record the intermediate process: an open set and a closed set. The former is used to store the nodes that have not passed under the reachable condition, while the latter is used to store all nodes that have passed or are not reachable. Thus, the method can be presented as Algorithm 2, and the algorithm flowchart is shown in Figure 3. In addition, we also give example graph of fully connected paths with obstacle avoidance generated by the improved A∗ algorithm as shown in Figure 4.Algorithm 2: Improved A∗ algorithm.
Input: Open set, closed set for all nodes on a mapOutput: Full direct path graph1 add all nodes with obstacles to the closed set2 set the starting node to the current node and add it to the open set.3 for all reachable nodes4  collect the current node that are not in the closed set5 Endfor6 while (the open set is not empty)7    calculatef(n) of each reachable node, and add it to the open set8    find a node with the smallestf(n) from the open set9    set the node as the current node, and add it to the open set10    set its parent node as the previous current node11    delete the previous current node from the open set and add it to the closed set12    for all reachable nodes in the open set14      select the node with the smallestg(n) in the open combinationAs the current node, and add it to the open set15    End for16 End doFigure 3
Flowchart of an improved A∗ algorithm.Figure 4
Fully connected path graph with obstacle avoidance.


#### 3.2.2. Trajectory Optimization

The path search algorithm does not take into account dynamic constraints such as speed and acceleration when the robot is moving on the way. From the geometric of a view, the planned path is all splicing of line segments, which leads to the connection between the paths by turning points. It is impossible to strictly follow this path in the process, because the motion parameters such as speed and acceleration at the turning point will change unless it is assumed that the speed and acceleration at the turning point are reduced to 0, which will, of course, reduce the motion efficiency of motions. Then, a polynomial-based trajectory optimization method was presented by minimizing jerk (snap) to ensure that the generated trajectory polynomial is continuous and smooth in both speed and acceleration [32–34]. The continuous trajectories between two ends in a two-dimensional plane can be described by the analytic polynomials xt and Yt about time t, taking the x-axis as an example, the polynomial expression is as equation (5).
(5)Xt=pntn+pn−1tn−1+⋯+p1t+p0=∑n=0kpntn,where k is the order of polynomial,p0,p1⋯pn is a polynomial coefficient, the corresponding velocity Vt, acceleration At, and acceleration Sanpt can be expressed as equations (6)–(8). Thus, the trajectory Xt can also be expressed in a matrix form as equation (9).
(6)Vt=X1t=∑n=0kpn.ntn−1,(7)At=X2t=∑n=0kpn.nn−1tn−2,(8)Snapt=X4t=∑n=0kpn.n!n−4!tn−4,(9)Xt=1,t,t2,⋯,tn·p0,p1,⋯,pnT.Similarly, the motion state description functions such asXt and At can be expressed in matrix form. In path planning, the multisegment trajectory between multiple target nodes can be denoted as equation (10).
(10)Xt=1,t,t2,⋯,tn·P1Tt0≤t<t1,1,t,t2,⋯,tn·P2Tt1≤t<t2,⋯1,t,t2,⋯,tn·PkTtk−1≤t<tk.To determine the trajectory of each path, it is necessary to define the value of each coefficient matrixP, minimize the Snapt function of multistage trajectories to obtain high-order continuous and smooth trajectories, that is, to ensure that the coordinates, velocities, and accelerations at the breakpoint between each trajectory are continuous, to ensure that the actual trajectory of the robot is in line with the dynamic constraints in the real world, and can maintain a steady-state continue to be in a stable state of motion. Considering that Snapt can be negative and square, the objective function of minimizing snap can be established as follows:
(11)JT=minimizesnapT2=∫Ti−1TiX4t2dt=PTQP.Among them, the total time ofT segment trajectory, P is the parameter matrix of each section, and Q is the weight matrix. After the establishment of the objective function, the constraint conditions are specified, and the initial state values of the positions, velocities, and acceleration at both ends and internal nodes of a trajectory can be specified:
(12)∑n=0kpntn=x0,∑n=0kpn.ntn−1=v0,∑n=0kpn.nn−1tn−2=a0.Then, we can establish equality constraints on matrixP:
(13)APi=C,where A is the coefficient matrix of T, and C is the initial matrix. It can be seen from equations (11)–(13) that what needs to be solved is a quadratic programming problem (QP). There are many algorithms for solving QPs omitted here. Similarly, we use the same method for y-axis to find the polynomial parameters.


#### 3.2.3. Solve the Weighted MTSP Problem with Balanced Energy

The energy-balanced MTSP problem extends the MTSP problem modeled in Section2.2. Considering the different traffic energies of each path section, we let the unit energy (UE) of connecting nodes ith and jth is vij, and the energy Cij consumed by the robot traveling between nodes i and j is denoted as equation (14).
(14)Cij=dij×Energyij,where dij is the distance between nodes i and j. To find the total energy of all robots and the smallest route plan, the objective function equation (1) must be rewritten as equation (15). At the same time, it is required to minimize the variance of the energy of traveling between robots by equation (16).
(15)minZ=∑k=1m∑i=1n∑j=1ndijvijpijk,(16)miniY=∑i=1mCi−C¯2n.(1) Establish the Path Weight between Nodes. The goal of this stage is to transform the problem into a weighted MTSP problem in which the weights between nodes are the energies of obstacle constraints and path length. Based on the fully connected path graph with obstacle avoidance (as shown in Figure 4 generated by the improved A∗ algorithm), considering the increase of energy consumption between nodes due to the need to avoid obstacles, the unit energy (UE) consumed by a robot traveling two nodes is approximately defined as eq. (17).
(17)Energy=x1−x2+y1−y2x1−x22+y1−y22,where the upper of eq. (17) actually is the Manhattan distance between two nodes, while the lower is the Euclidean distance directly connected between two nodes. If there are obstacles between two nodes, as shown in Figures 5 and 6 intercepted from the upper right corner of Figure 4, the unit energies (UEs) indicate the difficulties of moving on the direct paths between the two nodes, which are obviously more expensive than those without obstacles. The estimation matrix of UEs does not require additional information about obstacle avoidances. Therefore, when considering the obstructed MTSP problem with energies consumed by robots traveling between nodes, the objective is solved as eq. (15) that is a preparation for the optimal path of energy balance.Figure 5
Path section with obstacle avoidance.Figure 6
Sketch of UE between nodes.(2) Parallel PSO-AC Algorithm for Weighted MTSP with Energy-Balanced. In this section, we propose a hybridization of PSO and ACO algorithm to solve the weighted MTSPs. In our method, the PSO k-means algorithm is used to find clustering central nodes for each robot. Because the right initial clustering center can speed up the convergence and improve the clustering quality, this paper uses a rough classification method to classify the initial nodes by using equations (18) and (19), where the total energy Ri is consumed by a robot traveling from each node to other cities, and the total energy R is consumed by robots traveling the global path sections.
(18)Ri=∑j=0ndijvij,i=0,1,2⋯,n,(19)R=∑i=0n∑j=0ndijvij,i≠j.Then, sort the distances from the samples to the clustering centers and then traverse the samples in turn. If the traversal condition satisfies equation (20), the nodes that have been traversed are classified into one route, and the clustering centers are updated using equation (21). Assume that m is the number of robots and the number of clusters, and f is a Boolean label variable. If the kth robot visits ith node, it will be 1, and otherwise, it will be 0. In the iterative process, the reclassification process is restricted by the condition of equation (20). The purpose is to ensure that the total energy for each route is roughly balanced to m robots.
(20)∑i=0nfikRi≤Rm,(21)Cjp=∑i=1nωijXip∑i=1Nωij,whereωij=11Thesamplebelongstoroutej,0Thesampledoesnotbelongtoroutej.Next, ACO is utilized to calculate the minimum energy of traveling around a node that is currently classified as a route of nodes. After that, the pheromone and taboo tables of the algorithm are updated, and the fitness of the current particle, fitness, is calculating by equation (22), where Ck is the minimum energy, SCk represents the node dispersion of a route by variance function S, and the smaller the variance, the more balanced approximately the traversal energy consumption for robots. The normalization method, in addition, is used to uniformly quantify their energies and the dispersions as equation (22), where the adjusting coefficient q is [0,1]. When q=0, the problem is a common weighted MTSP. On the other word, the larger q, the greater impact of the energy balance on the convergence of the algorithm.
(22)fitness=Ck∑i=0tCk+qSCk∑i=0tSCk.To make full use of the parallel characteristics of the swarm algorithm, we present a hybridization of PSO and ACO to solve this stage of problem. The hybrid scheme is developed on GPU parallel computing platform with CUDA parallelism and multistream processor (SM), so as to enhance the convergence accuracy and executive efficiency of the hybrid swarm intelligence method described as Algorithm3.Algorithm 3: GPSO-AC.
Input: Data after modeling for each node in the MTSP problemOutput: The result of the MTSP problem1 initialize swarm size of particles and ants2do3Parallel_for particle in particle swarm4parallel{update position and speed}5parallel {//reclassification6for cluster center in all cluster results7    calculate the distance between the sample and the current cluster center8Sort sample by distance9    cluster center selection sample10end}11parallel {//calculate fitness12for cluster center in all clustering results13    initialize the ant colony14do15Parallel_for ant in an ant colony16parallel{search path}17parallel{update pheromone}18end19while stop condition reached20    calculate optimal path fitness21end}22parallel{update individual optimal}23parallel{update group optimal}24end25while stop condition reached


#### 3.2.4. Trajectory Optimization and Smoothing

(1) Constructing the Optimal Grid Path. From equations (14) and (17), it can be seen that the energy consumed by a robot traveling between two nodes is simplified as Cij=x1−x2+y1−y2, which is actually the Manhattan distance of the two nodes on direct path after the obstacle constraint is added. Thus, the total energy of a robot traveling around is the total length of a grid path with obstacle constraints. In summary, the balanced energy in the obstructed MTSP is also the equalized length of the gridded path in path planning. For the purpose of multirobot route planning, after an optimal weighted direct path is obtained, we need to convert the direct path as shown in Figure 7 to the path on which robots travel between nodes when bypassing the obstacles. At this stage, it is necessary to use the A∗ algorithm to restore the gridded path between the target nodes, so far, the initial planned path with obstacle avoidance is completed as shown in Figure 8.Figure 7
Gridded rough path section.Figure 8
Gridded path section with obstacle avoidance.(2) Smoothing Path. Because the generated path based on the gridded environment can only be turned by a multiple of 90°, there are a large number of turning points on the path like sawtooth as shown in Figure 9, which will increase both difficulty and traveling distance of the robot movement. In addition, too many invalid turning points will also hinder the polynomial trajectory optimization process, because turning points are used as constraints for trajectory optimization, and useless turning points must be deleted before trajectory optimization. Therefore, an improved RRT∗ is used to smooth trajectories that have fewer natural turning points to facilitate the follow-up operation of the minimum snap algorithm, so as to reduce useless nodes as much as possible to optimize the subsequent polynomial trajectory. If the raster path is not smoothed, there will be too many useless interference nodes, making the trajectory like S shape generated by the minimum snap algorithm [35], as shown in Figure 10. This paper also extends the A∗ algorithm and proposes a smooth grid path algorithm to reduce useless nodes, and finally, the smoothed trajectory can be obtained as shown in Figure 11.Figure 9
Serrated path.Figure 10
Optimized trajectory.Figure 11
Smoothed trajectory.(3) Solving Trajectory Polynomial. After the smooth processing, all breakpoints remaining on the path can be considered as the minimum effective breakpoints that are the necessary nodes to avoid obstacles, like the target nodes. The motion parameters can be quantified as the equality constraints in the minimum snap algorithm, and finally, used to solve the polynomial coefficients. Assumes that it takes time for the robot to perform the task at the target node, then, it can be considered that the speed and acceleration of the robot are 0 at this time, and at the same time, it needs to ensure that the robot does not change the motion state at the inflection node, that is, the polynomial of the speed and acceleration at the inflection node is smooth and continuous. The process of constructing equality constraints is given as follows.Takingx-axis as an example, y-axis similarly, given a grid path LAB, there are n breakpoints on it, and the coordinates of two endpoints are CA and CB. The LAB can be divided into n+1 segments according to the Cnn=1,⋯,n coordinate at the breakpoint. First, make sure that the static acceleration of the robot at both ends is 0, described as equations (23)–(25), then, ensure that the robot runs to the breakpoint at Tn time, and at the breakpoints, the coordinates, velocities, and accelerations of the front and back trajectories should be consistent at t time.
(23)XT0=CAX1T0=0X2T0=0⇒A3×1T0P=CA00,XTn+1=CBX1Tn+1=0X2Tn+1=0⟹A3×1Tn+1P=CB00,(24)XT1=C1⋯XTn=Cn⟹A1×1T1P=C1,⋯A1×1TnP=Cn,(25)XiT=Xi+1TXi1T=Xi+11TXi2T=Xi+12T⟹AiT−Ai+1TPiPi+1=0,where A is the weight matrix at time t, and P is the coefficient matrix to be solved. By taking these equality constraints into the QP objective function of equation (11), the P matrix can be obtained, and the polynomial analytic equation of the path planning trajectory can be obtained.(4) Smooth Grid Path Algorithm. On the basis of the serrated path generated by the A∗ algorithm, aiming at the shortcoming of the path, a smooth raster path algorithm is proposed in this section to solve the invalid turning point on the serrated path. The algorithm involves two processes of trajectory optimization and smooth described in pseudocode as the following Algorithm 4. If there is no obstacle between the two target nodes, the invalid turning point is deleted.Algorithm 4: Smoothing grid path algorithm.
Input: A continuous path on a raster mapOutput: Path to remove useless kinks1 index begin =02 index last =03 new route list record index (begin)4while (begin!=n) do5  last=n6while(end >begin) do7if(no collision between connecting line and obstacles (begin, end)) then8    new route list record index (last)9    begin = last10break11end if12  last = last -113end while14end while


## 3.1. Problem Description and Modeling

This paper focuses on the problem of multirobot motion planning, the first stage of which can be abstracted as a single-point MTSP problem. Assume thatm traveling salesmen (denoted as b1,b2⋯bm) will visit n cities (denoted as T0,T1⋯Tn) to sell their goods, and all traveling salesmen start from the same city T1 and finally meet in city T1; it is required that there must be only one salesman to visit each city, and the total energy and minimum route scheme of all salesmen are obtained [12]. The objective function is
(1)minZ=∑k=1m∑i=1n∑j=1ndijpijk,where pijk is denoted as equation (2) that is the direction marking variable between cities i and j of the traveling salesman with serial number k.
(2)pijk=1,travelingsalesmanktravelsfromcityitoj,2,else.Then, we assume that a given environment as shown in Figure2, in which there are a large number of obstacles (irregular polygons), m target nodes (triangles) Qii=1,⋯,, a given starting node P (diamond at the bottom left). Now, n mobile robots will visit to m target nodes to perform tasks, each node is only visited by one robot, and all robots must return to the starting node after finishing their tasks.Figure 2
A schematic map of the environment with obstacles.


## 3.2. Proposed Method

In this section, let us first explain what is energy balance and why we should balance the energy of multirobots. When each robot carries the same energy at the starting node, how to ensure that the total traveling distance of all robots is the shortest, and how to avoid the imbalance of energy consumption of each robot, that is, some robots stop on the way due to insufficient energy, while the others return to the starting node with a large amount of energy remaining. Thus, we present our motion planning scheme in this paper that has two optimization objectives, one is to minimize the total length of the multirobot traveling path, and another is to minimize the deviation of the path length of the multirobot to ensure the energy consumption balance. For these purposes, in our scheme, the environment information is simplified and merged according to the target to be optimized, the quantitative data and the model are established to solve the initial planning path in line with the optimization target, and then the initial planning path is added with obstacles constraints and dynamic constraints so that the planned path can be truly used in the actual environment. The scheme for multirobot motion planning in obstacles is first proposed as Algorithm1, and its methodology stages of pathfinding, trajectory optimization, and smoothing can be described in details next sections.Algorithm 1: Parallel swarm intelligent motion planning scheme.
Input: Structured 2D plan map of the environmentOutput:m motion trajectories1 define the starting node, target node, and obstacles2 parameter settings, swarm size, the adjustment coefficientq3 for the structured map of the environment4 discretize for using griding method in section2.15 create a gridded mesh and discretized matrix6 until all target nodes and obstacles are discretized on the mesh7 initialize the open set and closed set of nodes by calling A∗ algorithm8 while the open set is not empty9 generate all direct path between nodes10 end while//all nodes are reachable on the fully connected path graph11 while two nodes is not on a direct path12 calculate the unit energies for all not direct path13 Endwhile modelling the problem of MTSP with energies14 for each robot ofm, using GPSO-AC (args)15 generatem route trajectories that met the objectives16 Endfor17 form route trajectories using minimum snap algorithm18 trajectories optimization to reduce unnecessary turning points19 Endfor20 form route trajectories using improve A∗ and RRT algorithm21 solving trajectory polynomial and remove all breakpoints22 Endfor generate m smoothed motion trajectories


### 3.2.1. ImprovedA∗ Algorithm for Path Search

Based on the gridded environment, the A∗ algorithm can be selected for pathfinding with efficiency. The core evaluation function of the A∗ algorithm is as follows:
(3)fn=gn+hn,where fn is the total evaluation value of the current node n, gn is the evaluation generation value from the starting node to the current node in the map, and hn is the evaluation generation value from the current node to the endpoint. In the algorithm, Manhattan distance between nodes is usually used as the evaluation basis:
(4)Dn=xd−xn+yd−yn.The A∗ algorithm also needs two sets to record the intermediate process: an open set and a closed set. The former is used to store the nodes that have not passed under the reachable condition, while the latter is used to store all nodes that have passed or are not reachable. Thus, the method can be presented as Algorithm 2, and the algorithm flowchart is shown in Figure 3. In addition, we also give example graph of fully connected paths with obstacle avoidance generated by the improved A∗ algorithm as shown in Figure 4.Algorithm 2: Improved A∗ algorithm.
Input: Open set, closed set for all nodes on a mapOutput: Full direct path graph1 add all nodes with obstacles to the closed set2 set the starting node to the current node and add it to the open set.3 for all reachable nodes4  collect the current node that are not in the closed set5 Endfor6 while (the open set is not empty)7    calculatef(n) of each reachable node, and add it to the open set8    find a node with the smallestf(n) from the open set9    set the node as the current node, and add it to the open set10    set its parent node as the previous current node11    delete the previous current node from the open set and add it to the closed set12    for all reachable nodes in the open set14      select the node with the smallestg(n) in the open combinationAs the current node, and add it to the open set15    End for16 End doFigure 3
Flowchart of an improved A∗ algorithm.Figure 4
Fully connected path graph with obstacle avoidance.


### 3.2.2. Trajectory Optimization

The path search algorithm does not take into account dynamic constraints such as speed and acceleration when the robot is moving on the way. From the geometric of a view, the planned path is all splicing of line segments, which leads to the connection between the paths by turning points. It is impossible to strictly follow this path in the process, because the motion parameters such as speed and acceleration at the turning point will change unless it is assumed that the speed and acceleration at the turning point are reduced to 0, which will, of course, reduce the motion efficiency of motions. Then, a polynomial-based trajectory optimization method was presented by minimizing jerk (snap) to ensure that the generated trajectory polynomial is continuous and smooth in both speed and acceleration [32–34]. The continuous trajectories between two ends in a two-dimensional plane can be described by the analytic polynomials xt and Yt about time t, taking the x-axis as an example, the polynomial expression is as equation (5).
(5)Xt=pntn+pn−1tn−1+⋯+p1t+p0=∑n=0kpntn,where k is the order of polynomial,p0,p1⋯pn is a polynomial coefficient, the corresponding velocity Vt, acceleration At, and acceleration Sanpt can be expressed as equations (6)–(8). Thus, the trajectory Xt can also be expressed in a matrix form as equation (9).
(6)Vt=X1t=∑n=0kpn.ntn−1,(7)At=X2t=∑n=0kpn.nn−1tn−2,(8)Snapt=X4t=∑n=0kpn.n!n−4!tn−4,(9)Xt=1,t,t2,⋯,tn·p0,p1,⋯,pnT.Similarly, the motion state description functions such asXt and At can be expressed in matrix form. In path planning, the multisegment trajectory between multiple target nodes can be denoted as equation (10).
(10)Xt=1,t,t2,⋯,tn·P1Tt0≤t<t1,1,t,t2,⋯,tn·P2Tt1≤t<t2,⋯1,t,t2,⋯,tn·PkTtk−1≤t<tk.To determine the trajectory of each path, it is necessary to define the value of each coefficient matrixP, minimize the Snapt function of multistage trajectories to obtain high-order continuous and smooth trajectories, that is, to ensure that the coordinates, velocities, and accelerations at the breakpoint between each trajectory are continuous, to ensure that the actual trajectory of the robot is in line with the dynamic constraints in the real world, and can maintain a steady-state continue to be in a stable state of motion. Considering that Snapt can be negative and square, the objective function of minimizing snap can be established as follows:
(11)JT=minimizesnapT2=∫Ti−1TiX4t2dt=PTQP.Among them, the total time ofT segment trajectory, P is the parameter matrix of each section, and Q is the weight matrix. After the establishment of the objective function, the constraint conditions are specified, and the initial state values of the positions, velocities, and acceleration at both ends and internal nodes of a trajectory can be specified:
(12)∑n=0kpntn=x0,∑n=0kpn.ntn−1=v0,∑n=0kpn.nn−1tn−2=a0.Then, we can establish equality constraints on matrixP:
(13)APi=C,where A is the coefficient matrix of T, and C is the initial matrix. It can be seen from equations (11)–(13) that what needs to be solved is a quadratic programming problem (QP). There are many algorithms for solving QPs omitted here. Similarly, we use the same method for y-axis to find the polynomial parameters.


### 3.2.3. Solve the Weighted MTSP Problem with Balanced Energy

The energy-balanced MTSP problem extends the MTSP problem modeled in Section2.2. Considering the different traffic energies of each path section, we let the unit energy (UE) of connecting nodes ith and jth is vij, and the energy Cij consumed by the robot traveling between nodes i and j is denoted as equation (14).
(14)Cij=dij×Energyij,where dij is the distance between nodes i and j. To find the total energy of all robots and the smallest route plan, the objective function equation (1) must be rewritten as equation (15). At the same time, it is required to minimize the variance of the energy of traveling between robots by equation (16).
(15)minZ=∑k=1m∑i=1n∑j=1ndijvijpijk,(16)miniY=∑i=1mCi−C¯2n.(1) Establish the Path Weight between Nodes. The goal of this stage is to transform the problem into a weighted MTSP problem in which the weights between nodes are the energies of obstacle constraints and path length. Based on the fully connected path graph with obstacle avoidance (as shown in Figure 4 generated by the improved A∗ algorithm), considering the increase of energy consumption between nodes due to the need to avoid obstacles, the unit energy (UE) consumed by a robot traveling two nodes is approximately defined as eq. (17).
(17)Energy=x1−x2+y1−y2x1−x22+y1−y22,where the upper of eq. (17) actually is the Manhattan distance between two nodes, while the lower is the Euclidean distance directly connected between two nodes. If there are obstacles between two nodes, as shown in Figures 5 and 6 intercepted from the upper right corner of Figure 4, the unit energies (UEs) indicate the difficulties of moving on the direct paths between the two nodes, which are obviously more expensive than those without obstacles. The estimation matrix of UEs does not require additional information about obstacle avoidances. Therefore, when considering the obstructed MTSP problem with energies consumed by robots traveling between nodes, the objective is solved as eq. (15) that is a preparation for the optimal path of energy balance.Figure 5
Path section with obstacle avoidance.Figure 6
Sketch of UE between nodes.(2) Parallel PSO-AC Algorithm for Weighted MTSP with Energy-Balanced. In this section, we propose a hybridization of PSO and ACO algorithm to solve the weighted MTSPs. In our method, the PSO k-means algorithm is used to find clustering central nodes for each robot. Because the right initial clustering center can speed up the convergence and improve the clustering quality, this paper uses a rough classification method to classify the initial nodes by using equations (18) and (19), where the total energy Ri is consumed by a robot traveling from each node to other cities, and the total energy R is consumed by robots traveling the global path sections.
(18)Ri=∑j=0ndijvij,i=0,1,2⋯,n,(19)R=∑i=0n∑j=0ndijvij,i≠j.Then, sort the distances from the samples to the clustering centers and then traverse the samples in turn. If the traversal condition satisfies equation (20), the nodes that have been traversed are classified into one route, and the clustering centers are updated using equation (21). Assume that m is the number of robots and the number of clusters, and f is a Boolean label variable. If the kth robot visits ith node, it will be 1, and otherwise, it will be 0. In the iterative process, the reclassification process is restricted by the condition of equation (20). The purpose is to ensure that the total energy for each route is roughly balanced to m robots.
(20)∑i=0nfikRi≤Rm,(21)Cjp=∑i=1nωijXip∑i=1Nωij,whereωij=11Thesamplebelongstoroutej,0Thesampledoesnotbelongtoroutej.Next, ACO is utilized to calculate the minimum energy of traveling around a node that is currently classified as a route of nodes. After that, the pheromone and taboo tables of the algorithm are updated, and the fitness of the current particle, fitness, is calculating by equation (22), where Ck is the minimum energy, SCk represents the node dispersion of a route by variance function S, and the smaller the variance, the more balanced approximately the traversal energy consumption for robots. The normalization method, in addition, is used to uniformly quantify their energies and the dispersions as equation (22), where the adjusting coefficient q is [0,1]. When q=0, the problem is a common weighted MTSP. On the other word, the larger q, the greater impact of the energy balance on the convergence of the algorithm.
(22)fitness=Ck∑i=0tCk+qSCk∑i=0tSCk.To make full use of the parallel characteristics of the swarm algorithm, we present a hybridization of PSO and ACO to solve this stage of problem. The hybrid scheme is developed on GPU parallel computing platform with CUDA parallelism and multistream processor (SM), so as to enhance the convergence accuracy and executive efficiency of the hybrid swarm intelligence method described as Algorithm3.Algorithm 3: GPSO-AC.
Input: Data after modeling for each node in the MTSP problemOutput: The result of the MTSP problem1 initialize swarm size of particles and ants2do3Parallel_for particle in particle swarm4parallel{update position and speed}5parallel {//reclassification6for cluster center in all cluster results7    calculate the distance between the sample and the current cluster center8Sort sample by distance9    cluster center selection sample10end}11parallel {//calculate fitness12for cluster center in all clustering results13    initialize the ant colony14do15Parallel_for ant in an ant colony16parallel{search path}17parallel{update pheromone}18end19while stop condition reached20    calculate optimal path fitness21end}22parallel{update individual optimal}23parallel{update group optimal}24end25while stop condition reached


### 3.2.4. Trajectory Optimization and Smoothing

(1) Constructing the Optimal Grid Path. From equations (14) and (17), it can be seen that the energy consumed by a robot traveling between two nodes is simplified as Cij=x1−x2+y1−y2, which is actually the Manhattan distance of the two nodes on direct path after the obstacle constraint is added. Thus, the total energy of a robot traveling around is the total length of a grid path with obstacle constraints. In summary, the balanced energy in the obstructed MTSP is also the equalized length of the gridded path in path planning. For the purpose of multirobot route planning, after an optimal weighted direct path is obtained, we need to convert the direct path as shown in Figure 7 to the path on which robots travel between nodes when bypassing the obstacles. At this stage, it is necessary to use the A∗ algorithm to restore the gridded path between the target nodes, so far, the initial planned path with obstacle avoidance is completed as shown in Figure 8.Figure 7
Gridded rough path section.Figure 8
Gridded path section with obstacle avoidance.(2) Smoothing Path. Because the generated path based on the gridded environment can only be turned by a multiple of 90°, there are a large number of turning points on the path like sawtooth as shown in Figure 9, which will increase both difficulty and traveling distance of the robot movement. In addition, too many invalid turning points will also hinder the polynomial trajectory optimization process, because turning points are used as constraints for trajectory optimization, and useless turning points must be deleted before trajectory optimization. Therefore, an improved RRT∗ is used to smooth trajectories that have fewer natural turning points to facilitate the follow-up operation of the minimum snap algorithm, so as to reduce useless nodes as much as possible to optimize the subsequent polynomial trajectory. If the raster path is not smoothed, there will be too many useless interference nodes, making the trajectory like S shape generated by the minimum snap algorithm [35], as shown in Figure 10. This paper also extends the A∗ algorithm and proposes a smooth grid path algorithm to reduce useless nodes, and finally, the smoothed trajectory can be obtained as shown in Figure 11.Figure 9
Serrated path.Figure 10
Optimized trajectory.Figure 11
Smoothed trajectory.(3) Solving Trajectory Polynomial. After the smooth processing, all breakpoints remaining on the path can be considered as the minimum effective breakpoints that are the necessary nodes to avoid obstacles, like the target nodes. The motion parameters can be quantified as the equality constraints in the minimum snap algorithm, and finally, used to solve the polynomial coefficients. Assumes that it takes time for the robot to perform the task at the target node, then, it can be considered that the speed and acceleration of the robot are 0 at this time, and at the same time, it needs to ensure that the robot does not change the motion state at the inflection node, that is, the polynomial of the speed and acceleration at the inflection node is smooth and continuous. The process of constructing equality constraints is given as follows.Takingx-axis as an example, y-axis similarly, given a grid path LAB, there are n breakpoints on it, and the coordinates of two endpoints are CA and CB. The LAB can be divided into n+1 segments according to the Cnn=1,⋯,n coordinate at the breakpoint. First, make sure that the static acceleration of the robot at both ends is 0, described as equations (23)–(25), then, ensure that the robot runs to the breakpoint at Tn time, and at the breakpoints, the coordinates, velocities, and accelerations of the front and back trajectories should be consistent at t time.
(23)XT0=CAX1T0=0X2T0=0⇒A3×1T0P=CA00,XTn+1=CBX1Tn+1=0X2Tn+1=0⟹A3×1Tn+1P=CB00,(24)XT1=C1⋯XTn=Cn⟹A1×1T1P=C1,⋯A1×1TnP=Cn,(25)XiT=Xi+1TXi1T=Xi+11TXi2T=Xi+12T⟹AiT−Ai+1TPiPi+1=0,where A is the weight matrix at time t, and P is the coefficient matrix to be solved. By taking these equality constraints into the QP objective function of equation (11), the P matrix can be obtained, and the polynomial analytic equation of the path planning trajectory can be obtained.(4) Smooth Grid Path Algorithm. On the basis of the serrated path generated by the A∗ algorithm, aiming at the shortcoming of the path, a smooth raster path algorithm is proposed in this section to solve the invalid turning point on the serrated path. The algorithm involves two processes of trajectory optimization and smooth described in pseudocode as the following Algorithm 4. If there is no obstacle between the two target nodes, the invalid turning point is deleted.Algorithm 4: Smoothing grid path algorithm.
Input: A continuous path on a raster mapOutput: Path to remove useless kinks1 index begin =02 index last =03 new route list record index (begin)4while (begin!=n) do5  last=n6while(end >begin) do7if(no collision between connecting line and obstacles (begin, end)) then8    new route list record index (last)9    begin = last10break11end if12  last = last -113end while14end while


## 3.2.1. ImprovedA∗ Algorithm for Path Search

Based on the gridded environment, the A∗ algorithm can be selected for pathfinding with efficiency. The core evaluation function of the A∗ algorithm is as follows:
(3)fn=gn+hn,where fn is the total evaluation value of the current node n, gn is the evaluation generation value from the starting node to the current node in the map, and hn is the evaluation generation value from the current node to the endpoint. In the algorithm, Manhattan distance between nodes is usually used as the evaluation basis:
(4)Dn=xd−xn+yd−yn.The A∗ algorithm also needs two sets to record the intermediate process: an open set and a closed set. The former is used to store the nodes that have not passed under the reachable condition, while the latter is used to store all nodes that have passed or are not reachable. Thus, the method can be presented as Algorithm 2, and the algorithm flowchart is shown in Figure 3. In addition, we also give example graph of fully connected paths with obstacle avoidance generated by the improved A∗ algorithm as shown in Figure 4.Algorithm 2: Improved A∗ algorithm.
Input: Open set, closed set for all nodes on a mapOutput: Full direct path graph1 add all nodes with obstacles to the closed set2 set the starting node to the current node and add it to the open set.3 for all reachable nodes4  collect the current node that are not in the closed set5 Endfor6 while (the open set is not empty)7    calculatef(n) of each reachable node, and add it to the open set8    find a node with the smallestf(n) from the open set9    set the node as the current node, and add it to the open set10    set its parent node as the previous current node11    delete the previous current node from the open set and add it to the closed set12    for all reachable nodes in the open set14      select the node with the smallestg(n) in the open combinationAs the current node, and add it to the open set15    End for16 End doFigure 3
Flowchart of an improved A∗ algorithm.Figure 4
Fully connected path graph with obstacle avoidance.


## 3.2.2. Trajectory Optimization

The path search algorithm does not take into account dynamic constraints such as speed and acceleration when the robot is moving on the way. From the geometric of a view, the planned path is all splicing of line segments, which leads to the connection between the paths by turning points. It is impossible to strictly follow this path in the process, because the motion parameters such as speed and acceleration at the turning point will change unless it is assumed that the speed and acceleration at the turning point are reduced to 0, which will, of course, reduce the motion efficiency of motions. Then, a polynomial-based trajectory optimization method was presented by minimizing jerk (snap) to ensure that the generated trajectory polynomial is continuous and smooth in both speed and acceleration [32–34]. The continuous trajectories between two ends in a two-dimensional plane can be described by the analytic polynomials xt and Yt about time t, taking the x-axis as an example, the polynomial expression is as equation (5).
(5)Xt=pntn+pn−1tn−1+⋯+p1t+p0=∑n=0kpntn,where k is the order of polynomial,p0,p1⋯pn is a polynomial coefficient, the corresponding velocity Vt, acceleration At, and acceleration Sanpt can be expressed as equations (6)–(8). Thus, the trajectory Xt can also be expressed in a matrix form as equation (9).
(6)Vt=X1t=∑n=0kpn.ntn−1,(7)At=X2t=∑n=0kpn.nn−1tn−2,(8)Snapt=X4t=∑n=0kpn.n!n−4!tn−4,(9)Xt=1,t,t2,⋯,tn·p0,p1,⋯,pnT.Similarly, the motion state description functions such asXt and At can be expressed in matrix form. In path planning, the multisegment trajectory between multiple target nodes can be denoted as equation (10).
(10)Xt=1,t,t2,⋯,tn·P1Tt0≤t<t1,1,t,t2,⋯,tn·P2Tt1≤t<t2,⋯1,t,t2,⋯,tn·PkTtk−1≤t<tk.To determine the trajectory of each path, it is necessary to define the value of each coefficient matrixP, minimize the Snapt function of multistage trajectories to obtain high-order continuous and smooth trajectories, that is, to ensure that the coordinates, velocities, and accelerations at the breakpoint between each trajectory are continuous, to ensure that the actual trajectory of the robot is in line with the dynamic constraints in the real world, and can maintain a steady-state continue to be in a stable state of motion. Considering that Snapt can be negative and square, the objective function of minimizing snap can be established as follows:
(11)JT=minimizesnapT2=∫Ti−1TiX4t2dt=PTQP.Among them, the total time ofT segment trajectory, P is the parameter matrix of each section, and Q is the weight matrix. After the establishment of the objective function, the constraint conditions are specified, and the initial state values of the positions, velocities, and acceleration at both ends and internal nodes of a trajectory can be specified:
(12)∑n=0kpntn=x0,∑n=0kpn.ntn−1=v0,∑n=0kpn.nn−1tn−2=a0.Then, we can establish equality constraints on matrixP:
(13)APi=C,where A is the coefficient matrix of T, and C is the initial matrix. It can be seen from equations (11)–(13) that what needs to be solved is a quadratic programming problem (QP). There are many algorithms for solving QPs omitted here. Similarly, we use the same method for y-axis to find the polynomial parameters.


## 3.2.3. Solve the Weighted MTSP Problem with Balanced Energy

The energy-balanced MTSP problem extends the MTSP problem modeled in Section2.2. Considering the different traffic energies of each path section, we let the unit energy (UE) of connecting nodes ith and jth is vij, and the energy Cij consumed by the robot traveling between nodes i and j is denoted as equation (14).
(14)Cij=dij×Energyij,where dij is the distance between nodes i and j. To find the total energy of all robots and the smallest route plan, the objective function equation (1) must be rewritten as equation (15). At the same time, it is required to minimize the variance of the energy of traveling between robots by equation (16).
(15)minZ=∑k=1m∑i=1n∑j=1ndijvijpijk,(16)miniY=∑i=1mCi−C¯2n.(1) Establish the Path Weight between Nodes. The goal of this stage is to transform the problem into a weighted MTSP problem in which the weights between nodes are the energies of obstacle constraints and path length. Based on the fully connected path graph with obstacle avoidance (as shown in Figure 4 generated by the improved A∗ algorithm), considering the increase of energy consumption between nodes due to the need to avoid obstacles, the unit energy (UE) consumed by a robot traveling two nodes is approximately defined as eq. (17).
(17)Energy=x1−x2+y1−y2x1−x22+y1−y22,where the upper of eq. (17) actually is the Manhattan distance between two nodes, while the lower is the Euclidean distance directly connected between two nodes. If there are obstacles between two nodes, as shown in Figures 5 and 6 intercepted from the upper right corner of Figure 4, the unit energies (UEs) indicate the difficulties of moving on the direct paths between the two nodes, which are obviously more expensive than those without obstacles. The estimation matrix of UEs does not require additional information about obstacle avoidances. Therefore, when considering the obstructed MTSP problem with energies consumed by robots traveling between nodes, the objective is solved as eq. (15) that is a preparation for the optimal path of energy balance.Figure 5
Path section with obstacle avoidance.Figure 6
Sketch of UE between nodes.(2) Parallel PSO-AC Algorithm for Weighted MTSP with Energy-Balanced. In this section, we propose a hybridization of PSO and ACO algorithm to solve the weighted MTSPs. In our method, the PSO k-means algorithm is used to find clustering central nodes for each robot. Because the right initial clustering center can speed up the convergence and improve the clustering quality, this paper uses a rough classification method to classify the initial nodes by using equations (18) and (19), where the total energy Ri is consumed by a robot traveling from each node to other cities, and the total energy R is consumed by robots traveling the global path sections.
(18)Ri=∑j=0ndijvij,i=0,1,2⋯,n,(19)R=∑i=0n∑j=0ndijvij,i≠j.Then, sort the distances from the samples to the clustering centers and then traverse the samples in turn. If the traversal condition satisfies equation (20), the nodes that have been traversed are classified into one route, and the clustering centers are updated using equation (21). Assume that m is the number of robots and the number of clusters, and f is a Boolean label variable. If the kth robot visits ith node, it will be 1, and otherwise, it will be 0. In the iterative process, the reclassification process is restricted by the condition of equation (20). The purpose is to ensure that the total energy for each route is roughly balanced to m robots.
(20)∑i=0nfikRi≤Rm,(21)Cjp=∑i=1nωijXip∑i=1Nωij,whereωij=11Thesamplebelongstoroutej,0Thesampledoesnotbelongtoroutej.Next, ACO is utilized to calculate the minimum energy of traveling around a node that is currently classified as a route of nodes. After that, the pheromone and taboo tables of the algorithm are updated, and the fitness of the current particle, fitness, is calculating by equation (22), where Ck is the minimum energy, SCk represents the node dispersion of a route by variance function S, and the smaller the variance, the more balanced approximately the traversal energy consumption for robots. The normalization method, in addition, is used to uniformly quantify their energies and the dispersions as equation (22), where the adjusting coefficient q is [0,1]. When q=0, the problem is a common weighted MTSP. On the other word, the larger q, the greater impact of the energy balance on the convergence of the algorithm.
(22)fitness=Ck∑i=0tCk+qSCk∑i=0tSCk.To make full use of the parallel characteristics of the swarm algorithm, we present a hybridization of PSO and ACO to solve this stage of problem. The hybrid scheme is developed on GPU parallel computing platform with CUDA parallelism and multistream processor (SM), so as to enhance the convergence accuracy and executive efficiency of the hybrid swarm intelligence method described as Algorithm3.Algorithm 3: GPSO-AC.
Input: Data after modeling for each node in the MTSP problemOutput: The result of the MTSP problem1 initialize swarm size of particles and ants2do3Parallel_for particle in particle swarm4parallel{update position and speed}5parallel {//reclassification6for cluster center in all cluster results7    calculate the distance between the sample and the current cluster center8Sort sample by distance9    cluster center selection sample10end}11parallel {//calculate fitness12for cluster center in all clustering results13    initialize the ant colony14do15Parallel_for ant in an ant colony16parallel{search path}17parallel{update pheromone}18end19while stop condition reached20    calculate optimal path fitness21end}22parallel{update individual optimal}23parallel{update group optimal}24end25while stop condition reached


## 3.2.4. Trajectory Optimization and Smoothing

(1) Constructing the Optimal Grid Path. From equations (14) and (17), it can be seen that the energy consumed by a robot traveling between two nodes is simplified as Cij=x1−x2+y1−y2, which is actually the Manhattan distance of the two nodes on direct path after the obstacle constraint is added. Thus, the total energy of a robot traveling around is the total length of a grid path with obstacle constraints. In summary, the balanced energy in the obstructed MTSP is also the equalized length of the gridded path in path planning. For the purpose of multirobot route planning, after an optimal weighted direct path is obtained, we need to convert the direct path as shown in Figure 7 to the path on which robots travel between nodes when bypassing the obstacles. At this stage, it is necessary to use the A∗ algorithm to restore the gridded path between the target nodes, so far, the initial planned path with obstacle avoidance is completed as shown in Figure 8.Figure 7
Gridded rough path section.Figure 8
Gridded path section with obstacle avoidance.(2) Smoothing Path. Because the generated path based on the gridded environment can only be turned by a multiple of 90°, there are a large number of turning points on the path like sawtooth as shown in Figure 9, which will increase both difficulty and traveling distance of the robot movement. In addition, too many invalid turning points will also hinder the polynomial trajectory optimization process, because turning points are used as constraints for trajectory optimization, and useless turning points must be deleted before trajectory optimization. Therefore, an improved RRT∗ is used to smooth trajectories that have fewer natural turning points to facilitate the follow-up operation of the minimum snap algorithm, so as to reduce useless nodes as much as possible to optimize the subsequent polynomial trajectory. If the raster path is not smoothed, there will be too many useless interference nodes, making the trajectory like S shape generated by the minimum snap algorithm [35], as shown in Figure 10. This paper also extends the A∗ algorithm and proposes a smooth grid path algorithm to reduce useless nodes, and finally, the smoothed trajectory can be obtained as shown in Figure 11.Figure 9
Serrated path.Figure 10
Optimized trajectory.Figure 11
Smoothed trajectory.(3) Solving Trajectory Polynomial. After the smooth processing, all breakpoints remaining on the path can be considered as the minimum effective breakpoints that are the necessary nodes to avoid obstacles, like the target nodes. The motion parameters can be quantified as the equality constraints in the minimum snap algorithm, and finally, used to solve the polynomial coefficients. Assumes that it takes time for the robot to perform the task at the target node, then, it can be considered that the speed and acceleration of the robot are 0 at this time, and at the same time, it needs to ensure that the robot does not change the motion state at the inflection node, that is, the polynomial of the speed and acceleration at the inflection node is smooth and continuous. The process of constructing equality constraints is given as follows.Takingx-axis as an example, y-axis similarly, given a grid path LAB, there are n breakpoints on it, and the coordinates of two endpoints are CA and CB. The LAB can be divided into n+1 segments according to the Cnn=1,⋯,n coordinate at the breakpoint. First, make sure that the static acceleration of the robot at both ends is 0, described as equations (23)–(25), then, ensure that the robot runs to the breakpoint at Tn time, and at the breakpoints, the coordinates, velocities, and accelerations of the front and back trajectories should be consistent at t time.
(23)XT0=CAX1T0=0X2T0=0⇒A3×1T0P=CA00,XTn+1=CBX1Tn+1=0X2Tn+1=0⟹A3×1Tn+1P=CB00,(24)XT1=C1⋯XTn=Cn⟹A1×1T1P=C1,⋯A1×1TnP=Cn,(25)XiT=Xi+1TXi1T=Xi+11TXi2T=Xi+12T⟹AiT−Ai+1TPiPi+1=0,where A is the weight matrix at time t, and P is the coefficient matrix to be solved. By taking these equality constraints into the QP objective function of equation (11), the P matrix can be obtained, and the polynomial analytic equation of the path planning trajectory can be obtained.(4) Smooth Grid Path Algorithm. On the basis of the serrated path generated by the A∗ algorithm, aiming at the shortcoming of the path, a smooth raster path algorithm is proposed in this section to solve the invalid turning point on the serrated path. The algorithm involves two processes of trajectory optimization and smooth described in pseudocode as the following Algorithm 4. If there is no obstacle between the two target nodes, the invalid turning point is deleted.Algorithm 4: Smoothing grid path algorithm.
Input: A continuous path on a raster mapOutput: Path to remove useless kinks1 index begin =02 index last =03 new route list record index (begin)4while (begin!=n) do5  last=n6while(end >begin) do7if(no collision between connecting line and obstacles (begin, end)) then8    new route list record index (last)9    begin = last10break11end if12  last = last -113end while14end while


## 4. Simulation Experiment

### 4.1. Experimental Platform and Experiment Settings

In order to verify the feasibility of the proposed method, the experimental platform as shown in Figure12 is developed using C++ on a PC with CPU Intel® Core™ i5-8400, RAM 8 GB, GPU NVIDIA GeForce GTX 1060 under Linux Ubuntu 18.04.5 LTS. This platform provides some interactive functions including algorithm testing, map grid modeling, obstacle setting, MTSP path generation and optimization, motion trajectory planning, and data saving functions. In our platform development, an open-source SDL2.0 (Simple DirectMedia Layer) is used to the graphics layer rendering library which can make full use of GPU acceleration. For the convenience of debugging, we choose the rendering UI components (ImGUI) that support almost all C++ compilers, which can be embedded in the algorithm for debugging at any time. Because the core algorithm of path planning is to solve the weighted MTSP problem, this experimental platform also provides simulation support for MTSP algorithm experiments.Figure 12
Multirobot path planning experimental simulation platform.


### 4.2. Benchmark Tests and Comparisons

The multirobot motion planning proposed in this paper is a complete solution, which involves a variety of algorithms, such as map modeling, parallel path search, and trajectory optimization and smoothing. We select several TSPLIB datasets to be used to evaluate the performance of our GPSO-AC in path search by metrics of mean running time (MeanRT) and average acceleration ratio (aveRatio).First, for the dataset eil51 selected in TSPLIB, we assume that the number of traveling salesman is 3, the weight between nodes is set to 1 regardless of the cost balance, and the problem is a single node departure one. In this experiment, the proposed GPSO-AC algorithm and the PSO-AC algorithm [16] without GPU acceleration are run independently for 30 times, respectively, in different swarm sizes. The statistical results are shown in Table 1. From Table 1, it can be seen that PSO-AC on eil51 run much longer than the former, and its mean running time increases linearly with the swarm sizes, so it is difficult to application with high real-time requirement; in contrast, when the swarm size increases, the algorithm GPSO-AC on eil51 run much faster, and its mean running time increases less and the average acceleration ratio is greater, because the instruction alienation of clustering in GPU is greatly reduced, which enhances the parallel running efficiency.Table 1
Speedup ratio comparisons of runtimes.
Swarm sizeMean running time (MeanRT)Average acceleration ratio (AveRatio)GPUCPU3252.19 s260.96 s56452.97 s553.96 s10.4612858.06 s1145.35 s19.7325659.07 s1634.82 s27.68Then, the proposed GPSO-AC algorithm is tested on 6 datasets by comparing with others PSO-AC [16], TPHA [18], and Kmeans-AC [33], assuming that the number of traveling salesman is set to 3, the swarm size is 64, the maximum iteration is 500, and the learning factors C1=C2=1.97, α=1, and β=3 according to the previous literatures. The four algorithms run independently for 30 times for each problem, and the statistical metrics of their mean running time, worst solution, optimal solution, mean solution, and standard deviation (StD) are shown in Table 2. It can be seen from Table 2 that although the convergence result of PSO-AC is slightly better than the latter two algorithms, the mean running time is long; the algorithms follow the principle of classification before calculation, which runs faster than the first two hybrid algorithms, but the convergence accuracy is poor; the convergence accuracy of GPSO-AC is always better than others within a reasonable time, as two algorithms use clustering to effectively group nodes. TPHA only considers grouping and does not optimize the grouping in the later stage, resulting in poor convergence results.Table 2
Experimental results comparisons of four algorithms.
ProblemsPerformancesTPHAKmeans-ACPSO-ACGPSO-ACbayg29MeanRT0.23 s0.42 s461.28 s47.11 sWorest13184.913135.110706.110706.1Optimum11791.11173310403.210403.2Mean1218112494.510413.310413.3StD538.296375.31455.3155.31berlin52MeanRT0.73 s1.4 s896.03 s64.52 sWorest9077.089042.338525.28591.67Optimum8774.058809.458423.248470.35Mean8974.178906.138493.738505.86StD72.9952.9825.4336.31st70MeanRT1.38 s2.72 s1245.94 s77.47 sWorest1013.24992.144855.231867.028Optimum977.15964.325830.09826.925Mean989.44979.85841.9477.47StD8.717.249.529.41eil101MeanRT3.23 s6.19 s2117.81 s158.65 sWorest847.11826.824706.65712.31Optimum780.26776.452695.27681.1Mean802.6796.72702.55690.9StD17.2714.413.057.95ch150MeanRT8.47 s16 s3669.83 s237.8 sWorest9072.978873.17607.787022.99Optimum8488.818504.537347.336673.28Mean8810.758719.87432.46833.43StD125.193.668.43101.06kroA200MeanRT18.11 s33.63 s4603.84 s292.34 sWorest38417.837485.633820.933603.5Optimum36926.436563.131940.731271.1Mean37609.737076.232740.332723.6StD377.98279.49442.56547.5


### 4.3. Demonstration of Experimental Results

In the experiment, we assume that the proposed methods are used to planning the motion trajectory for 3 robots that visit the nodes to finish tasks in the environment as shown in Figure13, and the original map is discretely gridded as a mesh scene as shown in Figure 14. Then, the following Figures 15–18 demonstrate the intermediate results of the problem solving. Figure 15 shows a solution after modeling the map information into an MTSP problem which is a rough solution without considering obstacle constraints. While a gridded path with obstacle constraints is generated by the improved A∗ algorithm as shown in Figure 16, from that, there are many useless turning points on the path at this time.Figure 13
Original map.Figure 14
Gridded map.Figure 15
Results of solving MTSP problem algorithm.Figure 16
Building the grid path using A∗ algorithm.Figure 17
New path after smoothing grid path.Figure 18
Optimized path with the minimum-snap algorithm.On the basis of Figure16, after optimizing the path using the smooth grid path algorithm, a new optimized trajectory is obtained as shown in Figure 17. The path does not have any meaningless turning points on the basis of ensuring that the path does not collide with obstacles. Finally, a path that meets the dynamic constraints is generated by using the minimized snap algorithm. As shown in Figure 18, it can be seen that at the turning point between the two target nodes, the path trajectory becomes smooth and arc-shaped, which conforms to the actual situation of the robot.


### 4.4. Experimental Result Analysis

In the experiment, we assume that the proposed methods with parameters the same as the above subsections are tested to planning the motion trajectory for 4 robots that visit 75 nodes to finish tasks in the environment with 28 obstacles as shown in Figure2. The two algorithms run independently for 20 times under different adjustment coefficient q, and the statistical metrics of the total distance and standard deviation for each robot are recorded in Table 3 to investigate the adjustment coefficient of influence on generation path before and after optimization, while the relationship between the adjustment coefficient and the total length of the path and the relationship between the adjustment coefficient and the standard deviation of its total length for each robot are plotted in Figures 19 and 20, respectively.Table 3
Influence of different adjustment coefficients on generation path.
Adjustment coefficientqBefore and after optimizationRobot 1Robot 2Robot 3Robot 4Total distanceStandard deviationq=0A∗ grid path16502220238015007750428.05Smoothed grid path1403.521893.31938.271273.686508.77337.92q=0.025A∗ grid path20301540247018007840394.55Smoothed grid path1709.261322.252142.841521.136695.48350.31q=0.05A∗ grid path16902260225016807880329.14Smoothed grid path1404.081887.271781.561442.546515.45241.75q=0.075A∗ grid path19102200193019608000134.9Smoothed grid path1608.631825.321697.471619.066750.4899.99q=0.1A∗ grid path1980208020001990805045.73Smoothed grid path1672.181742.481659.951667.456742.0538.31q=0.15A∗ grid path2060205021202080831030.96Smoothed grid path1728.81697.911754.521825.257006.4854.27q=0.2A∗ grid path2140211021402170856024.49Smoothed grid path1854.171741.491761.511909.587266.7579.02q=0.25A∗ grid path2160217021402130860018.26Smoothed grid path1855.91772.031694.561851.217173.7176.32q=0.3A∗ grid path220021902190218087608.16Smoothed grid path1899.911804.661878.381895.447478.3944.27q=0.4A∗ grid path224022302230224089405.77Smoothed grid path1887.131845.5718671973.627573.3156.16q=0.5A∗ grid path228022702270227090905Smoothed grid path1877.951929.971998.081865.287671.2760.38q=0.6A∗ grid path223022302230224089305Smoothed grid path1876.771852.41947.231856.317532.7244.02q=0.7A∗ grid path219022002200220087905Smoothed grid path1786.81847.491891.221840.257365.7842.82q=0.8A∗ grid path222022302230223089105Smoothed grid path1840.441847.11788.941926.377402.8556.74Figure 19
Effects of adjustment coefficient on the path lengths.Figure 20
Influence of adjustment coefficient on the deviation of path length.From Table3 and Figures 19 and 20, we can see that the total length of the path after smoothing is much shorter than that before, which shows that the proposed smooth grid path algorithm is effective and greatly reduces the energy consumption of the robot. Meanwhile, without considering the balance of energy consumption, i.e., setting the adjustment coefficient q=0, the path length deviation of each robot is the largest, at this time, the total path length is the shortest. Then, gradually increasing the value of q, it is found that the standard deviation of the grid path will decrease rapidly whether it is smoothed or not, and then as the value of q continues to increase, the standard deviation tends to a smaller value. According to the proposed algorithm, we find that the energy of path smoothing increases rapidly with the increase of the standard deviation of path length, which demonstrates that the equalization algorithm is effective, and balancing the path length of each robot is the guarantee of balancing energy consumption. In addition, as the adjustment coefficient q increases, the path length shows an upward trend, because at this stage, the algorithm focuses on the energy balance when calculating the fitness score and ignores the impact of the global optimal energy on the fitness. Therefore, it can be concluded that in the path standard, it is meaningless to continue to increase the value of q after the difference tends to a stable small value. Instead, it will increase the path length and increase the energy consumption of the robot.


## 4.1. Experimental Platform and Experiment Settings

In order to verify the feasibility of the proposed method, the experimental platform as shown in Figure12 is developed using C++ on a PC with CPU Intel® Core™ i5-8400, RAM 8 GB, GPU NVIDIA GeForce GTX 1060 under Linux Ubuntu 18.04.5 LTS. This platform provides some interactive functions including algorithm testing, map grid modeling, obstacle setting, MTSP path generation and optimization, motion trajectory planning, and data saving functions. In our platform development, an open-source SDL2.0 (Simple DirectMedia Layer) is used to the graphics layer rendering library which can make full use of GPU acceleration. For the convenience of debugging, we choose the rendering UI components (ImGUI) that support almost all C++ compilers, which can be embedded in the algorithm for debugging at any time. Because the core algorithm of path planning is to solve the weighted MTSP problem, this experimental platform also provides simulation support for MTSP algorithm experiments.Figure 12
Multirobot path planning experimental simulation platform.


## 4.2. Benchmark Tests and Comparisons

The multirobot motion planning proposed in this paper is a complete solution, which involves a variety of algorithms, such as map modeling, parallel path search, and trajectory optimization and smoothing. We select several TSPLIB datasets to be used to evaluate the performance of our GPSO-AC in path search by metrics of mean running time (MeanRT) and average acceleration ratio (aveRatio).First, for the dataset eil51 selected in TSPLIB, we assume that the number of traveling salesman is 3, the weight between nodes is set to 1 regardless of the cost balance, and the problem is a single node departure one. In this experiment, the proposed GPSO-AC algorithm and the PSO-AC algorithm [16] without GPU acceleration are run independently for 30 times, respectively, in different swarm sizes. The statistical results are shown in Table 1. From Table 1, it can be seen that PSO-AC on eil51 run much longer than the former, and its mean running time increases linearly with the swarm sizes, so it is difficult to application with high real-time requirement; in contrast, when the swarm size increases, the algorithm GPSO-AC on eil51 run much faster, and its mean running time increases less and the average acceleration ratio is greater, because the instruction alienation of clustering in GPU is greatly reduced, which enhances the parallel running efficiency.Table 1
Speedup ratio comparisons of runtimes.
Swarm sizeMean running time (MeanRT)Average acceleration ratio (AveRatio)GPUCPU3252.19 s260.96 s56452.97 s553.96 s10.4612858.06 s1145.35 s19.7325659.07 s1634.82 s27.68Then, the proposed GPSO-AC algorithm is tested on 6 datasets by comparing with others PSO-AC [16], TPHA [18], and Kmeans-AC [33], assuming that the number of traveling salesman is set to 3, the swarm size is 64, the maximum iteration is 500, and the learning factors C1=C2=1.97, α=1, and β=3 according to the previous literatures. The four algorithms run independently for 30 times for each problem, and the statistical metrics of their mean running time, worst solution, optimal solution, mean solution, and standard deviation (StD) are shown in Table 2. It can be seen from Table 2 that although the convergence result of PSO-AC is slightly better than the latter two algorithms, the mean running time is long; the algorithms follow the principle of classification before calculation, which runs faster than the first two hybrid algorithms, but the convergence accuracy is poor; the convergence accuracy of GPSO-AC is always better than others within a reasonable time, as two algorithms use clustering to effectively group nodes. TPHA only considers grouping and does not optimize the grouping in the later stage, resulting in poor convergence results.Table 2
Experimental results comparisons of four algorithms.
ProblemsPerformancesTPHAKmeans-ACPSO-ACGPSO-ACbayg29MeanRT0.23 s0.42 s461.28 s47.11 sWorest13184.913135.110706.110706.1Optimum11791.11173310403.210403.2Mean1218112494.510413.310413.3StD538.296375.31455.3155.31berlin52MeanRT0.73 s1.4 s896.03 s64.52 sWorest9077.089042.338525.28591.67Optimum8774.058809.458423.248470.35Mean8974.178906.138493.738505.86StD72.9952.9825.4336.31st70MeanRT1.38 s2.72 s1245.94 s77.47 sWorest1013.24992.144855.231867.028Optimum977.15964.325830.09826.925Mean989.44979.85841.9477.47StD8.717.249.529.41eil101MeanRT3.23 s6.19 s2117.81 s158.65 sWorest847.11826.824706.65712.31Optimum780.26776.452695.27681.1Mean802.6796.72702.55690.9StD17.2714.413.057.95ch150MeanRT8.47 s16 s3669.83 s237.8 sWorest9072.978873.17607.787022.99Optimum8488.818504.537347.336673.28Mean8810.758719.87432.46833.43StD125.193.668.43101.06kroA200MeanRT18.11 s33.63 s4603.84 s292.34 sWorest38417.837485.633820.933603.5Optimum36926.436563.131940.731271.1Mean37609.737076.232740.332723.6StD377.98279.49442.56547.5


## 4.3. Demonstration of Experimental Results

In the experiment, we assume that the proposed methods are used to planning the motion trajectory for 3 robots that visit the nodes to finish tasks in the environment as shown in Figure13, and the original map is discretely gridded as a mesh scene as shown in Figure 14. Then, the following Figures 15–18 demonstrate the intermediate results of the problem solving. Figure 15 shows a solution after modeling the map information into an MTSP problem which is a rough solution without considering obstacle constraints. While a gridded path with obstacle constraints is generated by the improved A∗ algorithm as shown in Figure 16, from that, there are many useless turning points on the path at this time.Figure 13
Original map.Figure 14
Gridded map.Figure 15
Results of solving MTSP problem algorithm.Figure 16
Building the grid path using A∗ algorithm.Figure 17
New path after smoothing grid path.Figure 18
Optimized path with the minimum-snap algorithm.On the basis of Figure16, after optimizing the path using the smooth grid path algorithm, a new optimized trajectory is obtained as shown in Figure 17. The path does not have any meaningless turning points on the basis of ensuring that the path does not collide with obstacles. Finally, a path that meets the dynamic constraints is generated by using the minimized snap algorithm. As shown in Figure 18, it can be seen that at the turning point between the two target nodes, the path trajectory becomes smooth and arc-shaped, which conforms to the actual situation of the robot.


## 4.4. Experimental Result Analysis

In the experiment, we assume that the proposed methods with parameters the same as the above subsections are tested to planning the motion trajectory for 4 robots that visit 75 nodes to finish tasks in the environment with 28 obstacles as shown in Figure2. The two algorithms run independently for 20 times under different adjustment coefficient q, and the statistical metrics of the total distance and standard deviation for each robot are recorded in Table 3 to investigate the adjustment coefficient of influence on generation path before and after optimization, while the relationship between the adjustment coefficient and the total length of the path and the relationship between the adjustment coefficient and the standard deviation of its total length for each robot are plotted in Figures 19 and 20, respectively.Table 3
Influence of different adjustment coefficients on generation path.
Adjustment coefficientqBefore and after optimizationRobot 1Robot 2Robot 3Robot 4Total distanceStandard deviationq=0A∗ grid path16502220238015007750428.05Smoothed grid path1403.521893.31938.271273.686508.77337.92q=0.025A∗ grid path20301540247018007840394.55Smoothed grid path1709.261322.252142.841521.136695.48350.31q=0.05A∗ grid path16902260225016807880329.14Smoothed grid path1404.081887.271781.561442.546515.45241.75q=0.075A∗ grid path19102200193019608000134.9Smoothed grid path1608.631825.321697.471619.066750.4899.99q=0.1A∗ grid path1980208020001990805045.73Smoothed grid path1672.181742.481659.951667.456742.0538.31q=0.15A∗ grid path2060205021202080831030.96Smoothed grid path1728.81697.911754.521825.257006.4854.27q=0.2A∗ grid path2140211021402170856024.49Smoothed grid path1854.171741.491761.511909.587266.7579.02q=0.25A∗ grid path2160217021402130860018.26Smoothed grid path1855.91772.031694.561851.217173.7176.32q=0.3A∗ grid path220021902190218087608.16Smoothed grid path1899.911804.661878.381895.447478.3944.27q=0.4A∗ grid path224022302230224089405.77Smoothed grid path1887.131845.5718671973.627573.3156.16q=0.5A∗ grid path228022702270227090905Smoothed grid path1877.951929.971998.081865.287671.2760.38q=0.6A∗ grid path223022302230224089305Smoothed grid path1876.771852.41947.231856.317532.7244.02q=0.7A∗ grid path219022002200220087905Smoothed grid path1786.81847.491891.221840.257365.7842.82q=0.8A∗ grid path222022302230223089105Smoothed grid path1840.441847.11788.941926.377402.8556.74Figure 19
Effects of adjustment coefficient on the path lengths.Figure 20
Influence of adjustment coefficient on the deviation of path length.From Table3 and Figures 19 and 20, we can see that the total length of the path after smoothing is much shorter than that before, which shows that the proposed smooth grid path algorithm is effective and greatly reduces the energy consumption of the robot. Meanwhile, without considering the balance of energy consumption, i.e., setting the adjustment coefficient q=0, the path length deviation of each robot is the largest, at this time, the total path length is the shortest. Then, gradually increasing the value of q, it is found that the standard deviation of the grid path will decrease rapidly whether it is smoothed or not, and then as the value of q continues to increase, the standard deviation tends to a smaller value. According to the proposed algorithm, we find that the energy of path smoothing increases rapidly with the increase of the standard deviation of path length, which demonstrates that the equalization algorithm is effective, and balancing the path length of each robot is the guarantee of balancing energy consumption. In addition, as the adjustment coefficient q increases, the path length shows an upward trend, because at this stage, the algorithm focuses on the energy balance when calculating the fitness score and ignores the impact of the global optimal energy on the fitness. Therefore, it can be concluded that in the path standard, it is meaningless to continue to increase the value of q after the difference tends to a stable small value. Instead, it will increase the path length and increase the energy consumption of the robot.


## 5. Conclusion

Multirobot motion path planning is one of the hottest topics in robot technology. It is particularly difficult for multiple mobile robots in a complex obstacle environment where all robots need to execute their tasks, respectively. To speed up the running speed and guarantee the power supply of robots in a limited time, an energy-balanced parallel swarm intelligent optimizer (GPSO-AC) based on GPUs is presented for multirobot to find the shortest path in an obstacle environment where the energy consumption balance in the process of robot movement is modeled as a weighted MTSP problem. The presented method combines with grid constructing and improved the A∗ searching, smooth gridding, and minimum shaping algorithms to find a feasible oscillation-free, sharp turn-free, and collision-free path. To test the effectiveness of the proposed method, we develop an experimental platform based on GPUs where several scenes with multiple irregularly-shaped obstacles are generated to verify the performance of the presented method. In our experiments, the proposed GPSO algorithm is test on several datasets to evaluate the performances by comparing other similar approaches, and our method on different scenes has found the shortest path in which the multirobot can faster, smoother, and energy-balanced complete the tasks in a reasonable time. In practical applications, the trade-off between the shortest path and the minimum path length should be made. According to the experimental conclusion, the q value should be increased after the path length deviation is taken to a small value within a reasonably acceptable range. However, if each robot is regarded as a dynamic obstacle, it should be further studied in the future.



---

*Source: 8902328-2021-08-30.xml*