A Mixed-Initiative Formation Control Strategy for Multiple Quadrotors

Abstract

In this paper, we present a mixed-initiative motion control strategy for multiple quadrotor aerial vehicles. The proposed approach incorporates formation specifications and motion-planning commands as well as inputs by a human operator. More specifically, we consider a leader–follower aerial robotic system, which autonomously attains a specific geometrical formation, by regulating the distances among neighboring agents while avoiding inter-robot collisions. The desired formation is realized by a decentralized prescribed performance control strategy, resulting in a low computational complexity implementation with guaranteed robustness and accurate formation establishment. The multi-robot system is safely guided towards goal configurations, by employing a properly defined navigation function that provides appropriate motion commands to the leading vehicle, which is the only one that has knowledge of the workspace and the goal configurations. Additionally, the overall framework incorporates human commands that dictate the motion of the leader via a teleoperation interface. The resulting mixed-initiative control system has analytically guaranteed stability and convergence properties. A realistic simulation study, considering a team of five quadrotors operating in a cluttered environment, was carried out to demonstrate the performance of the proposed strategy.

1. Introduction

During recent years, multi-rotor aerial robots have been established as a popular solution for a variety of autonomous or semi-autonomous aerial tasks. Concurrently, as the technology of sensors and actuators rapidly advances, multi-rotor aerial robots evolve with improved flight endurance, maneuverability and payload efficiency. In critical aerial missions such as search and rescue, load transportation, precision agriculture, field surveillance and monitoring, the coordinated deployment of a multi-agent system excels in single-agent operation in terms of perception and data collection, speed of completion and fault tolerance. On the other hand, many of the aforementioned missions (e.g., search and rescue) require enhanced cognitive capabilities for efficient decision-making. However, cognitive robotic skills are still in their early stages; hence mixed-initiative strategies allowing a human operator with enhanced cognitive and decision capabilities to provide high level commands and affect online the motion of a robotic swarm, may significantly improve the safety and effectiveness of a mission.

1.1. Related Work

In general, formation of multi-robot systems can be either rigid or flexible with respect to the inter-robot distance specifications [1,2,3]. Two main formation strategies can be distinguished: (a) the leader–follower configuration and (b) the behavior-based approach [4]. In a leader–follower scheme, the motion of the system is dictated by the leader, while the following vehicles scatter in relatively close ranges extending the searching area. A leader–follower scheme is energy efficient and usually avoids instabilities related to information dissemination [5,6]. Nevertheless, in its nominal form, it is possible to appear robustness issues, because in the case the leader fails, the overall system collapses [7]. Formation control of multi-rotors is considered a rather interesting topic by the robotics community [8,9] and spans over a variety of applications such as evasion maneuvering [10], collision-free trajectory tracking [11], aggressive formation [12], target interception [13] and enclosing [14]. Frequently, the primary goal of a formation strategy is to maintain a specific structure of the involved agents, while simultaneously conform with a set of constraints [15,16,17]. In this respect, numerous control protocols can be found in the previous related studies, including backstepping approaches [18,19], model predictive control [20], feedback linearization [21], linear quadratic control [22], game theory approaches [23], prescribed performance methods [24] and iterative learning control [25]. A comparative study on formation control is presented in [26]. Although swarm control strategies can be employed for the autonomous operation of many Unmanned Aerial Vehicles (UAVs), effective and task-driven swarm navigation (e.g., search and rescue, autonomous exploration) is still in its early stages, due to the limited cognitive robotic skills. In this vein, mixed-initiative control strategies that allow a human operator to provide high level commands and affect the initial plan of a UAV swarm based on his/her enhanced cognitive and decisional capabilities may significantly improve the perceptional information acquired during a specific air mission. A substantial number of works have investigated the human-robot interaction in terms of sensor readings and command interfaces ranging from simple joystick commands using visual feedback from onboard cameras to more sophisticated gesture-based controls employing virtual reality [27,28,29]. However, sensing and direct teleoperation control in human-robot collaboration accounts for just a small part of the overall challenges faced when dealing with such missions. The human-robot collaboration should be perceptual and cognitive at the same time; humans should make rapid decisions for the robots, by interpreting in real time the sensor feedback (e.g., video, audio, thermal imaging) provided by the robots. Therefore, there is a great need for shared autonomy between human and robots to allow fast and efficient decision-making.

In a mixed-initiative autonomy mode, the robot can accept varying levels of human intervention, even from multimodal frameworks [28,29,30], hence relieving the latter from the tedious task of direct control. In fact, experimental procedures indicate that collaborative control can increase the performance of the overall system even in highly complicated workspaces [31]. Moreover, mixed-initiative control schemes can be employed successfully in cases where the robotic agent is assigned to perform various tasks such as navigation operations (e.g., take-off, hover, waypoint fly, land, etc.), but also tasks that require proximity and contact with surfaces (e.g., docking, undocking, sample picking, wall inspection, etc.) [32,33], where in such cases force feedback plays an important role in the mixed-initiative process. In any case, the element of trust is of utmost importance in human-robot collaboration; hence, methods towards formulating decision-analytical-based measures of trust in various cooperative tasks have been also studied [32,33,34]. In the same spirit, the robot should be able to recognize when help is actually needed from the operator and can refuse to undertake commands that may pose a threat to itself or its surroundings [32,35]. At this point, we should highlight that fusing motion-planning and control with mixed-initiative strategies is a rather tricky undertaking, since stability and convergence of the overall scheme are not always guaranteed. Towards this direction, very few studies have been conducted that blend human inputs with motion controllers to intervene with the robot guidance laws, although the resulting scheme exhibits guaranteed convergence properties [36].

1.2. Contribution

In this work and contrary to the related literature, a mixed-initiative formation control strategy (Preliminary results on prescribed performance distance-based formation control without incorporating human commands were published in the Proceedings of the 2020 International Conference on Unmanned Aircraft Systems (ICUAS’20), Athens, Greece [37]. The main theoretical contribution of the present work is the stability analysis of the integrated system, which incorporates the trajectory tracking of the leader, the prescribed distance-based formation of the agent strategy and finally the mixed-initiative scheme introducing human commands in the loop. To the best of our knowledge, similar mixed-initiative approaches accompanied with a rigorous stability analysis have not yet appeared in the related literature.) is proposed for multiple quadrotor aerial vehicles that incorporates for the first time formation specifications, motion-planning commands as well as human inputs. We summarize the main contributions of this paper as follows:

• Low complexity, decentralized formation with prescribed performance: The proposed formation control protocol is model-free, and the control output does not require complex calculations, hence it is ideal for real-time implementation on embedded computing units. It is decentralized, since each vehicle requires only position measurements that are easy to acquire via the onboard sensors, without the necessity for information exchange, while each control scheme runs on the local computing system of each agent. Moreover, the mathematical formulation is quite lean avoiding exhaustive numerical methods that are usually required by other approaches that attempt to tackle similar issues, such as Model Predictive Control or Reinforcement Learning. Finally, the transient and steady state response is predefined via the selection of specific performance functions.
• Safety in navigation: The multi-agent system is safely guided towards position goal configurations, while simultaneously avoiding obstacles, by employing a properly defined navigation function which calculates the motion commands for the leader vehicle.
• Flexibility: The overall framework incorporates human commands for the desired motion of the leader via a teleoperation interface. Hence, the overall system is implicitly guided by the mixed-initiative motion of the leader combined with predefined distance formation specifications, which proves to be rather efficient in cases where either an alternate path should be followed or an ad-hoc release from local minima is required, especially during obstacle avoidance. It should be noted that the proposed strategy successfully allows the human operator to intervene to the motion of the formation without however compromising safety (e.g., collisions and connectivity breaks) or performance.

1.3. Outline

The rest of the paper is organized as follows: In Section 2, some preliminary information regarding the quadrotor equations of motion, the formation of the multi-agent system and the mathematical description of the workspace is presented along with the problem formulation. Section 3 describes rigorously the proposed method consisting of the decentralized prescribed performance formation controller, the navigation function for the guidance of the leader as well as the overall mixed-initiative scheme that incorporates the human inputs. Section 4 demonstrates the performance of the proposed architecture via realistic simulation tests. Finally, Section 5 concludes the paper.

2. Preliminaries and Problem Formulation

In this section, we briefly present the quadrotor equations of motion and the low-level control architecture. Next, we provide some preliminary information regarding the distance-based formation control as well as the mathematical description of the operating workspace. Finally, we formulate the problem in hand for the envisioned multi-robot system.

2.1. Quadrotor Equations of Motion

The quadrotor actuation system consists of four motors, where the opposite pairs are rotating clockwise and counterclockwise. Thus, the dynamic motion of the system is induced via the torque and thrust applied by each actuation module (motor and propeller) [38,39].

We define a body-fixed frame ${\mathbf{B}}_i=\{e_{B_{x_i}},e_{B_{y_i}},e_{B_{z_i}}\}$ located at the i-th vehicle gravity center, as depicted in Figure 1, and the inertial frame $\mathbf{I}=\{e_{I_x},e_{I_y},e_{I_z}\}$ attached at a fixed position ${\mathbf{O}}_{\mathbf{I}}$ inside the vehicles workspace. According to the Newton-Euler method [39], the dynamic equations of motion for each vehicle may be formulated as follows:

$$\begin{array}{c}{}^I{\dot{\mathbf{p}}}_{B_i}={}^I{\mathbf{v}}_{B_i}\hfill \\ \\ {}^I{\dot{\mathbf{v}}}_{B_i}=m_i^{-1}\left({}^I{\mathbf{R}}_{B_i}\right){\mathbf{F}}_{B_i}\hfill \\ \\ {\dot{\mathbf{\omega }}}_{B_i}={\mathbf{J}}_{{}_{B_i}}^{-1}{\mathbf{M}}_{B_i}\hfill \end{array}$$

(1)

where ${}^I{\mathbf{p}}_{B_i}={\left[\begin{array}{ccc}{}^I{x}_{B_i}& {}^I{y}_{B_i}& {}^I{z}_{B_i}\end{array}\right]}^T$, ${}^I{\mathbf{v}}_{B_i}={\left[\begin{array}{ccc}{}^I{v}_{x_{B_i}}& {}^I{v}_{y_{B_i}}& {}^I{v}_{z_{B_i}}\end{array}\right]}^T$ the position and linear velocities for vehicle i with respect to $\mathbf{I}$, ${\mathit{\omega }}_{B_i}={\left[\begin{array}{ccc}{p}_{B_i}& q_{B_i}& r_{B_i}\end{array}\right]}^T$ the angular velocities in ${\mathbf{B}}_i$, $m_i$ is the mass, ${}^I{\mathbf{R}}_{B_i}$ is the rotation matrix from frame ${\mathbf{B}}_i$ to $\mathbf{I}$ and ${\mathbf{J}}_{{}_{B_i}}$ is the matrix describing the inertia in ${\mathbf{B}}_i$. The overall forces and torques acting on the i-th vehicle are given by the following equations (refer to [38,39] for more details):

$$\begin{array}{c}{\mathbf{F}}_{B_i}={\mathbf{F}}_{M_i}+{\mathbf{F}}_{d_i}+{\mathbf{F}}_{g_i}\hfill \\ \\ {\mathbf{M}}_{B_i}={\mathbf{M}}_{M_i}+{\mathbf{M}}_{d_i}\hfill \end{array}$$

(2)

where:

• ${\mathbf{F}}_{d_i}=-C_{d,F}\left({}^{B_i}{\mathbf{R}}_I\right)∥{}^I{\mathbf{v}}_{B_i}-{}^I{{\mathbf{v}}_{\mathbf{w}}}_{B_i}∥\left({}^I{\mathbf{v}}_{B_i}-{}^I{{\mathbf{v}}_{\mathbf{w}}}_{B_i}\right)$ are the drag forces and $C_{d,F}$ denotes the drag force coefficient, while ${}^I{{\mathbf{v}}_{\mathbf{w}}}_{B_i}$ is the wind velocity vector
• ${\mathbf{M}}_{d_i}=-C_{d,M}\left|{\dot{\mathbf{\omega }}}_{B_i}\right|{\dot{\mathbf{\omega }}}_{B_i}$ are the drag moments and $C_{d,M}$ denotes the drag moment coefficient
• ${\mathbf{F}}_{g_i}=m_i\left({}^{B_i}{\mathbf{R}}_I\right){\left[\begin{array}{ccc}0& 0& g_e\end{array}\right]}^T$ is the gravity vector and $g_e$ being the gravitational acceleration
• ${\mathbf{F}}_{M_i}={\left[\begin{array}{ccc}0& 0& -\left(T_1+T_2+T_3+T_4\right)\end{array}\right]}^T$ is the vector of the motor thrusts
• ${\mathbf{M}}_{M_i}={\left[\begin{array}{cc}\left(T_4-T_2\right)l_m& \\ \left(T_1-T_3\right)l_m& \\ \left(-M_1+M_2-M_3+M_4\right)\end{array}\right]}^T$ is the motor torque vector and $l_m$ is the distance of each motor as shown in Figure 1
• $T_i=C_T{\overline{{\mathit{\omega }}_i}}^2$, $i=1\dots 4$ is the thrust of each individual thruster, $C_T>0$ denotes the thrust coefficient and $\overline{{\mathit{\omega }}_i}$ is the speed of the i-th thruster
• $M_i=C_Q{\overline{{\mathit{\omega }}_i}}^2$, $i=1\dots 4$ is the thruster reaction torque while $C_Q$ denotes the torque coefficient

2.2. Quadrotor Low-Level Control

Regarding the low-level control of each vehicle, we assume the architecture proposed in [39], which is also implemented in the employed simulation environment. It consists of a fast inner loop responsible for controlling the rotational dynamics and an outer loop handling the translational ones. The overall scheme is implemented using a set of cascaded PI and PID controllers. The proposed architecture guarantees that for motions close to the hovering state individual control of each Degree of Freedom (DoF) is feasible. The reference inputs to the low-level control scheme are the desired linear velocities and yaw rate in ${\mathbf{B}}_i$ and the desired position and orientation $z_{d_i}$, ${\psi }_{d_i}$ with respect to $\mathbf{I}$. The calculated commanded thrust and torques are then converted to motor voltages by the servo control system. For more information regarding the adopted low-level control strategy please refer to [39].

2.3. Formation of the Multi-Agent System

In this work, we consider $N+1$ identical quadrotors in a leader–follower configuration. The leading vehicle is guided under a mixed-initiative control framework, consisting of a Navigation Function-based scheme combined with human teleoperation system. The leader is the only agent that has global knowledge of the environment (i.e., global positioning, position of obstacles and desired waypoints). The $N+1$ quadrotors are using only onboard sensor data and more specifically inter-robot ranges and headings. As with [2], we investigate a formation case where all vehicles should regulate their distances to each other to achieve the desired formation geometry.

We proceed with the modeling of the formation using undirected graphs as depicted in Figure 2. Let us define a graph $G\triangleq (\mathcal{V},\mathcal{E})$ which consists of l edges and $N+1$ vertices. Each vertex corresponds to a quadrotor, hence $\mathcal{V}=\{0,1,...,N\}$ is the set of vertices and $\mathcal{E}\subset \mathcal{V}\times \mathcal{V}$ is the set of edges, with index 0 referring to the leading quadrotor. We define as $N_i\left(\mathcal{E}\right)=\{j\in \mathcal{V}|(i,j)\in \mathcal{E}\}$ the set of neighbors of i-th quadrotor. To simplify the presentation, we define ${\mathbf{p}}_i\triangleq {}^I{\mathbf{p}}_{B_i}$ where ${\mathbf{p}}_i\in {\Re }^3,i=0,1,\dots ,N$ is the position of each vehicle and $\overline{\mathbf{p}}\triangleq \mathbf{col}\left({\mathbf{p}}_i\right)\in {\Re }^{3(N+1)}$ represents the realization of G in ${\Re }^3$. Thus, the framework of G is defined by the pair $F\triangleq (G,\overline{\mathbf{p}})$. Considering an arbitrary sequence of edges in $\mathcal{E}$, a rigidity function ${\mathsf{\Phi }}_G:{\Re }^{3(N+1)}\to {\Re }^l$ related to F can be formulated as:

$${\mathsf{\Phi }}_G\left(\overline{\mathbf{p}}\right)=[...,||{\mathbf{p}}_i-{\mathbf{p}}_j{\left|\right|,\dots ]}^T,(i,j)\in \mathcal{E}$$

(3)

To achieve the operational specifications, we synthesize a minimally and infinitesimally rigid framework F, with the following rigidity matrix R:

$$R\left(\overline{\mathbf{p}}\right)=\frac{\partial {\mathsf{\Phi }}_G\left(\overline{\mathbf{p}}\right)}{\partial \overline{\mathbf{p}}}$$

(4)

with each row of $R\left(\overline{\mathbf{p}}\right)$ formulated as follows:

$$\left[{\mathbf{O}}_{1\times m}^T,...,{\left(\frac{{\mathbf{p}}_i-{\mathbf{p}}_j}{\left|\right|{\mathbf{p}}_i-{\mathbf{p}}_j\left|\right|}\right)}^T,...,{\mathbf{O}}_{1\times m}^T,...,{\left(\frac{{\mathbf{p}}_j-{\mathbf{p}}_i}{\left|\right|{\mathbf{p}}_i-{\mathbf{p}}_j\left|\right|}\right)}^T,...,{\mathbf{O}}_{1\times m}^T\right]$$

(5)

where ${\mathbf{O}}_{1\times m}^T$ a vector of length m containing zeros. Since the framework F is infinitesimally rigid it holds that:

$$\mathbf{rank}\left[R\left(\overline{\mathbf{p}}\right)\right]=3(N+1)-6$$

(6)

and consequently the corresponding graph consists of at least $3(N+1)-6$ edges, i.e., $\left|\mathcal{E}\right|\ge 3(N+1)-6$.

We invoke the following Lemma from [13], which will be employed next in the stability analysis of the proposed decentralized control scheme(Section 3.2).

Lemma 1.

If the framework $F=(G,\overline{\mathbf{p}})$ is minimally and infinitesimally rigid, then the matrix $R\left(\overline{\mathbf{p}}\right)R{\left(\overline{\mathbf{p}}\right)}^T$ is positive definite.

Detailed information about distance-based formation can be found in [40].

2.4. Description of the Workspace

In this work, we consider a team of $N+1$ quadrotors which should be autonomously navigated within a workspace, while simultaneously avoiding scattered obstacles and workspace boundaries. Hence, we consider a team of quadrotors operating in a workspace $\mathbf{W}\subset {\Re }^3$ with boundaries $\partial \mathbf{W}=\{{\mathbf{p}}_{\mathbf{i}}\in {\Re }^3:{\mathbf{p}}_{\mathbf{i}}\in \mathrm{cl}\left(\mathbf{W}\right)\setminus \mathrm{int}\left(\mathbf{W}\right)\}$. We may consider that the overall vehicle formation and the obstacles are modeled by spheres. Let $\mathbf{B}({\mathbf{p}}_c,r_c)$ be a sphere that surrounds the vehicles formation where ${\mathbf{p}}_c\in {\Re }^3$ is the formation’s centroid that coincides with the leader’s position and $r_c$ the radius selected accordingly to encapsulate all vehicles. Moreover, we may define M spherical obstacles ${\pi }_m=\mathbf{B}({\mathbf{p}}_{{\pi }_m},r_{{\pi }_m}),m\in \{1,\dots ,M\}$, where ${\mathbf{p}}_{{\pi }_{\mathbf{m}}}\in {\Re }^3$ is the center and $r_{{\pi }_m}>0$ the radius of obstacle ${\pi }_m$. According to the spherical world representation as described in [41], the next inequality holds for two obstacles $m,m^{\prime }\in \{1,\dots ,M\}$:

$$\begin{array}{c}\hfill \left|\right|{\mathbf{p}}_{{\pi }_m}-{\mathbf{p}}_{{\pi }_{m^{\prime }}}\left|\right|>2r_c+r_{{\pi }_m}+r_{{\pi }_{m^{\prime }}}\end{array}$$

(7)

which means that for any pair of obstacles, there exists sufficient space such that the entire volume of the formation may travel between them. A graphical representation of the workspace is given in Figure 3. Finally, more complex geometries can be dealt by employing topological equivalent transformations as described in [41] or [42].

2.5. Problem Statement

In the proposed mixed-initiative formation control strategy, we identify the following specifications:

• We consider a leader–follower scheme consisting of $N+1$ quadrotor vehicles.
• The $N+1$ multi-robot system should be safely guided towards specific waypoints inside a workspace with internal obstacles.
• The $N+1$ multi-robot system should always retain an enclosing formation around the leader. Hence, each vehicle should uphold a set of distance specifications $d_{ij}^{\star }$, $(i,j)\in \mathcal{E}$ among the agents with prescribed performance, as depicted in Figure 2.
• The leader’s desired position coincides with the formation centroid.
• Only the leader vehicle has global knowledge of the workspace along with the location of the waypoint goals.
• A human operator may influence the motion behavior of the multi-robot system without compromising the obstacle avoidance and the distance formation properties.

Hence, given that the initial framework $(G,\mathbf{p}(0\left)\right)$ is minimally and infinitesimally rigid, we will proceed with the design of a mixed-initiative formation control strategy capable of accomplishing the desired formation as well as tracking a collision-free trajectory towards the goal waypoints. Furthermore, we consider that the only sensor measurements required for the operation of the proposed scheme are acquired from the sensor suite of each vehicle, including the relative distance/heading measurements among neighbors; thus, no explicit information is exchanged by the aerial agents at any time. Only for the case of the leader, we assume that it has global knowledge of the workspace and the position of the desired waypoints. Finally, we consider that the motion trajectory of the multi-robot system may be modified by a human operator via a teleoperation (joystick) interface that sends desired velocity reference signals to the leader. The overall control architecture is presented in Figure 4.

3. Methodology

In this section, we provide the analytical derivation of the mixed-initiative control scheme. First, we design the Navigation Function for the leading robot and show how it is combined with the human input commands, resulting in a smooth trajectory profile. Next, we proceed with the analytical formulation of the distance-based formation controller, which incorporates smooth reference trajectories for the leader. Finally, we provide a stability analysis for the overall mixed-initiative scheme.

3.1. Mixed-Initiative Control

We consider that the leading vehicle has global knowledge of the goal waypoints and obstacles’ location. Hence, its motion control objective is to safely guide the overall formation towards the goal waypoints while avoiding collisions with the scattered obstacles. On the other hand, the followers will implicitly navigate along with the leader as imposed by the decentralized distance-based formation scheme described in Section 3.2.

We denote as ${\mathbf{p}}_L$ and ${{\mathbf{p}}_L}^d$ the current and desired position of the leading quadrotor, respectively. Next, we calculate a feasible trajectory within the workspace $\mathbf{W}$, by a properly designed Navigation Function [41], as follows:

$$\begin{array}{c}\hfill {\varphi }_L({\mathbf{p}}_L;{\mathbf{p}}_L^d)=\frac{\gamma ({\mathbf{p}}_L-{\mathbf{p}}_L^d)}{{[{\gamma }^k({\mathbf{p}}_L-{\mathbf{p}}_L^d)+\beta \left({\mathbf{p}}_L\right)]}^{\frac{1}{k}}}\end{array}$$

(8)

where ${\varphi }_L:\stackrel{\mathbf{W}-\stackrel{M}{\underset{m=1}{\cap }}\mathbf{B}({\mathbf{p}}_{{\pi }_m},r_{{\pi }_m}+r_c)}{\to }[0,1)$ is the potential that determines a safe motion vector field inside the free workspace $\mathbf{W}-\stackrel{M}{\underset{m=1}{\cap }}\mathbf{B}({\mathbf{p}}_{{\pi }_m},r_{{\pi }_m}+r_c)$. We define $k>1$ as a design constant, $\gamma ({\mathbf{p}}_L-{\mathbf{p}}_L^d)>0$ with $\gamma \left(\mathbf{0}\right)=0$ as the attractive to the goal ${\mathbf{p}}_L^d$ potential field and $\beta \left({\mathbf{p}}_L\right)>0$ with:

$$\underset{{\mathbf{p}}_L\to \{\begin{array}{c}\mathrm{Boundary}\\ \mathrm{Obstacles}\end{array}}{lim}\beta \left({\mathbf{p}}_L\right)=0$$

(9)

as the repulsive potential field owing to the obstacles and workspace boundaries. As shown in [41], ${\varphi }_L({\mathbf{p}}_L;{\mathbf{p}}_L^d)$ has a global minimum at ${\mathbf{p}}_L^d$ and no other local minima for sufficiently large values of k. Hence, an obstacle-free path from any initial location (except for a set of measure zero [41]) to the goal can be generated by following the negated gradient of ${\varphi }_L({\mathbf{p}}_L;{\mathbf{p}}_L^d)$. As a result, the desired velocity profile of the leading vehicle can be designed as follows:

$${\mathbf{v}}_L^d\left(t\right)=-K_{NF}{\nabla }_{{\mathbf{p}}_L}{\varphi }_L({\mathbf{p}}_L\left(t\right),{\mathbf{p}}_L^d)$$

(10)

where $K_{NF}>0$ is a positive gain.

Following [36], we proceed with the formulation of a mixed-initiative desired velocity profile, incorporating the human motion intention commands as follows:

$$\begin{array}{c}\hfill {\mathbf{v}}_{L_{mi}}^d\left(t\right)={\mathbf{v}}_L^d\left(t\right)+r\left({\mathbf{p}}_L\right)\mathbf{u}\left(t\right)\end{array}$$

(11)

In (11), $\mathbf{u}\left(t\right)$ denotes the bounded linear velocity command vector provided by the human operator via a joystick teleoperation interface. The weighting function $r\left({\mathbf{p}}_L\right)$ becomes null when the leader is close to the workspace and the obstacles boundaries, while it equals to one, when the leader is far away from them, to assure safe interference with the motion planner (11). According to [36], we select the following smooth function:

$$\begin{array}{c}\hfill r\left({\mathbf{p}}_L\right) =\frac{\upsilon \left(\beta \left({\mathbf{p}}_L\right)-r_s\right)}{\upsilon \left(\beta \left({\mathbf{p}}_L\right)-r_s\right) + \left(\epsilon +r_s-\beta \left({\mathbf{p}}_L\right)\right)}\end{array}$$

(12)

where $\epsilon >0$, $r_s\ge 0$ and $\beta \left({\mathbf{p}}_L\right)$ is the obstacle function employed in the Navigation Function ${\varphi }_L$ and the function $\upsilon \left(t\right)$ is defined as follows:

$$\begin{array}{c}\hfill \upsilon \left(t\right) \stackrel{\Delta }{=} \left\{\begin{array}{c}{e}^{-1/t},t>0\\ 0,t\le 0\end{array}\right.\end{array}$$

(13)

Finally, according to the stability analysis provided in [36], the proposed interconnection exhibits global input to state stability properties, with respect to the human commands.

3.2. Distance-Based Formation Control

At first, we define the distance errors for each edge of the rigid graph, as:

$$e_{ij}\left(t\right)=\parallel {\mathbf{p}}_i\left(t\right)-{\mathbf{p}}_j\left(t\right)\parallel -d_{ij}^{\star },\forall (i,j)\in \mathcal{E}.$$

(14)

Then, we define the distance specifications that should be met by the multi-robot system, and more specifically the inter-robot collision avoidance and the sensor range limitations. Regarding collision avoidance, the distance of neighboring agents should be kept greater than a safety zone $\underline{d}<d_{ij}^{\star }$. Concerning the sensor range limitations, the distance of the agents should be maintained below a radius $\overline{d}>d_{ij}^{\star }$ to ensure connectivity and allow them the required proximity to achieve reliable relative localization. Given an initial vehicle configuration, where the above specifications hold, the decentralized control objective is expressed as follows:

$$-{\underline{\rho }}_{ij}\left(t\right)<e_{ij}\left(t\right)<{\overline{\rho }}_{ij}\left(t\right),\forall (i,j)\in \mathcal{E}$$

(15)

for all $t\ge 0$, where ${\underline{\rho }}_{ij}\left(t\right),{\overline{\rho }}_{ij}\left(t\right)$ denote strictly positive and decreasing performance functions [43] that satisfy ${lim}_{t\to \infty }{\underline{\rho }}_{ij}\left(t\right)\triangleq {\underline{\rho }}_{ij}^{\infty }>0$ and ${lim}_{t\to \infty }{\overline{\rho }}_{ij}\left(t\right)\triangleq {\overline{\rho }}_{ij}^{\infty }>0$, respectively. By selecting ${\underline{\rho }}_{ij}\left(0\right)=d_{ij}^{\star }-\underline{d}$ and ${\overline{\rho }}_{ij}\left(0\right)=\overline{d}-d_{ij}^{\star }$, then satisfying (15) for all time guarantees inter-robot collision avoidance as well as connectivity maintenance, due to the decreasing behavior of ${\underline{\rho }}_{ij}\left(t\right),{\overline{\rho }}_{ij}\left(t\right)$. Furthermore, by selecting appropriately the steady state value and decreasing rate of ${\underline{\rho }}_{ij}\left(t\right),{\overline{\rho }}_{ij}\left(t\right)$ we can impose steady state and transient performance specifications on the errors $e_{ij}\left(t\right)$. More specifically, we select the following decaying performance functions:

$$\begin{array}{c}\hfill {\underline{\rho }}_{ij}\left(t\right)= \left(d_{ij}^{\star }-\underline{d}-{\rho }_{\infty }\right)exp(-\lambda t)+{\rho }_{\infty }\end{array}$$

(16)

$$\begin{array}{c}\hfill {\overline{\rho }}_{ij}\left(t\right)= \left(\underline{d}-d_{ij}^{\star }-{\rho }_{\infty }\right)exp(-\lambda t)+{\rho }_{\infty }\end{array}$$

(17)

where ${\rho }_{\infty }$ and $\lambda$ are the maximum allowable steady state error and decaying rate, respectively.

Remark 1.

The proposed strategy does not include any specific design for the orientation of the vehicles. Instead, the low-level orientation controller of each vehicle can be employed (see Section 2.2), to either retain a fixed heading, or track a specific reference profile independently.

3.2.1. Control Design

We proceed with the design, selecting as control outputs the linear body velocities of each vehicle participating in the formation. The respective outputs will be fed as reference signals to each quadrotor’s low-level controller as described in Section 2.2. We consider a desired smooth and bounded velocity profile ${\mathbf{v}}_{L_{mi}}^d\left(t\right)$, as denoted in (11), and any initial configuration close to the desired formation within the acceptable collision avoidance and connectivity bounds. At first, we select the performance functions ${\underline{\rho }}_{ij}\left(t\right),{\overline{\rho }}_{ij}\left(t\right),\forall (i,j)\in \mathcal{E}$ as in (16) and (17) and the parameter values ${\rho }_{\infty }$ and $\lambda$ in order to prescribe the desired performance specifications of the steady state error and the speed of convergence.

Next, for all the vehicles participating in the formation, we design the following linear velocity control scheme:

$${\mathbf{v}}_I=-k_E{R}^T{\Xi }_{E}E+I_L{\mathbf{v}}_{mi}^d,k_E>0$$

(18)

which is expressed in the inertial frame, where $R\in {\Re }^{l\times 3(N+1)}$ is the rigidity matrix as defined in (4), ${\mathbf{v}}_{mi}^d\left(t\right)\triangleq {[{\mathbf{v}}_{L_{mi}}^d\left(t\right),{\mathbf{v}}_{L_{mi}}^d\left(t\right),\dots ,{\mathbf{v}}_{L_{mi}}^d\left(t\right)]}^T\in {\Re }^{3N\times 1}$, $I_L\in {\Re }^{3(N+1)\times 3}= \left[I_{3x3},O_{3x3},,O_{3x3},\dots ,O_{3x3}\right]$ is a selection matrix activating the mixed-initiative control only for the leading vehicle, E is the vector of modulated errors given as follows:

$$E\triangleq \mathbf{col}{\left(ln\left(\frac{1+\frac{e_{ij}\left(t\right)}{{\underline{\rho }}_{ij}\left(t\right)}}{1-\frac{e_{ij}\left(t\right)}{{\overline{\rho }}_{ij}\left(t\right)}}\right)\right)}_{(i,j)\in \mathcal{E}}\in {\Re }^l$$

(19)

and ${\Xi }_E$ is a diagonal matrix containing the partial derivatives of the modulated errors with respect to the distance errors:

$${\Xi }_E\triangleq \frac{\partial E}{\partial e_{ij}}=\mathbf{diag}{\left(\frac{{\underline{\rho }}_{ij}\left(t\right)+{\overline{\rho }}_{ij}\left(t\right)}{({\underline{\rho }}_{ij}\left(t\right)+e_{ij}\left(t\right))({\overline{\rho }}_{ij}\left(t\right)-e_{ij}\left(t\right))}\right)}_{(i,j)\in \mathcal{E}}\in {\Re }^{l\times l}.$$

(20)

The linear velocity profile is then expressed in the body frame of each vehicle via the respective rotation matrix of each vehicle:

$${\mathbf{v}}_d\triangleq [{\mathbf{v}}_{0d}^T,{\mathbf{v}}_{1d}^T,\dots ,{\mathbf{v}}_{N_d}^T]={}^I{\mathbf{R}}_{\mathbf{B}}^{-1}{\mathbf{v}}_I,$$

(21)

where ${}^I{\mathbf{R}}_B=\mathrm{diag}\left[{}^I{\mathbf{R}}_{B_0},{}^I{\mathbf{R}}_{B_1},\dots ,{}^I{\mathbf{R}}_{B_N}\right]$. Finally, the body velocity commands (21) are directed as references to the low-level control architecture (Section 2.2) of each quadrotor.

3.2.2. Stability Analysis

We proceed with the stability analysis of the proposed controller, which is summarized as follows:

Theorem 1.

Consider a group of $N+1$ quadrotors in a leader–follower formation modeled by (1), at an initial minimally and infinitesimally rigid configuration. The decentralized control protocol proposed in Section 3.2.1 guarantees that the group safely navigates within a cluttered environment while simultaneously: (i) achieving a predefined rigid formation shape around the leading vehicle with prescribed transient and steady state performance, (ii) avoiding inter-agent collisions and connectivity breaks, and (iii) incorporating human input commands without affecting the global stability of the system.

Proof. 

Since the initial distance errors satisfy $-{\underline{\rho }}_{ij}\left(0\right)<e_{ij}\left(0\right)<{\overline{\rho }}_{ij}\left(0\right)$, for all $j\in {\mathcal{N}}_i$ and $i=0,1,\cdots ,N$, Theorem 54 (p. 476) in [44] guarantees that $-{\underline{\rho }}_{ij}\left(t\right)<e_{ij}\left(t\right)<{\overline{\rho }}_{ij}\left(t\right)$ for a maximal interval with ${\tau }_f\in \{{\Re }_{+}^{\ast },\infty \}$. Subsequently arguments will be invoked to prove that ${\tau }_f=\infty$. Therefore, consider the following positive definite function of the modulated distance errors E:

$$V=\frac{1}{2}{E}^{T}E.$$

(22)

Differentiating (22) with respect to time, we obtain:

$$\dot{V}=E^T\frac{\partial E}{\partial e_{ij}}\mathbf{col}\left({\dot{e}}_{ij}\left(t\right)\right)+E^T\frac{\partial E}{\partial {\overline{\rho }}_{ij}}\mathbf{col}\left({\dot{\overline{\rho }}}_{ij}\left(t\right)\right)+E^T\frac{\partial E}{\partial {\underline{\rho }}_{ij}}\mathbf{col}\left({\dot{\underline{\rho }}}_{ij}\left(t\right)\right).$$

(23)

Employing the fact that $\frac{\partial E}{\partial e_{ij}}\triangleq {\Xi }_E$, $\frac{\partial E}{\partial {\overline{\rho }}_{ij}}\triangleq {\Xi }_E{\Xi }_{{\overline{\rho }}_{ij}}$, $\frac{\partial E}{\partial {\underline{\rho }}_{ij}}\triangleq {\Xi }_E{\Xi }_{{\underline{\rho }}_{ij}}$ for some bounded diagonal matrices ${\Xi }_{{\underline{\rho }}_{ij}}$, ${\Xi }_{{\overline{\rho }}_{ij}}$ and $\mathbf{col}\left({\dot{e}}_{ij}\left(t\right)\right)=R\left({}^I{R}_B\right){\mathbf{v}}_d$, where R is the rigidity matrix and ${\mathbf{v}}_d$ is defined in (21), we have:

$$\dot{V}=E^T{\Xi }_{E}R{\mathbf{v}}_I+E^T{\Xi }_E\left({\Xi }_{{\overline{\rho }}_{ij}}\mathbf{col}\left({\dot{\overline{\rho }}}_{ij}\left(t\right)\right)+{\Xi }_{{\underline{\rho }}_{ij}}\mathbf{col}\left({\dot{\underline{\rho }}}_{ij}\left(t\right)\right)\right).$$

(24)

Finally, invoking (18), we obtain:

$$\begin{array}{cc}\hfill \dot{V}& =-k_E{E}^T{\Xi }_{E}R{R}^T{\Xi }_{E}E+E^T{\Xi }_E(R{\mathbf{v}}_{mi}^d\left(t\right)\hfill \\ \hfill & +{\Xi }_{{\overline{\rho }}_{ij}}\mathbf{col}\left({\dot{\overline{\rho }}}_{ij}\left(t\right)\right)+{\Xi }_{{\underline{\rho }}_{ij}}\mathbf{col}\left({\dot{\underline{\rho }}}_{ij}\left(t\right)\right))\hfill \end{array}$$

(25)

Notice that all terms in the right parenthesis are bounded by construction or by assumption. Moreover, Lemma 1 guarantees that the square matrix $R{R}^T$ is positive definite. Hence, we may conclude that the modulated error vector E in (19) remains bounded. As a result, inequalities (15) are strictly satisfied for all $t\in [0,{\tau }_f)$, thus concluding ${\tau }_f=\infty$ by Proposition C.3.6 (p. 481) in [44]. Furthermore, since E was proven bounded, the velocity profiles ${\mathbf{v}}_I$ and consequently ${\mathbf{v}}_d$ remain also bounded. Lastly, the formation centroid ${\mathbf{p}}_c\left(t\right)\triangleq \frac{1}{N}{\sum }_{i=0}^{N-1}{\mathbf{p}}_i\left(t\right)$, which practically coincides with the leader’s position for sufficient small allowable steady state errors (i.e., ${\rho }_{\infty }<<1$) obeys ${\dot{\mathbf{p}}}_{\mathbf{c}}={\mathbf{v}}_{L_{mi}}^d$, from which we conclude that the overall formation navigates safely towards the desired waypoint, while incorporating the human commands without compromising the operational specifications. □

4. Results and Discussion

4.1. System Description

The performance of the overall mixed-initiative scheme is demonstrated via a set of simulation scenarios conducted in ROS and Gazebo [45,46]. The package $hector\_quadrotor$ [47] was incorporated and modified accordingly to synthesize a multi-vehicle waypoint tracking scenario with simultaneous obstacle avoidance. We consider $N+1=5$ identical quadrotors ($Q_i$) with $Q_0$ designating the leader that has global knowledge of the environment (goals, obstacles configurations and workspace boundaries), as depicted in Figure 3. The $N=4$ followers are using relative only range measurements, without explicit information exchange, which is a practical and realistic assumption that can be realized via a common (e.g., acoustic) onboard sensor suite.

4.2. Simulation Results

We conducted two different mission scenarios, namely Scenario 1 and 2. In both cases, the goal was to autonomously guide the multi-robot system to a set of desired waypoints, located inside a bounded workspace with scattered obstacles. Our strategy (Figure 4) implements a leader–follower configuration, where only the leading quadrotor has knowledge of the goal waypoints location and the workspace constraints. The Navigation Function presented in (10) is responsible for calculating the velocity profile for the leader to reach safely the desired waypoints. The followers are implicitly guided along with the leader under the influence of the decentralized formation controller, which is responsible for maintaining the desired inter-agent distance specifications. Hence, the motion control of each agent is calculated via the implementation of (18). During Scenario 1, the human operator does not provide any input to the system (i.e., $\mathbf{u}\left(t\right) =0$), thus (11) is simplified to (10). On the other hand, during Scenario 2 and at specific time instances, the human applies additional velocity commands to the leader, via a joystick device (i.e., $\mathbf{u}\left(t\right) \ne 0$), hence the velocity inputs are mixed accordingly via the implementation of (11). The goal of mixing the control inputs in Scenario 2, is to demonstrate that the overall trajectory of the formation can be influenced at any time by the human operator, without compromising safety (e.g., collision) or performance (e.g., connectivity breaks). The orientation of the vehicles is kept constant in both scenarios; thus, no angular velocity control input is applied.

The workspace is considered bounded and modeled as a sphere ${\mathbf{B}}_{\mathbf{w}}=\mathbf{B}\left({\mathbf{p}}_{\mathbf{w}},r_w\right)$, with ${\mathbf{p}}_{\mathbf{w}}={\left[0,0,0\right]}^T$ and $r_w=40$. We also follow a spherical representation for the obstacles ${\pi }_m=\mathbf{B}({\mathbf{p}}_{{\pi }_m},r_{{\pi }_m}),m\in \{1,\dots ,3\}$, where ${\mathbf{p}}_{{\pi }_1}= \left[10,0,4\right]$, ${\mathbf{p}}_{{\pi }_1}= \left[20,20,8\right]$, ${\mathbf{p}}_{{\pi }_1}= \left[30,0,12\right]$ are the centers and $r_{{\pi }_1}=r_{{\pi }_2}=r_{{\pi }_3}=2$ the radii respectively. We assume that the multi-agent system is enclosed inside a virtual sphere ${\mathbf{B}}_{\mathbf{c}}=\mathbf{B}\left({\mathbf{p}}_{\mathbf{c}},r_c\right)$, with ${\mathbf{p}}_{\mathbf{c}}$ being the formation centroid and $r_c=3.5$. The desired waypoints are located in ${\mathbf{w}}_{p_1}= \left[20,4,10\right]$, ${\mathbf{w}}_{p_2}= \left[30,-8,10\right]$, ${\mathbf{w}}_{p_3}= \left[20,28,10\right]$. We consider that each waypoint has been reached when the following inequality holds: $\parallel {\mathbf{p}}_c-{\mathbf{w}}_{p_i}\parallel \le 1$.

We choose the formation to be the one of a triangular right pyramid, where the followers are located at the pyramid’s apexes and the leader at the centroid. We consider a pyramid with height $h=2.45$ and base length $a=3.0$. Hence, all quadrotors must conform to the necessary distance specifications to achieve and maintain the triangular formation. In particular, the desired inter-agent distances are set $d_{10}^{\star }=(1/3)h$, $d_{12}^{\star }=a$, $d_{13}^{\star }=a$, $d_{14}^{\star }=a$, $d_{23}^{\star }=a$, $d_{24}^{\star }=a$, $d_{30}^{\star }=(2/3)h$, $d_{34}^{\star }=a$, $d_{40}^{\star }=(2/3)h$. We choose the safety inter-distance to avoid collisions as $d_{ij}^{\star }>\underline{d}=0.5$, and the distance for maintaining connectivity as $d_{ij}^{\star }<\overline{d}=4.5$, $(i,j)\in \mathcal{E}$. Furthermore, the specifications for minimum convergence speed and maximum steady state error (during formation) are set as $exp\left(-\frac{t}{5}\right)$ and $0.55$ respectively. This intuitively means that the errors should reach close to zero in approximately 20 s. We selected the performance functions to be as (16) and (17) to satisfy the specific performance requirements. The framework is minimally and infinitesimally rigid since graph $G= \left(\{0,1,2,3,4\},\left\{\right(1,0),(1,2),(1,3),(1,4),(2,3),(2,4),(3,0),(3,4),(4,0\left)\right\}\right)$ has exactly $3\times 5-6=9$ edges. The gains are set as $k=5$, $k_E=0.1$ and $k_{NF}=0.75$.

The evolution of Scenario 1 is depicted in Figure 5 at 12 sequential time instances, respectively. The trajectory of the vehicles in 3D is depicted in Figure 6. At this Scenario, no human input was commanded to the leader, hence the latter was guided only by the Navigation Function and the formation controller. The 3D trajectory of the leader vehicle versus the centroid of the formation is depicted in Figure 7. The errors of the inter-distances converge close to zero within the prescribed time, while the upper and lower bound specifications are constantly satisfied as shown in Figure 8. The control velocity commands (inertial frame) as dictated by (18), are depicted in Figure 9, Figure 10 and Figure 11.

The evolution of Scenario 2 is depicted in Figure 12 at 12 sequential time instances, respectively. The trajectory of the vehicles in 3D is depicted in Figure 13. At this Scenario, and at specific time instances, the human operator commands additional velocities to the leader as depicted in Figure 14. Hence, the latter was guided by the mixed-initiative control scheme (Equation (11)) and the formation controller. The 3D trajectory of the leader vehicle versus the centroid of the formation is shown in Figure 15. We may notice that the trajectories of the vehicles and the formation centroid were clearly affected by the application of the human commands. More specifically, we can observe that the route from the starting position to ${\mathbf{w}}_{p_1}$ as well as the route from ${\mathbf{w}}_{p_2}$ to ${\mathbf{w}}_{p_3}$ are completely different in comparison to Scenario 1, due to the influence of the human commands. Nevertheless, the errors of the inter-distances converge close to zero within the prescribed time, while the upper and lower bound specifications are constantly satisfied as shown in Figure 16. As indicated by the results, the human operator intervened to the motion of the formation (i.e., the resulting trajectories are not topologically equivalent to those from Scenario 1), without however compromising safety (e.g., collision) or performance (e.g., connectivity breaks). The control velocity commands (inertial frame) as dictated by (18), are depicted in Figure 17, Figure 18 and Figure 19.

5. Conclusions

This work presented a mixed-initiative motion control strategy for multiple quadrotors. The proposed approach incorporates simultaneously formation specifications, motion-planning commands as well as inputs by a human operator. More specifically, we consider a leader–follower aerial system, which autonomously attains a specific geometrical formation, by regulating the distances among neighboring agents and simultaneously avoiding inter-collisions. The desired formation is realized by a decentralized prescribed performance control strategy, which is computationally efficient, robust and straightforward regarding its implementation. The multi-agent system is safely guided towards goal configurations, by employing a properly defined navigation function which calculates the motion commands for the leading vehicle. In the proposed strategy, only the leader has knowledge of the workspace and the goal configurations. Additionally, the overall framework incorporates human commands for the desired motion of the leader via a teleoperation interface. Hence, the overall system is implicitly guided by the mixed-initiative motion of the leader in combination with the sustenance of distance formation specifications. The resulting mixed-initiative control system was evaluated via realistic simulation tests.

In our future research, we will proceed with the experimental validation of the proposed control strategy, to assess the efficacy of the overall architecture in the presence of external disturbances and sensor noise. Moreover, we aim to extend the proposed work in more complex workspaces (e.g., dense obstacle environments or generic geometry) using Harmonic Maps and replacing the Navigation Function with a Harmonic Potential Field controller [42,48]. Finally, we aim to employ our mixed-initiative control strategy for training motion planners to avoid local minima.