Formation control of unmanned rotorcraft systems with state constraints and inter-agent collision avoidance

Abstract

We present in this paper a novel framework and distributed control laws for the formation of multiple unmanned rotorcraft systems, be it single-rotor helicopters or multi-copters, with physical constraints and with inter-agent collision avoidance, in cluttered environments. The proposed technique is composed of an analytical distributed consensus control solution in the free space and an optimization based motion planning algorithm for inter-agent and obstacle collision avoidance. More specifically, we design a distributed consensus control law to tackle a series of state constraints that include but not limited to the physical limitations of velocity, acceleration and jerk, and an optimization-based motion planning technique is utilized to generate numerical solutions when the consensus control fails to provide a collision-free trajectory. Besides, a sufficiency condition is given to guarantee the stability of the switching process between the consensus control and motion planning. Finally, both simulation and real flight experiments successfully demonstrate the effectiveness of the proposed technique.

1 Introduction

Autonomous unmanned systems, such as unmanned aerial vehicles (UAVs) (also commonly termed as unmanned aerial systems or UAS), unmanned underwater vehicles (UUVs), unmanned ground vehicles (UGVs), and unmanned surface vehicles (USVs), play important roles in many industrial applications, such as geographic mapping, powerline and pipeline inspections, security surveillance, logistic delivery, and warehouse management. Autonomous unmanned vehicles have thus grown into a new research focus with rapid development [1]. Compared with a single agent, multi-agent systems have higher scalability and can achieve more complex tasks. It has thus drawn increasing attention from academia into the research and development of multi-agent systems techniques (see, for example, [2–6]). In contrast to most of the results related to multi-agent systems in the literature, we focus our attention in this work on the formation control of unmanned rotorcraft systems, either single-rotor or multiple-rotor UAVs, in cluttered environments with obstacle and inter-agent collision avoidance for systems with physical limitations on their velocities, accelerations and/or jerks. These limitations will eventually become constraints on system state variables in our problem formulation.

The shape of a formation for multi-agent systems is usually specified by the absolute position, relative position [7], bearing, or distance. Consensus control, a commonly used formation approach, is one depends on the relative position of the local coordinate. In the leader-follower structure, the trajectory generated by the leader usually acts as a reference for the followers to track, and the leader is usually formulated as a linear time-invariant system. There were many references (see, for example, [8, 9]) assuming that the leader has zero input, which implies that its trajectory is totally determined by its initial condition and the system matrix. Hence agents can not adjust their trajectories automatically to adapt to environmental changes. Even though the leader is subject to a bounded input in [10, 11], the input is given by a human operator. Therefore, the above methods are widely implemented in obstacle-free scenarios.

Each unmanned rotorcraft system is known to have physical limitations of velocity, acceleration, and jerk, and these limitations will be formulated as constraints on the input and system state variables. To the best of our knowledge, the great majority of the control laws [11, 12] do not consider both input and state constraints. Some of them only solve input saturation [13, 14] or actuator position and rate saturation [15]. We designed a novel consensus control law in this paper to tackle the physical constraints. Another important problem associated with the formation control of a multi-agent system is inter-agent collision avoidance. Although such a problem is solved for first-order [16, 17] and second-order systems [18], it is challenging and difficult to design an analytical control law for triple-integrator or general linear systems. In this work, we introduce an optimization-based motion planning technique to solve the collision avoidance problem.

The formation control is also studied as a multi-agent motion planning problem in the robotics society. Instead of consensus or stability, their focus is to find a dynamically feasible and collision free trajectory to bring each agent to its designated target state. The multi-agent motion planning problem is essentially a non-convex optimization problem that can be solved numerically. The solving procedure is usually initialized by certain types of graph-based algorithms. If the graph is provided or preconstructed, classical algorithms like A* [19] can be applied. Otherwise, the graph needs to be constructed along with the searching process such as described in [20] and [21]. With the initial guess provided, various numerical optimization methods can then be applied to find the optimal trajectory. In offline applications, the planning horizon of the numerical optimization is usually not explicitly limited [22] which allows the generation of a complete plan for each vehicle. In online applications, however, due to the realtime requirement, the planning horizon is usually limited to adapting to the computational resource [23] and the replanning is required at a certain frequency. Thus the motion planning becomes a model predictive control (MPC) problem.

In this paper, we aim to establish a novel framework and distributed control laws for the formation of multiple unmanned rotorcraft systems with physical constraints and with inter-agent collision avoidance, in cluttered environments. The proposed method is composed of an analytical distributed consensus control solution in the free space and an optimization-based motion planning protocol for inter-agent and obstacle collision avoidance in cluttered environments. Unlike the control laws proposed in [24, 25] where the leader is a linear system with zero input and without saturation problems, a distributed consensus control law is first designed to tackle the physical limitations of velocity, acceleration and jerk, for formation tracking of a trajectory generated by the leader with a bounded and unknown input. An optimization-based motion planning algorithm is then used to generate numerical solutions when the consensus control law can not provide a collision-free trajectory. The stability of the switching process between the consensus control and motion planning is guaranteed by a sufficiency condition. We should note that the works in [26–28] also combined consensus control and MPC for formation control of UAVs, but they are only suitable for obstacle-free environments. Specifically, authors of [26, 27] address the inter-vehicle collision avoidance problem by taking actions only in the vertical direction, thus, the methods may fail in voiding obstacles. The reference [28] focuses on solving the collision avoidance problem of two UAV teams, instead of the formation control of a rotorcraft team in cluttered environments.

The organization of this paper is as follows. Section 2 recalls some preliminary materials related to rotorcraft dynamics and some basic concepts in graph theory and consensus control. We then present our problem formulation and proposed framework in Sect. 3. The detailed solutions to the problems are given in Sect. 4, and the simulation and experimental results are presented in Sect. 5. Finally, we draw some concluding remarks in Sect. 6.

2 Preliminaries

In this section, we begin with presenting some background materials in rotorcraft dynamics, which shows the reason why rotorcraft can be represented by a chain of integrators. Then, some necessary definitions required for formulating formation control problems are provided.

2.1 UAS structure and rotorcraft dynamics

As depicted in Fig. 1, it is very common for an unmanned aerial system to adopt a two-layer flight control structure, i.e., an inner-loop control layer and an outer-loop layer. The attitude of the rotorcraft system is stabilized via the inner loop control whereas its position and velocity is controlled by the outer loop. It also has two layers for motion planning, i.e., i) the path planning block is to generate a collision-free path for the unmanned system to navigate through a cluttered environment; and ii) the trajectory generation module is to produce trajectory references that can be realized by the hardware platform. How to plan feasible paths and trajectories for a group of rotorcraft systems is a subject to be addressed later in this paper. In what follows, we detail how the rotorcraft dynamics inside the dash-line box in Fig. 1 can be approximated by a chain of integrators with a properly designed inner-loop control law.

Figure 1

Block diagram of the internal structure of an unmanned aerial system

For a single-rotor copter, its overall dynamics are generally highly nonlinear and of high dynamical order (see, e.g., [29, 30]). With a properly designed inner-loop control law using $H_{\infty}$ optimization technique, Cai et al. proved through frequency domain analysis that the resulting dynamics inside the dash-line box can be safely characterized by a double-integrator system with its position and velocity being the system state variables within the working bandwidth fully determined by the physical system and the inner-loop controller. For the unmanned helicopter studied in [30], the dynamics model inside dash-line box is given as (Eqns. (8.29)–(8.31) in [30])

$$ \begin{bmatrix} \dot{p} \\ \dot{v} \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix} \begin{bmatrix} p \\ v \end{bmatrix} + \begin{bmatrix} 0 \\ 1 \end{bmatrix} a, $$

where p, v and a, are the position, velocity and acceleration of the unmanned helicopter, respectively. The working bandwidth of the system is about 1 rad/sec.

For a multicopter, such as a commonly seen quadrotor, its inner-loop is structurally decoupled even though there are some small nonlinear terms, which can be washed out by a feedback linearization technique (see, e.g., Phang [31]). As shown in Phang [31] (see, e.g., Eqns. (6.17)–(6.19)), the dynamics of a quadrotor once again can be approximated by a chain of integrators in each channel. As such, we assume throughout the rest of this paper that the rotorcraft dynamics are characterized by an integrator system with appropriate physical limitations in velocity and acceleration. That is why the rotorcraft can be characterized by (1).

2.2 Notations and definitions

Notations. Throughout this paper, $I_{n}$ denotes the identity matrix with n dimension. $1_{n}$ is an n-dimensional column vector with all elements being 1. $X^{\mathrm{T}}$ denotes the transpose of the matrix or vector X. Given a signal $x:\mathbb{R}_{+}\to \mathbb{R}^{s}$, $x=[x_{1},\ldots ,x_{s}]^{\mathrm{T}}$, $|x|$ is the Euclidean norm, and $\|x\|_{\infty}=\max_{i}\|x_{i}\|$.

Rotorcraft team. Consider a team of N rotorcraft navigating in a cluttered environment. For each rotorcraft $i\in \mathcal{N}=\{1,2,\ldots ,N\}\subset \mathbb{N}$, we denote by $p_{i}(t)\in \mathbb{R}^{3}$ its position at time t. We use $\mathcal{P}(p)\subset \mathbb{R}^{3}$ to represent the volume occupied by a rotorcraft at position p. Given a (virtual) leader, the communication among the rotorcraft and the leader is denoted by a graph $\mathcal{G}=\{\mathcal{V},\mathcal{E}\}$ where $\mathcal{V}=\{0,1,\ldots ,N\}$ and $\mathcal{E}=\mathcal{V}\times \mathcal{V}$. For $i,j\in \mathcal{V}$, $(j,i)\in \mathcal{E}$ if and only if rotorcraft i has access to the information of rotorcraft. Let $\mathcal{A}=[a_{ij}]\in \mathbb{R}^{(N+1)\times N+1}$ be the adjacency matrix of the graph $\mathcal{G}$, where $a_{ij}=1$ if $(j,i)\in \mathcal{E}$, otherwise, $a_{ij}=0$. The Laplacian matrix is denoted as $\mathcal{L}=[l_{ij}]\in \mathbb{R}^{N\times N}$ with $l_{ij}=a_{ij}$ if $i\ne j$ and $l_{ii}=\sum_{j=0}^{N}a_{ij}$. In this paper, we assume that there exists at least one direct path between any two rotorcraft.

Formations. Suppose that there are $m\in \mathbb{N}$ pre-defined template formations, such as circle, line or square. Each template $f\in \mathcal{I}=\{1,2,\ldots ,m\}$ is determined by a reference point $c^{f}$ and its relative position between other rotorcraft $\{\Delta {\mathrm{p}}_{1}^{f},\ldots ,\Delta{\mathrm{p}}_{n}^{f}\}$. In other words, the absolute position of rotorcraft i is given by $p_{i}^{f}=c^{f}+\Delta {\mathrm{p}}_{i}^{f}$. In this paper, the template formations are given priorities.

Field of view. We assume that all rotorcraft have the same limited field of view, and it is assumed as a sphere with a given radius and with its center being located at the rotorcraft’s position. This field of each rotorcraft is denoted by $\mathcal{B}_{i}\in \mathbb{R}^{3}$.

Static obstacles. Assume there are many static obstacles $\mathcal{O}\subset \mathbb{R}^{3}$, and the obstacles seen by the ith rotorcraft is $\mathcal{O}_{i}:=\mathcal{B}_{i}\cap \mathcal{O}$. We further denote by $\bar{ \mathcal{O}}_{i}$ a dilatation of $\mathcal{O}_{i}$ within its visual space, that is,

$$ \bar{ \mathcal{O}}_{i}:=\bigl\{ p\in \mathbb{R}^{3}| \mathcal{P}(p)\cap \mathcal{O}_{i}\neq \emptyset \bigr\} . $$

If rotorcraft i is in $\bar{ \mathcal{O}}_{i}$, it will collide with obstacles.

Position-time obstacles. At current time $t_{0}$, for a time horizon τ which is usually several seconds in the near future, the position-time obstacles seen by rotorcraft i is denoted by

$$ \hat {\mathcal{O}}_{i}(t_{0})=\bar{ \mathcal{O}}_{i} \times [0,\tau ] \subset \mathbb{R}^{4}, $$

where × is the Cartesian product of the obstacles’ space and the time dimension.

Position-time free space. At time t, the free space of rotorcraft i is defined as

$$ \bar {\mathcal{F}}_{i}(t)=\mathbb{R}^{3}\times [0,\tau ] \backslash \hat {\mathcal{O}}_{i}(t), $$

which is the positions where rotorcraft would not collide with any obstacles within the time horizon τ.

3 Problem formulation and proposed framework

In this section, we define three problems that together form the formation control problem of multiple rotorcraft systems. Next, a novel framework is given to solve the three problems.

3.1 Problem formulation

We now formally introduce the flight formation of multiple rotorcraft systems. The outer dynamics of each rotorcraft can be safely characterized by the following triple integrator

$$ \dot{x}_{i}=Ax_{i} + Bu_{i}, \quad i\in \mathcal{N}, $$

(1)

where

$$ A= \begin{bmatrix} 0_{3} & I_{3} & 0_{3} \\ 0_{3} & 0_{3} & I_{3} \\ 0_{3} & 0_{3} & 0_{3} \end{bmatrix} , \qquad B= \begin{bmatrix} 0_{3} \\ 0_{3} \\ I_{3} \end{bmatrix} , $$

(2)

and $x_{i}=[p_{i}^{\mathrm{T}},v_{i}^{\mathrm{T}},a_{i}^{\mathrm{T}}]^{\mathrm{T}}\in \mathbb{R}^{9}$ being its position, velocity and acceleration, $u_{i}(t)$ being its jerk. The physical constraints are

$$ \Vert v_{i} \Vert \le v_{\max}, \qquad \Vert a_{i} \Vert \le a_{\max}, \quad \Vert j_{i} \Vert \le j_{ \max}. $$

(3)

Consider a team of rotorcraft satisfying (1)–(3), a communication graph $\mathcal{G}$, m pre-defined template formations that are known to all rotorcraft, a set of static obstacles, and a limited field of view of each rotorcraft. In this paper, we aim to tackle the distributed formation control of multiple rotorcraft systems in cluttered environments. The whole problem can be defined as the following three problems, and we will solve them jointly.

Problem 1

(Target formation selection)

At time $t_{0}$, design an approach to determine whether or not a formation can be formed at time $t_{0}+\tau$ where τ is a prediction time horizon such that all rotorcraft’s position at time $t_{0}+\tau$ are collision free. Find the target formation $f^{*}\in \mathcal{I}$ if one kind of formation can be formed.

Problem 1 derives from the fact that rotorcraft will change formations to adapt to different environments. It is also impossible for rotorcraft to form any formation when the obstacles are dense. Suppose that rotorcraft are and will be in formation $f_{1}\in \mathcal{I}$ at time $t_{0}$ and $t_{0}+\tau$, respectively. Then, is there an analytical solution navigating rotorcraft in formation $f_{1}$ during $(t_{0},t_{0}+\tau ]$ satisfying the physical constraints?

Problem 2

(Analytical control law)

Assume that rotorcraft are in formation $f_{1}\in \mathcal{I}$ at time $t_{0}$ and will keep this formation during $[t_{0},t_{0}+\tau ]$. Design an analytical control law that satisfies the physical constraints (3) for rotorcraft to maintain formation $f_{1}$ during $(t_{0},t_{0}+\tau ]$.

Another problem associated with the formation control of multiple rotorcraft is to guarantee the inter-vehicle collision avoidance when they switch from formation $f_{1}$ to $f_{2}$ where $f_{2}\ne f_{1}$. In such a case, the problem is formulated into a nonconvex optimization problem which will be solved numerically using motion planning algorithms.

Problem 3

(Collision avoidance)

Design or find a motion planning algorithm for rotorcraft to switch from formation $f_{1}$ to $f_{2}$ where $f_{2}\ne f_{1}$ such that rotorcraft do not collide with each other and with obstacles.

One can note that in a formation $f\in \mathcal{I}$, the positions of vehicles rely on the position of the reference point $c^{f}$. And the trajectory generation of the reference point is also formulated as an optimization problem and is solved by motion planning algorithms. Moreover, Problem 2 and Problem 3 illustrate that vehicles will switch between the analytical control law and the motion planning algorithm. It is important to ensure the stability of the switching process when designing both methods.

3.2 Framework overview

In the following, we give a novel framework connecting Problems 1–3, followed by detailed methods for these problems in Sect. 4.

In this paper, receding horizon control (RHC) is taken to generate trajectories for the team of rotorcraft. As illustrated in Fig. 2, each time interval consists of three subintervals: the communication part, the motion part, and the non-execution part. During each cycle, the following steps that form our framework are executed, which is also demonstrated in Fig. 3.

Figure 2

Determination of time intervals

Figure 3

Overview of the proposed framework. The blue parts represent local steps. In the orange parts, rotorcraft will communicate with each other to perform parameter consistency

1) At each initial time $t_{0}$, the individual obstacle-free convex region of each rotorcraft, denoted by $\mathcal{C}_{i}$, is computed (One algorithm is given in Sect. 4.1).

2) The common obstacle-free convex region of all rotorcraft is computed in a distributed way by ${\mathcal{C}}=\bigcap_{i\in \mathcal{N}}{\mathcal{C}}_{i}$ (One algorithm is given in Sect. 4.1).

3) Each rotorcraft computes the trajectory for the virtual leader for time $(t_{0},t_{0}+\Delta t_{3}]$, using the numerical solution (One numerical solution is given in Sect. 4.4).

4) The trajectory of the virtual leader is finally obtained by all rotorcraft performing consistency by selecting the one with the minimum cost that is defined in (10) among all trajectories generated in Step 3).

5) Given the common obstacle-free convex region computed in Step 2), all rotorcraft come to an agreement about whether they can keep current formation $f_{1}\in \mathcal{I}$ at time $t_{0}+\Delta t_{3}$ or not.

• (i) If formation $f_{1}$ can be kept (i.e., $\mathcal{P}(c^{f_{1}}(t_{0}+\Delta t_{3})+\Delta {\mathrm{p}}_{i}^{f_{1}}) \subset \mathcal{C}$, $\forall i\in \mathcal{N}$), consensus-based formation control is implemented during the motion interval $[t_{0}+\Delta t_{1}, t_{0}+\Delta t_{2}]$ (One consensus control is given in Sect. 4.2). Then, return to Step 1).

• (ii) In case of formation $f_{1}$ cannot be kept, they will reach consistency on if or not they can form other template formations. The template formations are checked from the highest priority to the lowest priority. If formation $f_{2}\ne f_{1}\in \mathcal{I}$ can be formed (i.e., $\mathcal{P}(c^{f_{2}}(t_{0}+\Delta t_{3})+\Delta {\mathrm{p}}_{i}^{f_{2}}) \subset \mathcal{C}$, $\forall i\in \mathcal{N}$), numerical solution is used to navigate rotorcraft to reach the set defined in (14) with $f=f_{2}$ (a small neighboring set around the formation $f_{2}$) to form formation $f_{2}$. Then, return to Step 1) with the updated initial time and formation. In case none of the formations can be formed, the numerical solution is used by each rotorcraft to reach its goal respectively. Then, return to Step 1).

The above steps repeat with the updated information until all rotorcraft navigate to their final targets.

Remark 1

The trajectory of the virtual leader is predicted for time $\Delta t_{3}$, which is longer than the motion time $\Delta t_{2}$. It can be inferred from the definition of formation that the trajectory of the virtual leader stands for that of the reference point.

Remark 2

Note that MPC can also be used for formation control. However, the optimal solution at each sampling time cannot guarantee the stability of the MPC algorithm. This strategy we designed aims to use the consensus control law for formation control as much as possible because the stability of the consensus control law can be proved.

Analytical solution-based formation control. In this paper, the virtual leader also satisfies the dynamics (1). During the time interval $[t,t+\Delta t_{3})$, its trajectory is generated by all rotorcraft with some communication using a motion planning algorithm that is to be introduced in Sect. 4.4.

The leader together with the N rotorcraft forms a team of $N+1$ rotorcraft, where the leader is numbered as rotorcraft 0 and the follower rotorcraft are numbered as 1 to N, respectively. They form the following rotorcraft systems

$$ \textstyle\begin{cases} \dot{x}_{0}=Ax_{0} + Bu_{0}, \\ \dot{x}_{i}=Ax_{i} + Bu_{i}, &i\in \mathcal{N}, \end{cases} $$

(4)

where for $i=0,1,\ldots ,N$, $x_{i}=[p_{i}^{\mathrm{T}},v_{i}^{\mathrm{T}},a_{i}^{\mathrm{T}}]^{\mathrm{T}}$, and the input $u_{i}$ being the jerk. Chosen a template formation f, if there is a control law of the form $u_{i}(t)=g_{i} (x_{i}(t),x_{0}(t),u_{0}(t) )$, so that $\lim_{t\to \infty}(p_{i}(t)-p_{0}(t)-\Delta {\mathrm{p}}_{i}^{f})=0$, then such a law is an analytical solution for the formation control.

For a template formation f, the relative positions of the rotorcraft and the virtual leader are denoted by $\{\Delta {\mathrm{p}}_{1}^{f},\ldots , \Delta{\mathrm{p}}_{N}^{f}\}$. The formation control $\lim_{t\to \infty}(p_{i}(t)-p_{0}(t))=\Delta {\mathrm{p}}_{i}^{f}$ is equivalent to $\lim_{t\to \infty}(p_{i}(t)-\Delta {\mathrm{p}}_{i}^{f}-p_{0}(t))=0$. If the state of followers is re-formulated as

$$ \hat{x}_{i}= \begin{bmatrix} \hat{p}_{i} \\ v_{i} \\ a_{i} \end{bmatrix} = \begin{bmatrix} p_{i}-\Delta p_{i} \\ v_{i} \\ a_{i} \end{bmatrix}, $$

(5)

the systems in (4) are converted to

$$ \textstyle\begin{cases} \dot{x}_{0}=Ax_{0} + Bu_{0}, \\ \dot {\hat{x}}_{i}=A\hat{x}_{i}+Bu_{i}, &i\in \mathcal{N}. \end{cases} $$

(6)

Then, the formation control is transformed into the leader-following consensus control problem.

Even though the consensus-based formation control law gives an analytical solution for the formation control, it can not solve potential collision problems among agents. Instead, the collision avoidance constraint can be formulated into a nonconvex optimization problem.

Nonconvex optimization problem. The numerical solution is applied in two cases, 1) the trajectory generation of the virtual leader, 2) the motion planning of all rotorcraft when none of the template formations can be performed or when rotorcraft switch formations.

Consider system (1) with state and input constraints (3). The formula

$$ \mathcal{P}\bigl(p_{i}(t)\bigr)\notin \mathcal{O}, \quad \forall t\ge t_{0}, $$

(7)

where $\mathcal{O}$ denotes the space occupied by obstacles, defines the obstacle collision free constraint, and the inter-vehicle collision free constraint is

$$ \mathcal{P}\bigl(p_{i}(t)\bigr)\cap \mathcal{P}\bigl(p_{j}(t) \bigr)\in \emptyset , \quad \forall t\ge t_{0}, \forall i\ne j\in \mathcal{N}. $$

(8)

The controller regulates the system (1) from any initial states to desired points $[p_{i,\mathrm{d}},0_{3},0_{3}]$. The initial condition constraint is

$$ x_{i}(t_{0})=x_{i,0}. $$

(9)

For rotorcraft i, we use $u_{i}(t)$, $x_{i}(t)$, $t\in [t_{0},t_{0}+T]$ to represent its input trajectory and state trajectory, respectively, where T denotes the planning horizon. The local motion planning satisfying the above constraints can be formulated as an optimization problem that solves

$$ J_{i} = \xi _{i} \bigl(x_{i}(t_{0}+T) \bigr) + \int _{t=t_{0}}^{t_{0}+T}L_{i} \bigl(x(t),u_{i}(t) \bigr). $$

(10)

Functions $\xi _{i}$ and $L_{i}$ are the user-defined terminal and running costs, respectively.

• For $i=0$, the virtual leader, there is no need to consider the inter-vehicle collision free constraint (8).

• For rotorcraft 1 to N, the problem is subject to system (1) and constraints (3), (7)–(9). Constraint (8) is considered because inter-vehicle collisions must be avoided.

We aim to find the position, velocity, acceleration, and jerk of the rotorcraft and the virtual leader. Because of the collision avoidance constraints (7) and (8), especially constraint (8), the optimization problem is nonconvex, which is NP-hard to solve.

4 Solutions to the problems

In this section, we present the details of the three parts of our framework, which are the common obstacle-free convex region, consensus control, and numerical solution-based algorithm we used. Then, we analyzed the stability and convergence of the framework. The three parts are respectively solutions to the three problems defined in Sect. 3.1.

4.1 Solution to Problem 1: common obstacle-free convex region

Rotorcraft may form different obstacle maps since each of them has a limited field of view, which may result in different target formations. Therefore, rotorcraft should compute a common obstacle-free convex region.

In this paper, the method of Deits and Tedrake [32] is chosen, which solves the optimization problem recurrently to compute $\mathcal{C}_{i}$. Assume that the separating hyperplanes rotorcraft i obtains are $H_{i,k}x=b_{i,k}$, for $H_{i,k}\in \mathbb{R}^{l_{i}\times 4}$, $b_{i,k}\in \mathbb{R}^{l_{i}}$ where $l_{i}$ is the number of hyperplanes. Let $H_{i}=[H_{i,1}^{\mathrm{T}},\ldots ,H_{i,l_{i}}^{\mathrm{T}}]^{\mathrm{T}}$ and $b_{i}=[b_{i,1}^{\mathrm{T}},\ldots ,b_{i,l_{i}}^{\mathrm{T}}]^{\mathrm{T}}$, then, the set of points satisfying

$$ \mathcal{C}_{i} = \bigl\{ x\in \mathbb{R}^{4}|H_{i}x \le b_{i}\bigr\} $$

(11)

form the convex polytope. Since the obstacles has been detected by rotorcraft are in position-time space, so is the convex polytope. Therefore, the hyperplanes are defined in 4-dimensional space instead of a 3-dimensional space.

As shown in Algorithm 1, the common convex region is computed in a distributed and iterative way. Before transmitting information, the separating hyperplanes of each rotorcraft are set as an empty set (line 1). Then, only new hyperplanes are transmitted to and from their neighbors to reduce communication costs (lines 3 and 4). The whole process ends after d rounds of communication. The convex region shrinks after each iteration and it finally converges to the intersection of all rotorcraft’s convex regions. An example of an obstacle-free convex region obtained is demonstrated in Fig. 4.

Algorithm 1

Distributed intersection of convex regions

Figure 4

An illustration example of the obtained common convex region. The dash lines denote the hyperplanes of all rotorcraft, the light purple area is the common obstacle-free convex region, and the four red dots denote vehicles and the grey blocks are obstacles

It is proved in [33] that the resulting convex region $\mathcal{C}$ is obstacles free since it does not intersect with any obstacle in the field of rotorcraft for the time period $[t_{0},t_{0}+\Delta t_{3}]$.

4.2 Solution to Problem 2: leader-following consensus control with state constraint

To achieve the leader’s trajectory tracking, it is reasonable to assume that the leader’s maximum velocity, acceleration and jerk are less than that of the followers. For simplicity, we denote $v_{\max}-v_{0,\max}=\delta _{v}$, $a_{\max}-a_{0,\max}=\delta _{a}$. We suppose that agents can share information if they are within the communication range, so the communication graph is undirected.

Regarding the systems (6), consider the following control law for rotorcraft i, $i=1,2,\ldots ,N$,

$$ \begin{aligned}[b] u_{i}(t)&=-\mu B^{\mathrm{T}}P \Biggl(\sum _{j=1}^{N}a_{ij}(\hat{x}_{i}-\hat{x}_{j})+a_{i0}( \hat{x}_{i}-x_{0}) \Biggr)\\ &\quad {} -\gamma f_{i}(t), \end{aligned} $$

(12)

where $\gamma =j_{0,\max}$, the maximum jerk of leader. $\mu \ge 1/ (2\lambda _{\min}(\mathcal{L}))$ is a positive constant where $\lambda _{\min}(\mathcal{L})$ is the minimum eigenvalue of the Laplacian matrix. Denote $\zeta _{i}(t)=\sum_{j=1}^{N}a_{ij}(\hat{x}_{i}-\hat{x}_{j})+a_{i0}( \hat{x}_{i}-x_{0})$, $f_{i}(t)$ is designed as

$$ f_{i}(t)=\textstyle\begin{cases} \frac{B^{\mathrm{T}}P\zeta _{i}(t)}{ \Vert B^{\mathrm{T}}P\zeta _{i}(t) \Vert }, &\text{if } \zeta _{i}(t)\ne 0, \\ 0,&{\text{otherwise}}. \end{cases} $$

P is the solution of the following Algebraic Riccati Equation (ARE)

$$ A^{\mathrm{T}}P(\epsilon )+P(\epsilon )A-P(\epsilon )BB^{\mathrm{T}}P(\epsilon )=- \epsilon I_{9}. $$

(13)

From [34], we have $\lim_{\epsilon \to 0}P(\epsilon )=0$. Then, we have the following theorem.

Theorem 1

Consider the multiagent agent system (6). For $i=1,2,\dots ,N$, for any $v_{i}(0)-v_{0}(0)\le \delta _{v}$, $a_{i}(0)-a_{0}(0)\le \delta _{a}$, and $\|v_{i}(0)\|\le v_{\max}$, $\|a_{i}(0)\|\le a_{\max}$, there exists an $\epsilon ^{*}\in (0,1]$, such that for any $\epsilon \in (0,\epsilon ^{*}]$, the solution of the closed-loop system consisted of (6) and (12) satisfies $\|v_{i}(t)\|\le v_{\max}$, $\|a_{i}(t)\|\le a_{\max}$, $\|u_{i}\|\le j_{\max}$, and $\lim_{t\to \infty}(\hat{x}_{i}(t)-x_{0}(t))=0$.

Proof

See Appendix. □

Remark 3

Theorem 1 indicates that the control law (12) can tackle the state constraints in velocity, acceleration and the input constraint (jerk). It also shows that if the initial states of agents are within a small neighborhood of the target formation, they will converge to the formation without inter-agent collisions. Inter-agent collisions may happen when agents change formation configuration, because the current state is outside the small neighborhood of the target formation to be formed. In this paper, a motion planing algorithm is introduced to solve the problem.

4.3 Stability and convergence analysis

The individual stability of the analytical solution and numerical solution has been proved by theoretical analysis and experiments. The potential instability may occur in the switching process.

(i) When consensus control switches to the numerical solution, the end state of the consensus control is the initial state of the non-convex optimization problem (10). Given any initial state in (9), the numerical solution will generate trajectories satisfying constraints (3) and (7)-(9).

(ii) For system (1), following (5), the actual position of vehicles is recovered by $p_{i}=\hat{p}_{i}+\Delta {\mathrm{p}}_{i}^{f}$. One can note from Sect. 4.2 that the inter-vehicle collision avoidance problem is not considered. Given a chosen formation template f, the expected initial state of the consensus control is assumed to be $[(p_{i}^{f})^{\mathrm{T}},0_{1\times 3},0_{1\times 3}]^{\mathrm{T}}$, where $p_{i}^{f}=p_{0}^{f}+\Delta {\mathrm{p}}_{i}^{f}$. To solve the inter-vehicle collision avoidance problem of the consensus-based formation, we define a set for each vehicle

$$\begin{aligned} \mathcal{T}_{i}&=\bigl\{ d_{i}=\bigl(d_{i,1}^{\mathrm{T}},d_{i,2}^{\mathrm{T}},d_{i,3}^{ \mathrm{T}} \bigr)^{\mathrm{T}}\in \mathbb{R}^{9}| \bigl\Vert d_{i,1}-p_{i}^{f} \bigr\Vert \le r_{i}, \\ &\quad \Vert d_{i,2} \Vert \le \rho _{i}, \Vert d_{i,3} \Vert \le \kappa _{i}\bigr\} , \end{aligned}$$

(14)

where $r_{i}$, $0<\rho _{i}<\delta _{v}$ and $0<\kappa _{i}<\delta _{a}$ are small numbers. The three constraints in (14) ensure that vehicles navigate to a small neighborhood of the expected initial state. As illustrated in Fig. 5, there exist $r_{i}$ and $r_{j}$ such that $\forall i\ne j$, $\mathcal{T}_{i}\cap \mathcal{T}_{j}=\emptyset$ and $\|d_{i,1}-d_{j,1}\|>2r$ where r is the radius of vehicles. The constraint $\|d_{i,1}-d_{j,1}\|>2r$ guarantees vehicles in set $\mathcal{T}_{i}$ are at a distance of at least 2r so that vehicles will not collide with each other.

Figure 5

(a) If the initial state $x_{i}(t_{0})$ is in a small neighborhood of the target formation, i.e., $x_{i}(t_{0})\in \mathcal{T}_{i}$, and $\mathcal{T}_{i}\cap \mathcal{T}_{j}=\emptyset$, then their trajectories $x_{i}(t)$ will not intersect; (b) If $\mathcal{T}_{i}\cap \mathcal{T}_{j}\neq \emptyset$, their trajectories may intersect

Theorem 1 shows that for $[p_{i}^{\mathrm{T}}(0),v_{i}^{\mathrm{T}}(0),a_{i}^{\mathrm{T}}(0)]^{\mathrm{T}}\in \mathcal{T}_{i}$, there exists an $\epsilon _{i}^{*}$ such that for any $\epsilon \in (0,\epsilon _{i}^{*}]$, the state $[p_{i}^{\mathrm{T}}(t),v_{i}^{\mathrm{T}}(t),a_{i}^{\mathrm{T}}(t)]^{\mathrm{T}}\in \mathcal{T}_{i}$. Therefore, when switching to consensus control, if rotorcraft navigate to the set $\mathcal{T}_{i}$, they will not collide with each other during the consensus control process, which is a sufficient condition for the switching stability.

4.4 Solution to Problem 3: MPC-based trajectory generation

As introduced in Sect. 1, there are several motion planning techniques. To achieve real time planning, we extend the work of Lai et al. [35], which uses MPC for trajectory generation of a single agent with boundary state constraint, for trajectory generation of multiple rotorcraft.

The cost function of the optimization problem is

$$\begin{aligned} J_{i}&=\bigl(x_{i}(t_{0}+T)-x_{tg} \bigr)^{\mathrm{T}}Q_{i}\bigl(x_{i}(t_{0}+T)-x_{tg} \bigr) \\ &\quad {}+ \int _{t=t_{0}}^{t_{0}+T}u_{i}(t)^{\mathrm{T}}R_{i}u_{i}(u), \end{aligned}$$

where $x_{tg}$ denotes the state of the target, and $Q_{i}$ and $R_{i}$ are positive definite matrices, which obviously satisfies the form of (10). The constraints are (3) and (7)-(9).

Remark 4

Let us note that MPC is one of the motion planning methods to solve the collision avoidance problems. In this framework, it can be replaced by other algorithms like the sampling-based motion planning methods. We use MPC in this paper because it runs faster than the sampling-based motion planning methods.

5 Simulation and experimental results

Both simulations and experiments are done in this section to verify the effectiveness of our method, using a group of quadrotors whose dynamics satisfy (1). In both simulation and experiment, there are four quadrotors whose state constraints are

$$\begin{aligned} &v_{0,max}=1, \qquad a_{0,max}=2, \qquad u_{0,max}=4, \\ &v_{1,max}=2,\qquad a_{1,max}=3, \qquad u_{1,max}=5. \end{aligned}$$

(15)

The consensus control law is derived from the one in (12) with $\epsilon =0.1$ and $\mu =2$. Quadrotors will choose one formation from line and square, and the priority of line is lower than square. The formation templates are shown in Fig. 6.

Figure 6

Distribution of MAVs in the square formation and line formation. MAVs are distributed in the line evenly

In simulation, the computations are performed in MATLAB R2020a with a laptop (Intel Core i7 CPU@1.8G Hz). The group of quadrotors navigates in a $6{~\mathrm{m}}\times 5{~\mathrm{m}}$ area containing six convex obstacles. The obstacles are placed randomly in the area, except two cuboid obstacles (the two ones in the bottom of the area) are placed in parallel so that the line formation have the possibility to be formed. Four targets are given. The next target will not be given, unless the virtual leader enters the reaching radius (0.15 m) of the current one. Formations are recurrently predicted at 2 Hz and the algorithm is recurrently implemented at 4 Hz. Figure 7 shows quadrotors’ trajectories, obstacle-free convex regions, and expected formations at several time instants.

Figure 7

Snapshots of the simulation results at $t=0.5\text{ s}$, $t=2.5\text{ s}$, $t=9.25\text{ s}$ and $t=29.75\text{ s}$, respectively. (a) The light purple areas denote the common obstacle-free convex regions, red dots are the quadrotor positions, and the blue stars denote the target formation for the next cycle; (b) The red lines and the blue lines represent trajectories of the four quadrotors’ and the virtual leader, respectively

In the experiment, the obstacles are placed at the same position as in the simulation to show the effectiveness of our method. We use a kind of nano quadrotor helicopter Crazyflie 2.1 as the flying platform, and the localization system we use to localize quadrotors and obstacles is VICON. The Crazyflie 2.1 is an open source flying development platform that weighs only 27 g. It is also equipped with low-latency/long-range radio. Therefore, by combining with the Crazyradio PA, we can control it and display data using our computer. A laptop with Intel Core i7 CPU is used to do online planning. The hardware used in the experiment is shown in Fig. 8 which also illustrates the information flow. The experiment environment is shown in Fig. 9 and the actual flight video captured can be found at YouTube: https://youtu.be/XGehuDDPxVk. The real trajectories and velocities of quadrotors in the experiment are shown in Fig. 10 and Fig. 11, respectively. Figure 10 shows that quadrotors’ trajectories are almost the same as the ones in simulation in Fig. 7.

Figure 8

The hardware used in the experiment

Figure 9

The experimental environment of our flight test

Figure 10

Real trajectories of rotorcraft in the experiment

Figure 11

Velocities of rotorcraft in the experiment

In this example, the four quadrotors start in a square formation. Firstly, the consensus-based formation control law is applied by quadrotors to main the square formation, until they meet a narrow corridor at $t=2.5$ s. They predict that they can not pass the narrow corridor with the square formation. Hence, they change to line formation using MPC, and we say rotorcraft change to the line successfully if each of them reaches a small neighborhood of the line formation $\mathcal{T}_{i}$ which is defined in (14). The parameters that are defined in $\mathcal{T}_{i}$ are set as $r_{i}=0.05$, $\rho _{i}=0.02$, $\kappa _{i}=0.02$. Then, they pass the corridor in line using the consensus control until $t=9.25$ s, from which they return to the square using MPC with the same set $\mathcal{T}_{i}$. Finally, they navigate to the last target in the square using consensus control.

6 Conclusion

In this paper, we have presented a novel formation shaping framework that combines an analytical and a numerical solution for quadrotors navigating in cluttered environments. More specifically, the analytical solution we used is consensus control, and the consensus control law we designed can handle a set of state constraints. More importantly, the stability of the switching process is proved by both theoretical analysis and experiments.

Appendix

Proof of Theorem 1

Let $\tilde{x}_{i}=\hat{x}_{i}-x_{0}$, $\tilde{x}_{f}=[\tilde{x}_{1}^{\mathrm{T}},\ldots ,\tilde{x}_{N}^{\mathrm{T}}]^{ \mathrm{T}}$, and $\zeta =[\zeta _{1}^{\mathrm{T}},\ldots ,\zeta _{N}^{\mathrm{T}}]^{\mathrm{T}}$. We have $\zeta =(\mathcal{L}\otimes I_{9})\tilde{x}_{f}$. Then, the closed-loop system can be written in terms of $\zeta (t)$. Since $\mathcal{L}$ is invertible, the convergence of $\zeta (t)$ means that of $\tilde{x}_{f}(t)$.

We construct the following Lyapunov function

$$ V_{\zeta}(t)=\zeta ^{\mathrm{T}}(I_{N}\otimes P)\zeta, $$

(16)

where P is the solution of the ARE (13). Since the initial condition satisfies the state constraint, there exists some bounded and impact sets $\mathcal{X}_{i0}$ for $i=1,2,\ldots ,N$, such that $\hat{x}_{i}(0)\in \mathcal{X}_{i0}$ and $x_{0}(0)\in \mathcal{X}_{00}$.

Let c be a constant such that

$$ \sup_{\hat{x}_{i}(0)\in \mathcal{X}_{i0},x_{0}(0)\in \mathcal{X}_{00}} V_{\zeta}(t)\le c. $$

Define $L_{V}(c):=\{\zeta \in \mathbb{R}^{9N}:V_{\zeta}(t)\le c\}$. Because $L_{V}(c)$ is bounded and $\lim_{\epsilon \to 0}P=0$, there exists an $\epsilon ^{*}\in (0,1]$, such that for all $\epsilon \in (0,\epsilon ^{*}]$, $\zeta \in L_{V}(c)$ means

$$\begin{aligned} & \Biggl\Vert -\mu B^{\mathrm{T}}P \Biggl(\sum_{j=1}^{N}a_{ij}( \hat{x}_{i}-\hat{x}_{j})+a_{i0}( \hat{x}_{i}-x_{0}) \Biggr) \Biggr\Vert \\ &\quad = \bigl\Vert -\mu B^{\mathrm{T}}P\zeta _{i}(t) \bigr\Vert \le j_{\max}-j_{0,\max}, \end{aligned}$$

which further implies

$$ \Vert u_{i} \Vert = \bigl\Vert -\mu B^{\mathrm{T}}P\zeta _{i}(t)-\gamma f_{i} \bigr\Vert \le j_{\max}. $$

Thus, the input of the each rotorcraft satisfies the jerk constraint.

Substituting the control law (12) into the system (6) gives

$$ \dot {\hat{x}}_{i}=A\hat{x}_{i}-\mu BB^{\mathrm{T}}P\zeta _{i}(t)-\gamma Bf_{i}(t), $$

whose compact form is

$$ \dot {\hat{x}}_{f}=(I_{N}\otimes A)\hat{x}_{f}-\mu \bigl(I_{N}\otimes BB^{ \mathrm{T}}P\bigr)\zeta (t)-\gamma (I_{N}\otimes B)\bar{f}(t) $$

with $\bar{f}(t)=[f_{1}^{\mathrm{T}}(t),\ldots ,f_{N}^{\mathrm{T}}(t)]^{\mathrm{T}}$. Denote $\bar{x}_{0}(t)=1_{N}\otimes x_{0}(t)$ and $\bar{u}_{0}=1_{N}\otimes u_{0}(t)$, it follows that

$$\begin{aligned} \dot {\tilde{x}}_{f}&=(I_{N}\otimes A)\tilde{x}_{f}- \mu \bigl(I_{N}\otimes BB^{ \mathrm{T}}P\bigr)\zeta (t)-\gamma (I_{N}\otimes B)\bar{f}(t) \\ &\quad {}-(I_{N}\otimes B)\bar{u}_{0}. \end{aligned}$$

Since $\zeta =(\mathcal{L}\otimes I_{9})\tilde{x}_{f}$, we have

$$ \begin{aligned} \dot{\zeta}(t)&= \bigl[I_{N}\otimes A-\mu \bigl(\mathcal{L}\otimes BB^{\mathrm{T}}P\bigr)\bigr] \zeta (t)\\ &\quad {}-\gamma (\mathcal{L}\otimes B)\bar{f}(t)-( \mathcal{L}\otimes B) \bar{u}_{0}. \end{aligned} $$

Then, the derivative of the Lyapunov function is

$$\begin{aligned} \dot{V}_{\zeta}& = \zeta ^{\mathrm{T}}\bigl[I_{N}\otimes \bigl(A^{\mathrm{T}}B+BA\bigr)-\mu \bigl( \mathcal{L}^{\mathrm{T}}+\mathcal{L} \bigr)\otimes PBB^{\mathrm{T}}P\bigr]\zeta \\ &\quad{}-2\gamma \zeta ^{\mathrm{T}}(\mathcal{L}\otimes PB)\bar{f}(t)-2\gamma \zeta ^{\mathrm{T}}(\mathcal{L}\otimes PB)\bar{u}_{0}. \end{aligned}$$

(17)

Besides, since $\mu \ge 1/(2\lambda _{\min}(\mathcal{L}))$, we have

$$\begin{aligned} &I_{N}\otimes \bigl(A^{\mathrm{T}}B+BA\bigr)-\mu \bigl( \mathcal{L}^{\mathrm{T}}+\mathcal{L}\bigr) \otimes PBB^{\mathrm{T}}P \\ &\quad \le I_{N}\otimes \bigl(A^{\mathrm{T}}B+BA-2\mu \lambda ( \mathcal{L})PBB^{\mathrm{T}}P\bigr) \\ &\quad \le -I_{N}\otimes \epsilon I_{9}. \end{aligned}$$

(18)

On the other hand, based on the definition of $f_{i}(t)$, it is easy to obtain that $\zeta _{i}^{\mathrm{T}}(t)PBf_{i}(t)=\|B^{\mathrm{T}}P\zeta _{i}(t)\|$ and $\zeta _{i}^{\mathrm{T}}(t)PBf_{j}(t)\le \|B^{\mathrm{T}}P\zeta _{i}(t)\|$ for $j\ne i$. Then, we have

$$\begin{aligned} &-2\gamma \zeta ^{\mathrm{T}}(\mathcal{L}\otimes PB)\bar{f}(t) \\ &\quad =2\gamma \sum_{i=1}^{N}\zeta _{i}^{\mathrm{T}}PB \Biggl(\sum_{j=1}^{N}a_{ij} \bigl(f_{j}(t)-f_{i}(t)\bigr)-a_{i0}f_{i}(t) \Biggr) \\ &\quad \le -2\gamma \sum_{i=1}^{N}\zeta _{i}^{\mathrm{T}}PBa_{i0}f_{i}(t)=-2 \gamma \sum _{i=1}^{N}a_{i0} \bigl\Vert \zeta _{i}^{\mathrm{T}}PB \bigr\Vert , \end{aligned}$$

(19)

and

$$\begin{aligned} -2\gamma \zeta ^{\mathrm{T}}(\mathcal{L}\otimes PB)\bar{u}_{0} &=2 \sum_{i=1}^{N}a_{i0} \zeta _{i}^{\mathrm{T}}PB u_{0} \\ &\le 2\gamma \sum_{i=1}^{N}a_{i0} \bigl\Vert \zeta _{i}^{\mathrm{T}}PB \bigr\Vert . \end{aligned}$$

(20)

Substituting (18), (19) and (20) into (17) gives

$$ \dot{V}_{\zeta}(t)\le -\zeta ^{\mathrm{T}}(I_{N}\otimes \epsilon I_{9}). $$

Since $V_{\zeta}(t)$ decreases over time, $\zeta (t)$ and $\tilde{x}_{i}$ also decrease. Because $v_{i}(0)-v_{0}(0)\le \delta _{v}$, $a_{i}(0)-a_{0}(0)\le \delta _{a}$, and $\|v_{i}(0)\|\le v_{\max}$, $\|a_{i}(0)\|\le a_{\max}$, we have $\|v_{i}(t)\|\le v_{0}(t)+\delta _{v}\le v_{\max}$ and $\|a_{i}(t)\|\le a_{0}(t)+\delta _{a}\le a_{\max}$. Thus, the trajectory of the closed-loop system satisfy the state condition. Besides, $V_{\zeta}=0$ leads to $\zeta =0$, Since $\mathcal{L}$ is invertible, it means $\lim_{t\to \infty}(\hat{x}_{i}(t)-x_{0}(t))=0$. □