Decentralized Path Planning for Multi-Objective Robot Swarm System

The multi-robot path planning aims to explore a set of non-colliding paths with the shortest sum of lengths for multiple robots. The most popular approach is to artificially decompose the map into discrete small grids before applying heuristic algorithms. To solve the path planning in continuous environments, we propose a decentralized two-stage algorithm to solve the path-planning problem, where the obstacle and inter-robot collisions are both considered. In the first stage, an obstacle- avoidance path-planning problem is mathematically developed by minimizing the travel length of each robot. Specifically, the obstacle-avoidance trajectories are generated by approximating the obstacles as convex-concave constraints. In the second stage, with the given trajectories, we formulate a quadratic programming (QP) problem for velocity control using the control barrier and Lyapunov function (CBF-CLF). In this way, the multi-robot collision avoidance as well as time efficiency are satisfied by adapting the velocities of robots. In sharp contrast to the conventional heuristic methods, path length, smoothness and safety are fully considered by mathematically formulating the optimization problems in continuous environments. Extensive experiments as well as computer simulations are conducted to validate the effectiveness of the proposed path-planning algorithm.


Introduction
In the early days, mobile robots were used only to help with production in factories. However, with the development of science and technology, such mobile robots are gradually applied to disaster relief, education, medical care, agricultural production, information and communication fields. When performing a locating pathfinder mission, the robot is equipped with many advanced equipments, the main functions of these devices include mapping of the environment, obstructive detection, control the motion and use navigation techniques to avoid obstacles. The most important function of any navigation for a robot is to find and plan a safe route from start position to terminal point.
The most common approach for path planning is to divide the continuous environment into a discrete gridded space. Considerable investigations have been devoted into this scheme. For instance, the genetic algorithm (GA) [1] and particle swarm optimization (PSO) [2] were developed to find the suitable trajectory with the shortest length in a discrete gridded environment. However, the computational complexity as well as time cost may become extremely high due to the NP-hard nature of this approach [3]. Furthermore, the gridded environments may result in unrealistic trajectories, where sharp turns are inevitable.
To address these challenges, an enhanced genetic algorithm (EGA) was developed in [4], where five genetic operators were designed to improved the performance at each iteration in terms of the predefined objective function. In [5], an informative path-planning problem was derived and resolved using an evolutionary strategy. Unfortunately, the aforementioned algorithms are not suitable for dynamic scenarios since the training process is highly depended. In [6], a decentralized method using velocity control was derived for multi-vehicle traffic networks, where the path-planning problem was only investigated in two-lane scenarios. The control barrier function (CBF) is utilized to guarantee the safe distance among vehicles.
The existing methods for multi-robot path planning can be classified into two categories, namely centralized and decoupled methods. For the centralized approaches, the constraints and trajectory generation for all robots are combined into one optimization problem such that the solutions are near to the global optimum. However, the time complexity may become extremely high due to the combination of multiple constraints and configurations. Therefore, the centralized approaches usually cannot adapt dynamic environments. In contrast, decoupled methods first derive an individual trajectory for each robot and then resolve the possible conflicts of the generated trajectories. A typical decoupled approach was proposed in [7], where the wait-strategy is adopted by predefining the priority of robots. After the trajectories are individually generated, the robot with lower priority is ordered to wait temporarily if two robots are collision-prone. Despite the collisions are avoided, this wait-strategy is nor time-efficient, neither feasible in practice.
In this work, we propose a decoupled approach for multi-robot path planning in which two stages are adopted. In the first stage, obstacle-avoidance trajectories are generated for the individual robots. In contrast to the heuristic algorithms based on the gridded environments, such as A* and Dijkstra [8], the proposed trajectory-generation algorithm mathematically formulates an optimization problem by approximating the obstacles into convex-concave constraints. After obtained the trajectories, we control the velocities of the robots to avoid possible inter-robot collision while considering the time efficiency. The proposed approach is decentralized since each robot only computes its own trajectory and velocity according to the surrounding environments.
In summary, the main contributions of this work can be summarized as follows: 1) A two-stage decoupled approach is developed for multi-robot path planning in continuous environments. The proposed algorithm can be implemented in decentralized online operations.
2) As compared to the conventional heuristic algorithms that decompose the spaces into discrete grids, we derive the obstacle-avoidance trajectories by formulating an optimization problem in continuous environments, where the obstacles are circular-approximated.
3) The inter-robot collisions of the planned trajectories are avoided using velocity control by formulating a quadratic programming (QP) problem. In the meanwhile, the travel time of robots is minimized by adopting the control Lyapunov function (CLF). Finally, experiments and simulations are implemented to validate the effectiveness of the proposed approach. The remainder of this paper is organized as follows. Section II provides the brief review of CBF-CLF used in this work. Next, the multi-robot system model and problem statement are presented in Section

III.
To solve the path-planning problem, we first generate the obstacle-avoidance trajectories using DCCP in Section IV. Next, the possible collisions of robots are avoided by solving the QP problem derived in Section V. Furthermore, performance of the proposed approach is evaluated in Section VI, and Section VII concludes this work.
Notation: Uppercase boldface and lowercase boldface letters are used to denote matrices and vectors, respectively. A stands for the 2  norm of A while Ȧ denotes the derivative of A. LfA is the Lie derivative of function A to f , Â the estimator of Â, where Â can be a matrix, vector, or scalar. A T stands for the transpose of matrix A. Finally, f     represents the gradient of function f (A).

A brief review of CBF-CLF problem formulation
Firstly, we provide an elegant introduction of control barrier functions (CBFs). The complete details can be found in [9]. A continuous function Γ(x) is defined as a class K function if Γ(x) is strictly increasing and Γ(0) = 0 for x≤0.
In this paper, we consider a continuous-time affine control system formulated by Where function B(x) for a control system is defined by the following inequalities: for all x ∈ C. L f and L g are the Lie derivatives of the function B(x) to f and g, respectively. The Lie derivatives are defined x is a scalar.For a CBF B(x) and γ > 0, any Lipschitz continuous controller u with renders set C forward invariant [9] for the affine control system in Equation (1). A continuously differentiable function n n V : R R  is an exponentially stabilizing control Lyapunov function (CLF) if there exist positive constants c 1 , c 2 , c 3 > 0 such that for any n x R  , the following inequalities hold: Given a CLF ( ) V X , any Lipschitz continuous controller u holds The CLF and CBF can be combined into a single controller through a quadratic programming (QP) problem represented as Where C3≥0 is a constant and α(h(x)) is a class K function. H x (x) is a positive definite and In addition,δ 0 is the relaxation factor on the CLF.

Multi-robot system model
Consider a swarm of Z robots moving in a continuous environment. All the robots are equipped with LiDARs and RGB-D cameras to detect the surrounding situations. The velocity and position of adjacent robots as well as the locations of M obstacles can be measured using the equipped sensors. The obstacles in arbitrary shapes are centered We assume the start and destination locations are given for each robots. The task for each robots is efficiently traveling from the start point to its destination in a continuous environment while avoiding collisions with obstacles and other robots. Furthermore, we assume the robots consist of motion primitives that allow them to deterministically move from z ,n y to  are the set of sampled points in the trajectory of the z-th robot. The maximal velocity and acceleration of a robot are given as v max and a max , respectively.

Path-Planning Problem Statement
For the multi-objective path planning, the designated robots are assigned to achieve their own destinations. The objectives and constraints are summarized as follows: • Path Length: The sum of path lengths is one of the most important metric for the performance of the proposed path-planning algorithm. Let L z the path length of the z-th robot that travels from the start point to its destination. The objective function can be modeled as the minimization of . Obviously, the possible collisions among the robots will increase the max travel time.
• Safety: The safety constraints can be divided into two categories, namely static and dynamic collision-avoidance con-straints. Firstly, the static collision avoidance refers to the avoidance of static obstacles in the given environment. Next, the inter-robot collisions are considered as dynamic collision avoidance. The safety distance must be satisfied with both of the constraints during the movement of the robots.
• Smoothness: A smooth and continuous UGV's trajectory without sharp turns is desired in practice. We will first characterize the smoothness of the generated trajectories. For any three ordered positions {y z,n−1 , y z,n−1 , y z,n+1 } with n = 2, · · · , N − 1, the turning angle is defined as We propose to minimize the following metric to ensure the smoothness: In this work, we aim to achieve the aforementioned objectives subject to the constraints for each robot.

Obstacle-Avoidance Path Planning for A Single Robot
In the sequel, we investigate the path-planning problem for a single robot. The inter-robot collisions are neglected temporarily. The goal of path planning is to find the shortest trajectory connecting initial point s int and destination s des that avoids Mobstacles, which are approximated as M circles using Algorithm 1. Apart from obstacles, the available road denoted by Γ is assumed to be known in advance. For instance, the available roads and corridors are usually constrained by the kerbstones or walls.
We assume a continuous trajectory is discretized into N points. The problem is represented as where L z is the path length for z = 1,..., Z. C 1 provides the start and destination locations while C 2 ensures the smoothness of the generated trajectory. A larger N will produce a smoother trajectory but the computational complexity is also improved as the increment of variables. In addition, C 3 guarantees the safety distance between the robot and circular-approximated obstacles.
The constraint C 3 is a convex-concave constraint, which cannot be solved by generic solvers. To cope with this issue, we linearize the problem in Equation (13) where we set z,n m F(y )=r and z ,n z ,n m G(y )= y p  , The subscript (i) denotes the i-th iteration for the optimization of Equation (13). Furthermore, β is added to the objective function as a regularization term.
Using the sequential convex programming algorithm in [11], the problem in Equation (14) can be iteratively solved via efficient solver, such as DCCP [12]. This problem can be regarded as a difference of two convex (DC) problem, thereby the convergence of Equation (14) is guaranteed. It is worth noting that the obtained trajectories cannot be verified as global optimums due to the non-convex nature.
Once the trajectories for all of the robots are generated, the sum of path lengths is lower bounded by where L * z is the obtained path length from Equation (13). We will validate the effectiveness of the proposed algorithm by comparing the path lengths with other algorithms in Section VI.

Collision-aware path planning for multi-robot scenarios
With the given trajectory for all of the robots, the collision-avoidance problem is addressed by velocity control in the sequel. Based on CBF-CLF reviewed in Section II, the time-varying velocities of robots are obtained by solving a quadratic programming (QP) problem. For each robot, it only needs to acquire the location and velocity of the nearest robot. Thus, the operation is decentralized and can be implemented online. Once the trajectories of robots are generated in Section IV, we now consider the collision-aware path planning by velocity control. In the decentralized dynamic model, we assume one robot is able to obtain the location and velocity of its nearest robot. Since no central coordinator exists, each robot can only control its own velocity. Thus, we propose a leader-follower (LF) dynamic model, where the velocity of the leader robot cannot be changed while the follower robot executes the decentralized algorithm.  Fig. 2, the velocity components between two robots are denoted by: Where the l  and f  are given as follows Furthermore, the distance of two robots is denoted by Given x(t) = (ṽ f (t), ṽ l (t), D(t)), the dynamics of an LF system can be compactly represented as where F r (v a (t)) = f 0 + f 1 ṽ f (t) + f 2 ṽ f (t) 2 is the aerodynamic drag; the coefficients f 0 , f 1 , and f 2  . αg(t) is the random noise of the measured velocity of the leader robot respect to a Gaussian distribution with zero mean. Next, we consider solving the decentralized collision-aware path planning by formulating a QP problem at the t-th instant. For notation simplicity, the state variable x(t) is written as x temporarily.

Travel Time Minimization
The minimum travel time can be achieved when the robots move with the maximal velocity. We sort the path lengths from the largest to the smallest, i.e., L z ≥ L z+1 . Thus, the lower bound of the travel time to accomplish the tasks is To minimize the travel time, we should select the robot with a larger path length as a leader robot since the follower robot may reduce its velocity to avoid the inter-robot collisions. In this way, the robot with a larger path length can keep a relatively larger velocity.
Given the dynamic model, we give a "soft constraint" for each robot to achieve a desired velocity: This constraint only works when the safety distance is ensured. The soft constraint can be written in terms of a candidate CLF: For any c > 0, the following inequality verifies that

CBF-CLF QP formulation
Now we consider the constraints for inter-robot collision avoidances. We express the safety distance as a "hard constraint" given by where τ is a constant reaction time for the follower robot. The hard constraint ensures the safety distance of two robots. We consider the function ( ) Recalling Equation (6),we set 1 which validates B(x) is a CBF. As a result, a CLF-CBF QP is formulated by combining the above constraints [14]: The constraint C 1 is generated by CLF and the additive parameter σ makes the CLF constraint "soft". The constraint C 2 guarantees the "hard" safety distance for two robots while a fc is the acceleration limit of a robot in constraint C 3 . It is worth noting that the combination of CLF and CBF assures the requirements are not in conflict.
In addition, the cost function related to the control yields the following function in terms of u: In each instant, the left travel distances of robots are sorted. The robot with a larger left distance is selected as the leader robot. In this way, the travel time can be minimized according to Section V-B.
The decentralized path planning algorithm is elaborated in Algorithm 1, where the DCCP and CBF-CLF are implemented. Specifically, the obstacle-avoidance trajectories for each robots are first generated by temporarily ignoring the possible collisions using DCCP. Next, the inter-robot collisions are considered by formulating a QP problem using CBF-CLF. In the time-varying dynamic system, the possible collision is avoided by controlling the velocities of robots.

Performance evaluation
To evaluate the performance of the proposed multi-objective path-planning algorithm, the concept-proof experiments are presented in this section. We use multiple RoboMaster produced by Da-Jiang Innovations (DJI) to accomplish the experiments. The RoboMaster can be controlled via computer with the SDK provided by DJI 1 . Each RoboMaster is equipped with a LiDar and a RGB-D camera such that the state of other robots can be detected. We assume the geographical information of the given environment is acknowledged.
a fc 20

Velocity Control for LF Dynamic Model
We first investigate the velocity control of two robots in the LF dynamic model. The leader robot is observed by the follower robot. After obtained the position and velocity of the leader robot, the follower robot dynamically control its velocity to avoid possible collisions. If no collision exists, the follower robot is prone to move with the maximum velocity. In Fig. 3, the velocities of two robots are illustrated by exploiting the CBF-CLF-based algorithm derived in Section V.     Next, we consider the multi-objective path planning for the robot swarm. Assume that four RoboMasters are assigned tasks to achieve different destinations in a continuous environment, where the obstacles are randomly deployed.
The left side of Fig. 5 shows the trajectories generated using the circular-approximated obstacles corresponding to the practical environment in the right side.

Conclusion and Future Work
In this work, a decentralized path-planning approach is proposed for multi-robot scenarios. Based on DCCP and CBF-CLF, the decoupled problems are separately resolved in two stages. Firstly, the trajectories are generated by neglecting the inter-robot collisions. Next, we formulate a QP problem using CBF-CLF to design the velocity controller for each robot. In this way, the travel time as well as the trajectory length are minimized. The decentralized algorithm operates on the robots by implementing the LF model. Finally, extensive experiments verify the advantages of the proposed decentralized approach, showing the improvements on efficiency and solution quality. In our future works, we will consider the dynamic path planning problem for the robots. For instance, one of the most important path-planning problems is the existing of the dynamic obstacles. It requires much more computational resource for the computer equipped on the robot. Moreover, the path-planning problem can be also extended to the 3-D scenarios, such as the unmanned aerial vehicles (UAVs).