Distributed Motion Planning for Multiple Quadrotors in Presence of Wind Gusts

Abstract

This work demonstrates distributed motion planning for multi-rotor unmanned aerial vehicle in a windy outdoor environment. The motion planning is modeled as a receding horizon mixed integer nonlinear programming (RH-MINLP) problem. Each quadrotor solves an RH-MINLP to generate its time-optimal speed profile along a minimum snap spline path while satisfying constraints on kinematics, dynamics, communication connectivity, and collision avoidance. The presence of wind disturbances causes the motion planner to continuously regenerate new motion plans, thereby significantly increasing the computational time and possibly leading to safety violations. Control Barrier Functions (CBFs) are used for assist in collision avoidance in the face of wind disturbances while alleviating the need to recalculate the motion plans continually. The RH-MINLPs are solved using a novel combination of heuristic and optimal methods, namely Simulated Annealing and interior-point methods, respectively, to handle discrete variables and nonlinearities in real-time feasibly. The framework is validated in simulations featuring up to 50 quadrotors and Hardware-in-the-loop (HWIL) experiments, followed by outdoor field tests featuring up to 6 DJI M100 quadrotors. Results demonstrate (1) fast online motion planning for outdoor communication-centric multi-quadrotor operations and (2) the utility of CBFs in providing effective motion plans.

1. Introduction

Safe and reliable motion/formation coordination of networked multiple rotor-based unmanned aerial vehicles (UAVs) for surveillance/cargo transport is constrained by UAV kinematics and dynamics, environmental conditions such as wind gusts and shears, and the need to avoid collisions [1].

Since the seminal studies by Schouwenaars et al. [2] and Richards and How [3], the body of work that presents centralized and decentralized Mathematical Programming (MP) or Mathematical Optimization-based approaches for solving multi-vehicle motion planning problems (MVMPs) for ground [4,5], underwater [6,7], and aerial robot systems [8,9] has steadily grown. Mixed-integer nonlinear programs (MINLPs) can be used to capture all MVMP constraints effectively. However, the solutions to MINLP in the context of MVMP problems are computationally intensive (NP-hard), thus rendering real-time implementations and scalability challenging [10]. One approach to address this challenge is the use of receding horizon (RH) or model predictive control (MPC) methods. These methods have proven to be suitable for online and real-time motion planning for multi-robot systems in dynamic and partially unknown environments [11,12]. A key benefit of using MP-based approaches is that they allow for effectively studying the trade-off between mobility (speed, acceleration, waypoints), wireless communications (bandwidth, buffer size, path loss, delay), and energy consumption [8,13]. In this study, a distributed MINLP-based motion planner for multiple quadrotors is evaluated wherein each quadrotor locally computes and communicates (with others) its future trajectories. Figure 1 depicts snapshots of multiple quadrotor during the outdoor flight tests.

For quadrotors operating outdoors, solutions to MVMPs are exacerbated in the presence of wind disturbances. Despite significant work on multi-quadrotor motion planning, simulation or experimental approaches that explicitly incorporate effective strategies to counter wind gusts are only recently being reported in the literature [14,15,16,17]. Simulation approaches use wind models such as the Dryden and Von Karman models to incorporate realistic wind gusts during motion planning [18,19]. Experimental approaches to estimate wind speed and direction during motion planning use sensors such as ultrasonic anemometers, pitot tubes, accelerometers, gyroscopes, and barometers [20]. In this work, we use the Dryden model to simulate realistic wind gusts in simulations and Hardware-in-the-loop (HWIL) experiments.

Wind gusts continually disturb the quadrotors off their planned positions, thus requiring fast re-computation of the MP solutions to mitigate any increased risk of collisions [21]. Such repeated computations limit the effectiveness of MP frameworks in windy situations. As such, there is a need for a novel approach to reduce the repeated computation of MPs while ensuring that mission safety is not compromised. In this study, we present a novel MINLP solution approach that combines heuristic and optimal methods, namely simulated annealing and interior-point methods, respectively, to handle discrete variables and nonlinearities feasibly in real-time.

Control Barrier Functions (CBFs) are being increasingly used to provide robust collision avoidance for multi-vehicle systems [22]. CBFs have been applied to single UAV problems to demonstrate collision avoidance while exploring a cave environment and seamlessly integrate with an existing motion control algorithm [23]. CBFs have also been used for collision-free multi-quadrotor systems operating indoors and outdoors to demonstrate formation control and safe human teleoperation [24,25,26]. While CBFs are attractive, there is a varying level of robustness they can accord depending on the intrusiveness of the barrier constraint [27]. This study extends these use cases by applying CBFs for transit of quadrotors subjected to potentially dangerous wind gusts. Since the seminal studies by Schouwenaars et al. [2] and Richards and How [3], the body of work that presents centralized and decentralized Mathematical Programming (MP) or Mathematical Optimization-based approaches for solving multi-vehicle motion planning problems (MVMPs) for ground [4], underwater [6,7], and aerial robot systems [8,9] has steadily grown. Mixed-integer nonlinear programs (MINLPs) can be used to capture all MVMP constraints effectively [28]. However, the solutions to MINLP in the context of MVMP problems are computationally intensive (NP-hard), thus rendering real-time implementations and scalability challenging [10]. One approach to address this challenge is the use of receding horizon (RH) or model predictive control (MPC) methods [29,30]. These methods have proven to be suitable for online and real-time motion planning for multi-robot systems in dynamic and partially unknown environments [11,12]. A key benefit of using MP-based approaches is that they allow for effectively studying the trade-off between mobility (speed, acceleration, waypoints), wireless communications (bandwidth, buffer size, path loss, delay), and energy consumption [8,13]. In this study, a distributed MINLP-based motion planner for multiple quadrotors is evaluated wherein each quadrotor locally computes and communicates (with others) its future trajectories.

The main contributions of this study are as follows:

• An RH-MINLP and CBF-based framework for real-time, MVMP of multiple, networked quadrotors, operating outdoors in the presence of wind gusts. The resulting optimization problem is solved using a fast MINLP solution approach that combines Simulated Annealing and Interior point methods.
• Real-time validation of the framework with Hardware-in-the-loop (HWIL) experiments, followed by outdoor field tests featuring up to 6 DJI M100 quadrotors.
• Integration of the Dryden wind gust model with RH-MINLP and CBFs to perform realistic simulations (featuring up to 50 quadrotors) and HWIL testing with DJI Matrice M100 quadrotors. Source code has been made available at https://github.com/radlab-sketch/ (accessed on 24 December 2022).

Extensive performance results of the proposed framework are documented in this study.

2. Preliminaries

2.1. Quadrotor Motion and Paths

Consider a group of n networked quadrotors operating in appropriately defined inertial (East-North-Up (ENU)), body, and body-fixed frames of reference. Each quadrotor $i=1,\dots ,n$ has a start (origin) point $o^i$ and an end (goal) point $e^i$. O is the set of all start (origin) points. $o^i\in O,i=1,\dots ,n$. E is the set of all end points. $e^i\in E,i=1,\dots ,n$. The Euclidean distance between two quadrotors i and j is denoted by $d^{ij}$, $i=1,\dots ,n,j\ne i$.

All quadrotors are required to maintain a minimum safe distance $d_{\mathrm{safe}}$ to avoid collisions with each other. For each quadrotor i, the total arc length of its path is calculated using the Gaussian quadrature and is denoted by $U^i$. The linear and angular speeds of quadrotor i along its fixed path are denoted by $s^i$ and ${\omega }^i$, each expressed in the body-fixed frame of the quadrotor. p${}^i\left(t\right)$ = $(x^i\left(t\right),y^i\left(t\right),z^i\left(t\right))$ is the location of quadrotor i in the NED frame on its path at time t, and is calculated using the speed $s^i\left(t\right)$. The path for quadrotor i is represented by a 3-dimensional, piecewise 7th order spline curve ${\mathbb{SP}}^i\left(u\right)$, where the parameter u is the normalized arc length along the curve. The curves are obtained by combining three 1-dimensional piecewise 7th order splines. The piecewise 7th order spline curves feature continuous first derivatives (slope), second derivatives (curvature), third derivatives (jerk), and fourth derivatives (snap) along the curve. Figure 2 depicts the spline curve paths for a 6-quadrotor scenario.

The differential flatness of quadrotor dynamics, use of 3-dimensional piecewise 7th order spline curves, and the constraints on the speed, acceleration, snap, and yaw rate results in a kino-dynamically feasible speed profile for quadrotors [31,32]. A detailed discussion on spline curve design and analysis can be found in [33]. Let ${\kappa }^i\left(u\right)$ be the curvature along the spline curve i. The angular speed ${\omega }^i\left(u\right)$ can be computed from the linear speed and spline curvature as follows:

$${\omega }^i\left(u\right)=s^i\left(u\right){\kappa }^i\left(u\right).$$

(1)

2.2. Receding Horizon (RH)

$T_{\mathrm{hor}}$ denotes the length of the receding horizon and is determined by the capabilities of the embedded flight planning and control system. At any discrete time step t, each quadrotor must calculate its plan for $t+1$, …, $t+T_{\mathrm{hor}}$ and communicate this plan with other quadrotors in the network. This plan is denoted by ${\mathcal{P}}^i\left(t\right)=({\mathbf{p}}^i(t+1),\dots ,{\mathbf{p}}^i(t+T_{\mathrm{hor}}))$. Only the plan for time $t+1$ is implemented, and the planning process is restarted at time step $t+1$. At time step $T_{\mathrm{mission}}$, the mission is completed when the last-arriving quadrotor reaches its end point. If a quadrotor reaches its goal point before $T_{\mathrm{mission}}$, it hovers there until the mission is over.

2.3. Communication Modeling and Ordering

Each quadrotor is equipped with a mobile ad-hoc network (MANET) radio to exchange RH-MINLP solutions with other quadrotors. MANET communication is accomplished via direct one-hop connection or multi-hop relaying, thus resulting in a fully connected network. A round-robin scheduling technique for sequential wireless communication is used for fair distribution of MANET bandwidth and fault tolerance. The sequence of the quadrotors for the round-robin scheduling is determined randomly at the beginning of the mission. The first quadrotor in the round-robin is also the first to take off.

2.4. Dryden Wind Model and Spline Regeneration

The continuous Dryden turbulence model is used to simulate wind gusts and study their effects on the motion planning solutions [34]. We provided the first-in-literature, open-source implementation of the Dryden wind turbulence model in [35]. When the quadrotors are displaced off their pre-defined spline paths due to wind gusts, a rapid spline regeneration process ensures that each quadrotor has an updated spline path starting from the displaced location to the endpoint. Spline regeneration is effectively visualized in the accompanying video submission.

2.5. Control Barrier Functions and Safety Barrier Certificates

Each quadrotor is encapsulated in a super-ellipsoidal, Exponential Control Barrier Function (ECBF) depicted in Figure 2. For a pair of quadrotors $(i,j)$, the pairwise safe set ${\mathcal{B}}^{ij}$ and the pairwise super ellipsoid $h^{ij}(q^i,q^j)$ are defined as:

$${\mathcal{B}}^{ij}=\left\{(q^i,q^j)\right|h^{ij}(q^i,q^j)\ge 0\},$$

(2)

$$h^{ij}(q^i,q^j)={(x^i-x^j)}^4+{(y^i-y^j)}^4+{(\frac{z^i-z^j}{c})}^4-d_{safe}^4$$

(3)

where $q^i$ represents the full state of the quadrotor i, $d_{\mathrm{safe}}$ is the safety distance, c is the scaling factor along the Z-axis caused by airflow disturbance from the downwash of one quadrotor flying over of another quadrotor. In this study, the quadrotors flew at a safe distance from each other along the vertical direction. As such, c was chosen to be equal to one (1). The two quadrotors are considered safe when the two super-ellipsoids do not intersect. Readers are referred to [36] and its references for information about the mathematical construction, verification, and incorporation of ECBFs in motion planners through the use of Quadratic Programming (QP).

3. Optimization Model

The optimization (MINLP) problem ${\mathcal{O}}^i\left(t\right)$ solved by each quadrotor is expressed in (4)–(14). The optimization is performed for speed $s^i$ of the quadrotor along its specified path and auxiliary variables are used for position ($x^i$, $y^i$, $z^i$), arc length ($u^i$), acceleration ($a^i$), and communication connectivity ($C^{ij}$, $j\ne i$). Each quadrotor i uses the most current plans of all other quadrotors while solving ${\mathcal{O}}^i\left(t\right)$.

Problem Size:

$\mathcal{O}\left(t\right)$ has $(n+5)T_{\mathrm{hor}}$ decision variables, $(2n+5)T_{\mathrm{hor}}$ constraints, and variables bounds. The problem size grows linearly with the number of quadrotors n.

3.1. Objective Function

The objective function (4) forces the quadrotors to minimize the total distance (arc length) between their current location and the goal position over the entire receding horizon. It prioritizes the quadrotors’ movement to their goal positions.

$$\begin{array}{ccc}\hfill \underset{s,x,y,z,u,a,C}{\mathrm{minimize}}& & {\displaystyle \sum _{k=t+1}^{t+T_{hor}}(U^i-u^i\left(k\right))}\hfill \\ \hfill \mathrm{subject} \mathrm{to}& & \forall k\in \{t+1,\dots ,T_{\mathrm{hor}}\}\hfill \end{array}$$

(4)

$$\begin{array}{ccc}& & (x^i\left(k\right),y^i\left(k\right),z^i\left(k\right))={\mathbb{SP}}^i\left(u^i\left(k\right)\right)\hfill \end{array}$$

(5)

$$\begin{array}{ccc}& & u^i\left(k\right)=u^i(k-1)+s^i\left(k\right)\Delta t\hfill \end{array}$$

(6)

$$\begin{array}{ccc}& & u^i\left(t\right)\le u^i\left(k\right)\le U^i\hfill \end{array}$$

(7)

$$\begin{array}{ccc}& & s^i\left(k\right)=s^i(k-1)+a^i\left(k\right)\Delta t\hfill \end{array}$$

(8)

$$\begin{array}{ccc}& & s_{\min }\le s^i\left(k\right)\le s_{\max }\hfill \end{array}$$

(9)

$$\begin{array}{ccc}& & a_{\min }\le a^i\left(k\right)\le a_{\max }\hfill \\ & & \forall j\in \{1,\dots ,n\},j\ne i\hfill \end{array}$$

(10)

$$\begin{array}{ccc}& & {\left(d^{ij}\left(k\right)\right)}^2\ge d_{\mathrm{safe}}^2\hfill \end{array}$$

(11)

$$\begin{array}{ccc}& & {\left(d^{ij}\left(k\right)\right)}^2\le M(1-C^{ij}\left(k\right))+{\eta }_d^2\hfill \end{array}$$

(12)

$$\begin{array}{ccc}& & {\displaystyle \sum _{j:j\ne i}{C}^{ij}\left(k\right)\ge n_{\mathrm{conn}}}\hfill \end{array}$$

(13)

$$\begin{array}{ccc}& & C^{ij}\left(k\right)\in \{0,1\}\hfill \end{array}$$

(14)

3.2. Path (Kinematic) Constraints

Constraints (5)–(7) define the quadrotor’s location along its path. Location can be defined in two ways: arc length traversed along the quadrotor’s path ($u^i$) and the coordinates of its location ($x^i$,$y^i$,$z^i$). The former is used to define kinematic and dynamic constraints, while the latter is used to calculate Euclidean distances between quadrotors for collision avoidance and communication connectivity. Constraint (5) defines the 7th order spline curves ${\mathbb{SP}}^i\left(u^i\left(k\right)\right)$ that link these two sets of variables. Given its location at the beginning of the planning period, constraint (6) increments the arc length at each time step based on the speed of the quadrotor. $\Delta t$ is the DJI Matrice M100’s flight controller update rate obtained directly during actual tests. Constraint (7) ensures that the quadrotor will remain between its location at the beginning of the planning period and its goal position at all times.

3.3. Speed and Acceleration (Dynamic) Constraint

Given the quadrotor’s linear speed $s^i$ at the beginning of the planning period, constraint (8) increments the speed at each time step based on its acceleration $a^i$. Constraints (9)–(10) are dynamic constraints that ensure that the linear speed (and hence, the angular speed) and acceleration for each quadrotor at each time step are sufficiently bounded. The bounds are determined by the capabilities of the DJI M100 quadrotor and the curvature of the spline paths to ensure that the quadrotor motors do not saturate. Since the 7th order spline curves are generated while minimizing snap (4th derivative), the paths allow for smooth trajectory following. The angular speed required by the quadrotors corresponding to the optimal linear speed is always achievable and is determined using a relationship (1).

3.4. Collision Avoidance Constraint

The non-convex constraint (11) defines the required minimum distance $d_{\mathrm{safe}}$ between each pair of quadrotors at all times to avoid collisions.

3.5. Communication Connectivity Constraint

The binary variable $C^{ij}\left(t\right)$ indicates whether two quadrotors are in one-hop communication range ${\eta }_d$ of each other, as shown in the following equation:

$$C^{ij}\left(t\right)=\left\{\begin{array}{l}1,\hspace{1em}\mathrm{if}{d}^{ij}\le {\eta }_d,\\ 0,\hspace{1em}\mathrm{otherwise}.\end{array}\right.$$

(15)

4. Optimization Solution Technique

The numerical optimization solution technique for solving (4)–(14) is divided into three levels—the outer level, the middle level, and the lower level.

Since there are multiple decision makers (i.e., multiple quads), the outer level establishes the initial conditions of multi-quad distributed decision making in terms of decision ordering and associated motion plan data at each time step t.

The middle and lower levels are rooted in well-established numerical optimization solution approaches for solving mixed integer nonlinear programming (MINLP) problems [37,38]. In the middle level, the algorithm handles the discrete variables and communication constraints of each quadrotor’s problem $O^i\left(t\right)$ via Simulated Annealing (SA). The SA approach establishes the discrete variable, thus turning the rest of the optimization problem into a continuous nonlinear programming problem. This underlying continuous nonlinear programming problem is solved in the lower level using interior-point methods to determine each quadrotor’s speed during the time horizon.

4.1. Outer Level: Distributed Decision Making

All quadrotors are in the communication range of each other at time 0. As discussed, a round-robin decision-making and communication order is enforced among quadrotors. The ordering can be randomly assigned or can be assigned a priori. It is assumed without loss of generality that the ordering is represented as $1,\dots ,n$.

Each quadrotor i sequentially solves the problem ${\mathcal{O}}^i(t+1)$ at time t by taking into account the following plans:

• Plans ${\mathcal{P}}^j(t+1)$ for quadrotors $j<i$, as these quadrotors have already calculated their new plans, and 
• Plans ${\mathcal{P}}^j\left(t\right)$ for quadrotors $j>i$, as these quadrotors have yet to calculate their new plans.

If ${\mathcal{O}}^i(t+1)$ is feasible, the new plan becomes ${\mathcal{P}}^i(t+1)$. If it is infeasible, then the quadrotors simply hover in place for safety till an operator takes over. Each plan is then communicated to the rest of the quadrotors.

4.2. Middle Level: Simulated Annealing for Discrete Variables and Communication Constraints

The outer level requires the solution of ${\mathcal{O}}^i(t+1)$ for each quadrotor i at each time step t. The communication variables and the connectivity constraints (12) contribute significantly to the computational complexity of ${\mathcal{O}}^i(t+1)$: the communication variables are binary, there are $n\ast T_{\mathrm{hor}}$ of them in each instance (the remaining variables do not change by fleet size), and there are $n\ast T_{\mathrm{hor}}$ communication constraints at each instance. As the problem size scales in n, these Big-M communication constraints that include integers and the parameter M introduce non-trivial computational effort in finding a feasible solution [39]. The middle level of the numerical solution process tackles this computational burden to generate real-time solutions.

When deciding upon an appropriate method to handle the communication variables and constraints, it should be first noted that a feasible solution to ${\mathcal{O}}^i(t+1)$ is implementable, even if it is not optimal. From a solution approach perspective, a well-established method such as branch-and-bound (BB) with deep dives can be used to solve MINLPs [40,41], but this may still require the solution of multiple instances of the RH-MINLP (and hence more computational effort/time) before identifying a feasible solution, especially as the problem size grows.

By contrast, Simulated Annealing (SA) [42] is a stochastic method to quickly identify effective feasible solutions for MINLPs [43,44]. The sampling approach for neighbors of an incumbent and solution quality evaluation are critical to SA. At each iteration of SA, we sample a small number of neighbors of the current solution (also known as an “incumbent”). Any neighbor can replace the incumbent with probability 1 if it is better than the incumbent or with probability $ex{p}^{-\Delta /Temp}$ if it is worse than the incumbent, where $\Delta$ measures the change in solution quality and $Temp$ is the current temperature of the system. $Temp$ is reduced at every iteration of SA, which promotes localization around a good incumbent in the later iterations. The heuristic is stopped when $Temp$ is sufficiently small or when a time limit is reached.

Sampling Neighbors Approach:

As described in Algorithm 1, the binary values for the communication variables were chosen to attain or promote feasibility of the solution. The condition $C^{ij}=0$ does not always imply that quadrotor i and j are not within communication distance. This condition indicates that the quadrotors do not need to be within communication distance. The incumbent solutions which satisfy the $n_{\mathrm{conn}}$ constraint with equality indicate that the quadrotors are required to maintain at least $n_{\mathrm{conn}}$, and not exactly $n_{\mathrm{conn}}$. This method for generating a neighbor of the incumbent solution for N waypoints is depicted in Algorithm 1.

Algorithm 1 Generating a neighbor of an incumbent solution for Simulated Annealing

1: Input: The sets $\{1,\dots ,N-1\}$ and $\{1,\dots ,T_{hor}\}$, ${\mathcal{C}}_{k^{\prime }}$ 2: Output: ${\mathcal{C}}_{k^{\prime }}$ 3: Randomly select $k^{\prime }\in \{1,\dots ,T_{hor}\}$. 4: Randomly select $j^{\prime }\in {\mathcal{C}}_{k^{\prime }}$ and $j^{″}\notin {\mathcal{C}}_{k^{\prime }}$. 5: Set $C^{i{j}^{\prime }}\left(k^{\prime }\right)=0$, $C^{i{j}^{″}}\left(k^{\prime }\right)=1$. 6: Update: ${\mathcal{C}}_{k^{\prime }}\leftarrow {\mathcal{C}}_{k^{\prime }}\backslash j^{\prime }\bigcup j^{″}$.

Evaluating Solution Quality: Once the communication variables are fixed and constraint (13) satisfied, a reduced form of ${\mathcal{O}}^i\left(t\right)$, denoted by ${\mathcal{O}}_{\mathrm{reduced}}^i\left(t\right)$, is created where constraints (12)–(14) are replaced by

$${\left(d^{ij}\left(k\right)\right)}^2\le {\eta }_d^2\mathrm{if}{C}_{ij}\left(k\right)=1$$

(16)

The objective function value of ${\mathcal{O}}_{\mathrm{reduced}}^i\left(t\right)$ is used as the metric of solution quality by SA (chosen as ∞ for an infeasible instance).

Moreover, ${\mathcal{O}}_{\mathrm{reduced}}^i\left(t\right)$ features a reduction in the computational burden as n increases when compared to (4)–(14) due to the lack of big-M constraints and integer variables.

Reduced Problem Size: The resulting optimization problem is also significantly smaller in size, with $5T_{\mathrm{hor}}$ variables and $(N+n_{conn}+4)T_{\mathrm{hor}}$ constraints.

4.3. Lower Level: Nonlinear Optimization

${\mathcal{O}}_{\mathrm{reduced}}^i\left(t\right)$ is a non-linear, non-convex program and can be solved using an interior-point method for nonlinear optimization adapted from [45]. Readers are referred to [45] and its references for further details on this nonlinear optimization technique.

5. Experimental Setup and Results

The RH-MINLP framework was implemented in numerical simulations, HWIL, and outdoor flight tests. The optimization model ${\mathcal{O}}^i$, optimization solver (all three levels), and CBFs were implemented in Python 3.7. Custom spline generation functions implemented in Python were used to generate the minimum snap 7th order splines passing through N pre-selected waypoints. The Dryden Model to generate wind gusts was also implemented on the ground station computer in Python.

Numerical simulations were carried out using a desktop computer fitted with an Lenovo i5-6400 Intel CPU 2.70 GHz Quad-core processor and 8 GB RAM. HWIL and outdoor flight tests were conducted with DJI M100 quadrotors fitted with a Raspberry Pi 4 (Quad-core Cortex-A72 (ARM v8) 64-bit SoC @ 1.5 GHz and 4 GB RAM) computer. Each quadrotor was fitted with a transceiver radio module operating in the 915 MHz license-free frequency bands, providing half-duplex bi-directional RF links at 300 Kbps. A ground station computer (Lenovo Intel Dual Core i5 @ 3.3 GHz Processor and 8 GB RAM) was part of the MANET and hosted a web-based user interface for mission configuration, clock synchronization, and mission upload. The GPS sensor’s positional accuracy and sampling rate onboard the DJI M100 are 1 m (average) and 4 Hz, respectively. The fused telemetry stream data from the DJI M100 were sampled at 50 Hz to obtain state feedback (real-time locations and speeds). The quadrotors were equipped with the TB48D battery that has a capacity of 5700 mAh [46].

Table 1. Motion and Communication Parameters used in HWIL, numerical Simulations, and field tests.

$d_{\mathrm{safe}}$	3 m	$s_{\min }$	0	$s_{\max }$	0.5 m/s
$a_{min}$	−0.25 m/s${}^2$	$a_{\max }$	0.25 m/s${}^2$	$n_{conn}$	1
${\eta }_d$	5 m	$P_{tr}$	100 mW	$T_{\mathrm{hor}}$	3

lists key motion and communications parameters used in HWIL, numerical simulations, and field tests.

The HWIL tests were executed using the DJI Assistant 2 flight simulation software. The software provided real-time emulation of the telemetry outputs and velocity yaw rate inputs to the DJI N1 flight controller based on the rigid body dynamics of the DJI M100. During HWIL testing, the quadrotors and ground station were placed in line with approximately 0.5 m between each quadrotor and approximately 1.5-m separation from the ground station.

The Python implementation interfaced with DJI M100 real-time telemetry data via UART and a Linux SDK provided by the DJI M100 N1 flight computer. The speed obtained by solving the $\mathcal{O}$ served as the set-point for the DJI M100’s N1 flight controller, which exerted the appropriate control effort to achieve the set-point. During outdoor tests, a manual override was implemented to stall the quadrotor in place until a human operator took over in case of degraded MANET performance (average inter-quadrotor wireless latency greater than 0.33 s), heavy wind gusts (>18 m/s mean value), or critically low battery in the quadrotors (<5% battery power).

Numerical simulations focused on exploring scalability (up to 50 quadrotors) of the RH-MINLP + CBF framework and comparing the computational efficiency of the SA + Interior Point Method solver with another established method (Branch-and-Bound (BB) + Interior Point Method). HWIL tests were performed with up to 6 DJI M100 to study (1) the effect of wind disturbances on safety violations, (2) the safety accorded by CBFs, and (3) the effect of wind disturbances and CBFs on $T_{\mathrm{mission}}$. Actual flight tests followed the simulations and HWIL tests in outdoor, windy settings to validate the real-time feasibility of the RH-MINLP + CBF approach.

Each flight in numerical simulations, HWIL tests, and outdoor tests was conducted in a 40 m × 40 m × 20 m airspace and consisted of the following three sub-tasks:

• Take-off and Formation: The quadrotors would take off one at a time and move to a preset location to organize themselves in a geometric formation. Example initial formations included a straight line, triangle, and rectangle as shown in Figure 1. The initial height of the formation was 12.5 m above ground level (AGL).
• Waypoint-based Transit: Once in formation, the quadrotors would start their transit while maintaining their flight formation, to the extent possible, and visit a total of 5 waypoints spread out across an area of 40 m × 40 m × 20 m.
• Return-To-Home (RTH): Once the quadrotors visited all the waypoints, they sequentially executed an RTH procedure to land at their take-off locations safely.

Key evaluation metrics are defined as:

• $T_{\mathrm{mission}}$: denotes the transit time between the start of the multiple quadrotor formation and their return to their start points averaged over multiple runs.
• $n_{\mathrm{CBF}}$: number of CBF activations averaged over multiple runs and rounded to the nearest integer.
• $T_{\mathrm{comp}}$: Solver computation time averaged over all quadrotors, further averaged over multiple runs.
• RR: Optimization based motion-plan re-planning rate in Hz, average over multiple runs.
• $T_{\mathrm{saving}}$: denotes the percentage of $T_{\mathrm{comp}}$ time saved when using SA instead of the BB method.
• $D_{\mathrm{obj}-\mathrm{gap}}$: denotes the percentage gap in the objective function values (as defined by (4)) between BB and SA.

Each numerical simulation was repeated five (5) times, HWIL experiments were repeated nine (9) times, and outdoor flight tests were conducted five (5) times. Dryden gusts with average 9 m/s speed and standard deviation of 1.23 m/s were used in numerical simulations and HWIL.

5.1. HWIL: Flight Safety Accorded by CBFs

For a fixed $d_{\mathrm{safe}}$, Figure 3 (top) depicts the number of times the inter-quadrotor distance ($d^{ij}$) was less than $d_{\mathrm{safe}}$ during windy conditions. These actual safety violations were observed because the wind gusts disturbed the quadrotors off their planned course despite recalculating $\mathcal{O}$. This observation underscores the need for additional safeguards to prevent in-flight collisions.

Once CBFs were added to the motion-planning process, as depicted in Figure 3 (bottom), there were zero (0) actual safety violations during flight despite windy conditions, i.e., several potential safety violations (in the future) were avoided due to CBF activations.

Also, the number of potential safety violations (in the future) that were prevented by adding the CBFs were significantly less than the case when no CBFs were present. This is due to the fact that the quadrotors were moving in a safer fashion from the very beginning of their mission due to the activation of the CBFs. Hence, there was a relatively lower number of potential safety violations in future time steps. We note that in the absence of Dryden wind gusts, no safety violations were observed while solving $\mathcal{O}$.

5.2. HWIL: Effect of Wind Disturbances and CBFs on $T_{mission}$

For a fixed $d_{\mathrm{safe}}$, Figure 4 depicts the change in $T_{\mathrm{mission}}$ in the absence and presence of wind disturbances. It is observed that $T_{\mathrm{mission}}$ increases significantly in the presence of the Dryden wind gusts because $\mathcal{O}$ is solved more often (the wind gusts cause the quadrotors to move off-course).

Figure 4 also depicts the change in $T_{\mathrm{mission}}$ in the presence of wind disturbances while using CBFs (blue-colored data series). It is observed that the $T_{\mathrm{mission}}$ increases with CBFs in windy conditions as compared to the no wind case. However, it is also observed that in the presence of CBFs, $T_{\mathrm{mission}}$ is lower than the $T_{\mathrm{mission}}$ when no CBFs are present. This is due to the fact that the CBF activations afford collision-avoidance from the very beginning of the mission thereby reducing the number of times $\mathcal{O}$ needs to be resolved. A reduced $T_{\mathrm{mission}}$ also led to lower use of quadrotor battery. Averaged over the 9 runs, the battery savings per quadrotor in the case of the 2, 3, 4, 5, and 6 quadrotor test were $25.14\%$, $20.39\%$, $18.52\%$, $11.81\%$, and $16.4\%$, respectively.

5.3. Numerical Simulations for Scalability

Numerical simulations were performed for up to 50 quadrotors operating outdoors to study computational scalability in the presence of Dryden wind gusts.

Table 2. Numerical simulation results to study scalability of the RH-MINLP framework. Data averaged over 5 runs.

n	SA		BB
	${\mathit{T}}_{\mathbf{comp}}$	${\mathit{n}}_{\mathbf{CBF}}$	${\mathit{T}}_{\mathbf{comp}}$	${\mathit{n}}_{\mathbf{CBF}}$	${\mathit{T}}_{\mathbf{saving}}$	${\mathit{D}}_{\mathbf{obj}\mathbf{-}\mathbf{gap}}$
10	132.3	14	195.2	16	47.54%	−5.29%
15	186.8	17	311.9	19	66.97%	−4.66%
20	271.1	19	648.5	18	139.21%	0.2%
25	383.9	22	902.4	21	135.06%	−3.10%
30	562.1	27	1376.2	30	144.83%	−1.98%
40	819.3	31	2314.7	31	182.52%.	−7.81%
50	1184.5	38	3920.7	42	231%	−6.52%

notes results for each of these scenarios. The SA + interior-point numerical solution method was compared with a BB + interior point method as presented in [40,41].

As is observed, for all cases, $n_{\mathrm{CBF}}$, and $T_{\mathrm{comp}}$ increase with n. The increase is pronounced at higher n due to increased collision possibilities and an increase in the number of variables and constraints for $\mathcal{O}$.

It is observed that SA outperforms BB in terms of $T_{\mathrm{comp}}$ as indicated by $T_{\mathrm{saving}}$. These $T_{\mathrm{comp}}$ savings can be attributed to the fact that BB fixes a subset of the binary variables (and leaves the rest as continuous, between 0 and 1) at any iteration which in turn, causes the nonlinear solver to face larger problems with BB than with SA. However, when it concludes, BB yields superior objective function values for all tests except in the case of $n=20$, which is expected since it focuses on global optimality. As the number of quadrotors is scaled up, BB may not conclude in real-time, either due to the size of the problem or the nonconvexity of the underlying nonlinear problem due to the collision avoidance constraints. On the other hand, SA is a search heuristic that focuses first on attaining feasibility and then on further improving the objective function value, and can be stopped within the real-time requirements.

5.4. Outdoor Flight Tests

Outdoor field tests were conducted with up to 6 DJI M100 quadrotors to validate the RH-MINLP + CBF framework. The test was conducted in an open field at a park in Philadelphia. The wind disturbances during the flight tests were measured using an FT205 ultrasonic anemometer mounted on a DJI M100 quadrotor using a 20-inch long 3D printed pole as shown in Figure 1. The FT205 was sampled at a frequency of 2 Hz. The wind measurements were performed with the quadrotor hovering at a height of 12.5 m above ground level. The quadrotor was flown approximately 50 m away from the multi-quadrotor system to maintain safety. During the field test with 2 quadrotors, the mean wind speed was 12.23 m/s with a standard deviation 3.49 m/s. During the field test with 3 quadrotors, the mean wind speed was 10.33 m/s with a standard deviation 1.54 m/s. During the field test with 6 quadrotors, the mean wind speed was 13.5 m/s with a standard deviation 2.0 m/s.

Figure 1 depicts a snapshot of a 2, 3, and 6-quadrotor tests.

Table 3. Outdoor flight tests results demonstrate the feasibility of the RH-MINLP + CBF framework. Data averaged over 5 flight tests per n.

n	Wind Speed (mean, stdev) (m/s)	${\mathit{T}}_{\mathbf{mission}}$ (s)	${\mathit{n}}_{\mathbf{CBF}}$	${\mathit{T}}_{\mathbf{comp}}$ (ms)	RR (Hz)
2	12.23, 3.49	141.52	1	26.8	30
3	10.33, 1.54	258.10	2	37.2	24
6	13.5, 2.0	829.47	4	74.6	12

depicts the average $T_{\mathrm{mission}}$, $n_{\mathrm{CBF}}$, $T_{\mathrm{comp}}$, and re-planning rate (RR). $T_{\mathrm{comp}}$ varied from 26.8 ms to 74.6 ms. Re-planning rates varied between 12 Hz to 30 Hz.

Readers are referred to the accompanying video for footage of the flight tests. For the 2-quadrotor test, a CBF was activated when the quadrotors completed a concentric-spiral path mission and the two quadrotors were about to breach the safety distance threshold. One of the quadrotors came to a hover, allowing the other quadrotor to pass. Similarly, for the 3-quadrotor test, the CBF was activated twice, once each between two different pairs. For the 6-quadrotor test, 4 CBFs were activated between different pairs when the quadrotors came close to each other due to wind gusts.

6. Discussion

• Due to the inherent distributed nature of the decision making, certain quadrotors’ decisions may render the coordination problem difficult or infeasible to solve for other quadrotors. In some cases, reassigning a different decision order helped improve overall solutions. Safety is always maintained since the quadrotors can hover in place in case of infeasibility.
• Even when CBFs are used to provide an additional layer of assistance in collision avoidance, guaranteeing robustness to collisions is non-trivial because of the non-linearities in the multi-UAV system and unknown wind disturbances. Future work will explore the study of robust control barrier functions for constrained stabilization of the multi-UAV system, as noted in [27].
• While rare, satisfying the communication constraint can still result in network partitions. Strategies to mitigate such unusual circumstances have been developed and presented in [47].
• As observed in Figure 4, as the number of quadrotors increase, the difference between $T_{\mathrm{mission}}$ without CBFs and $T_{\mathrm{mission}}$ with CBFs decreases. The effectiveness of CBFs in multi-robot systems at scale is an open area of research and will be investigated in future work [48].

7. Conclusions

This work validated a distributed, RH-MINLP + CBF-based motion planner for multiple quadrotors operating in windy outdoor environments. The validation was performed via numerical simulations, HWIL, and outdoor flight tests. A novel 3-level optimization solver that used Simulated Annealing was developed to expedite the optimization solution process resulting in feasible online motion-planning. The Dryden wind gust model provided realistic wind effects during the simulations and HWIL experiments. The use of CBFs provided protection against safety violations and potential mid-flight collisions that could have resulted due to the wind disturbances, in turn reducing mission times. The simulation and experiment results demonstrated the computational time savings accorded by the SA-based numerical solution process when compared to a well-established method, thus demonstrating the scalability of this framework. Future work will focus on incorporating online estimates of wind gusts in the planning using a non-sequential MANET communication techniques, and approaches to improve the sampling in Algorithm 1 based on current distances and past plans.