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.
Similar content being viewed by others
Avoid common mistakes on your manuscript.
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 map**, 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.
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])
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,
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
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
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
where
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
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.
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
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
the systems in (4) are converted to
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
where \(\mathcal{O}\) denotes the space occupied by obstacles, defines the obstacle collision free constraint, and the inter-vehicle collision free constraint is
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
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
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
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.
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\),
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
P is the solution of the following Algebraic Riccati Equation (ARE)
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
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.
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
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
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.
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.
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.
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 sha** 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.
Availability of data and materials
Not applicable.
Code availability
Not Applicable.
Abbreviations
- N :
-
the number of rotorcraft
- m :
-
the number of predefined template formation
- \(p_{0}(t)\) :
-
position of the virtual leader
- \(p_{i}(t)\) :
-
position of rotorcraft i
- \(v_{i}(t)\) :
-
velocity of rotorcraft i
- \(a_{i}(t)\) :
-
acceleration of rotorcraft i
- \(j_{i}(t)\) :
-
jerk of rotorcraft i
- \(u_{i}\) :
-
control input of rototcraft i
- \(\Delta {\mathrm{p}}_{i}^{f_{i}}\) :
-
expected relative position between rotorcraft i and the virtual leader in formation \(f_{i}\)
- \(c^{f_{i}}\) :
-
position of the virtual leader
- \(\mathcal{B}_{i}\) :
-
field of view of rotorcraft i
- \(\mathcal{P}(p)\) :
-
the volume occupied by a rotorcraft at position p
- \(\mathcal{O}\) :
-
static obstacles
- \(\mathcal{O}_{i}\) :
-
obstacles seen by rotorcraft i
- \(\bar {\mathcal{O}}_{i}\) :
-
a dilatation of \(\mathcal{O}_{i}\)
- \(\hat {\mathcal{O}}_{i}\) :
-
position-times obstacles seen by rotorcraft i
- \(\hat {\mathcal{F}}_{i}(t)\) :
-
free space of rotorcraft i at time t
- \(\mathcal{C}_{i}\) :
-
the obstacle-free convex region of rotorcraft i
- \(\mathcal{C}\) :
-
the common obstacle-free convex region
- \(\hat{p}_{i}\) :
-
equals \(p_{i}-\Delta {\mathrm{p}}_{i}\)
- \(\hat{x}_{i}\) :
-
equals \([\hat{p}_{i}^{\mathrm{T}},v_{i}^{\mathrm{T}},a_{i}^{\mathrm{T}}]^{\mathrm{T}}\)
- \(\Delta t_{1}\) :
-
communication time in each cycle of RHC
- \(\Delta t_{2}\) :
-
motion time in each cycle of RHC
- \(\Delta t_{3}\) :
-
predictive horizon
- P :
-
solution of the ARE (13)
- \(\delta _{v}\), \(\delta _{a}\) :
-
small positive constants
References
B.M. Chen, On the trends of autonomous unmanned systems research. Engineering 12, 20–23 (2021). https://doi.org/10.1016/j.eng.2021.10.014.
S. Zhao, Affine formation maneuver control of multiagent systems. IEEE Trans. Autom. Control 63(12), 4140–4155 (2018)
X. Ge, Q.L. Han, Distributed formation control of networked multi-agent systems using a dynamic event-triggered communication mechanism. IEEE Trans. Ind. Electron. 64(10), 8118–8127 (2017)
Y. Li, H. Fang, J. Chen, Anomaly detection and identification for multiagent systems subjected to physical faults and cyberattacks. IEEE Trans. Ind. Electron. 67(11), 9724–9733 (2019)
M. Wang, J. Sun, J. Chen, Input-to-state stability of perturbed nonlinear systems with event triggered receding horizon control scheme. IEEE Trans. Ind. Electron. 66(8), 6393–6403 (2018)
P. Zhou, B.M. Chen, Semi-global leader-following output consensus of heterogeneous systems with position and rate-limited actuators via state feedback, in Proceedings of the 40th Chinese Control Conference, Shanghai, China, July 26-28, 2021, 2021, pp. 5295–5302
X. Dong, Y. Zhou, Z. Ren, Y. Zhong, Time-varying formation tracking for second-order multi-agent systems subjected to switching topologies with application to quadrotor formation flying. IEEE Trans. Ind. Electron. 64(6), 5014–5024 (2016)
X. Dong, G. Hu, Time-varying formation tracking for linear multiagent systems with multiple leaders. IEEE Trans. Autom. Control 62(7), 3658–3664 (2017)
L. Ding, Q.L. Han, G. Guo, Network-based leader-following consensus for distributed multi-agent systems. Automatica 49(7), 2281–2286 (2013)
S. Zhao, D. Zelazo, Translational and scaling formation maneuver control via a bearing-based approach. IEEE Trans. Control Netw. Syst. 4(3), 429–438 (2015)
Y. Hua, X. Dong, G. Hu, Q. Li, Z. Ren, Distributed time-varying output formation tracking for heterogeneous linear multiagent systems with a nonautonomous leader of unknown input. IEEE Trans. Autom. Control 64(10), 4292–4299 (2019)
Y. Su, J. Huang, Stability of a class of linear switching systems with applications to two consensus problems. IEEE Trans. Autom. Control 57(6), 1420–1430 (2011)
H. Su, M.Z.Q. Chen, X. Wang, J. Lam, Semiglobal observer-based leader-following consensus with input saturation. IEEE Trans. Ind. Electron. 61(6), 2842–2850 (2013)
Y. Lv, J. Fu, G. Wen, T. Huang, X. Yu, Distributed adaptive observer-based control for output consensus of heterogeneous MASs with input saturation constraint. IEEE Trans. Circuits Syst. I, Regul. Pap. 67(3), 995–1007 (2019)
Z. Zhao, H. Shi, Semi-global containment control for linear systems in the presence of actuator position and rate saturation. Int. J. Robust Nonlinear Control 29(1), 1–18 (2019)
L. Kou, Z. Chen, J. **ang, Cooperative fencing control of multiple vehicles for a moving target with an unknown velocity. IEEE Trans. Autom. Control 67(2), 1008–1015 (2021)
R. Falconi, L. Sabattini, C. Secchi, C. Fantuzzi, C. Melchiorri, Edge-weighted consensus-based formation control strategy with collision avoidance. Robotica 33(2), 332–347 (2015)
L. Kou, Y. Huang, Z. Chen, S. He, J. **ang, Cooperative fencing control of multiple second order vehicles for a moving target with and without velocity measurements. Int. J. Robust Nonlinear Control 31(10), 4602–4615 (2021)
P.E. Hart, N.J. Nilsson, B. Raphael, A formal basis for the heuristic determination of minimum cost paths. IEEE Trans. Cybern. 4(2), 100–107 (1968)
S. Karaman, E. Frazzoli, Incremental sampling-based algorithms for optimal motion planning, in Robotics Science and Systems VI, 104(2) (2010), pp. 267–274
J.D. Gammell, S.S. Srinivasa, T.D. Barfoot, Batch informed trees (BIT*): sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs, in Proceeding of the 2015 IEEE International Conference on Robotics and Automation (ICRA) (2015), pp. 3067–3074
W. Wu, Temporal scheduling and optimization for multi-mav planning. PhD thesis (2019)
S.H. Arul, D. Manocha, DCAD: decentralized collision avoidance with dynamics constraints for agile quadrotor swarms. IEEE Robot. Autom. Lett. 5(2), 1191–1198 (2020)
Y. Su, J. Huang, Cooperative output regulation of linear multi-agent systems. IEEE Trans. Autom. Control 57(4), 1062–1066 (2011)
Y. Dong, Z. Lin, An event-triggered observer and its applications in cooperative control of multi-agent systems. IEEE Trans. Autom. Control 67(7), 3647–3654 (2022)
Y. Kuriki, T. Namerikawa, Formation control with collision avoidance for a multi-UAV system using decentralized MPC and consensus-based control. SICE J. Control Meas. Syst. Integr. 8(4), 285–294 (2015)
C. Chang, J. Shiau, Quadrotor formation strategies based on distributed consensus and model predictive controls. Appl. Sci. 8(11), 2246 (2018)
M. Ille, T. Namerikawa, Collision avoidance between multi-UAV-systems considering formation control using MPC, in 2017 IEEE International Conference on Advanced Intelligent Mechatronics (AIM) (2017), pp. 651–656
G. Cai, B.M. Chen, T.H. Lee, K. Lum, Comprehensive nonlinear modeling of a miniature unmanned helicopter. J. Am. Helicopter Soc. 57(1), 1–13 (2012)
G. Cai, B.M. Chen, T.H. Lee, Unmanned Rotorcraft Systems (Springer, Berlin, 2011)
S.K. Phang, System design methodology and implementation of micro-unmanned aerial vehicles. PhD thesis, National University of Singapore (2014)
R. Deits, R. Tedrake, Computing large convex regions of obstacle-free space through semidefinite programming, in Algorithmic Foundations of Robotics XI (Springer, Berlin, 2015), pp. 109–124
J. Alonso-Mora, E. Montijano, T. Nageli, O. Hilliges, M. Schwager, D. Rus, Distributed multi-robot formation control in dynamic environments. Auton. Robots 43(5), 1079–1100 (2019)
Z. Lin, Low Gain Feedback (Springer, London, 1999)
S. Lai, M. Lan, B.M. Chen, Model predictive local motion planning with boundary state constrained primitives. IEEE Robot. Autom. Lett. 4(4), 3577–3584 (2019)
Acknowledgements
We would like to thank Mr. Jihan Zhang of the Department of Mechanical and Automation Engineering, The Chinese University of Hong Kong, for his help in conducting the experiments.
Funding
This paper was supported by in part the Research Grants Council of Hong Kong SAR (Grant No: 14206821 and Grant No: 14217922), and in part by the Hong Kong Centre for Logistics Robotics (HKCLR).
Author information
Authors and Affiliations
Contributions
All the authors provided ideas for this paper. P. Zhou prepared the manuscript initially and performed all the steps of the proofs in this research. All authors read and approved the final manuscript.
Corresponding author
Ethics declarations
Ethics approval and consent to participate
Not Applicable.
Consent for publication
Not Applicable.
Competing interests
Prof. Ben M. Chen is an editorial board member for Autonomous Intelligent Systems and was not involved in the editorial review, or the decision to publish, this article. All authors declare that there are no other competing interests.
Additional information
Publisher’s Note
Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.
Appendix
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
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
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
which further implies
Thus, the input of the each rotorcraft satisfies the jerk constraint.
Substituting the control law (12) into the system (6) gives
whose compact form is
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
Since \(\zeta =(\mathcal{L}\otimes I_{9})\tilde{x}_{f}\), we have
Then, the derivative of the Lyapunov function is
Besides, since \(\mu \ge 1/(2\lambda _{\min}(\mathcal{L}))\), we have
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
and
Substituting (18), (19) and (20) into (17) gives
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\). □
Rights and permissions
Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article’s Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article’s Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecommons.org/licenses/by/4.0/.
About this article
Cite this article
Zhou, P., Lai, S., Cui, J. et al. Formation control of unmanned rotorcraft systems with state constraints and inter-agent collision avoidance. Auton. Intell. Syst. 3, 4 (2023). https://doi.org/10.1007/s43684-023-00049-3
Received:
Revised:
Accepted:
Published:
DOI: https://doi.org/10.1007/s43684-023-00049-3