# Multi-Robot Trajectory Planning with Feasibility Guarantee and Deadlock
Resolution: An Obstacle-Dense Environment

Yuda Chen, Chenghan Wang, and Zhongkui Li  The authors are with the State Key
Laboratory for Turbulence and Complex Systems, Department of Mechanics and
Engineering Science, College of Engineering, Peking University, Beijing
100871, China (e-mail: zhongkli@pku.edu.cn).

###### Abstract

This article presents a multi-robot trajectory planning method which
guarantees optimization feasibility and resolves deadlocks in an obstacle-
dense environment. The method is proposed via formulating an optimization
problem, where the modified buffered Voronoi cell with warning band is
utilized to avoid the inter-robot collision and the deadlock is resolved by an
adaptive right-hand rule. Meanwhile, a novel safe corridor derived from
historical planned trajectory is proposed to provide a proper space for
obstacle avoidance in trajectory planning. Comparisons with state-of-the-art
works are conducted to illustrate the safety and deadlock resolution in
cluttered scenarios. Additionally, hardware experiments are carried out to
verify the performance of the proposed method where eight nano-quadrotors fly
through a $0.6$m cubic framework.

###### Index Terms:

Trajectory generation, motion planning, multi-robot system, collision
avoidance, deadlock resolution.

##  I Introduction

Collision-free trajectory planning plays an essential role in the missions
preformed by a swarm of robots in a shared environment, such as cooperative
inspection and transportation [1, 2]. Currently, optimization-based methods,
such as model predictive control [3] and sequential convex program [4], are
widely employed to handle collision avoidance by introducing different kinds
of constraints. However, the constrained optimization problem may suffer from
infeasibility leading to the failure of replanning, and such a phenomenon
occurs more frequently in a crowded environment. Furthermore, in obstacle-
dense situations, the robots in the swarm are more likely to get stuck with
each other, which is also known as deadlock [5].

Concerning the multi-robot trajectory planning problem in an obstacle-dense
environment, we propose a novel method to ensure the optimization feasibility
and handle the deadlock problem simultaneously. In this work, the modified
buffered Voronoi cell with warning band (MBVC-WB) [6] is utilized to deal with
inter-robot collision avoidance, and the assorted adaptive right hand rule is
brought in for deadlock resolution. Furthermore, in order to avoid obstacles
in the environment, a safe corridor is generated to provide a feasible space
for trajectory replanning. Spcifically, we firstly adopt a search-based path
planning method ABIT∗ [7] to determine an approximated path in the complex
environment. Then, the separating hyperplane is constructed between the
imminent obstacles and the existing planned trajectory based on a quadratic
program.

The main contributions of this work are summarized as follows.

  * $\bullet$

A novel safe corridor constituted by a sequence of polyhedron is proposed for
obstacle avoidance, and it is formed via an online method, different from
those offline ones as in [8, 9]. In contrast to the rectangular safe corridor
in [10, 11, 12], the presented corridor provides a more reasonable planning
space by considering the motion tendency of robots and the distribution of
obstacles in the environment.

  * $\bullet$

Different from [6] where the deadlock resolution is performed in a free space,
this work considers an obstacle-dense environment, which is evidently more
challenging. In addition, the penalty term related to the warning band is
replaced by a quadratic one which considerably mitigates the computation time.

  * $\bullet$

Comparisons with state-of-the-art results [11, 13, 14] are made in several
cluttered scenarios, illustrating that the proposed method has a better
performance in terms of guaranteeing the collision avoidance as well as
handling the deadlock problem.

  * $\bullet$

Hardware experiments are executed to verify the validation in real-world
cases, including eight crazyfiles passing though a 3D framework (Fig. 1), four
crazyfiles transitting in a polygonal environment, and six and eight robots
going through “H”-and “n”-shaped narrow passages.

Figure 1: Eight nano-quadrotors fly through a cubic framework.

##  II Related Work

###  II-A Optimization-based Trajectory Planning

In optimization-based multi-robot planners, trajectory planning is formulated
as a numerical optimization problem, where the inter-robot collision avoidance
is leveraged by adding convex [3, 4] or non-convex [14, 15] constraints.
Nonetheless, most of the existing methods encounter a challenge that the
optimization may be infeasible under these constraints. To overcome this
drawback, the method in [13] adopts a soft constraints instead of a hard one,
which however may lead to the result that the safety of planned trajectory
cannot be ensured. Other methods [11] solve the feasibility problem where the
relative safe corridor is used to guarantee the feasibility. Unfortunately, it
computes the trajectories sequentially instead of in a concurrent way, e.g.,
[3, 16]. This indicates that the robot would waste a large amount of time in
waiting for others replanning. Besides the optimization feasibility, another
problem in trajectory planning is the deadlock, which refers to the fact that
robots will be trapped by each other in collision avoidance [5, 11, 17]. A
common solution is the right-hand rule [16] based on an artificial
perturbation [8], but the inter-robot collision avoidance cannot be ensured.
Our previous work [6] is well performed in deadlock resolution via an adaptive
right-hand rule, which however is only applicable in obstacle-free space. In
conclusion, feasibility guarantee and deadlock resolution in an obstacle-dense
environment is still an open problem for multi-robot trajectory planning.

###  II-B Safe corridor

An early work related to the safe corridor is [18], which generates the
corridor through semi-definite programming. The work [19] produces the safe
corridor using two steps, namely, sampling-based path planning and geometry-
based corridor construction, and achieves a high-speed replanning for
quadrotors. Unfortunately, the constraints introduced by the corridor cannot
always be satisfied, which implies the optimization may be infeasible, and the
same problem can also be observed in [8, 20]. In addition, the authors in [10,
11, 12] propose a construction method by expanding the rectangular corridor in
a grid map. Despite this method is computationally efficient, the constructed
corridors are restricted by its rectangular shape that may have a lower space
efficiency. The method in [21] generates a safe corridor by using supported
vector machine, which has a higher utility rate of space but is centralized
and offline. Another way to construct a safe corridor is voxel expansion [8,
9], where a performed corridor is constituted based on a grid map. However,
the construction there is achieved offline as well, and cannot handle dynamic
missions such as changing targets.

##  III Problem Statement

This section formulates the optimization-based trajectory planning problem in
a cluttered environment with dimension $d={2,3}$. The goal is to drive $N$
robots from their initial positions to respective destinations in an
environment with obstacles. During this period, a robot cannot collide with
_any_ other robot or _any_ obstacle. Despite every robot can only determine
its own control input, the information of others can be obtained via wireless
communication. The trajectory is replanned and executed every sampling time
step and the replanning is reformulated as a numerical optimization with
finite variables.

###  III-A Trajectory Representation

Let $h$ denote the sampling time step. In the time step $t$ of replanning, the
planned trajectory for robot $i$ is defined as
$\mathcal{P}^{i}(t)=[p^{i}_{1}(t),p^{i}_{2}(t),\ldots,p^{i}_{K}(t)]$ where
$p^{i}_{k}(t)$, $k\in\mathcal{K}:=\\{1,2,\cdots,K\\}$, is the planned position
at time $t+kh$ and $K$ is the length of horizon. Similarly, let
$v^{i}_{k}(t)$, $k\in\mathcal{K}$ denote the velocity at time $t+kh$ and let
$u^{i}_{k}(t)$, $k=\\{0,1,\cdots,K-1\\}$ denote the control input. The
dynamics of the robot are formulated as

$x_{k}^{i}(t)=\mathbf{A}x_{k-1}^{i}(t)+\mathbf{B}u_{k-1}^{i}(t),\;k\in\mathcal{K},$
(1)

where $x^{i}_{k}(t)=[p^{i}_{k}(t),v^{i}_{k}(t)]$ is the planned state at time
$t+kh$, $x^{i}_{0}(t)=x^{i}(t)$ and

$\mathbf{A}=\left[\begin{array}[]{ccc}\mathbf{I}_{d}&h\mathbf{I}_{d}\\\
\mathbf{0}_{d}&\mathbf{I}_{d}\\\
\end{array}\right],\quad\mathbf{B}=\left[\begin{array}[]{ccc}\frac{h^{2}}{2}\mathbf{I}_{d}\\\
h\mathbf{I}_{d}\end{array}\right].$ (2)

Additionally, the velocity and input constraints are given as

$\|\Theta_{a}u^{i}_{k-1}(t)\|_{2}\leq a_{\text{max}},\ k\in\mathcal{K},$ (3)
$\|\Theta_{v}v_{k}^{i}\|_{2}\leq v_{\text{max}},\ k\in\mathcal{K},$ (4)

where $\Theta_{v},\Theta_{a}$ are positive-definite matrices, and
$v_{\text{max}},a_{\text{max}}$ denote the maximum velocity and acceleration,
respectively.

Assume that once the planned trajectory is updated, the lower feedback
controller can perfectly track it in the time interval $\left[t,t+h\right]$.
As a result, when replanning at time $t+h$, the current state of the robot
satisfies $x^{i}(t+h)=x^{i}_{1}(t)$. Fortunately, the current tracking
controller, e.g. [22], is qualified to fulfill this assumption and is adopted
in our hardware experiments.

In cooperative navigation, the information of different robots is exchanged by
wireless communication. Moreover, it is assumed that the modified historically
planned trajectory is informed and called predetermined trajectory, defined as
$\overline{P}^{i}(t)=[\overline{p}_{1}^{i}(t),\overline{p}_{2}^{i}(t),\ldots,\overline{p}_{K}^{i}(t)]$,
where
$\overline{p}_{k}^{i}(t)=p^{i}_{k+1}(t-h),k\in\tilde{\mathcal{K}}:=1,2,\ldots,K-1$
and $\overline{p}^{i}_{K}(t)=p^{i}_{K}(t-h)$.

###  III-B Collision Avoidance

####  III-B1 Inter-robot Collision Avoidance

To avoid the collision among robots, the minimum distance allowed between any
pair of robots is set to be $r_{\text{min}}>0$, implying that a collision
happens when $\|p^{i}-p^{j}\|_{2}\leq r_{\text{min}}$ 111For the sake of
simplicity, the time index $t$ will be omitted whenever ambiguity is not
caused. For example, $p^{i}(t)$ will be rewritten as $p^{i}$. . Moreover, the
replanned trajectories of robots $i$ and $j$ are collision-free if the
positions of different pairs of robots are larger than $r_{\text{min}}$ at not
only the sampling time $t+kh$, $k\in\mathcal{K}$ but also during the interval
time.

####  III-B2 Obstacle Avoidance

Let $\mathcal{O}$ denote the collection of obstacles. Then the obstacle
avoidance requires that the occupied circle or sphere for a robot in the
configuration space should not have contact with any obstacle, i.e.,
$\left(p^{i}\oplus r_{a}\mathbf{I}_{d}\right)\cap\mathcal{O}=\emptyset$, where
$\oplus$ is the Minkowski sum and $r_{a}$ is the radius of agents.
Furthermore, a planned trajectory $\mathcal{P}$ is collision-free, if its
position is collision-free at not only the sampling time but also during the
interval time. Similar to [20, 23], we assume that the obstacles are convex-
shaped.

##  IV Trajectory Planning Method

The trajectory planning method will be provided in this section. We deal with
the inter-robot collision avoidance, deadlock resolution and then obstacle
avoidance. Subsequently, the complete trajectory planning method is
summarized, followed by the proof for the feasibility guarantee of the
proposed method.

###  IV-A Inter-robot Collision Avoidance

For inter-robot collision avoidance, we introduce the modified buffered
Voronoi cell with warning band (MBVC-WB) [6] as depicted in Fig. 2. Define the
following parameters:

$a_{k}^{ij}=\frac{\overline{p}_{k}^{i}-\overline{p}_{k}^{j}}{\|\overline{p}_{k}^{i}-\overline{p}_{k}^{j}\|_{2}},\quad
b_{k}^{ij}=a_{k}^{ij^{T}}\frac{\overline{p}_{k}^{i}+\overline{p}_{k}^{j}}{2}+\frac{r_{\min}^{\prime}}{2},$
(5)

where
$r^{\prime}_{\text{min}}=\sqrt{r_{\text{min}}^{2}+h^{2}v_{\text{max}}^{2}}$
denotes the extended minimum distance. Then, the MBVC-WB can be formulated by

$\displaystyle{a_{k}^{ij}}^{T}p_{k}^{i}\geq b_{k}^{ij},\ \forall j\neq
i,k\in\tilde{\mathcal{K}},$ (6a) $\displaystyle{a_{K}^{ij}}^{T}p_{K}^{i}\geq
b_{K}^{ij}+w^{ij},\forall j\neq i,$ (6b)

where $w^{ij}$ is an additional variable added to the optimization and
satisfies

$0\leq w^{ij}\leq\epsilon.$ (7)

In (7), $\epsilon$ is referred to as the maximum width of warning band.

The additional penalty term added to the cost function is given by

$C_{w}^{i}=\sum_{j\neq i}\frac{1}{\epsilon\gamma^{ij}}\rho_{ij}{w^{ij}}^{2},$
(8)

where $\gamma^{ij}(t)=(1-\beta)\gamma^{ij}(t-h)+\beta w^{ij}(t-h)$,
$\beta\in(0,1)$ and $\gamma^{ij}(t_{0})~{}=~{}\epsilon$. Notably, the modified
function (8) is a quadratic term and is computationally efficient in the sense
that $\gamma^{ij}(t)$ related to the result of $w^{ij}$ in last time step
$t-h$ is utilized to adjust the weight of this penalty. Additionally,
$\rho_{ij}$ is an important coefficient designed to execute adaptive the
right-hand rule and will be presented later.

###  IV-B Deadlock Resolution

To deal with the possible deadlock problem in inter-robot collision avoidance,
we propose a detection-resolution mechanism. For deadlock detection, a notion
of terminal overlap is introduced as follows: If there holds $p^{i}_{K}(t)\neq
p^{i}_{\text{target}}$, $p^{i}_{K}(t)=p^{i}_{K}(t-h)$, and
$p^{i}_{K}(t)=p^{i}_{K-1}(t)$, we say that a terminal overlap happens and it
is denoted by $b^{i}_{\text{TO}}=True$. Regarding deadlock resolution, the
adaptive right-hand rule is carried out by adjusting $\rho^{ij}$ as follows

$\rho^{ij}=\rho_{0}\,e^{\eta^{i}(t)\,\sin\theta^{ij}},$ (9)

where

$\eta^{i}(t)=\left\\{\begin{array}[]{rll}&\eta^{i}(t-h)+\Delta\eta&\mbox{if}\;b^{i}_{\text{TO}}=True,\\\
&0&\mbox{if}\;\ w^{ij}=0,\forall j\neq i,\\\
&\eta^{i}(t-h)&\text{else}.\end{array}\right.$ (10)

In (9) and (10), $\rho_{0}>0$ and $\Delta\eta>0$ are the coefficients; the
initial condition of $\eta^{i}$ is $\eta^{i}(t_{0})=0$; the parameter
$\theta^{ij}$ is defined as the angle in $x-y$ plane between the projection of
$p^{i}_{K}$ to $p^{i}_{\text{target}}$ and $p^{i}_{K}$ to $p^{j}_{K}$.

Figure 2: Illustration of the MBVC-WB where the green area is the feasible
space and the orange one is the warning band. Shared space is split at each
horizon (Left). In particular, for terminal horizon, warning band is added
(Right).

###  IV-C Obstacle Avoidance

The obstacle avoidance is realized by restricting the planned trajectory in a
safe corridor. The corridor is constituted by a sequence of convex polyhedra
in which the edge can separate the planned positions $p^{i}_{k}$ and inflated
obstacle $\tilde{\mathcal{O}}$, i.e., the obstacle inflated by $r_{\text{a}}$
in (Fig. 3, the blue polygons shifted by green obstacles). In our method, the
corridor is formed upon the predetermined trajectory. As an example in Fig.
3(d), three intersected convex polyhedra make up a corridor. Based on the safe
corridor, the obstacle avoidance constraint can be written as

${a_{k}^{i,o}}^{T}p_{k}^{i}\geq b_{k}^{i,o},$ (11)

where $a_{k}^{i,o}$ and $b_{k}^{i,o}$ constitute the edge of corridor for
robot $i$ at horizon $k\in\mathcal{K}$. In the following subsections, the
method of constructing these planes will be clarified in detail.

####  IV-C1 Path Planning

To begin with, a path is required to indicate an approximated direction for a
robot to the destination. This path is a collision-free polyline that connects
the robot and its target, i.e., connecting terminal horizon position of
predetermined trajectory, $\overline{p}^{i}_{K}$ and target,
$p^{i}_{\text{target}}$ as shown in Fig 3(a). RRT∗-based methods are qualified
to find such a feasible path, and among these methods, the Advanced Batch
Informed Trees (ABIT∗) [7] has a higher path quality. Thus, ABIT∗ is utilized
to find the path and the length of this path is chosen as objective to be
minimized.

Once the path is found, a point $p^{i}_{\text{tractive}}$, called the tractive
point, will be chosen in this path. It is determined as the closet point in
the path to the target such that the line segment between this point and
$\overline{P}^{i}_{K}$ is collision-free. A demo is presented in Fig. 3(b).
Then, if a terminal overlap does not happen, i.e., $b^{i}_{\text{TO}}=False$,
we add the tractive point to the end of predetermined trajectory to form the
extended predetermined trajectory (EPT). In practice, we find that, in most of
situations, a tractive point can be obtained via previously planned path. In
other words, the path is unnecessary to be updated in every replanning. Thus,
the path planning is triggered when it is needed such as changing the target
or failing to find a tractive point.

####  IV-C2 Segment Division

After obtaining EPT, we need to divide the points of EPT into several segments
so as to decrease the computation requirements as shown in Fig 3(b). Firstly,
we choose the end point of EPT as the start of the first segment. Next, from
the second end point of EPT to the start, the point in EPT will be added into
the current segment one by one. We will stop to add the next point into the
current segment until the convex hull of the contained points is not
collision-free anymore. Then, a new segment will begin from the end point of
the last one. The above process will be repeatedly carried out until the
beginning point $\overline{p}^{i}_{1}$ is added into the segment.

(a) Path planning.

(b) The process of choosing a tractive point and splitting the segments $S1$,
$S2$, $S3$.

(c) Form the separation hyperplane via linear program.

(d) For a given predetermined trajectory, form a safe corridor to provide a
feasible space for replanning.

Figure 3: Process of forming safe corridor.

input : $\mathcal{O}$, $\overline{\mathcal{P}}^{i}(t)$

output : $ObCons$

1 if  _Need Path Planning_ then

2 $Path^{i}(t)\leftarrow
ABIT^{*}(\overline{p}^{i}_{K},p^{i}_{\text{target}},\mathcal{O})$;

3

4else

5 $Path^{i}(t)\leftarrow Path^{i}(t-h)$;

6

7 end if

8$ObCons\leftarrow\emptyset$;

9
$Segmentlist\leftarrow\text{SegmentDivision}(\mathcal{O},\overline{\mathcal{P}}^{i}(t),Path^{i}(t))$;

10 for  _$Segment\ \in\ Segmentlist$_ do

11 $Corridor\leftarrow\text{GetCorridor}(Segment,\mathcal{O})$;

12 $ObCons\cup\text{AddConstraints}(Corridor)$;

13

14 end for

Algorithm 1 GetCorridor()

####  IV-C3 Separating Plane

After diving points of EPT into several segments, we will construct the
separating plane between the segment and obstacles. Since the convex hull
formed by the points in each segment is obstacle-free and also the obstacles
are convex-shaped, a separating plane exists according to separating
hyperplane theorem [24]. Then, as shown in Fig 3(c), an optimization-based
method can be provided as follows:

$\displaystyle\max_{a,b,\gamma}\;\gamma,$ (12) $\displaystyle{\rm s.t.},$
$\displaystyle{a}^{T}p^{\text{free}}\geq\gamma+b,$
$\displaystyle{a}^{T}p^{\text{obstacle}}\leq b,$
$\displaystyle{\left\|a\right\|}_{2}=1,$ $\displaystyle\gamma\geq 0,$

where $a$ and $b$ determine the separating plane; $p^{\text{free}}$ and
$p^{\text{obstacle}}$ denote the points of segments and obstacles
respectively; $\gamma$ is the margin variable. Such an optimization can be
further transformed to the following quadratic program (QP):

$\displaystyle\min_{a^{\prime},b^{\prime}}\ \left\|a^{\prime}\right\|_{2}^{2}$
(13) $\displaystyle{\rm s.t.},$
$\displaystyle{a^{\prime}}^{T}p^{\text{free}}\geq 1+b^{\prime},$
$\displaystyle{a^{\prime}}^{T}p^{\text{obstacle}}\leq b^{\prime},$

where $a^{\prime}=\frac{a}{\gamma}$ and $b^{\prime}=\frac{b}{\gamma}$. By
solving the QP (13), the separating plane can be obtained as
$a=\frac{a^{\prime}}{\|a^{\prime}\|_{2}}$ and
$b=\frac{b^{\prime}}{\|a^{\prime}\|_{2}}$, which forms the edge of a convex
polyhedron. Moreover, $a_{k}^{i,o}$ and $b_{k}^{i,o}$ are chosen as $a$ and
$b$, respectively, to formulate the constraints (11).

In implementation, the separation between segments and obstacles are
unnecessary to be constructed in some occasions that the obstacle locates out
of the planning region. Therefore, we come up with the following manipulation
to simplify the planning process. Specifically, the separating plane will be
computed from the nearest obstacle to the farthest one. Regarding each
obstacle, a separating plane will be built if this obstacle has a contact with
the currently formed convex polyhedra. Otherwise, such an obstacle will be
omitted. In addition, if the distance between the obstacle and the robot is
larger than $Khv_{\text{max}}$, the obstacle will not be considered either.

####  IV-C4 Safe Corridor Construction

All above, the algorithm of forming a safe corridor is concluded in Alg. 1,
where the detail process has been illustrated sequentially in previous sub-
subsections. This safe corridor generation method has some critical
properties, which is summarized as follows.

######  Lemma 1.

The safe corridor formulation method provided in Alg. 1 has the following
three properties:

  1. 1.

If the predetermined trajectory $\overline{\mathcal{P}}$ is obstacle-free, a
safe corridor can be generated.

  2. 2.

The underlying predetermined trajectory $\overline{\mathcal{P}}$ satisfies the
formulated constraints (11), i.e., ${a_{k}^{i,o}}^{T}\overline{p}_{k}^{i}\geq
b_{k}^{i,o}$, $k\in\mathcal{K}$.

  3. 3.

If the constraints (11) formulated by the safe corridor are satisfied, the
planned trajectory $\mathcal{P}$ is obstacle-free.

###### Proof.

1) Based on the above-mentioned method, we can conclude that a division of
segment can be found if a predetermined trajectory is obstacle-free. This is
because the line between the tractive point and $\overline{p}^{i}_{K}$ is
collision-free, which obviously leads to a collision-free EPT. In this case,
the worst and most conservative division is all the segments are only formed
by the adjacent two points of EPT. Additionally, since there exist separating
planes between the points of segment and any of obstacle, the convex polyhedra
can be formed. Consequently, the safe corridor can be constituted by a
sequence of polyhedra.

2) Since the constraints (11) are obtained from the optimization (12) with
$p^{\text{free}}$ chosen as the positions from predetermined trajectory, it
clear that the predetermined trajectory $\overline{\mathcal{P}}$ satisfies
these constraints.

3) As aforementioned, for a collision-free predetermined trajectory, a
division of segment can be found. Then, due to the segment division rule, it
is obvious that the adjacent points of predetermined trajectory, e.g.,
$\overline{p}^{i}_{k}$ and $\overline{p}^{i}_{k+1}$, must be contained in a
common segment. Thus, if the constraints is enforced, the corresponding
planned points $p^{i}_{k}$ and $p^{i}_{k+1}$ must be restricted to a common
convex polyhedron. Consequently, the line segment between them must be
obstacle-free. Then, regarding a finished segment division, all points of
predetermined trajectory are included because the segment division can be
completed if and only if the point is counted sequentially until the last one.
Thus, all the line segments of the trajectory is covered by a collision-free
safe corridor. ∎

###  IV-D Trajectory Planning Algorithm

In the previous subsections, the methods dealing with inter-robot and robot-
obstacle collision avoidance are formulated to be certain kinds of constraints
into the optimization problem. Furthermore, another constraint is introduced
to ensure the feasibility of the underlying optimization, that is,

$v^{i}_{K}=\mathbf{0}_{d}.$ (14)

Moreover, we enforce that $x^{i}_{k}=x^{i}_{K}$, $k>K$ and
$u^{i}_{k}=\textbf{0}_{d}$.

In addition to the constraints, the proposed cost function for robot $i$ is
given by

$C^{i}=C^{i}_{p}+C^{i}_{w},$ (15)

where $C^{i}_{w}$ is provided in (8) and $C^{i}_{p}$ is given by

$C^{i}_{p}=\frac{1}{2}Q_{K}\|p_{K}^{i}-p_{\text{tractive}}^{i}\|_{2}^{2}+\frac{1}{2}\sum_{k=1}^{K-1}Q_{k}\|p_{k+1}^{i}-p_{k}^{i}\|_{2}^{2}.$
(16)

Note that $C^{i}_{p}$ is employed to drive the robots to the current tractive
point and $Q_{k}$, $k\in\mathcal{K}$ in (16) is the weight parameter.

Therefore, the optimization can reformulated as follows.

$\displaystyle\min_{\mathrm{u}^{i},x^{i},w^{ij}}C^{i}$ $\displaystyle\text{
s.t. }\eqref{eq: dynamic constraint},\eqref{eq: input-constraint},$
$\displaystyle\eqref{eq: state-constraint},\eqref{eq: inter-
constraint},\eqref{eq: w-constraint},\eqref{eq: obstacle-
constraint},\eqref{eq: terminal-constraint}.$

Based on this optimization, the proposed trajectory planning method is
summarized in Algorithm 2. The inputs are the initial position $p^{i}(t_{0})$,
the target position $p^{i}_{\text{target}}$ and obstacles $\mathcal{O}$. To
begin with, the predetermined trajectory is initialized as
$\overline{\mathcal{P}}^{i}(t_{0})=[p^{i}(t_{0}),\ldots,p^{i}(t_{0})]$ and
$b^{i}_{\text{TO}}$ is set as False. After this initialization, in the main
loop, each robot run their respective algorithm in parallel (Line 2). At the
beginning, the predetermined trajectory is informed among the robots (Line 2),
followed by the procedure that the functions related to inter-robot and robot-
obstacle collision avoidance are realized (Line 2-2). After obtaining the
current state, the optimization (17) is formulated and resolved (Line 2).
Afterwards, the deadlock detection is made (Line 2) based on the resolution of
the optimization and the predetermined trajectory in next step is derived
thereafter (Line 2). Finally, the planned trajectory is executed (Line 2).

Input : $p^{i}(t_{0})$, $p^{i}_{\text{target}}$,$\mathcal{O}$

1
$\overline{\mathcal{P}}^{i}(t_{0})\leftarrow\text{InitialPredTraj}(p^{i}(t_{0}))$
;

2 $b^{i}_{\text{TO}},\leftarrow\textbf{False}$;

3 while  _not all robots at target_ do

4 for  _$i\in\mathcal{N}$ concurrently _ do

5

6
$\overline{\mathcal{P}}^{j}(t)\leftarrow\text{Communicate}(\overline{\mathcal{P}}^{i}(t))$
;

7

8
$cons^{i}\leftarrow\text{GetInterCons}(\overline{\mathcal{P}}^{i}(t),\overline{\mathcal{P}}^{j}(t))$;

9

10 $cons^{i}\leftarrow
cons^{i}\cup\text{GetCorridor}(\mathcal{O},\overline{\mathcal{P}}^{i}(t))$ ;

11

12 $x^{i}(t)\leftarrow\text{GetCurrentState}()$;

13

14 $\mathcal{P}^{i}(t),w^{ij}\leftarrow\text{Optimization}(cons^{i},x^{i}(t))$
;

15

16
$b^{i}_{\text{TO}}\leftarrow\text{DeadlockDetection}(\mathcal{P}^{i}(t),w^{ij})$
;

17

18
$\overline{\mathcal{P}}^{i}(t+h)\leftarrow\text{GetPredTraj}(\mathcal{P}^{i}(t))$
;

19

20 $\text{ExecuteTrajectory}(\mathcal{P}^{i}(t))$;

21

22 end for

23 $t\leftarrow t+h$;

24

25 end while

Algorithm 2 The Complete Algorithm

###  IV-E Feasibility Guarantee

Different from most existing optimization-based methods, the proposed planner
guarantees the feasibility of the optimization problem, which is illustrated
in the following theorem.

######  Theorem 1.

If the initial positions of all robots are collision-free, the robots will
never have collisions with each other or any obstacles.

###### Proof.

At beginning time $t_{0}$, the predetermined trajectory is initialized as
$\overline{\mathcal{P}}^{i}(t_{0})=[p^{i}(t_{0}),\ldots,p^{i}(t_{0})]$.
Choosing it as the planned trajectory, i.e.,
$p^{i}_{k}(t_{0})=\overline{p}^{i}_{k}(t_{0})$, it is naturally that this
planned trajectory satisfies all constraints listed in optimization (17).
Thus, at time $t_{0}$, the optimization is feasible for all robots.

Thereafter, we intend to prove that our algorithm is recursively feasible,
i.e., once the optimization at time step $t-h$ are feasible, we can find a
feasible solution at time $t$. Form Lemma 1, we already know that if planned
trajectory at $t-h$ is feasible, it must be obstacle-free and we can find a
safe corridor in this time’s replanning. Afterwards, the final optimization
(17) can be formulated. Given feasible solution at last time step,
$u^{i}_{k-1}(t-h)$ and $x^{i}_{k}(t-h)$ for $k\in\mathcal{K}$, we can provide
a feasible solution $x^{i}_{k}(t)=x^{i}_{k+1}(t-h)$,
$u^{i}_{k}(t)=u^{i}_{k+1}(t-h)$ and
$w^{ij}(t)=\min\\{\epsilon,\;{a_{K}^{ij}}^{T}(t)p_{K}^{i}(t)-b_{K}^{ij}(t)\\}$,
where we enforce that $x^{i}_{K}(t)=x^{i}_{K+1}(t-h)=x^{i}_{K}(t-h)$ and
$u^{i}_{K}(t)=u_{e}$.

First, as the result of optimization at time step $t-h$, $x^{i}_{k+1}(t-h)$
and $u^{i}_{k}(t-h)$ with $k\in\tilde{\mathcal{K}}$, satisfy the constraints
in (1), (3)-(4) naturally. In addition, since
$x^{i}_{K}(t)=x^{i}_{K+1}(t-~{}h~{})=x^{i}_{K}(t-~{}h~{})$ and
$u^{i}_{K-1}(t)=u^{i}_{K}(t-h)=u_{e}$ hold, $x^{i}_{K}(t)$ and
$u^{i}_{K-1}(t)$ also satisfy these constraints. In the meantime, as
$x^{i}_{K}(t)=x^{i}_{K+1}(t-h)=x^{i}_{K}(t-h)=x^{i}_{K-1}(t)$ holds, it is
evident that the constraint (14) holds as well. Then, towards constraints
related to MBVC-WB, i.e., (6), (7), the feasibility of the provided solution
have been proved in our previous work [6]. Lastly, regarding constraint (11),
as property 1) and 3) stated in Lemma 1, for given feasible solution in time
$t-h$, the previously planned trajectory is obstacle-free and a formulation of
safe corridor can be found at this time. Afterwards, according to property 3),
the provided solution, i.e., the predetermined trajectory, satisfies these
constraints evidently. Thus, the constraint (11) is feasible and the
optimization is feasible in a recursive way.

Since the initial optimizations as well as thees successive ones are feasible,
the included constraints are satisfied. According to property 3) in Lemma 1
and the property of MBVC-WB in [6], the obstacle and inter-robot collision can
be avoided. ∎

##  V Numerical Simulations and Experiments

In this section, we will analyze the validation and performance of the
proposed algorithm via numerical simulations and hardware experiments. The
algorithm is implemented in an Intel Core i9 @3.2GHz computer with Python3
programming language and publicly available at https://github.com/PKU-
MACDLab/IMPC-OB. We use CVXOPT [25] for quadratic programming and trajectory
optimization, and OMPL [26] for ABIT∗ path planning. Furthermore, comparisons
with Ego-swarm [13], MADER [14] and LSC [11] are carried out.

###  V-A Numerical Simulations and Comparison

The main parameters of robots are chosen as follows: the minimum inter-agent
distance $r_{\text{min}}=0.6{\rm m}$; the radius of agents $r_{a}=0.3{\rm m}$;
the maximum acceleration $a_{\text{min}}=2{\rm m/s^{2}}$; the maximum velocity
$v_{\text{max}}=3{\rm m/s}$.

(a) LSC [11]

(b) Ours

Figure 4: The scenario ’L’-passage for LSC [11] and ours. Figure 5: The
generated corridor for given pre-planned trajectory (Top: LSC [11] and Bottom:
Ours ). Noticeably, the provided corridor is the feasible space for the
centroid of the robot.

(a) ego-swarm[13]

(b) MADER[14]

(c) LSC[11]

(d) Ours

Figure 6: Four planners are performed in the forest scenario.

(a) ego-swarm[13]

(b) MADER[14]

(c) LSC[11]

(d) Ours

Figure 7: Four planners are performed in the “H” scenario. TABLE I: Comparison With State-of-arts. (Safety: no collision occurs. $T_{t}[{\rm s}]$: transition time. $L_{t}[{\rm m}]$: the length of transition. $T_{c}[{\rm ms}]$: mean computation time per replanning.)  | method | Safety | $T_{t}$ | $L_{t}$ | $T_{c}$
---|---|---|---|---|---
forest | Ego-swarm [13] | No | 9.2 | 105.8 | 9.6
MADER [14] | No | 22.3 | 111.1 | 104.0
LSC [11] | Yes | 22.3 | 114.34 | 53.2
Ours | Yes | 8.1 | 102.4 | 93.0
“H” | Ego-swarm [13] | No | 7.5 | 66.6 | 10.2
MADER [14] | No | 14.2 | 71.5 | 116.7
LSC [11] | Yes | - | - | 62.3
Ours | Yes | 9.3 | 67.7 | 86.8

To begin with, we will provide a single robot simulation to show the proposed
safe corridor, where the environment is a “L”-passage. Fig. 4 shows the
comparative simulation results of the method proposed in [11] and our method.
Notably, our method has a higher speed and smoother trajectory in the sense
that our planner cost $4.1s$ in comparison with $7.5{\rm s}$ for LSC. The
difference between the safe corridors are given in Fig. 5. Compared with the
rectangle corridor in LSC, our corridor is a trapezoid, which provides more
feasible space for upcoming turning.

Furthermore, comparisons with Ego-swarm, MADER and LSC are made throughout
forest-transition and “H”-transition. Trajectories of these scenarios are
illustrated in Fig. 6 and Fig. 7, respectively. The results are shown in Table
I. For computation time, the proposed method has a relatively long computation
time since it is programmed by Python3 that cannot take full advantage of
multi-core computation. But in prospective execution, the computation can be
carried out in multiple processors concurrently, and the time will decrease
considerably. Ego-swarm has an impressive computation time in addition to a
relative smooth and fast trajectories. Unfortunately, it cannot guarantee the
collision avoidance, since it adopts an unconstrained optimization in
planning. For MADER, the optimization under the strict constraints guarantees
the avoidance between robots. However, regarding obstacle-avoidance, MADER
seems to be maladaptive in an obstacle-dense environment as several collision
appears. LSC has the feasibility guarantee which ensures the safety of robots,
but a deadlock occurs in “H”-transition. Despite heuristic deadlock resolution
method is adopted in LSC, it is ineffective in this scenario. Though we
similarly utilize linear constraint to handle inter-agent collision, the extra
warning band introduce an elastic interaction instead of a hard one, based on
which, the adaptive right-hand rule is leveraged, resulting in right-hand
rotations in the bottleneck of the “H”. Moreover, for transition time and
length, the proposed planner has a considerable superiority which means a
faster and smoother trajectories.

(a) Left: the trajectories of this swarm where the ellipsoids indicate the
minimum inter-robot distance. Right: the distances between different pairs of
robots.

(b) left: four crazyfiles are deployed in a polyhedrom-shape enviroment.
Right: When a crazyfile go through the narrow passage, another quadrotor make
a way to let it pass by.

(c) Left: The hardware experiment of “H”-transition. Right: “n”-transition.

Figure 8: The real-world experiments.

###  V-B Hardware Experiments

Hardware experiments are executed on the platform of crazyswarm [27], where
multiple nano-quadrotors are flying under a motion capture system OptiTrack.
The computation of all robots is done on a central computer with frequency,
$5$Hz to comply with the sampling time step $h=0.2$s. For each crazyfile, a
feedback controller [22] is adopted to track the planned trajectory.

The first experiment is shown in Fig. 8(a), where $8$ crazyfiles fly through a
$0.6{\rm m}$ cubic framework. Considering the air turbulence, a crazyfile in
the inter-robot avoidance is represented as an ellipsoid with diameter $0.24$m
in the $x-y$ plane and $0.6$m in the $z$ axis. Owing to the deformation of
robots, the inter-robot constraints are accordingly adjusted by modifying
$a^{ij}_{k}$ and $b^{ij}_{k}$ as

$a_{k}^{ij}=E\frac{E(\overline{p}_{k}^{i}-\overline{p}_{k}^{j})}{\|E(\overline{p}_{k}^{i}-\overline{p}_{k}^{j})\|_{2}},\quad
b_{k}^{ij}=a_{k}^{ij^{T}}\frac{E(\overline{p}_{k}^{i}+\overline{p}_{k}^{j})}{2}+\frac{r_{\min}^{\prime}}{2},$

where $E={\rm diag}(1.0,1.0,\frac{0.24}{0.6})$;
$r_{\min}^{\prime}=\sqrt{r_{\text{min}}^{2}+v_{\text{max}}^{2}}$ and
$r_{\text{min}}=0.24$m. The radius of a crazyfile is set as $r_{a}=0.12$m.
From the result given in Fig 8(a), it is apparent that the crazyfiles can
achieve this transition.

In addition, four crazyfiles moving in a cluttered environment are shown in
Fig 8(b). Given initial positions, the targets are randomly chosen. After
arriving at the targets, the new one will be published immediately and this
process is repeated $5$ times. In this scenario, the feasible space is the
irregular-shaped passage at the interval of polygon-shaped obstacles where the
width of these passages ranges from $0.4$m to $0.7$m. By the help of MBVC-WB,
where the warning band introduces an elastic interaction between the robots, a
robot can squeeze out a way between the wall and other robots to avoid this
trap as shown in Fig. 8(b). Such an inter-robot coordination in this scenario
solves the deadlock problem.

At last, other two experiments as illustrated in Fig. 8(c) are carried out,
where the scenarios are “H”-transition and “n”-transition, respectively. In
these navigations, the safety is guaranteed and the coordination in narrow
passage is properly achieved. In “H”-transition, the right-hand rotation
appears as in the previous simulation. Regarding the “n”-transition, the
intersection of two groups of quadrotors at the top passage is the main
challenge for this mission. It can be seen that the proposed method resolves
it as the quadrotors fly through the passage without sacrificing any speed.
For a quadrotor, when encountering an oncoming agent, it can rapidly find a
side to avoid collisions by utilizing MBVC-WB.

##  VI Conclusion

This work has proposed a novel multi-robot trajectory planning method for
obstacle-dense environments wherein the collision avoidance is guaranteed and
the deadlock among robots is resolved. In contrast to state-of-the-art works,
the proposed method’s performance of safety and deadlock resolution in
cluttered scenarios can be ensured by theoretical proof and validated by
comprehensive simulations and hardware experiments.

## References

  * [1] S.-J. Chung, A. A. Paranjape, P. Dames, S. Shen, and V. Kumar, “A survey on aerial swarm robotics,” _IEEE Transactions on Robotics_ , vol. 34, no. 4, pp. 837–855, 2018.
  * [2] X. Zhou, X. Wen, Z. Wang, Y. Gao, H. Li, Q. Wang, T. Yang, H. Lu, Y. Cao, C. Xu, and F. Gao, “Swarm of micro flying robots in the wild,” _Science Robotics_ , vol. 7, no. 66, p. eabm5954, 2022.
  * [3] L. C. E. and A. P. Schoellig, “Trajectory generation for multiagent point-to-point transitions via distributed model predictive control,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 2, pp. 375–382, 2019\.
  * [4] F. Augugliaro, A. P. Schoellig, and R. D’Andrea, “Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach,” in _2012 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2012, pp. 1917–1922.
  * [5] J. Alonso-Mora, P. Beardsley, and R. Siegwart, “Cooperative collision avoidance for nonholonomic robots,” _IEEE Transactions on Robotics_ , vol. 34, no. 2, pp. 404–420, 2018.
  * [6] Y. Chen, M. Guo, and Z. Li, “Deadlock resolution and recursive feasibility in mpc-based multi-robot trajectory generation,” _arXiv preprint arXiv:2202.06071_ , 2022.
  * [7] M. P. Strub and J. D. Gammell, “Advanced BIT (ABIT): Sampling-based planning with advanced graph-search techniques,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_ , 2020, pp. 130–136.
  * [8] C. Toumieh and A. Lambert, “Decentralized multi-agent planning using model predictive control and time-aware safe corridors,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 11 110–11 117, 2022.
  * [9] F. Gao, L. Wang, B. Zhou, X. Zhou, J. Pan, and S. Shen, “Teach-repeat-replan: A complete and robust system for aggressive flight in complex environments,” _IEEE Transactions on Robotics_ , vol. 36, no. 5, pp. 1526–1545, 2020.
  * [10] J. Park, J. Kim, I. Jang, and H. J. Kim, “Efficient multi-agent trajectory planning with feasibility guarantee using relative bernstein polynomial,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_ , 2020, pp. 434–440.
  * [11] J. Park, D. Kim, G. C. Kim, D. Oh, and H. J. Kim, “Online distributed trajectory planning for quadrotor swarm with feasibility guarantee using linear safe corridor,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 2, pp. 4869–4876, 2022.
  * [12] J. Li, M. Ran, and L. Xie, “Efficient trajectory planning for multiple non-holonomic mobile robots via prioritized trajectory optimization,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 405–412, 2021\.
  * [13] X. Zhou, J. Zhu, H. Zhou, C. Xu, and F. Gao, “Ego-swarm: A fully autonomous and decentralized quadrotor swarm system in cluttered environments,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_ , 2021, pp. 4101–4107.
  * [14] J. Tordesillas and J. P. How, “Mader: Trajectory planner in multiagent and dynamic environments,” _IEEE Transactions on Robotics_ , pp. 1–14, 2021\.
  * [15] M. Kamel, J. Alonso-Mora, R. Siegwart, and J. Nieto, “Robust collision avoidance for multiple micro aerial vehicles using nonlinear model predictive control,” in _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2017, pp. 236–243.
  * [16] D. Zhou, Z. Wang, S. Bandyopadhyay, and M. Schwager, “Fast, on-line collision avoidance for dynamic vehicles using buffered Voronoi cells,” _IEEE Robotics and Automation Letters_ , vol. 2, no. 2, pp. 1047–1054, 2017.
  * [17] L. Wang, A. D. Ames, and M. Egerstedt, “Safety barrier certificates for collisions-free multirobot systems,” _IEEE Transactions on Robotics_ , vol. 33, no. 3, pp. 661–674, 2017.
  * [18] R. Deits and R. Tedrake, _Computing Large Convex Regions of Obstacle-Free Space Through Semidefinite Programming_. Springer International Publishing, 2015, pp. 109–124.
  * [19] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, “Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-D complex environments,” _IEEE Robotics and Automation Letters_ , vol. 2, no. 3, pp. 1688–1695, 2017.
  * [20] B. Şenbaşlar, W. Hönig, and N. Ayanian, “Rlss: Real-time multi-robot trajectory replanning using linear spatial separations,” _arXiv preprint arXiv:2103.07588_ , 2021.
  * [21] W. Hönig, J. A. Preiss, T. K. S. Kumar, G. S. Sukhatme, and N. Ayanian, “Trajectory planning for quadrotor swarms,” _IEEE Transactions on Robotics_ , vol. 34, no. 4, pp. 856–869, 2018.
  * [22] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in _2011 IEEE International Conference on Robotics and Automation (ICRA)_ , 2011, pp. 2520–2525.
  * [23] X. Ma, Z. Jiao, Z. Wang, and D. Panagou, “Decentralized prioritized motion planning for multiple autonomous uavs in 3d polygonal obstacle environments,” in _2016 International Conference on Unmanned Aircraft Systems (ICUAS)_ , 2016, pp. 292–300.
  * [24] S. Boyd, S. P. Boyd, and L. Vandenberghe, _Convex optimization_. Cambridge university press, 2004.
  * [25] A. Martin, D. Joachim, and V. Lieven, “Cvxopt,” Website, http://cvxopt.org/.
  * [26] I. A. Şucan, M. Moll, and L. E. Kavraki, “The Open Motion Planning Library,” _IEEE Robotics & Automation Magazine_, vol. 19, no. 4, pp. 72–82, December 2012, https://ompl.kavrakilab.org.
  * [27] J. A. Preiss, W. Hönig, G. S. Sukhatme, and N. Ayanian, “Crazyswarm: A large nano-quadcopter swarm,” in _2017 IEEE International Conference on Robotics and Automation (ICRA)_ , 2017, pp. 3299–3304.