Time Coordination and Collision Avoidance Using Leader-Follower Strategies in Multi-Vehicle Missions

Abstract

In recent years, the increasing popularity of multi-vehicle missions has been accompanied by a growing interest in the development of control strategies to ensure safety in these scenarios. In this work, we propose a control framework for coordination and collision avoidance in cooperative multi-vehicle missions based on a speed adjustment approach. The overall problem is decoupled in a coordination problem, in order to ensure coordination and inter-vehicle safety among the agents, and a collision-avoidance problem to guarantee the avoidance of non-cooperative moving obstacles. We model the network over which the cooperative vehicles communicate using tools from graph theory, and take communication losses and time delays into account. Finally, through a rigorous Lyapunov analysis, we provide performance bounds and demonstrate the efficacy of the algorithms with numerical and experimental results.

1. Introduction

Multi-vehicle cooperative missions have become the focus of many researches, because they allow for the completion of tasks in a faster, safer, and more efficient way than single vehicle missions. Therefore, many efforts have been devoted to developing control algorithms for such missions. Examples can be found in several different applications, such as logistic tasks in industrial warehouses, where Automated Guided Vehicles (AGV) transport and handle products [1]; search and rescue missions, where drones cooperatively search large portions of territory to aid people in distress [2,3]; and, surveillance, where UAVs gather images and information about specific targets or areas [4,5], etc.

In missions involving unmanned vehicles, safety is one of the main concerns and collision avoidance algorithms play an important role toward the safe execution of the tasks. Some of the techniques for collision avoidance found in the literature include (i) trajectory planning, where agents’ trajectories are planned to create collision-free paths [6,7,8]; (ii) velocity obstacle (VO) methods, where the set of velocities that will result in collisions is defined, and the vehicle’s velocity is adjusted accordingly [9,10,11]; (iii) machine learning based methods, such as deep reinforcement learning [12,13]; (iv) and game theory [14,15,16]. Many of the proposed solutions can only guarantee safety in a narrow range of scenarios (e.g., static or slow moving obstacles), and are not always well suited for multi-vehicle missions, as they do not ensure safety among agents.

Multi-vehicle missions pose an extra challenge, as they require control strategies to ensure safety against pop-up moving obstacles as well as inter-vehicle safety. In other words, when one vehicle deviates from its nominal trajectory in order to avert collision with an external obstacle, it must simultaneously consider the presence of cooperative vehicles sharing the same space. Some of the well known approaches that simultaneously address both of the requirements are based on potential fields, where agents are treated as charged particles and repel other cooperative or non-cooperative vehicles [17,18,19,20,21]. However, these techniques are often conservative, and they only provide guaranteed safety within small subsets of initial conditions and vehicle dynamics.

In this work, we consider scenarios in which it is undesirable for a vehicle to deviate from its nominal trajectory and, if possible, it is preferred to perform collision avoidance against moving obstacles by only changing the speed of the agents. Examples of such scenarios include, but are not limited to, air traffic management, where the direction taken by the aircraft is fixed and the speed can be varied within a feasibility range, and highway driving, where vehicles driving can vary their speeds while remaining in their lane. To address the problem at hand, the research group previously introduced a cooperative control framework for collision avoidance [22,23,24]. With this framework, safety is achieved by decoupling the cooperative collision avoidance problem into a coordination problem [25,26] to ensure inter-vehicle safety, and a collision-avoidance problem [23] to guarantee safety against dynamic obstacles. It is shown, using tools from graph theory and Lyapunov analysis, that safe operations are guaranteed under realistic assumptions on the communication network over which the vehicles communicate. Nevertheless, one main limitation of this work lies in the assumption that all of the agents are equipped with sensors that are able to measure position and speed of the obstacle in real-time. It is often more realistic to assume that only a subset of the vehicles (the leaders of the mission) have the necessary on-board equipment to acquire such information.

Concerning coordination, in [25], the authors introduce a time-critical coordination algorithm for the control of multiple vehicles. This time-coordination algorithm is based on a leader-follower structure and allows for a fleet of fixed-wing UAVs to coordinate with each other at a desired constant pace, known to the leaders only. One key limitation of the work in [25] lies in the assumption that the speed of the vehicles under consideration must be lower bounded by a positive constant. Therefore, making this algorithm not employable in missions involves vehicles that allow the existence of zero velocity vectors. In Refs. [26,27,28], the authors depart from [25], and propose a decentralized time-coordination algorithm that guarantees coordination for a broader class of vehicles. Coupled with trajectory generation methods, this time-coordination algorithm ensures spatial separation between the agents and, thus, guarantees inter-vehicle safety throughout the mission. On the other hand, in this work it is assumed that the coordination variables and the desired mission pace are known by all of the agents and not only to a subset of leaders.

Motivated by these ideas, we build on previous results on time-coordination [26,27,28,29], and propose a Lyapunov-based method to the cooperative collision avoidance problem. This work departs from previous solutions to this problem [22,23,24] by adopting a new time-coordination algorithm for inter-vehicle safety. The main novelty of this method concerns the structure of the control system. Conversely from the solutions that were proposed in [26,27,28], this algorithm relies on a leader-follower structure and can be applied to a wide range of autonomous missions. In the proposed scenario, once a possible collision with a non-cooperative obstacle is detected by mission’s leaders (or virtual leaders), these deviate from their nominal trajectory, while the followers, who have no information about the position of the obstacle, are also able to avoid the collision by coordinating with the leaders. Furthermore, we have extended the theoretical results in [24] and provided rigorous performance bounds. It is shown that the coordination and safety guarantees hold even in the presence of communication faults. When combined with trajectory generation algorithms [30,31] and trajectory tracking algorithms [25,32], the control laws that are proposed in this work provide a Cooperative Control Framework that enables safe operations for all agents. Figure 1 presents a schematic of the Cooperative Control Framework.

The paper is organized as follows: Section 2 presents the cooperative control framework that this work is based on. A formulation of the time-coordination problem is given in Section 3, while the collision-avoidance problem is stated in Section 4. A mathematical formulation of the proposed control laws is given in Section 5. Section 6 presents the main results, while the efficacy of the algorithms is demonstrated through the experimental results presented in Section 7. Finally, the conclusions are given Section 8.

2. Cooperative Control Framework

The cooperative control framework comprises (i) an offline optimal motion planning module that generates feasible paths for multiple vehicle missions, (ii) tracking algorithms that enable the vehicles to track virtual targets moving along these paths, (iii) time-coordination algorithms that adjust the rate of progression of the virtual target to follow a desired mission pace and to achieve inter-vehicle coordination, and (iv) collision-avoidance algorithms that adjust the desired pace of the mission to avert collisions with pop-up obstacles and uncooperative vehicles. The framework, which builds on the planning and control framework for multi-vehicle missions presented in previous work [33], is depicted in Figure 1 for the leader and follower, respectively. The present paper is focused on time-coordination and collision-avoidance. Nevertheless, for the sake of clarity and completeness, the remainder of this section introduces the optimal motion planning and virtual target tracking problems.

2.1. Optimal Motion Planning

Given a cooperative multi-vehicle mission involving N vehicles, the optimal motion planning problem can be defined as follows:

Problem 1

(Optimal motion planning). Compute N desired trajectories $p_i\left(t\right):[0,t_f]\to {\mathbb{R}}^d$, $d\in \{2,3\}$, $i\in \{1,2,\cdots ,N\}$, which minimize a given cost function, and satisfy boundary conditions, feasibility constraints, and mission-specific constraints.

In the above problem definition, the boundary constraints are the predefined initial and final conditions on trajectories, such as initial and final positions, speeds, and heading angles. The feasibility constraints include vehicle dynamics and input bounds, as well as constraints dealing with the avoidance of obstacles and spatial deconfliction between trajectories to avoid inter-vehicle collisions and ensure the safe simultaneous operation in a common airspace, i.e.,

$$\parallel p_i\left(t\right)-p_j\left(t\right)\parallel \ge d_{\mathrm{safe}},$$

(1)

for all $i,j\in \{1,\dots ,N\}$, $i\ne j$, with $d_{\mathrm{safe}}>0$. Finally, further mission-specific constraints, such as simultaneous or sequential arrival, can be imposed.

2.2. Virtual Target Tracking

Let the virtual time ${\gamma }_i\left(t\right)$, $t\ge 0$, be a temporal variable that is defined as follows:

$${\gamma }_i:{\mathbb{R}}^{+}\to [0,t_f],\forall i=1,\dots ,N.$$

(2)

Let $p_i\left({\gamma }_i\left(t\right)\right)$ be the virtual target that is assigned to the ith vehicle. Here, $p_i\left(t\right)$ is the ith trajectory generated at the motion planning level. With this definition, the virtual time is a variable, which conversely to the clock time, can be stretched or compressed to adjust the progression of the virtual target along the geometric path $p_i(·)$, in order to achieve some objectives, e.g., coordination or collision avoidance, as we will see later. Notice that, when ${\dot{\gamma }}_i\left(t\right)\equiv 1$, then the progression of the virtual time is equal to the progression of the clock time.

On the other hand, ${\dot{\gamma }}_i\left(t\right)>1$ implies that the virtual target that is assigned to the ith vehicle is moving faster than the nominal trajectory generated by the trajectory generation algorithm. Similarly, ${\dot{\gamma }}_i\left(t\right)<1$ implies a slower execution of the mission. With this setup, let the equations of motion of the ith vehicle be given by

$$\left\{\begin{array}{c}{\dot{\hat{x}}}_i\left(t\right)=f_i({\hat{x}}_i\left(t\right),{\hat{u}}_i\left(t\right)),{\hat{x}}_i\left(0\right)={\hat{x}}_{i,0}\hfill \\ {\hat{p}}_i\left(t\right)=g_i\left({\hat{x}}_i\left(t\right)\right),\hfill \end{array}\right.$$

(3)

where ${\hat{p}}_i\left(t\right)$ is the actual ith vehicle’s position, ${\hat{x}}_i$ is the state of the vehicle (e.g., position, attitude, velocity, etc.), ${\hat{u}}_i\left(t\right))$ is the control input, and $f_i(·)$ and $g_i(·)$ are vectors of nonlinear functions describing the vehicle’s dynamics. Subsequently, the objective of the virtual target tracking problem can be stated as follows:

Problem 2

(Virtual target tracking). Design a control law for ${\hat{u}}_i\left(t\right)$ such that the virtual target tracking error

$$x_{TT,i}\left(t\right)\triangleq p_i\left({\gamma }_i\left(t\right)\right)-{\hat{p}}_i\left(t\right)$$

(4)

converges to a neighborhood of zero.

Finally, feasibility constraints must be imposed to ensure that the problem above can be solved. In addition to the vehicles’ dynamics constraints, which are taken into account in Problem 1, it is necessary to ensure that the bounds on the dynamics of the virtual time are enforced, so that the virtual targets can be tracked by the agents. In particular, a virtual target can be tracked by the ith vehicle if its trajectory $p_i\left(t\right)$, $i=1,\dots ,N$ is a feasible solutions of Problem 1, and the derivatives of its virtual time satisfy

$$\begin{array}{c}1-{\dot{\gamma }}_{i,\max }\le {\dot{\gamma }}_i\left(t\right)\le 1+{\dot{\gamma }}_{i,\max },\end{array}$$

(5)

$$\begin{array}{c}|{\ddot{\gamma }}_i\left(t\right)|\le {\ddot{\gamma }}_{i,\max },\end{array}$$

(6)

with $0<{\dot{\gamma }}_{i,\max }<1$ and ${\ddot{\gamma }}_{i,\max }>0$.

As it will become clear later, the dynamics of the virtual time (in particular, its second derivative ${\ddot{\gamma }}_i\left(t\right)$) are explicitly used as control input to achieve time coordination and collision avoidance. For this reason, when deriving control laws for ${\ddot{\gamma }}_i\left(t\right)$, the bounds in (6) will be taken into consideration.

3. Time Coordination Problem

Recall that, at time t, the virtual target to be tracked by ith vehicle is given by $p_i\left({\gamma }_i\left(t\right)\right)$, where the path $p_i(·)$ is given by the optimal motion planning algorithm, and ${\gamma }_i\left(t\right)$ is the virtual time. The virtual time and its derivatives play an important role in defining the time-coordination problem. In fact, because the virtual target is parameterized by ${\gamma }_i\left(t\right)$, and the trajectories $p_i\left(t\right)$ satisfy spatial deconfliction constraints (see Equation (1)) and mission-specific requirements (e.g., simultaneous time of arrival, see Section 2), we say that, at time t, the vehicles are coordinated and inter-vehicle safety is guaranteed, if

$${\gamma }_i\left(t\right)={\gamma }_j\left(t\right),i,j\in \{1,2,\cdots ,N\}.$$

(7)

Furthermore, they are all progressing at the same desired pace, if

$${\dot{\gamma }}_i\left(t\right)={\dot{\gamma }}_d\left(t\right),i\in \{1,2,\cdots ,N\},$$

(8)

where ${\dot{\gamma }}_d\left(t\right)>0$ represents the desired pace of the mission. We recall that ${\dot{\gamma }}_d\left(t\right)=1$ means that the vehicles are required to proceed along the assigned trajectories at the rate that was established at the motion planning level.

As it will be clear later, the desired pace of the mission ${\dot{\gamma }}_d\left(t\right)$ is used as an extra degree of freedom by the collision-avoidance algorithm to adjust the rate of the mission in order to avert collision with obstacles, and it is set to 1 if no obstacle is present.

With this setup, the time-coordination problem can be defined as that of designing control laws for ${\ddot{\gamma }}_i\left(t\right)$ that ensure the satisfaction of Equations (7) and (8). To achieve this objective, information between the vehicles must be exchanged during the mission execution. Thus, we first state a set of assumptions that the communication network, over which the vehicles communicate, must satisfy in order to provide a mathematical formulation of the problem at hand. In what follows, tools from graph theory are used to formulate these assumptions. In particular, consider the Laplacian matrix for the time-varying communication graph $\Gamma \left(t\right)$, $L\left(t\right)\in {\mathbb{R}}^{N\times N}$, and let $Q\in {\mathbb{R}}^{(N-1)\times N}$ be a matrix that satisfies the following:

$$Q{1}_N=0,Q{Q}^{\top }=I_{N-1},Q^{\top }Q=I_N-\frac{1}{N}{1}_N{1}_N^{\top }.$$

(9)

Remark 1.

In [25], the existence of a matrix Q that satisfies the above properties is demonstrated. An iterative procedure to compute the matrix Q is presented in [26].

Let $\overline{L}\left(t\right)=QL\left(t\right)Q^{\top }$. The matrix $\overline{L}\left(t\right)$ has the same eigenvalues as the Laplacian matrix, but without the first eigenvalue ${\lambda }_1=0$. The following is assumed in terms of the communication graph.

Assumption 1.

• The ith vehicle can only communicate with a neighboring set of vehicles, here referred to as${\mathcal{N}}_i$.
• The communication between the vehicles is bidirectional and with no time delays.
• The connectivity of the communication graph $\Gamma \left(t\right)$ satisfies the Persistency of Excitation (PE)-like condition:

  $${\int }_t^{t+T}\overline{L}\left(\tau \right)d\tau \ge \mu I_{N-1},t\ge 0,$$

  (10)
  with $T>0$ and $\mu =(0,1]$.

Notice that the condition given by Equation (10) implies that the graph $\Gamma \left(t\right)$ must be connected in an integral sense, therefore allowing the graph to be disconnected point-wise in time. In this sense, the assumption above captures packet dropouts, communication losses, and switching topologies.

Finally, let $N_L\le N$ be the number of vehicles elected as leaders (and $N-N_L$ the number of followers). We assume that the desired mission pace ${\dot{\gamma }}_d\left(t\right)$ is known to the leaders only, e.g., vehicles $k=\{1,\dots ,N_L\}$. Thus, one of the main challenges is ensuring the satisfaction of Equation (8) for all $i\in \{1,2,\cdots ,N_L,\cdots ,N\},$ with the desired pace ${\dot{\gamma }}_d\left(t\right)$ only being known to a subset of vehicles.

Letting $\gamma \left(t\right)={[{\gamma }_1\left(t\right),\dots {\gamma }_N\left(t\right)]}^{\top }$, $\dot{\gamma }\left(t\right)={[{\dot{\gamma }}_1\left(t\right),\dots {\dot{\gamma }}_N\left(t\right)]}^{\top }$, we define the time coordination errors as

$$\xi \left(t\right)=Q\gamma \left(t\right),z\left(t\right)=\dot{\gamma }\left(t\right)-{\dot{\gamma }}_d\left(t\right)1_N.$$

(11)

Notice that $\xi \left(t\right)=0$ implies that ${\gamma }_i\left(t\right)={\gamma }_j\left(t\right)$ (see Equation (9)). Subsequently, the time-coordination problem can be formulated, as follows.

Problem 3

(Time coordination). Consider a cooperative mission with N vehicles. Assume that the vehicles are equipped with optimal motion planning and virtual target tracking algorithms that solve Problems 1 and 2, respectively. Finally, assume that the vehicles are supported by a communication network that satisfies Assumption 1, and that the desired mission pace is known to a subset of $N_L\le N$ vehicles only (leaders). Then, the objective of time-coordination is to design control laws for ${\ddot{\gamma }}_i\left(t\right)$, for all $i\in \{1,\dots ,N\}$, such that the time-coordination errors $\xi \left(t\right)$ and $z\left(t\right)$ defined in Equation (11) converge to a neighborhood of zero.

4. Collision Avoidance Problem

Consider a non-cooperative vehicle (or an obstacle) with nominal position ${\hat{p}}_o\left(t\right)$ and speed satisfying $\parallel {\dot{\hat{p}}}_o\left(t\right)\parallel >0$ for all $t\ge 0$. Furthermore, let there be values of the virtual time ${\gamma }_{*,i}\in [0,t^{\mathrm{f}}]$ and (clock) time $t_{*}\in [0,\infty )$, such that

$$\parallel p_i\left({\gamma }_{*,i}\right)-{\hat{p}}_o\left(t_{*}\right)\parallel \le d_{\mathrm{safe}},$$

(12)

with $d_{\mathrm{safe}}>0$. We recall that the planned position of the ith vehicle is given by $p_i\left({\gamma }_i\left(t\right)\right)$. Thus, Equation (12) implies that a collision between the obstacle and vehicle i occurs if ${\gamma }_i\left(t_{*}\right)\approx {\gamma }_{*,i}$, and it is the objective of the collision-avoidance algorithm to ensure that this condition is never met. Moreover, in order to take possible speed variations in the obstacle’s trajectory into consideration, let its trajectory be reparameterized using the virtual time variable ${\gamma }_o\left(t\right)$. Similarly to the vehicles’ trajectories, ${\dot{\gamma }}_o\left(t\right)=1$ indicates that the obstacle is moving according to its nominal trajectory. Subsequently, the trajectory of the non-cooperative vehicle can be rewritten as

$$p_o\left({\gamma }_o\left(t\right)\right)={\hat{p}}_o({\gamma }_o\left(t\right)-{\gamma }_{*,i}+t_{*}),{\gamma }_o\left(t\right)\in [{\gamma }_o\left(0\right),\infty ).$$

(13)

Assuming that the instantaneous speed of the obstacle can be measured (no such assumptions are made for the acceleration),the pace of the obstacle can be characterized as the ratio between the obstacle’s actual and nominal speed. In particular, we have

$${\dot{\gamma }}_o\left(t\right)=\frac{\parallel {\dot{p}}_o\left(t\right)\parallel }{∥\frac{d{\hat{p}}_o(\gamma -{\gamma }_{*,i}+t_{*})}{d\gamma }{|}_{\gamma ={\gamma }_o\left(t\right)}∥},{\gamma }_o\left(0\right)={\gamma }_{*,i}-t_{*}.$$

(14)

With this setup, assumptions on the dynamics of the obstacle can be formulated in terms of its virtual time. In particular, the following is assumed about the obstacle’s speed and acceleration:

$$\begin{array}{c}1-{\dot{\gamma }}_{o,\max }\le {\dot{\gamma }}_o\left(t\right)\le 1+{\dot{\gamma }}_{o,\max },\\ |{\ddot{\gamma }}_o\left(t\right)|\le {\ddot{\gamma }}_{o,\max },\end{array}$$

(15)

for some ${\dot{\gamma }}_{o,\max }$ and ${\ddot{\gamma }}_{o,\max }$.

Finally, we define

$${\gamma }_{\delta ,i}\left(t\right)={\gamma }_i\left(t\right)-{\gamma }_o\left(t\right),$$

as the difference in virtual time between the ith vehicle and the obstacle. From (13), it follows that if ${\gamma }_{\delta ,i}\left(t\right)=0$, with ${\gamma }_i\left(t\right)={\gamma }_o\left(t\right)={\gamma }_{*,i}$, then $\parallel p_i\left({\gamma }_i\left(t\right)\right)-p_o\left({\gamma }_o\left(t\right)\right)\parallel \le d_{\mathrm{safe}}$, where $p_i\left({\gamma }_i\left(t\right)\right)$ is the trajectory of the ith vehicle and $d_{\mathrm{safe}}$ is the required safety distance between the vehicle and the obstacle. Instead, keeping $|{\gamma }_{\delta ,i}\left(t\right)|\gg 0$ ensures that the vehicle and obstacle will reach the impact location at different times, therefore avoiding a collision. Thus, ${\gamma }_{\delta ,i}\left(t\right)\ne 0$ is a sufficient condition for ensuring safety. With this setup, we assume the existence of a minimum virtual time separation that guarantees collision avoidance. This is summarized in the following two cases:

1.

There exists ${\gamma }_{\mathrm{safe}}>0$ such that, if

$${\gamma }_{\delta ,i}\left(t\right)={\gamma }_i\left(t\right)-{\gamma }_o\left(t\right)\ge {\gamma }_{\mathrm{safe}},$$

(16)

for all ${\gamma }_i\left(t\right)\in [0,t_{\mathrm{f}}]$, then

$$\parallel p_i\left({\gamma }_i\left(t\right)\right)-p_o\left({\gamma }_o\left(t\right)\right)\parallel >d_{\mathrm{safe}},{\gamma }_i\left(t\right)\in [0,t_{\mathrm{f}}].$$

2.

There exists ${\gamma }_{\mathrm{safe}}<0$ such that, if

$${\gamma }_{\delta ,i}\left(t\right)={\gamma }_i\left(t\right)-{\gamma }_o\left(t\right)\le {\gamma }_{\mathrm{safe}},$$

(17)

for all ${\gamma }_i\left(t\right)\in [0,t_{\mathrm{f}}]$, then

$$\parallel p_i\left({\gamma }_i\left(t\right)\right)-p_o\left({\gamma }_o\left(t\right)\right)\parallel >d_{\mathrm{safe}},{\gamma }_i\left(t\right)\in [0,t_{\mathrm{f}}].$$

Recall, from Section 3, that the rate of progression of the virtual times ${\gamma }_i\left(t\right)$, $i\in \{1,\dots ,N\}$ (and, thus, the speed of the vehicles) can be controlled by adjusting ${\dot{\gamma }}_d\left(t\right)$. In fact, the use of the desired mission pace ${\dot{\gamma }}_d\left(t\right)$ becomes important in the presence of external obstacles to ensure that conditions (16) or (17) are satisfied for all $t\ge 0$. This leads to the formulation of the collision avoidance problem.

Problem 4

(Collision avoidance). Consider a cooperative mission with N vehicles. Assume that the vehicles are equipped with optimal motion planning and virtual target tracking algorithms that solve Problems 1 and 2, respectively, and with a time-coordination algorithm that solves Problem 3. Assume the presence of an obstacle with dynamics that satisfy the bounds in Equation (15) for some ${\dot{\gamma }}_{o,\max }$ and ${\ddot{\gamma }}_{o,\max }$. Subsequently, the objective of the collision-avoidance algorithm is to design control laws for ${\dot{\gamma }}_d\left(t\right)$, such that the safety conditions that are given by Equations (16) or (17) are satisfied for all $t\ge 0$.

Remark 2.

In this paper, we only consider the scenario in which one obstacle is present.

Remark 3.

In practice, the leaders can be "virtual", e.g., ground stations. Therefore, we believe that it is reasonable to assume that most of the computations and sensing necessary to guarantee collision avoidance can be performed by the leaders.

5. Control Law

This section provides a control strategy to solve the time-coordination and collision-avoidance problems.

5.1. Time-Coordination Control Law

To solve Problem 3, we let the evolution of the virtual time (for leaders and followers, respectively) be governed by:

$$\begin{array}{cc}\hfill & {\ddot{\gamma }}_i\left(t\right)=-b\left({\dot{\gamma }}_i\left(t\right)-{\dot{\gamma }}_d\left(t\right)\right)-a{\displaystyle \sum _{j\in {\mathcal{N}}_i}}({\gamma }_i\left(t\right)-{\gamma }_j\left(t\right)),\hfill \\ \hfill & i\in \{1,\dots ,N_L\},\hfill \\ \hfill & {\ddot{\gamma }}_i\left(t\right)=-b\left(\dot{{\gamma }_i}\left(t\right)-{\chi }_{I,i}\left(t\right)\right)-a{\displaystyle \sum _{j\in {\mathcal{N}}_i}}({\gamma }_i\left(t\right)-{\gamma }_j\left(t\right)),\hfill \\ \hfill & {\dot{\chi }}_{I,i}\left(t\right)=-k{\displaystyle \sum _{j\in {\mathcal{N}}_i}}({\gamma }_i\left(t\right)-{\gamma }_j\left(t\right)),i\in \{N_L+1,\dots ,N\},\hfill \end{array}$$

(18)

$$\begin{array}{c}{\gamma }_i\left(0\right)=0,{\dot{\gamma }}_i\left(0\right)=1,{\chi }_{I,i}\left(0\right)=1,\end{array}$$

where a, b, and k are the positive coordination control gains and ${\dot{\gamma }}_d\left(t\right)$ is a desired mission pace satisfying

$$\begin{array}{c}1-{\dot{\gamma }}_{d,\max }\le {\dot{\gamma }}_d\left(t\right)\le 1+{\dot{\gamma }}_{d,\max },\end{array}$$

(19)

$$\begin{array}{c}|{\ddot{\gamma }}_d\left(t\right)|\le {\ddot{\gamma }}_{d,\max },\end{array}$$

(20)

for some ${\dot{\gamma }}_{d,\max },{\ddot{\gamma }}_{d,\max }>0$. The desired pace ${\dot{\gamma }}_d\left(t\right)$ will be defined later when introducing the collision avoidance algorithm, and can now be considered as a desired time-varying value that is to be tracked by ${\dot{\gamma }}_i\left(t\right),i=1,\dots ,N$.

The time-coordination control law can also be written in matrix form as

$$\begin{array}{cc}\hfill & \ddot{\gamma }\left(t\right)=-aL\gamma \left(t\right)-b\left[\begin{array}{c}{D}^{\top }\dot{\gamma }\left(t\right)-{\dot{\gamma }}_d\left(t\right)1_{N_L}\\ C^{\top }\dot{\gamma }\left(t\right)-{\chi }_I\left(t\right)\end{array}\right]\hfill \\ \hfill & {\dot{\chi }}_I\left(t\right)=-k{C}^{\top }L\gamma \left(t\right),\hfill \end{array}$$

(21)

$$\begin{array}{c}\gamma \left(0\right)=0_N,\dot{\gamma }\left(0\right)=1_N,{\chi }_I\left(0\right)=1_{N-N_L},\end{array}$$

where

$$C^{\top }=\left[\begin{array}{cc}0& I_{N-N_L}\end{array}\right],$$

$$D^{\top }=\left[\begin{array}{cc}{I}_{N_L}& 0\end{array}\right].$$

The objective of the time-coordination control law is to ensure that Equations (7) and (8) are satisfied at all times, even if the desired mission pace $\dot{{\gamma }_d}\left(t\right)$ is known by the leaders only, as was mentioned earlier. For this reason, we introduce the term ${\chi }_I\left(t\right)$, which plays the role of an integral term ensuring satisfaction of Equation (8).

5.2. Collision-Avoidance Control Law

To solve Problem 4, the proposed collision-avoidance method uses ${\dot{\gamma }}_d\left(t\right)$ as a control input to appropriately adjust the speed of the mission. We formulate control laws based on two cases.

5.2.1. Case 1: Collision Avoidance by Setting ${\gamma }_{\delta ,i}\left(t\right)>{\gamma }_{\mathrm{safe}}$

In this scenario, the collision between the ith vehicle and an obstacle can be avoided by maintaining the virtual time ${\gamma }_i\left(t\right)$ larger than ${\gamma }_o\left(t\right)$ by a value that is greater or equal to ${\gamma }_{\mathrm{safe}}$. Therefore, we let ${\dot{\gamma }}_d\left(t\right)$ be governed by

$${\dot{\gamma }}_d\left(t\right)=1+\left\{\begin{array}{cc}0,\hfill & \omega \left(t\right)<1-\eta ,\hfill \\ \left(\omega \right(t)-1)\rho \left(\omega \right(t\left)\right),\hfill & 1-\eta \le \omega \left(t\right)\le 1,\hfill \\ \omega \left(t\right)-1,\hfill & \omega \left(t\right)>1,\hfill \end{array}\right.$$

(22)

where

$$\omega \left(t\right)=\frac{{\gamma }_{\mathrm{safe}}+ϵ}{{\gamma }_{\delta ,i}\left(t\right)}{\dot{\gamma }}_o\left(t\right),$$

$ϵ>0$ is a design parameter, and $\eta >0$ is a small positive number. Lastly, $\rho :[1-\eta ,1]\to [0,1]$ is a smooth function (an example for $\rho$ is $\rho \left(x\right)=1-3{\left(\frac{x-1}{\eta }\right)}^2-2{\left(\frac{x-1}{\eta }\right)}^3$) that satisfies

$$\begin{array}{c}\rho (1-\eta )=0,\frac{d\rho (1-\eta )}{d\omega }=0,\\ \rho \left(1\right)=1,\frac{d\rho (1-\eta )}{d\omega }=0.\end{array}$$

In the remainder of this section, we describe the physical meaning of the proposed control laws. We note that, in (22), ${\dot{\gamma }}_d\left(t\right)$ is a $C^1$-continuous approximation of a max function and satisfies

$${\dot{\gamma }}_d\left(t\right)\approx max\left\{1,\omega \left(t\right)\right\}=max\left\{1,\frac{{\gamma }_{\mathrm{safe}}+ϵ}{{\gamma }_{\delta ,i}\left(t\right)}{\dot{\gamma }}_o\left(t\right)\right\}.$$

(23)

This is motivated by the fact that ${\gamma }_{\delta ,i}\left(0\right)$ is initially already greater than ${\gamma }_{\mathrm{safe}}$. Thus, the pace of the mission may need to be increased, but never decreased. To this end, ${\dot{\gamma }}_d\left(t\right)$ is defined such that the right hand side of (23) is guaranteed to be always greater or equal to 1.

In the case in which $\omega \left(t\right)\ge 1$, the pace of the mission is adjusted as

$${\dot{\gamma }}_d\left(t\right)\approx \omega \left(t\right)=\frac{{\gamma }_{\mathrm{safe}}+ϵ}{{\gamma }_{\delta ,i}\left(t\right)}{\dot{\gamma }}_o\left(t\right).$$

This implies that, if ${\gamma }_{\delta ,i}\left(t\right)<{\gamma }_{\mathrm{safe}}+ϵ$, the proposed control law ensures that ${\dot{\gamma }}_d\left(t\right)$ (and, consequently, ${\dot{\gamma }}_i\left(t\right)$) are greater than ${\dot{\gamma }}_o\left(t\right)$, which, in turn, cause ${\gamma }_{\delta ,i}\left(t\right)$ to increase.

Conversely, using the fact that ${\dot{\gamma }}_d\left(t\right)$ is bounded from below (see Equation (23)), if ${\gamma }_{\delta ,i}\left(t\right)>{\gamma }_{\mathrm{safe}}+ϵ$, then ${\dot{\gamma }}_d\left(t\right)\approx \omega \left(t\right)<{\dot{\gamma }}_o\left(t\right)$, which implies that ${\dot{\gamma }}_d\left(t\right)$ is guaranteed to remain in a neighbourhood of 1.

5.2.2. Case 2: Collision Avoidance by Setting ${\gamma }_{\delta ,i}\left(t\right)<{\gamma }_{\mathrm{safe}}$

In this case, the vehicle can avoid the obstacle by maintaining ${\gamma }_i\left(t\right)$ lower than ${\gamma }_o\left(t\right)$ by a value lower or equal to ${\gamma }_{\mathrm{safe}}$. The pace of the mission may need to be decreased to achieve this.

The proposed control law is given, as follows:

$${\dot{\gamma }}_d\left(t\right)=1+\left\{\begin{array}{cc}\omega \left(t\right)-1,\hfill & \omega \left(t\right)<1,\hfill \\ \left(\omega \right(t)-1)\rho \left(\omega \right(t\left)\right),\hfill & 1\le \omega \left(t\right)\le 1+\eta ,\hfill \\ 0,\hfill & 1+\eta <\omega \left(t\right),\hfill \end{array}\right.$$

where $\omega \left(t\right)$ is now defined as

$$\omega \left(t\right)=\frac{{\gamma }_{\delta ,i}\left(t\right)}{{\gamma }_{\mathrm{safe}}-ϵ}{\dot{\gamma }}_o\left(t\right),$$

$ϵ>0$ is a design parameter and $\eta$ is a small positive number. Finally, $\rho :[1,1+\eta ]\to [0,1]$ is a smooth function that satisfies

$$\begin{array}{c}\rho \left(1\right)=1,\frac{d\rho \left(1\right)}{d\omega }=0,\\ \rho (1+\eta )=0,\frac{d\rho (1+\eta )}{d\omega }=0.\end{array}$$

6. Main Results

6.1. Time Coordination

Recall that the time-coordination objective is captured by the error variables $\xi \left(t\right)$ and $z\left(t\right)$ defined in Equation (11), and the time-coordination control law is given by Equation (21). Let us define the time-coordination error vector, as follows:

$$x_{TC}\left(t\right)={[\xi {\left(t\right)}^{\top },z{\left(t\right)}^{\top },{({\chi }_I\left(t\right)-{\dot{\gamma }}_d\left(t\right)1_{N-N_L})}^{\top }]}^{\top }.$$

Subsequently, the following theorem summarizes one of the main results of this paper.

Theorem 1.

Consider a cooperative mission involving N vehicles, with $N_L$ vehicles elected as leaders. Let the vehicles be equipped with optimal motion planning and virtual target tracking algorithms that solve Problems 1 and 2, respectively. Assume that the vehicles are supported by a communication network that satisfies Assumption 1, and that the desired mission pace ${\dot{\gamma }}_d\left(t\right)$ is known to a subset of $N_L\le N$ vehicles only (leaders). Let the evolution of the virtual time be governed by the control law that is given by (21). Let the time coordination error vector at time $t=0$, i.e., $x_{TC}\left(0\right)$, and the dynamic limits of the desired mission pace (see Equations (19) and (20)) satisfy

$${\dot{\gamma }}_{d,max}<{\dot{\gamma }}_{max},$$

(24)

and

$$\begin{array}{c}\hfill max\left\{\parallel x_{TC}\left(0\right)\parallel ,{\ddot{\gamma }}_{d,\max }\right\}<min\left\{\frac{{\dot{\gamma }}_{max}-{\dot{\gamma }}_{d,max}}{{\kappa }_1+{\kappa }_2},\frac{{\ddot{\gamma }}_{max}}{3b{\kappa }_1+3b{\kappa }_2+1}\right\},\end{array}$$

(25)

for given ${\dot{\gamma }}_{max},{\ddot{\gamma }}_{max}>0$ and for some positive constants ${\kappa }_1$ and ${\kappa }_2$. Subsequently:

1.

There exist time-coordination control gains a, b and k, such that the time-coordination error vector is bounded by

$$\begin{array}{cc}\hfill \left|\right|x_{TC}\left(t\right)\left|\right|\le & {\kappa }_1\left|\right|x_{TC}\left(0\right)\left|\right|e^{-{\lambda }_{TC}t}+{\kappa }_2{\displaystyle \underset{t\ge 0}{sup}}\left(\right|{\ddot{\gamma }}_d\left(t\right)\left|\right),\hfill \end{array}$$

(26)

with rate of convergence satisfying

$${\lambda }_{TC}<\frac{N_L}{N}\frac{1}{4b}\frac{\mu }{T{\left(1+\frac{a}{b}NT\right)}^2}.$$

2.

The feasibility bounds that are introduced in Equations (5) and (6) are never violated.

Proof. 

The proof of Theorem 1 is given in Appendix A, while a derivation of the dynamics is given in Appendix C. □

Remark 4.

For the sake of simplicity, in this paper we assume that the vehicles are able to perfectly track their virtual targets, i.e., $x_{TT,i}\left(t\right)=0$ (see Equation (4)). However, if this is not true, it can be demonstrated that similar coordination results can be achieved by adding an additional term in Equation (22) in order to take the tracking error into account. We refer the reader to [24], where the problem of coordination in the presence of tracking errors is addressed.

Remark 5.

We note that the rate of convergence is directly affected by the total number of vehicles, the number of leaders, and by the quality of the communication network (i.e., the parameters μ and T). Coordination is achieved with a slower rate of convergence in the case of switching topologies, dropouts, and communication delays, as emphasized by Theorem 1.

6.2. Collision Avoidance

The second and final result of this paper is summarized in the following theorem.

Theorem 2.

Let the desired pace of the mission ${\dot{\gamma }}_d\left(t\right)$ be governed by Equation (22). Let the dynamics of the obstacle satisfy the following

$$\begin{array}{c}{\dot{\gamma }}_{o,max}\le \frac{{\gamma }_{safe}}{{\gamma }_{safe}+ϵ}\left(1+{\dot{\gamma }}_{max}-\frac{{\gamma }_{safe}+ϵ}{{\gamma }_{safe}}\right),\\ {\ddot{\gamma }}_{var}\le {\ddot{\gamma }}_{max},\end{array}$$

where

$$\begin{array}{c}{\kappa }_1\parallel x_{TC}\left(0\right)\parallel +{\kappa }_2\underset{t^{\prime }\in [0,t^{\alpha }]}{\sup }\left(\right|{\ddot{\gamma }}_{d,max}\left(t^{\prime }\right)\left|\right)\le ϵ\le {\gamma }_{safe}{\dot{\gamma }}_{max},\\ 0\le \left|\right|x_{TC}\left(0\right)\left|\right|\le \frac{{\dot{\gamma }}_{max}-{\kappa }_2{\ddot{\gamma }}_{var}}{{\kappa }_1},\end{array}$$

with ${\ddot{\gamma }}_{\mathrm{var}}$ given in Equation (A19). Moreover, assume that the collision avoidance error at time $t=0$ satisfies ${\gamma }_{\delta ,i}\left(0\right)>{\gamma }_{\mathrm{safe}}$, with ${\gamma }_{\mathrm{safe}}$ satisfying

$${{\gamma }_{safe}}^2-\frac{\overline{\eta }{\ddot{\gamma }}_{d,max}(1+{\dot{\gamma }}_{o,max})}{{\ddot{\gamma }}_{max}}{\gamma }_{safe}-\frac{\overline{\eta }{\ddot{\gamma }}_{d,max}(1+{\dot{\gamma }}_{o,max})ϵ}{{\ddot{\gamma }}_{max}}>0.$$

(27)

Then:

1.

Collision avoidance is guaranteed for all $t\ge 0$, i.e.

$${\gamma }_{\delta ,i}\left(t\right)\ge {\gamma }_{safe},t\in [0,\infty ).$$

(28)

2.

The dynamic limits of ${\gamma }_d\left(t\right)$, namely ${\dot{\gamma }}_{d,max}$ and ${\ddot{\gamma }}_{d,max}$, satisfy the bounds given by Equations (24) and (25).

Proof. 

Appendix B provides the proof of Theorem 2. □

Remark 6.

We notice that the second result above—Theorem 2—guarantees that the dynamics of the desired mission pace never violate the assumptions of Theorem 1. In turn, Theorem 2 implies that time coordination and collision avoidance are both guaranteed with the control laws given by Equations (21)—coordination—and (22)—collision avoidance.

Remark 7.

For the sake of brevity, Theorem 2 has only focused on the control law of Section 5.2.1, i.e., case 1. However, identical results can be concluded for the control law of Section 5.2.2, i.e., case 2.

7. Experimental Results

7.1. Time Coordination

The time-coordination algorithm is demonstrated with the use of two AR Drones 2.0 [34]. The drones need to follow a circular trajectory with a radius of 1.5 m, such that they maintain a constant phase shift between each other and are able to guarantee inter-vehicle safety. The total time of the mission is $t_f=60$ s and ${\dot{\gamma }}_{i,max}=0.25$, ${\dot{\gamma }}_{d,max}=0.2$, ${\ddot{\gamma }}_{i,max}=0.1$ and ${\ddot{\gamma }}_{d,max}=0.1$.

The trajectories are generated with the use of the trajectory generation algorithm that is described in Section 2.1, and they are formulated as follows:

$$\begin{array}{cc}\hfill & x_1\left({\gamma }_1\left(t\right)\right)=1.5cos\left(0.15{\gamma }_1\left(t\right)\right),\hfill \\ \hfill & y_1\left({\gamma }_1\left(t\right)\right)=1.5sin\left(0.15{\gamma }_1\left(t\right)\right),\hfill \\ \hfill & x_2\left({\gamma }_2\left(t\right)\right)=1.5cos(0.15{\gamma }_2\left(t\right)+\pi ),\hfill \\ \hfill & y_2\left({\gamma }_2\left(t\right)\right)=1.5sin(0.15{\gamma }_2\left(t\right)+\pi ).\hfill \end{array}$$

(29)

The drones are enabled to follow their desired orbits by virtue of a simple PID-based trajectory tracking algorithm (see [26]). Finally, the quadrotors are equipped with the time-coordination algorithm that is proposed in this paper (see Equation (21)). The virtual target tracking and time-coordination algorithms are implemented on MATLAB Simulink, and run on a Lenovo ThinkPad P52 laptop with an Intel 8th generation i7 core. Real-time position measurements are performed using Vicon Motion Capture, and communicated to the controllers using the MATLAB ROS Toolbox. Finally, commands velocities in the x, y, and z directions are sent to the drones through ROS.

For this mission scenario, the drones are required to maintain a phase shift of $\pi$ rad. Vehicle 1 is elected as leader while vehicle 2 is the follower. The drones initially start moving according to a desired mission pace of ${\dot{\gamma }}_d=1$, but, as can be seen from Figure 2, the pace of the mission is decreased twice during the course of the mission. In fact, at $t\approx 23$ s, the pace is decreased to ${\dot{\gamma }}_d\left(t\right)=0.9$, and at $t\approx 44s$ it is decreased again to ${\dot{\gamma }}_d\left(t\right)=0.8$. The value of ${\dot{\gamma }}_d$ is only communicated to the leader, but the time-coordination algorithm ensures that the pace of the follower also converges to the desired pace of the mission. Furthermore, Figure 3 demonstrates the time-coordination, which shows the convergence of the virtual times to the same values, i.e., ${\gamma }_1\left(t\right)={\gamma }_2\left(t\right)$. From the definition of the desired drones’ trajectories described in (29), it follows that, if ${\gamma }_1\left(t\right)={\gamma }_2\left(t\right)$, then the desired phase shift between the drones is maintained. This can also be noticed from Figure 4. Here, the positions of the drones at three different time instances are shown. It can be seen that the drones are able to maintain a phase coordination throughout the mission.

7.2. Collision Avoidance

The efficacy of the collision-avoidance algorithm is demonstrated with experimental results involving three Turtlebot3 Burger ground robots [35] and one AR Drone 2.0. The mission requires the ground robots to cooperatively move along a circular trajectory with a radius of 1.5 m, while the drone is hovering. The drone is elected as a (virtual) leader while the robots are followers. Figure 5 shows the experimental setup for this mission, which is identical to the one used for the previous experiment.

The agents’ trajectories are planned offline, such that a minimum safety distance $d_{\mathrm{safe}}=0.4$ m is maintained at all time, and ${\gamma }_{\mathrm{safe}}=-2$ s, $t_f=80s$, ${\dot{\gamma }}_{i,max}=0.2$, ${\dot{\gamma }}_{d,max}=0.2$, ${\ddot{\gamma }}_{i,max}=0.1$ and ${\ddot{\gamma }}_{d,max}=0.1$.

The evolution of the mission can be seen in Figure 6. In the considered scenario, a dynamic obstacle is moving towards the vehicles. At $t\approx 15$ s, the obstacle starts decreasing its speed and now poses a threat to ground robot 1, which is shown by the blue circle in Figure 6. However, all three ground robots are able to avoid the collision, as it can be seen in the second panel of Figure 6, by virtue of the collision-avoidance algorithm. This can be better seen in Figure 7. The figure on the left shows the distance between the ground robots and the obstacle. It can be noticed that all the vehicles are able to successfully avoid the obstacle. However, this is not true in the case in which the collision-avoidance algorithm was not used, as shown by the light blue line. Furthermore, the right panel of Figure 7 shows the inter-vehicle distances between the ground robots. It can be noticed that, by virtue of the time-coordination algorithm, the agents are able to maintain a safe distance throughout the mission.

Figure 8 shows the evolution of ${\gamma }_{\delta ,i}\left(t\right)$, which in this case is the difference in virtual time between the leader and the obstacle, throughout the mission. At $t\approx 15$ s, when the obstacle starts to decrease its speed, ${\gamma }_{\delta ,1}\left(t\right)$ begins to increase. However, with the use of the collision-avoidance algorithm, ${\gamma }_{\delta ,1}\left(t\right)$ converges to a value that is less than ${\gamma }_{\mathrm{safe}}=-2$ s, guaranteeing that a minimum safe distance is kept and the collision can be avoided. The value of ${\gamma }_{\mathrm{safe}}$ is calculated based on the average nominal linear and angular velocities of a Turtlebot3 Burger and it is selected to achieve $d_{\mathrm{safe}}=0.4$ m.

The top row of Figure 9 shows the evolution of the pace of the mission. Initially, the desired pace is ${\dot{\gamma }}_d=1$, which indicates an ideal evolution of the mission. However, when the obstacle starts to endanger one of the vehicles, the pace of the mission is decreased to ${\dot{\gamma }}_d\approx 0.9$, according to the control law that is presented in this article. The bottom row of Figure 9 presents the evolution of the virtual time. It can be seen that, even with the collision-avoidance algorithm in use, the vehicles maintain time-coordination with ${\gamma }_1\left(t\right)={\gamma }_2\left(t\right)={\gamma }_3\left(t\right)={\gamma }_4\left(t\right)$. We note that, at $t=65$ s, the collision-avoidance algorithm is turned off, and ${\gamma }_{\delta ,i}\left(t\right)=-3$ s and ${\dot{\gamma }}_d=1$. However, this does not affect collision-avoidance, as the obstacle has already left the impact area. Lastly, Figure 9 shows the evolution of the coordination errors. It can be noticed that, despite changes in the desired pace throughout the mission, the coordination errors are quickly driven to 0.

It is highlighted here that only the drone is able to detect changes in the speed profile of the obstacle, but, by virtue of the combined action of the time-coordination and collision-avoidance algorithms, the ground robots are able to avoid the impact, even without knowing the actual position and speed of the obstacle.

8. Conclusions

In this paper, we propose a collision-avoidance method that is based on a leader-follower time-coordination algorithm. The time-coordination algorithm ensures that all of the the vehicles are able to not only coordinate with each other, but follow a desired mission pace, which is known to to be a subset of vehicles only. Furthermore, the collision-avoidance algorithm relies on the knowledge of the nominal trajectory of obstacles and takes bounded deviations in their speed profiles into account. Together, the time-coordination and collision avoidance algorithms guarantee the safe execution of cooperative multi-vehicle missions in the presence of moving obstacles. A rigorous mathematical analysis, as well as experimental results, are provided to demonstrate the efficacy of the algorithm.