# Integrated Decision Control Approach for Cooperative Safety-Critical Payload
Transport in a Cluttered Environment

Nishanth Rao, Suresh Sundaram Nishanth Rao is with the Artificial Intelligence
and Robotics Lab, Department of Aerospace Engineering, Indian Institute of
Science, Bengaluru, India, 560012<EMAIL_ADDRESS>Sundaram is
with the Artificial Intelligence and Robotics Lab, Department of Aerospace
Engineering, Indian Institute of Science, Bengaluru, India, 560012.
<EMAIL_ADDRESS>
###### Abstract

In this paper, the problem of coordinated transportation of heavy payload by a
team of UAVs in a cluttered environment is addressed. The payload is modeled
as a rigid body and is assumed to track a pre-computed global flight
trajectory from a start point to a goal point. Due to the presence of local
dynamic obstacles in the environment, the UAVs must ensure that there is no
collision between the payload and these obstacles while ensuring that the
payload oscillations are kept minimum. An Integrated Decision Controller (IDC)
is proposed, that integrates the optimal tracking control law given by a
centralized Model Predictive Controller with safety-critical constraints
provided by the Exponential Control Barrier Functions. The entire payload-UAV
system is enclosed by a safe convex hull boundary, and the IDC ensures that no
obstacle enters this boundary. To evaluate the performance of the IDC, the
results for a numerical simulation as well as a high-fidelity Gazebo
simulation are presented. An ablation study is conducted to analyze the
robustness of the proposed IDC against practical dubieties like noisy state
values, relative obstacle safety margin, and payload mass uncertainty. The
results clearly show that the IDC achieves both trajectory tracking and
obstacle avoidance successfully while restricting the payload oscillations
within a safe limit.

###### Index Terms:

Cooperative Payload Transport, Multi-UAV System, Linear Model Predictive
Control, Exponential Control Barrier Functions, Cluttered Environment

##  I Introduction

Recent advancements in Unmanned Aerial Vehicles (UAV) and sensor technology
has created a widespread interest in their ability to solve payload
transportation problems in the logistics sector. Collaborative interaction of
UAVs can efficiently solve many transportation problems, especially when there
are requirements for delivering versatile payloads that vary in size. This is
because utilizing multiple UAVs to lift payloads are economically feasible and
efficient, as opposed to utilizing a single UAV of a bigger form factor.
Industrial missions[1], military expeditions[2] and medical search-rescue
operations [3] demand that the UAVs have to ensure proper delivery of the
payload along a set pre-planned global trajectory, while at the same time,
preventing collisions with the local dynamic obstacles present around it and
maintaining minimal oscillations of the payload throughout the course.
However, when there are multiple UAVs interacting with one another to
transport a payload, existing path planning algorithms[4] fall short, as they
don’t consider the kinodynamic-maneuverability of the system and it’s highly
coupled complex dynamics.

Majority of existing literature [5], [6], [7] models the payload suspension as
a point mass cable suspension. This results in simplified system dynamics,
which is generally unrealistic in situations where there are requirements to
transport heavy payloads. In [8], a payload suspension model is used, and
formation control of UAVs are considered to negotiate a tight passage. However
formation control is not practically possible for rigid link payload
suspension with minimal oscillation constraints. Moreover, additional
mechanisms like rack-pinion attachment points must be employed during
formation control when rigid link suspensions are employed.

In [9], [10] the payload is modelled as an external disturbance rather than a
point mass. The direction of this external disturbance force is along the
cable connecting the payload and the UAV. This may lead to suboptimal results,
as the control law cannot leverage the benefits of knowing the actual dynamics
of the payload. With this approach, it is also hard to ensure bounds on the
states of the payload such as keeping the oscillations under certain
threshold. Further, Artificial Potential Fields (APFs) are considered for the
purpose of obstacle avoidance in [9]. However, APFs can have limitations in
certain scenarios like passing between closely spaced obstacles, exhibiting
oscillations or being trapped in local minima as discussed in [11]. Moreover,
Control Barrier Functions (CBFs) are more suited for obstacle avoidance than
APFs as discussed in [12].

In contrast, [13] models the cable-payload suspension as a series of mass-
spring-damper system, thus taking into account the elastic nature of the
cable. A null-space based controller is then designed for the purpose of
trajectory tracking and obstacle avoidance. While cable suspension of light,
point mass payload may be desirable in some circumstances, they aren’t
desirable when the payload is heavy, as they may cause undesirable
uncontrollable oscillations due to the cable elasticity. Further, with cable
suspension there can exist a point in the course of the mission where the
cables can become slack, causing the payload to become massless, or even
accelerate upwards. Controlling the states of the payload in these situations
becomes complicated and may not be feasible in practical situations where the
exact states of the payload like its relative position and linear acceleration
are not known, but needs to be estimated based on the cable orientation and
the tension in it. It can also lead to singularities in the dynamics of the
system as discussed in [14], resulting in extremely complex control laws.

Even though the aforementioned works address the problem of cooperative
payload transport, it is evident that there is a need to model heavy payload
as a rigid body, with rigid link suspension, that enables easy control over
the states of the payload. Moreover, safety-critical payload transport must be
ensured in a cluttered environment, with minimum deviation from the preset
global trajectory while avoiding the obstacles.

In this paper, the payload-UAV system is modelled end-to-end, keeping in mind
the necessity to control the states of the payload throughout the course of
the mission. The dynamics of the system is modelled using Lagrangian mechanics
as discussed in [15], and rigid, massless links are used in place of cable
suspension, as demonstrated in [16]. This makes it relatively easier to
calculate the payload states at all times. Moreover, using rigid links
provides complete control authority to the UAVs to handle the payload as
required. To ensure accurate tracking of the payload, a centralized Model
Predictive Controller (MPC) is employed. As of now, centralized MPC is shown
to outperform distributed MPC at a lower sampling rate in [16]. Furthermore,
the major advantage of using MPC based controllers is it’s inherent ability to
take into consideration the state and control input constraints. The concept
of receding horizon also ensures that future uncertainties and requirements
are taken into consideration while obtaining the control law at the current
time step. Inspired by the application of control barrier functions to safety-
critical systems, an obstacle avoidance controller is developed for the entire
payload-UAV system using Exponential Control Barrier Functions (ECBFs).
Barrier functions naturally provide safety-critical constraints that can be
leveraged in an optimization framework. The Integrated Decision Controller
fuses the optimal tracking control law as provided by the MPC into an
optimization-based obstacle avoidance decision controller. The IDC is designed
in a way that allows any trajectory tracking controller to be used with the
obstacle avoidance controller.

The rest of the paper is organized as follows: Section II covers the system
description and state space formulation. Section III covers the design and
development of the Integrated Decision controller for the payload-UAV system,
with brief discussion on Model Predictive Control and Exponential Barrier
Functions. In Section IV, the proposed controller is evaluated, and the
results for numerical simulations as well as a Gazebo simulation are presented
along with an ablation study demonstrating the robustness of the IDC. Finally,
conclusions and possibilities for future work is discussed in Section V.

$F_{j}=\mathcal{F}_{j}\boldsymbol{R}_{j}b_{j3}$$p_{i}$$q_{i}\in
S^{2}$$l_{i}$$m_{i},\boldsymbol{J}_{i}$$m_{0},\boldsymbol{J}_{0}$$(r_{i},\boldsymbol{R}_{i})\in
SE(3)$$(r_{0},\boldsymbol{R}_{0})\in SE(3)$xyz Figure 1: A Schematic diagram
of $N$ UAVs transporting a rigid body, attached via massless rigid links
present in the NED coordinate frame is shown. The black dot at the center of
the payload denotes its center of mass.

##  II System Model

In this section, a brief description of the equations of motion for the
payload-UAV system is provided. The derivation of the system dynamics
parallels the discussion presented in [15] except that the equations of motion
are resolved in the payload frame of reference [16].

###  II-A Payload-UAV System Description

There are $N$ UAVs that are collaboratively transporting a rigid payload, as
shown in Fig. 1. The UAVs are attached to the payload via massless rigid
links. The rigid links are connected to the payload and the UAVs via spherical
joints. This way, the attitude dynamics of the UAVs are decoupled from the
dynamics of the rest of the system.

Throughout this paper, the NED (North, East, Down) coordinate system is used,
with the positive $z$-axis pointing downward. The direction for positive $x$-,
$y$\- axis are then chosen on the basis of the right hand rule. The basis
vectors for the inertial frame and the body-fixed frames are denoted by
$\left\\{e_{1},e_{2},e_{3}\right\\}$ and
$\left\\{b_{i1},b_{i2},b_{i3}\right\\}$ respectively. The variables that refer
to the payload are subscripted by $0$, and the variables that refer to the
$i^{th}$-UAV is subscripted by $i$.

The location of the center of mass of the payload in the inertial frame is
denoted by $r_{0}\in\mathbb{R}^{3}$, and the location of the center of mass of
the $i^{th}$-UAV in the inertial frame is denoted by $r_{i}\in\mathbb{R}^{3}$.
The position vector of the attachment point of the rigid link of length
$l_{i}$ corresponding to the $i^{th}$-UAV in the payload frame is denoted by
$p_{i}\in\mathbb{R}^{3}$. The unit vector along the rigid link corresponding
to the $i^{th}$-UAV, pointing towards the payload is denoted by
$q_{i}\in\mathbb{S}^{2}$, where
$\mathbb{S}^{2}=\left\\{q_{i}\in\mathbb{R}^{3}\ |\
\|q_{i}\|_{{}_{2}}=1\right\\}$. The attitude of the payload is characterized
by $\boldsymbol{R}_{0}\in SO(3)$, the rotation matrix that rotates a vector in
the payload-fixed frame to the inertial frame. Similarly, the attitude of the
$i^{th}$-UAV is characterized by $\boldsymbol{R}_{i}\in SO(3)$, the rotation
matrix that rotates a vector in the $i^{th}$-UAV fixed frame to the inertial
frame. Let $\boldsymbol{J}_{0}\in\mathbb{R}^{3\times 3}$ denote the inertia
matrix of the payload with mass $m_{0}$, and
$\boldsymbol{J}_{i}\in\mathbb{R}^{3\times 3}$ denote the inertia matrix of the
$i^{th}$-UAV with mass $m_{i}$.

The force vector $F_{i}\in\mathbb{R}^{3}$ generated by the $i^{th}$-UAV in the
inertial frame is denoted by $F_{i}=\mathcal{F}_{i}\boldsymbol{R}_{i}b_{i3}$,
where $\mathcal{F}_{i}\in\mathbb{R}$ denotes the thrust value. In addition to
this, the torque vector generated by the $i^{th}$-UAV in the body-fixed frame
is denoted by $\tau_{i}\in\mathbb{R}^{3}$. Thus, the control inputs to the
payload-UAV system is given by
$\left\\{\mathcal{F}_{i},\tau_{i}\right\\},i\in\\{1,..,N\\}$.

###  II-B Payload-UAV System Dynamics

Based on the notations and the system description presented above, the
equations of motion are derived in the following section. As the connecting
links are rigid and massless, the position vector $r_{i}$ of the $i^{th}$-UAV
can be directly inferred as:

$\displaystyle r_{i}=r_{0}+\boldsymbol{R}_{0}\left(p_{i}-l_{i}q_{i}\right),\ \
\ i=1,\ldots,N$ (1)

The kinematic equations for the payload-UAV system are:

$\displaystyle\dot{r}_{0}$ $\displaystyle=\boldsymbol{R}_{0}v_{0}$ (2)
$\displaystyle\boldsymbol{\dot{R}}_{0}$
$\displaystyle=\boldsymbol{R}_{0}\omega_{0}^{\times}$ (3)
$\displaystyle\dot{r}_{i}$
$\displaystyle=\dot{r}_{0}+\boldsymbol{R}_{0}\left(\omega_{0}^{\times}p_{i}-l_{i}\Omega_{i}^{\times}q_{i}\right)$
(4) $\displaystyle\dot{q}_{i}$
$\displaystyle=\left(\Omega_{i}-\omega_{0}\right)^{\times}q_{i}$ (5)
$\displaystyle\boldsymbol{\dot{R}}_{i}$
$\displaystyle=\boldsymbol{R}_{i}\omega_{i}^{\times},\ \ \ \ \ \ \ \ \ \ \ \ \
\ \ \ \ \ i=1,\ldots,N$ (6)

where the hat map $(\cdot)^{\times}:\mathbb{R}^{3}\rightarrow SO(3)$ denotes
the skew-symmetric operator, $v_{0}\in\mathbb{R}^{3}$ denotes the velocity of
the payload in the payload-fixed frame, $w_{0},w_{i}\in\mathbb{R}^{3}$ denotes
the angular velocities of the payload and the $i^{th}$ UAV respectively and
$\Omega_{i}\in\mathbb{R}^{3}$ denotes the angular velocities of the $i^{th}$
link. Note that the orthogonal condition $q_{i}\cdot\Omega_{i}=0$ must be
satisfied for all time.

The kinetic energy $\mathcal{T}$ and the potential energy $\mathcal{U}$ of the
system are given by:

$\displaystyle\begin{split}\mathcal{T}={}&\frac{1}{2}m_{0}\|\dot{r}_{0}\|^{2}_{{}_{2}}+\frac{1}{2}\omega_{0}\cdot\boldsymbol{J}_{0}\omega_{0}\\\
&+\sum_{i=1}^{N}\left(\frac{1}{2}m_{i}\|\dot{r}_{i}\|^{2}_{{}_{2}}+\frac{1}{2}\omega_{i}\cdot\boldsymbol{J}_{i}\omega_{i}\right)\end{split}$
(7) $\displaystyle\begin{split}\mathcal{U}={}&-m_{0}g\mathbb{k}\cdot
r_{0}-\sum_{i=1}^{N}m_{i}g\mathbb{k}\cdot r_{i}\end{split}$ (8)

where $\mathbb{k}=\left[0\ 0\ 1\right]^{T}$. To derive the equations of motion
of the payload-UAV system, the Lagrangian $\mathcal{L}$ of the system must be
obtained:

$\displaystyle\mathcal{L}={}$ $\displaystyle\mathcal{T}-\mathcal{U}$ (9)

Once the Lagrangian $\mathcal{L}$ is formulated, the Euler-Lagrange equations
can be obtained from the Lagrange-d’Alembert Principle as described in the
Appendix section of [15] and are given as follows:

$\displaystyle\frac{d}{dt}\boldsymbol{D}_{\dot{r}_{0}}\mathcal{L}$
$\displaystyle-\boldsymbol{D}_{r_{0}}\mathcal{L}=\sum_{i=1}^{N}F_{i}$ (10)
$\displaystyle\frac{d}{dt}\boldsymbol{D}_{\omega_{0}}\mathcal{L}+\omega_{0}^{\times}\boldsymbol{D}_{\omega_{0}}\mathcal{L}$
$\displaystyle-\boldsymbol{d}_{\boldsymbol{R}_{0}}\mathcal{L}=\sum_{i=1}^{N}p_{i}^{\times}\boldsymbol{R}_{0}^{T}F_{i}$
(11) $\displaystyle
q_{i}^{\times}\frac{d}{dt}\boldsymbol{D}_{\dot{q}_{i}}\mathcal{L}$
$\displaystyle-
q_{i}^{\times}\boldsymbol{D}_{q_{i}}\mathcal{L}=-l_{i}q_{i}^{\times}F_{i}$
(12) $\displaystyle\frac{d}{dt}\boldsymbol{D}_{\omega_{i}}\mathcal{L}$
$\displaystyle+\omega_{i}^{\times}\boldsymbol{D}_{\omega_{i}}\mathcal{L}=\tau_{i}$
(13)

where $\boldsymbol{D}_{a}\mathcal{L}$ is the derivative of the Lagrangian
$\mathcal{L}$ w.r.t the vector $a$, and
$\boldsymbol{d}_{\boldsymbol{R}_{0}}\mathcal{L}$ are the left-trivialized
derivatives of the Lagrangian $\mathcal{L}$ [15]. Substituting for the
derivatives of the Lagrangian, and simplifying the equations leads to the
equations of motion of the payload-UAV system [16]:

$\displaystyle\begin{split}m_{T}\left(\dot{v}_{0}+\omega_{0}^{\times}v_{0}\right)+\sum_{i=1}^{N}m_{i}\left(-p_{i}^{\times}\dot{\omega}_{0}+l_{i}q_{i}^{\times}\dot{\Omega}_{i}+l_{i}q_{i}^{\times}\omega_{0}^{\times}\Omega_{i}\right)\\\
+\sum_{i=1}^{N}\left(m_{i}\left(\omega_{0}^{\times}\right)^{2}p_{i}+m_{i}l_{i}\|\Omega_{i}\|_{{}_{2}}^{2}q_{i}\right)=\boldsymbol{R}_{0}^{T}\left(m_{T}g\mathbb{k}+\sum_{i=1}^{N}F_{i}\right)\end{split}$
(14)
$\displaystyle\begin{split}\sum_{i=1}^{N}m_{i}p_{i}^{\times}\left(\dot{v}_{0}+\omega_{0}^{\times}v_{0}+l_{i}q_{i}\dot{\Omega}_{i}+l_{i}\|\Omega_{i}\|_{{}_{2}}^{2}q_{i}+l_{i}q_{i}^{\times}\omega_{0}^{\times}\Omega_{i}\right)+\\\
\boldsymbol{\bar{J}}_{0}\dot{\omega}_{0}+\omega_{0}^{\times}\boldsymbol{\bar{J}}_{0}\omega_{0}=\sum_{i=1}^{N}p_{i}^{\times}\boldsymbol{R}_{0}^{T}\left(F_{i}+m_{i}g\mathbb{k}\right)\
\ \ \ \ \ \ \ \ \hfill\end{split}$ (15)
$\displaystyle\begin{split}m_{i}l_{i}^{2}\left(\dot{\Omega}_{i}+\omega_{0}^{\times}\Omega\right)-m_{i}l_{i}q_{i}^{\times}\left(\dot{v}_{0}+\omega_{0}^{\times}v_{0}\right)+m_{i}l_{i}q_{i}^{\times}p_{i}^{\times}\dot{\omega}_{0}\\\
-m_{i}l_{i}q_{i}^{\times}\left(\omega_{0}^{\times}\right)^{2}p_{i}=-l_{i}q_{i}^{\times}\boldsymbol{R}_{0}^{T}\left(F_{i}+m_{i}g\mathbb{k}\right)\
\ \ \ \ \ \ \hfill\end{split}$ (16)
$\displaystyle\begin{split}\boldsymbol{J}_{i}\dot{\omega}_{i}+\omega_{i}^{\times}\boldsymbol{J}_{i}\omega_{i}=\tau_{i}\end{split}$
(17)

where $m_{T}=\left(m_{0}+\sum_{i=1}^{N}m_{i}\right)$ is the total mass of the
UAVs with the payload and
$\boldsymbol{\bar{J}}_{0}=\left(\boldsymbol{J}_{0}-\sum_{i=1}^{N}m_{i}\left(p_{i}^{\times}\right)^{2}\right)$.

###  II-C State-Space Representation and Model Linearization

The equations of motion described earlier can be rearranged, and a state-space
representation of the payload-UAV system can be obtained. Consider the state
vector $x\in\mathbb{R}^{12+16N}$ as follows:

$\displaystyle x=\left[\underbrace{r_{0}^{T}\ v_{0}^{T}\ \Theta_{0}^{T}\
\omega_{0}^{T}}_{\text{payload $\in\mathbb{R}^{12}$}}\ \underbrace{q_{i}^{T}\
\Omega_{i}^{T}\ \Theta_{i}^{T}\ \omega_{i}^{T}}_{\text{$i^{th}$ UAV
$\in\mathbb{R}^{16N}$}}\right]^{T},\ i\in\\{1,...,N\\}$ (18)

where $\Theta_{0}$, $\Theta_{i}\in\mathbb{R}^{3}$ are the ZYX Euler angle
parametrization of $\boldsymbol{R}_{0}$ and $\boldsymbol{R}_{i}$ respectively.
The state-space equations for $\dot{x}_{a}=\left[\dot{r}_{0}^{T}\
\dot{\Theta}_{0}^{T}\ \dot{q}_{i}^{T}\ \dot{\Theta}_{i}^{T}\right]^{T}$ can be
easily obtained from Eq. (2) - (6). The equations for
$\dot{x}_{b}=\left[\dot{v}_{0}^{T}\ \dot{\omega}_{0}^{T}\
\dot{\Omega}_{i}^{T}\ \dot{\omega}_{i}^{T}\right]^{T}$ can be obtained by
rearranging equations Eq. (14) - (17) and is given by:

$\displaystyle\dot{x}_{b}=\boldsymbol{P}^{-1}\boldsymbol{Q}$ (19)

where $\boldsymbol{P}=$

$\left[\begin{array}[]{cc|ccc}m_{T}\boldsymbol{I}_{3}&-\sum_{i=1}^{N}m_{i}p_{i}^{\times}&m_{1}l_{1}q_{1}^{\times}&\ldots&m_{N}l_{N}q_{N}^{\times}\\\
\sum_{i=1}^{N}m_{i}p_{i}^{\times}&\boldsymbol{\bar{J}}_{0}&m_{1}l_{1}p_{1}^{\times}q_{1}^{\times}&\ldots&m_{N}l_{N}p_{N}^{\times}q_{N}^{\times}\\\
\hline\cr
m_{1}l_{1}q_{1}^{\times}&m_{1}l_{1}q_{1}^{\times}p_{1}^{\times}&m_{1}l_{1}^{2}\boldsymbol{I}_{3}&\ldots&\boldsymbol{0}_{3}\\\
\vdots&\vdots&\vdots&\vdots&\vdots\\\
-m_{N}l_{N}q_{N}^{\times}&m_{N}l_{N}q_{N}^{\times}p_{N}^{\times}&\boldsymbol{0}_{3}&\ldots&m_{N}l_{N}^{2}\boldsymbol{I}_{3}\end{array}\right]$

(20)

and $\boldsymbol{Q}=$

$\left[\begin{array}[]{c}-m_{T}\omega_{0}^{\times}v_{0}-\sum_{i=1}^{N}\left\\{m_{i}\left(\omega_{0}^{\times}\right)^{2}p_{i}+m_{i}l_{i}\left\|\Omega_{i}\right\|_{{}_{2}}^{2}q_{i}\right.\\\
\left.+m_{i}l_{i}q_{i}^{\times}\omega_{0}^{\times}\Omega_{i}\right\\}+m_{T}g\boldsymbol{R}_{0}^{T}\mathbb{k}+\sum_{i=1}^{N}\boldsymbol{R}_{0}^{T}F_{i}\\\
\hline\cr-\omega_{0}^{\times}\bar{\boldsymbol{J}}_{0}\omega_{0}-\sum_{i=1}^{N}m_{i}\left\\{p_{i}^{\times}\omega_{0}^{\times}v_{0}+l_{i}p_{i}^{\times}q_{i}^{\times}\omega_{0}^{\times}\Omega_{i}\right.\\\
\left.+l_{i}p_{i}^{\times}\left\|\Omega_{i}\right\|_{{}_{2}}^{2}q_{i}\right\\}+\sum_{i=1}^{N}p_{i}^{\times}\boldsymbol{R}_{0}^{T}\left(F_{i}+m_{i}g\mathbb{k}\right)\\\
\hline\cr
m_{1}l_{1}\left\\{q_{1}^{\times}\left(\omega_{0}^{\times}\right)^{2}p_{1}-l_{1}\omega_{0}^{\times}\Omega_{1}+q_{1}^{\times}\omega_{0}^{\times}v_{0}\right\\}\\\
-l_{1}q_{1}^{\times}\boldsymbol{R}_{0}^{T}\left(F_{1}+m_{1}g\mathbb{k}\right)\\\
\hline\cr\vdots\\\ \hline\cr\\\
m_{N}l_{N}\left\\{q_{N}^{\times}\left(\omega_{0}^{\times}\right)^{2}p_{N}-l_{N}\omega_{0}^{\times}\Omega_{N}+q_{N}^{\times}\omega_{0}^{\times}v_{0}\right\\}\\\
-l_{N}q_{N}^{\times}\boldsymbol{R}_{0}^{T}\left(F_{N}+m_{N}g\mathbb{k}\right)\end{array}\right]$
(21)

where $\boldsymbol{0}_{n}\in\mathbb{R}^{n\times n}$ is the $n\times n$ zero
matrix and $\boldsymbol{I}_{n}\in\mathbb{R}^{n\times n}$ is the $n\times n$
identity matrix. The control input to the system is defined by the vector
$u=\left[\mathcal{F}_{1}\ \tau_{1}^{T}\ ..\ \mathcal{F}_{N}\
\tau_{N}^{T}\right]^{T}\in\mathbb{R}^{4N}$. The equilibrium points of the
system $\left\\{x_{e},u_{e}\right\\}$ satisfy the condition
$f\left(x_{e},u_{e}\right)=0$. One such equilibrium point ($x_{e},u_{e}$) for
the payload-UAV system is given in Eq. 22, and occurs when all the links are
vertical, and the orientation of the payload and all the UAVs are zero, thus
producing zero torques. In addition to this, the thrust generated by the UAVs
must balance the overall weight they are subjected to, maintaining net zero
acceleration:

$\displaystyle x_{e}=\left[r_{0}^{T}\ \mathbb{0}^{T}\ \mathbb{0}^{T}\
\mathbb{0}^{T}\ \mathbb{k}_{i}^{T}\ \mathbb{0}_{i}^{T}\ \mathbb{0}_{i}^{T}\
\mathbb{0}_{i}^{T}\right]^{T},i\in\\{1,..,N\\}$ (22a) $\displaystyle
u_{e}=\left[\mathcal{F}_{e_{i}}\
\mathbb{0}_{i}^{T}\right]^{T},i\in\\{1,..,N\\}\ \ \ \ \ \ \ \ \ \ \ \ \ $
(22b)

where $\mathbb{0}\in\mathbb{R}^{3}$ is the zero vector and
$\mathcal{F}_{e_{i}}=\left(m_{i}+m_{0}/4\right)g$. The non-linear state-space
equations can then be linearized about the equilibrium point
$\left(x_{e},u_{e}\right)$. The resultant discretized equations of motion at
time step $k$ is given by:

$\displaystyle\Delta x_{k+1}$ $\displaystyle=\boldsymbol{A}\Delta
x_{k}+\boldsymbol{B}\Delta u_{k}$ (23a) $\displaystyle\Delta y_{k}$
$\displaystyle=\boldsymbol{C}\Delta x_{k}+\boldsymbol{D}\Delta u_{k}$ (23b)

where $\boldsymbol{A}\in\mathbb{R}^{n\times n}$ is the state matrix,
$\boldsymbol{B}\in\mathbb{R}^{n\times m}$ is the input matrix111 Due to their
high dimensionality, the matrices are uploaded as .csv files, and can be
viewed here., $\boldsymbol{C}\in\mathbb{R}^{p\times n}$ is the output matrix
and $\boldsymbol{D}\in\mathbb{R}^{p\times m}$ is the input feedforward matrix.
The discrete state vector $\Delta x_{k}\in\mathbb{R}^{n}$ and the control
input vector $\Delta u_{k}\in\mathbb{R}^{m}$ is obtained as $\Delta
x_{k}=x_{k}-x_{e}$, $\Delta u_{k}=u_{k}-u_{e}$ respectively. Due to the
principle of receding horizon, it is assumed implicitly that the input $u_{k}$
cannot affect the output $y_{k}$ at the same time. Thus, throughout the
discussion that follows, it is assumed that $\boldsymbol{D}=\boldsymbol{0}$.

TrajectoryTrackingController$\bar{u}$OptimizationFramework$u^{*}=\min_{u}L$Integrated
Decision ControllerExponentialBarrierFunctions$\boldsymbol{A}_{obs}u\leq
B_{obs}$ObstacleAvoidanceController$u^{*}$PayloadUAVDynamics$\dot{x}=f(x,u)$SensorSuite$x_{obs}$$\dot{x}_{obs}$$x$$y=\boldsymbol{C}x$$y^{{}^{\prime}}$$y$$x$
Figure 2: Schematic Diagram of the Integrated Decision Controller. The IDC
(dashed box) comprises of a trajectory tracking controller and an obstacle
avoidance controller (dash-dot box)

##  III Integrated Decision Controller design for safety-critical navigation

Figure 3: The payload must track the desired trajectory $y^{\prime}$ amidst a
cluttered environment. The cluttered environment consists of static and
dynamic obstacles which are shown by spheres and cylinders. The formation of
the convex hull (dashed lines) around the payload-UAV system is shown in (a).
The convex hull encloses all the UAVs and the payload completely. In (b), the
point $c_{i},c_{j}$ are the points on the convex hull closest to the $i^{th}$
and the $j^{th}$ obstacle respectively.

In this section, an integrated decision controller (IDC) is designed, that
meets the requirements of trajectory tracking and obstacle avoidance.
Particularly, the IDC must ensure that the payload oscillations are minimum,
while tracking a set trajectory. Moreover, the IDC must be capable of
providing safety critical decision control in order to avoid dynamic obstacles
in the cluttered environment, while maintaining minimum deviation from the set
trajectory. The schematic of the IDC is shown in Fig. (2).

The structure of IDC ensures powerful versatility in the sense that any
tracking controller can be used in conjunction with the obstacle avoidance
controller. In this work, a Model Predictive Controller is designed for the
linear discretized system (23), with particular emphasis on the derivation of
state and control input constraints. This way, the bounds on payload
oscillations and tracking requirements can be automatically ensured in a
single framework.

###  III-A Safety critical payload transport in a cluttered environment

This work focuses on ensuring safety critical transportation of a payload in a
cluttered environment. In particular, there must be no collision of the
payload with any surrounding obstacles, and the payload oscillations must be
restricted under a given limit:

$\displaystyle\left|\phi_{0}\right|$ $\displaystyle\leq\delta_{\phi},$ (24a)
$\displaystyle\left|\theta_{0}\right|$ $\displaystyle\leq\delta_{\theta},$
(24b) $\displaystyle\left|\psi_{0}\right|$ $\displaystyle\leq\delta_{\psi}\ \
\ \forall t>t_{0}$ (24c)

where $\Theta_{0}=\left[\phi_{0}\ \theta_{0}\ \psi_{0}\right]^{T}$ is the
roll-pitch-yaw (rpy) of the payload, and
$\delta_{\phi},\delta_{\theta},\delta_{\psi}>0$ are small positive constants.
The cluttered environment consists of static and dynamic obstacles, whose
position $x_{obs}$ and velocity $\dot{x}_{obs}$ can be estimated by the
onboard UAV sensor suite.

###  III-B Trajectory Tracking Controller

A Linear Model Predictive Controller (LMPC) can be designed for the system
(23), such that the payload tracks a given reference trajectory $y^{\prime}$
as shown in in Fig. 3(a). The LMPC minimizes the cost function $J$ at every
time step $k$:

$J\left(\Delta x_{k},\Delta
U_{k}\right)=\left\|\boldsymbol{\bar{C}}X_{e}+\Delta
Y_{k}-Y^{\prime}_{k}\right\|^{2}_{\boldsymbol{\bar{Q}}}+\left\|\Delta
U_{k}\right\|^{2}_{\boldsymbol{\bar{R}}}\\\
+\left\|y_{k+N_{p}}-y^{\prime}_{k+N_{p}}\right\|^{2}_{\boldsymbol{Q}_{f}}\ \ \
\ \ \ \ \ \ \ \ \ \ \ \ $ (25)

where $\bar{\boldsymbol{C}}=\text{diag}(\boldsymbol{C},..,\boldsymbol{C})$,
$\bar{\boldsymbol{Q}}=\text{diag}(\boldsymbol{Q},..,\boldsymbol{Q})$,
$\bar{\boldsymbol{R}}=\text{diag}(\boldsymbol{R},..,\boldsymbol{R})$,
$X_{e}=\left[x^{T}_{e}\ ..\ x^{T}_{e}\right]^{T}$, $\Delta Y_{k}=\left[\Delta
y^{T}_{k}\ ..\ \Delta y^{T}_{k+N_{p}-1}\right]^{T}$ and $\Delta
U_{k}=\left[\Delta u^{T}_{k},..,\Delta u^{T}_{k+N_{p}-1}\right]^{T}$ are
defined over the prediction horizon $N_{p}$. The matrices
$\boldsymbol{Q},\boldsymbol{Q}_{f}\geq 0$ and $\boldsymbol{R}>0$ are the cost
weighting matrices and $y_{k}=\boldsymbol{C}\left(x_{e}+\Delta x_{k}\right)$.
The matrix $\boldsymbol{Q}_{f}$ is chosen to be equal to the solution of the
discrete algebraic Riccati equation for the discrete system in Eq. (23)[17].

The cost function in Eq. 25 is subject to linear state and control input
constraints. The nonlinear constraints derived in [16] needs to be linearized
about every time step along with the model. This can be computationally
demanding in a real time implementation. A much simplified set of constraints
can be derived for the discrete linear system in Eq. (23). Let the upper and
lower bounds for the input be $u_{ub}$ and $u_{lb}$ respectively. Thus,
$u_{lb}\leq u_{e}+\Delta u_{k+i}\leq u_{ub}$, which can also be expressed as

$\displaystyle\left[\begin{array}[]{l}\boldsymbol{I}_{m}\\\
-\boldsymbol{I}_{m}\end{array}\right]\Delta
u_{k+i}\leq\left[\begin{array}[]{l}\left(u_{ub}-u_{e}\right)\\\
\left(u_{e}-u_{lb}\right)\end{array}\right]$ (30)

Similarly, let the upper and lower bounds for the state be $x_{ub}$ and
$x_{lb}$ respectively, for the set of states described by
$\boldsymbol{c}_{z}x_{k+i}$, where the matrix $\boldsymbol{c}_{z}$ is a
diagonal matrix, with ones for the states that require constraints, and zeros
for the states that are free to take any values. The constraint
$x_{lb}\leq\boldsymbol{c}_{z}\left(x_{e}+\Delta x_{k+i}\right)\leq x_{ub}$ can
be expressed as

$\displaystyle\left[\begin{array}[]{l}\boldsymbol{c}_{z}\\\
-\boldsymbol{c}_{z}\end{array}\right]\Delta
x_{k+i}\leq\left[\begin{array}[]{l}\left(x_{ub}-\boldsymbol{c}_{z}x_{e}\right)\\\
\left(\boldsymbol{c}_{z}x_{e}-x_{lb}\right)\end{array}\right]$ (35)

The state vector $\Delta x_{k+i}$ can be recursively substituted in Eq. (23),
to obtain $\Delta X_{k}=\left[\Delta x^{T}_{k}\ ,..,\ \Delta
x^{T}_{k+N_{p}-1}\right]^{T}$ and thus, over the prediction horizon these
constraint equations can be compactly written as:

$\displaystyle\boldsymbol{M}_{U}\Delta U_{k}$ $\displaystyle\leq\Delta U_{b}$
(36) $\displaystyle\boldsymbol{M}_{x}\left(\boldsymbol{F}\Delta
x_{k}+\boldsymbol{H}\Delta U_{k}\right)$ $\displaystyle\leq\Delta Z_{b}$ (37)

where,

$\displaystyle\begin{aligned}
\boldsymbol{H}&=\left[\begin{array}[]{ccccc}\boldsymbol{0}&&&&\\\
\boldsymbol{B}&\boldsymbol{0}&&&\\\
\boldsymbol{AB}&\boldsymbol{B}&\boldsymbol{0}&&\\\
\vdots&\vdots&\ddots&\ddots&\\\
\boldsymbol{A}^{N_{p}-2}\boldsymbol{B}&\boldsymbol{A}^{N_{p}-3}\boldsymbol{B}&\ldots&\boldsymbol{B}&\boldsymbol{0}\end{array}\right]\\\
\boldsymbol{M}_{U}&=\left[\begin{array}[]{ccc}\left[\begin{array}[]{l}\boldsymbol{I}_{m}\\\
-\boldsymbol{I}_{m}\end{array}\right]&&\\\ &\ddots&\\\
&&\left[\begin{array}[]{l}\boldsymbol{I}_{m}\\\
-\boldsymbol{I}_{m}\end{array}\right]\end{array}\right]\\\
\boldsymbol{M}_{x}&=\left[\begin{array}[]{ccc}\left[\begin{array}[]{l}\boldsymbol{c}_{z}\\\
-\boldsymbol{c}_{z}\end{array}\right]&&\\\ &\ddots&\\\
&&\left[\begin{array}[]{l}\boldsymbol{c}_{z}\\\
-\boldsymbol{c}_{z}\end{array}\right]\end{array}\right]\\\ \Delta
U_{b}&=\left[\begin{array}[]{l}\left[\begin{array}[]{l}\left(u_{ub}-u_{e}\right)\\\
\left(u_{e}-u_{lb}\right)\end{array}\right]\\\ \ \ \ \ \ \ \ \ \ \vdots\\\
\left[\begin{array}[]{l}\left(u_{ub}-u_{e}\right)\\\
\left(u_{e}-u_{lb}\right)\end{array}\right]\end{array}\right],\
\boldsymbol{F}=\left[\begin{array}[]{l}\boldsymbol{I}\\\ \boldsymbol{A}\\\
\boldsymbol{A}^{2}\\\ \vdots\\\
\boldsymbol{A}^{N_{p}-1}\end{array}\right]\end{aligned}$ $\displaystyle\Delta
Z_{b}$
$\displaystyle=\left[\begin{array}[]{l}\left[\begin{array}[]{l}\left(x_{ub}-\boldsymbol{c}_{z}x_{e}\right)\\\
\left(\boldsymbol{c}_{z}x_{e}-x_{lb}\right)\end{array}\right]\\\ \ \ \ \ \ \ \
\ \ \vdots\\\
\left[\begin{array}[]{l}\left(x_{ub}-\boldsymbol{c}_{z}x_{e}\right)\\\
\left(\boldsymbol{c}_{z}x_{e}-x_{lb}\right)\end{array}\right]\end{array}\right],\
\ \ \ \ $

Eq. (37) can be rearranged, to obtain the constrain in terms of $\Delta
U_{k}$, as in Eq. (36). Once these constraints are obtained, the cost function
in Eq. (25) can be solved as:

$\displaystyle\Delta\bar{U}_{k}=\operatorname*{\arg\\!\min}_{\Delta
U_{k}}J(\Delta x_{k},\Delta U_{k})\ \ \ \ \ \ \ \ \ $ (38a)
$\displaystyle\text{s.t}\ \ \left[\begin{array}[]{l}\boldsymbol{M}_{U}\\\
\boldsymbol{M}_{x}\boldsymbol{H}\end{array}\right]\Delta
U_{k}\leq\left[\begin{array}[]{l}\ \ \ \ \ \Delta U_{b}\\\ \Delta
Z_{b}-\boldsymbol{M}_{x}\boldsymbol{F}\Delta x_{k}\end{array}\right]$ (38f)

and the optimal tracking control input over $N_{p}$ horizon $\bar{U}_{k}$ is
given by $\bar{U}_{k}=U_{e}+\Delta\bar{U}_{k}$, where
$U_{e}=\left[u_{e}^{T}\ldots u_{e}^{T}\right]^{T}$.

The constraints include bounds on the control input $u=\left[\mathcal{F}_{1}\
\tau_{1}^{T}\ldots\mathcal{F}_{N}\ \tau_{N}^{T}\right]^{T}$, and bounds on
states such as the $z-$ coordinates of the payload and the UAVs, which cannot
be positive (due to the NED convention) and the set of constraints in Eq.
(24). The reference trajectory $y^{{}^{\prime}}$ contains the desired position
of the payload, the desired orientation of the payload to be zero ensuring
minimal oscillations and the desired orientation of links to be vertical,
ensuring that the UAVs don’t collide with each other, and the system stays
close to the equilibrium point $\left(x_{e},u_{e}\right)$ as described in Eq.
(22). Thus, the optimal tracking control input $\bar{u}_{k}$ at the current
step $k$ is obtained as $\bar{u}_{k}=[\boldsymbol{I}\ \boldsymbol{0}\ \ldots\
\boldsymbol{0}]\bar{U}_{k}$.

###  III-C Obstacle Avoidance Controller

The obstacles are dynamic in nature, and it is assumed that the position and
velocity of the obstacle are available at every time step $k$. Due to the
spatial structure of the payload-UAV system, it must be ensured that the rigid
links and the propellers of the UAVs do not collide with the surrounding
obstacles. This is ensured by constructing a safe convex hull around the
payload-UAV system such that it fully encloses it, as shown in Fig. 3(a).

The collision avoidance problem is solved using the Exponential Control
Barrier Functions (ECBFs) [18]. In general, the control barrier function is
defined over the state-space, and provides safety-critical constraints, that
can be leveraged in an optimization framework. The ECBF naturally allows
extension of control barrier functions to higher relative-degree systems, such
as the payload-UAV system described in II.

Suppose there exists an r-times continuously differentiable function
$h(x):\mathcal{D}\rightarrow\mathbb{R},\ \mathcal{D}\subset\mathbb{R}^{n}$,
that is a function of the state vector $x\in\mathcal{D}$. Define a super level
set $\mathcal{C}$ over $h$ as:

$\displaystyle\mathcal{C}$ $\displaystyle=\left\\{x\in
D\subset\mathbb{R}^{n}:h(x)\geq 0\right\\}$ (39a)
$\displaystyle\partial\mathcal{C}$ $\displaystyle=\left\\{x\in
D\subset\mathbb{R}^{n}:h(x)=0\right\\}$ (39b)
$\displaystyle\text{Int}\left(\mathcal{C}\right)$ $\displaystyle=\left\\{x\in
D\subset\mathbb{R}^{n}:h(x)>0\right\\}$ (39c)

where $\partial\mathcal{C}$ represents the boundary of the set $\mathcal{C}$
and $\text{Int}(\mathcal{C})$ represents the interior of the set
$\mathcal{C}$. The set $\mathcal{C}$ is said to be forward invariant, if for
every $x(t_{0})=x_{0}\in\mathcal{C}$, the state trajectory
$x(t)\in\mathcal{C},\ \forall\ t>t_{0}$, i.e., the trajectory $x(t)$ never
leaves $\mathcal{C}$ if it starts in $\mathcal{C}$. For a system whose
dynamics can be written as $\dot{x}=f(x)+g(x)u$, the relative degree of $h(x)$
is defined as the number of times $h$ must be differentiated before the
control input $u$ appears explicitly. Thus, the function $h$ is said to have a
degree $r$ if $\mathfrak{L}_{g}\mathfrak{L}^{r-1}_{f}h(x)\neq 0$ and
$\mathfrak{L}_{g}\mathfrak{L}_{f}h(x)$ =
$\mathfrak{L}_{g}\mathfrak{L}^{2}_{f}h(x)$ = $\ldots$ =
$\mathfrak{L}_{g}\mathfrak{L}^{r-2}_{f}h(x)=0$, where
$\mathfrak{L}_{a}b(x)=a\cdot\frac{\partial b}{\partial x}$ denotes the lie
derivative of the vector field $b$ along the vector field $a$.

###### Theorem 1

([18]) Given a set $\mathcal{C}\subset\mathbb{R}^{n}$ defined as a superlevel
set of a r-times continuously differentiable function
$h:\mathcal{D}\rightarrow\mathbb{R}$, then $h$ is an Exponential Control
Barrier Function (ECBF) if there exists a row vector $K\in\mathbb{R}^{r}$ such
that

$\displaystyle\inf_{u}\left[\mathfrak{L}^{r}_{f}h(x)+\mathfrak{L}_{g}\mathfrak{L}^{r-1}_{f}h(x)u+K\eta(x)\right]\geq
0$ (40)

$\forall\ x\in\mathcal{C}$, where $\eta(x)=\left[h(x)\ \dot{h}(x)\ \ldots\
h^{(r-1)}(x)\right]^{T}$. The set $\mathcal{C}$ is then a forward invariant
set.

Consider the safe set $\mathcal{C}=\mathbb{R}^{3}-\mathcal{O}$, where the
obstacle set $\mathcal{O}$ is the space occupied by all the $N_{obs}$
obstacles. If it is ensured that the safe set $\mathcal{C}$ is forward
invariant for all time $t$, i.e., if the trajectory of the payload-UAV system
starts in $\mathcal{C}$, and forever be trapped inside $\mathcal{C}$, then
obstacle avoidance is guaranteed. To ensure that the set $\mathcal{C}$ is
forward invariant, construct the $i^{th}$ Exponential Control Barrier Function
$h_{i}(.):\mathbb{R}^{3}\rightarrow\mathbb{R}$ as:

$\displaystyle
h_{i}(x_{c})=\left\|x_{c}-x_{obs_{i}}\right\|^{2}_{{}_{2}}-R_{o_{i}}^{2},\
i=1,\ldots,N_{obs}$ (41)

where $x_{obs_{i}},\ R_{o_{i}}$ are the position and the radius of the
$i^{th}$ obstacle respectively, and $x_{c}$ is the position of a point $c_{i}$
on the surface of the convex hull around the payload-UAV system closest to the
$i^{th}$ obstacle. It should be noted that the form of $h_{i}(.)$ is dependent
on the $i^{th}$ obstacle shape. Eq. (41) holds for spherical obstacles only.
However, Eq. (41) can be modified to include cylindrical pole obstacles (see
Section IV-B), by extending $z-$axis linearly in both positive and negative
axis, and the vectors in the Eq. (41) only include $x-$, and $y-$ coordinates.
It can be easily verified that $h_{i}(.)$ satisfies the properties described
in Eq. (39). At this point, an assumption is made that the convex hull (and
thus all the points on its surface) translates with a velocity equal to the
translational velocity of the payload. This assumption is easily justified as
long as the payload-UAV system remains close to the equilibrium point of Eq.
(22). From Eq. (23), it can be inferred that the dynamics for the payload is
actually a double integrator system i.e., the control inputs appear only in
the acceleration equations of the payload222One can substitute the A and B
matrices given in the footnote 1 in Eq. (23) to obtain the equations for the
payload-UAV system.. This results in the relative degree of $h_{i}(.)$ to be
$2$. Thus, Eq. (40) reduces to:

$\displaystyle\ddot{h}_{i}\geq-K\left[h_{i}\ \ \dot{h}_{i}\right]^{T}$ (42)

where

$\displaystyle\begin{split}\dot{h}_{i}={}&2\left(x_{c}-x_{obs_{i}}\right)^{T}\left(\dot{x}_{c}-\dot{x}_{obs_{i}}\right)\end{split}$
(43a)
$\displaystyle\begin{split}\ddot{h}_{i}={}&2\left(x_{c}-x_{obs_{i}}\right)^{T}\left(\ddot{x}_{c}-\ddot{x}_{obs_{i}}\right)\\\
&+2\left\|\dot{x}_{c}-\dot{x}_{obs_{i}}\right\|_{{}_{2}}^{2}\end{split}$ (43b)

and $K\in\mathbb{R}^{2}$ is a row vector, chosen such that the poles of the
system (42) are all negative. Substituting acceleration equations of the
payload for $\ddot{x}_{c}$ and substituting Eqs. (43) in Eq. (42) and
rearranging the equations results in a set of $N_{obs}$ constraints in $\Delta
u_{k}$, of the form $\boldsymbol{A}_{obs}\Delta u_{k}\leq B_{obs}$333The
analytical forms of the matrix $\boldsymbol{A}_{obs}$ and $B_{obs}$ can be
found here. .

Thus, the obstacle avoidance controller can be designed as follows:

$\displaystyle\Delta u_{k}^{*}=\operatorname*{\arg\\!\min}_{\Delta
u_{k}}\frac{1}{2}\left\|\Delta
u_{k}-\Delta\bar{u}_{k}\right\|^{2}_{\boldsymbol{Q}_{obs}}$ (44a)
$\displaystyle\begin{split}{}&\text{s.t}\ \ \ \ \ \boldsymbol{A}_{obs}\Delta
u_{k}\leq B_{obs}\\\ {}&\left[\begin{array}[]{l}\boldsymbol{I}_{m}\\\
-\boldsymbol{I}_{m}\end{array}\right]\Delta
u_{k}\leq\left[\begin{array}[]{c}\left(u_{ub}-u_{e}\right)\\\
\left(u_{e}-u_{lb}\right)\end{array}\right]\end{split}$ (44c)

where $\boldsymbol{Q}_{obs}>0$ is a diagonal weighting matrix. The bounds on
the control input are reapplied, along with the barrier function constraints.
The optimal control law $\Delta u_{k}^{*}$ produced by the Integrated Decision
Controller is thus equal to the optimal tracking control law
$\Delta\bar{u}_{k}$ in the absence of obstacles, but while encountering the
obstacles, the optimal control law $\Delta u_{k}^{*}$ is different, ensuring
that the set of constraints in Eq. (LABEL:eq:con_avoid) is satisfied.

##  IV Simulation Results

In this section, the implementation details and simulation results are
discussed to evaluate the performance of the proposed controller. The Linear
Model Predictive Controller is implemented in C++ using the ACADO toolkit [19]
code generator. Multiple Shooting discretization technique is employed to
obtain a discrete system analogous to Eq. (23). The prediction horizon for the
LMPC is chosen to be $20$ time steps. The optimization problems in both Eq.
(25) and Eq. (44) is solved using the online active set strategy [20] as
implemented in the qpOASES library [21]. To construct the convex hull, and
find the point on the surface of the convex hull closest to the obstacle,
polytope distance algorithms [22] are used as implemented in the CGAL library
[23].

The advantage of detaching the obstacle avoidance controller from the MPC is
that it enables the user to use pre-existing, state-of-art MPC implementations
that are highly optimized for nonlinear systems, and semi-definite hessian
matrices that occur in the MPC optimization stage, as discussed in [19].

Table I: Parameter values for the payload-UAV system  | $m$ | $J_{xx}$ | $J_{yy}$ | $J_{zz}$
---|---|---|---|---
Payload | 3.1 | 0.29 | 0.29 | 0.55
UAVs | 0.7 | 0.01 | 0.01 | 0.01
|  | $p_{i}$ |  | $l_{i}$
UAV1 |  | $[0.5,\ 0.5,\ \shortminus 0.25]^{T}$ |  | 3.2
UAV2 |  | $[0.5,\ \shortminus 0.5,\ \shortminus 0.25]^{T}$ |  | 3.2
UAV3 |  | $[\shortminus 0.5,\ \shortminus 0.5,\ \shortminus 0.25]^{T}$ |  | 3.2
UAV4 |  | $[\shortminus 0.5,\ 0.5,\ \shortminus 0.25]^{T}$ |  | 3.2

###  IV-A Numerical Results

For the subsequent discussions, the parameters considered for the payload-UAV
system is shown in Table I, and the constants
$\delta_{\phi},\delta_{\theta},\delta_{\psi}$ of Eq. (24) are set to $5$
degrees. It is observed that assigning a value lower than 5 degrees to these
constants renders the optimization problem of Eq. (38) infeasible, especially
during the instants where obstacle avoidance occurs.

####  IV-A1 Trajectory Tracking

For a numerical evaluation of the algorithm, four UAVs are considered that
transport a rigid payload, as shown in Fig. 1. The desired trajectory for the
payload is chosen as a figure-eight $\infty$ contour, as described by Eq.
(45):

$\displaystyle r_{{0d}_{x}}$ $\displaystyle=6\sin\left(0.5t\right)$ (45a)
$\displaystyle r_{{0d}_{y}}$
$\displaystyle=-6\sin\left(0.5t\right)\cos\left(0.5t\right)$ (45b)
$\displaystyle r_{{0d}_{z}}$ $\displaystyle=-5u(t)$ (45c)

where $u(t)$ is the unit step function. From Fig. 4 and 5, it can be seen that
the LMPC controller successfully stabilizes the payload, while ensuring that
it tracks the desired trajectory. The attitude of the payload is limited to
within $5$ degrees during the abrupt take off, but dies down quickly once
stabilized. The yaw angles of the payload and UAVs are highly penalized, to
ensure that the optimization problem remains strictly convex[16]. A higher
cost is placed on the attitude and the position of the payload, and a slightly
lower cost is placed on the attitude of the links and UAVs in the weighting
matrix $\boldsymbol{Q}$ of Eq. (25). This ensures that the system can still
make slight deviations from its equilibrium points, making the system less
stiff, and slightly more flexible. A snapshot of the 3D simulation444 The
video demonstrations for both the numerical simulations as well as Gazebo
simulations can be found here: https://youtu.be/AMVlMNYQCLw is shown in Fig.
6, where the complete scenario is shown.

Figure 4: Position of the payload is shown in solid line, and the desired
trajectory is shown in dashed lines (no obstacles) Figure 5: Attitude of the
payload (no obstacles) Figure 6: A snapshot of the numerical simulation. The
reference trajectory is shown in red dotted lines, and the payload trajectory
is shown in black line. For the purpose of demonstration, the coordinates are
converted to ENU (east, north, up) from the NED coordinates. Figure 7:
Trajectory tracking with two surrounding obstacles. The instants of obstacle
avoidance is shown in red shaded area. Figure 8: Attitude of the payload with
two surrounding obstacles. The instants of obstacle avoidance is shown in red
shaded area. Figure 9: A snapshot of the obstacle avoidance simulation. The
sphere on the left is a static obstacle and the sphere on the right is a
dynamic obstacle. Figure 10: Figure on the left shows the payload-UAV system,
and the figure on the right shows the obstacle-avoidance environment course,
that consists of walls, poles and spherical obstacles. The goal point for the
payload is shown by a small black circle behind the rightmost sphere. Figure
11: Payload Trajectory for the obstacle environment course in Gazebo. Figure
12: The purple line shows the trajectory of the payload. The image is darkened
to make the purple line more prominent and noticeable.

####  IV-A2 Obstacle Avoidance

The numerical simulation consists of both static and dynamic obstacles. It is
assumed that the obstacles can be enclosed in sphere of radius $R_{o}$. For
demonstration purposes, the obstacle radius is considered to be $0.5$m and a
safety margin of $0.5$m around the obstacle is taken. Thus, $R_{0}=1$ meter.
The dynamic obstacle is modelled as a simple harmonic oscillator, oscillating
about its mean position $(6,0,\shortminus 5)$m. The mean position of the
dynamic obstacle coincides with a point on the desired trajectory, and the
obstacle timing is set in such a way that it directly confronts the payload-
UAV system at its mean position. The static obstacle is placed at
$(\shortminus 6,0,\shortminus 5)$m. Due to the dynamic nature of the obstacle,
the constraint matrices $\left(\boldsymbol{A}_{obs},B_{obs}\right)$ of Eq.
(44b) are time-varying in nature.

It can be observed from Fig. 7 that the trajectory only changes along the
$z-$axis, where the payload goes above the obstacle, as this is the most
feasible thing to do. As seen in Fig. 8, the payload oscillates while avoiding
the obstacles. This oscillation is however bounded under a small deviation of
$5$ degrees and occurs due to the fact that the payload-UAV system is avoiding
the obstacle while in motion, without slowing down. Due to the high cost
placed on the yaw angles of the payload and UAVs, there is almost no change in
the yaw configuration of the system throughout the course of motion. A bird’s
eye snapshot of the 3D obstacle avoidance simulation is shown in Fig. 9. The
spherical obstacle on the right oscillates along the $x-$axis, and
periodically meets the payload-UAV system at the point $(6,0,\shortminus 5)$,
while the spherical obstacle on the left remains stationary at the point
$(\shortminus 6,0,\shortminus 5)$.

Figure 13: Maximum roll and pitch angle vs the noise standard deviation
$\sigma$ ($m/s$ for linear velocity) and ($rad/s$ for angular velocity

###  IV-B Gazebo Simulation

The obstacle avoidance control algorithm is validated by testing it inside a
high-fidelity simulation environment. The Gazebo simulator [24] is used to
create an obstacle-avoidance environment course. For spawning the payload-UAV
system, the entire model is written in a urdf file, and the RotorS package
[25] is used for spawning four hummingbird drones. This is shown in Fig. 10.
The control algorithm is implemented in a C++ script, which communicates with
the Gazebo simulator via the ROS library. The inputs to the model in the
simulation are rotor rpm values, which can be easily obtained from the thrust
force and torques via a linear transformation matrix.

The environment in Gazebo consists of many obstacles including walls and poles
that are static and spherical obstacles that are dynamic. The desired
trajectory that the payload must track goes in between the walls, and then to
the goal point behind the right-most sphere (see Fig. 10). This ensures that
the payload-UAV system doesn’t go all the way around the walls and then to the
goal point.

The position of the leftmost and rightmost spherical obstacle is at
$(22,13,5)$ and $(22,37,5)$ respectively. The cylindrical poles are placed at
$(22,20,5)$, $(22,26,5)$, $(22,30,5)$ from left to right. It must be noted
that the coordinate conventions in Gazebo simulator is ENU (East-North-Up).
Because of this, there is a coordinate conversion between NED to ENU at the
interface of the ROS C++ script and the Gazebo APIs.

It can be observed from Fig. 11 that the payload successfully avoids both the
spherical obstacles, as well as the cylindrical pole obstacles. Unlike for
spherical obstacles where the payload dodges by going above them, due to the
height of the pole obstacles, the payload goes around them by maintaining a
safe distance from these poles. This is inferred better from Fig. 12, where
the purple line shows the trajectory history of the payload. The sphere near
the walls is a static sphere, where as the sphere towards the end is a dynamic
sphere oscillating about its mean position $(22,37,5)$. The control algorithm
is able to run at a real-time frequency of 20Hz, providing optimal rpm values
to the UAVs in the simulation.

###  IV-C Ablation Study

To evaluate the robustness of the proposed IDC, some of the practical
uncertainties are explored in the following paragraphs.

####  IV-C1 Mass Uncertainty

In practice, there is some uncertainty between the reported mass of the
payload and the actual mass of the payload. A $\pm 10\%$ variation is
considered here. From Table I, the value for the payload mass is taken as
$3kg$ during controller design, but the payload mass in the plant equations is
chosen to be either $2.7kg$ or $3.3kg$. It is observed that the performance of
the controller doesn’t change, however the control effort
$\left\|u\right\|_{2}$ required varies by $\pm 5\%$. This is justified, as the
UAVs need to compensate for the change in the weight, while meeting the
tracking requirements.

####  IV-C2 Noisy Payload States

The linear velocity and angular rates of the payload in Eq. (18) is subjected
to zero mean Gaussian noise $\mathcal{N}(0,\sigma)$. This is done to mimic the
actual payload state values obtained from a noisy rate sensor. For now, the
UAV states are ignored, due to the presence of onboard estimation algorithms
that can accurately infer the UAV states. The goal is to analyze the
robustness of the proposed IDC to the noisy state values of the payload i.e.,
to find the maximum value of the standard deviation $\sigma$ ($m/s$ for linear
velocity and $rad/s$ for angular velocity), such that the payload oscillations
remain under a certain threshold. The results are shown in Fig. 13. It can be
observed that the maximum roll and pitch angles of the payload increase
monotonically as the standard deviation $\sigma$ increases. Beyond a standard
deviation of $0.012$, the obstacle avoidance controller becomes unstable,
while the tracking controller continues to track the trajectory (with a noisy
performance) when assessed independently. If the maximum threshold for payload
oscillations is set at $10$ degrees, the standard deviation must not be above
$0.008$. In practice, most of the IMU’s standard deviation falls below or
occurs close the nominal value of $0.008$555A list of parameters for a few
commonly used rate sensors are available here:
https://github.com/rpng/kalibr_allan. It is also observed that the maximum
roll and pitch angles occur only at the time of avoiding the obstacles.

####  IV-C3 Relative Safety Margin

The IDC ensures that there is minimum deviation from the reference trajectory
while avoiding the obstacles. Earlier, a safety margin of $0.5$m was
considered. It is observed that by changing the safety margin the controller
performance doesn’t change; it only determines the relative proximity between
the payload-UAV system and the obstacle. In practice, the relative distance to
the obstacles may be underestimated or overestimated by some value. Thus,
considering the right safety margin is crucial. Setting a higher safety margin
may result in a higher deviation from the reference trajectory, which in some
cases may render the tracking controller unstable whereas setting a lower
safety margin may be undesirable, as the payload-UAV may come too close to the
obstacle, which could result in a collision. After various experimentation, a
thumb rule of choosing the safety margin equal to the obstacle radius is found
to provide best results.

##  V Conclusions

In this paper, an integrated decision control-based obstacle avoidance
controller is presented to solve the problem of collaborative payload
transportation in a cluttered environment. The IDC fuses the optimal tracking
control provided by the Model Predictive Controller, with the safety-critical
constraints provided by the Exponential Control Barrier Functions in an
optimization framework. The structure of the ECBFs depend on the shape of the
obstacle, which can be static or dynamic in nature. To ensure that the UAVs or
the payload don’t collide with the surrounding obstacles, a safe convex hull
is computed around the entire system at each time instant, and the point on
the convex hull closest to the surrounding obstacle is used to generate the
ECBF constraints. Numerical simulations are conducted on the proposed
controller to demonstrate its functionality, and eventually the algorithm is
deployed in a real time, high fidelity simulation using Gazebo. An ablation
study is conducted to demonstrate the robustness of the IDC to handle
practical unforeseen situations like payload mass uncertainties, noisy payload
states and choosing an optimal relative safety margin around the obstacles.

In the scope of future work, non-linear control laws will be explored that can
handle the problem of tracking and obstacle avoidance in a single framework.
Moreover, external disturbances and state estimation techniques can be used to
make the controller more robust to sensor noise and model uncertainties.
Further, with the help of additional supporting mechanisms like rack-pinion
joints, formation control of the UAVs for rigid payload can be made possible.

##  VI Acknowledgements

The authors would like to acknowledge the financial support from the Nokia CSR
grant on Network Robotics.

## References

  * [1] H. Y. Jeong, B. D. Song, and S. Lee, “The flying warehouse delivery system: A quantitative approach for the optimal operation policy of airborne fulfillment center,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 22, no. 12, pp. 7521–7530, 2021.
  * [2] Z. Xiaoning, “Analysis of military application of UAV swarm technology,” in _2020 3rd International Conference on Unmanned Systems (ICUS)_ , 2020, pp. 1200–1204.
  * [3] R. D. Arnold, H. Yamaguchi, and T. Tanaka, “Search and rescue with autonomous flying robots through behavior-based cooperative intelligence,” _Journal of International Humanitarian Action_ , vol. 3, no. 1, pp. 1–18, 2018.
  * [4] C. Pei, F. Zhang, P. Huang, and H. Yu, “Trajectory planning for collaborative transportation by tethered multi-UAVs,” in _2021 IEEE International Conference on Real-time Computing and Robotics (RCAR)_ , 2021, pp. 769–775.
  * [5] D. K. Villa, A. S. Brandao, and M. Sarcinelli-Filho, “A survey on load transportation using multirotor UAVs,” _Journal of Intelligent & Robotic Systems_, vol. 98, no. 2, pp. 267–296, 2020.
  * [6] K. Klausen, T. I. Fossen, T. A. Johansen, and A. P. Aguiar, “Cooperative path-following for multirotor UAVs with a suspended payload,” in _2015 IEEE Conference on Control Applications (CCA)_. IEEE, 2015, pp. 1354–1360.
  * [7] Y. Liu, F. Zhang, P. Huang, and X. Zhang, “Analysis, planning and control for cooperative transportation of tethered multi-rotor UAVs,” _Aerospace Science and Technology_ , vol. 113, p. 106673, 2021.
  * [8] T. Lee, “Collision avoidance for quadrotor UAVs transporting a payload via voronoi tessellation,” in _2015 American Control Conference (ACC)_. IEEE, 2015, pp. 1842–1848.
  * [9] I. H. B. Pizetta, A. S. Brandão, and M. Sarcinelli-Filho, “Avoiding obstacles in cooperative load transportation,” _ISA transactions_ , vol. 91, pp. 253–261, 2019.
  * [10] I. H. B. Pizetta, A. S. Brandao, and M. Sarcinelli-Filho, “Control and obstacle avoidance for an UAV carrying a load in forestal environments,” in _2018 International Conference on Unmanned Aircraft Systems (ICUAS)_. IEEE, 2018, pp. 62–67.
  * [11] Y. Koren, J. Borenstein _et al._ , “Potential field methods and their inherent limitations for mobile robot navigation.” in _ICRA_ , vol. 2, 1991, pp. 1398–1404.
  * [12] A. Singletary, K. Klingebiel, J. Bourne, A. Browning, P. Tokumaru, and A. Ames, “Comparative analysis of control barrier functions and artificial potential fields for obstacle avoidance,” _arXiv preprint arXiv:2010.09819_ , 2020\.
  * [13] J. Gimenez, D. C. Gandolfo, L. R. Salinas, C. Rosales, and R. Carelli, “Multi-objective control for cooperative payload transport with rotorcraft UAVs,” _ISA transactions_ , vol. 80, pp. 491–502, 2018.
  * [14] K. Sreenath and V. Kumar, “Dynamics, control and planning for cooperative manipulation of payloads suspended by cables from multiple quadrotor robots,” _rn_ , vol. 1, no. r2, p. r3, 2013.
  * [15] T. Lee, “Geometric control of quadrotor UAVs transporting a cable-suspended rigid body,” _IEEE Transactions on Control Systems Technology_ , vol. 26, no. 1, pp. 255–264, 2017.
  * [16] J. Wehbeh, S. Rahman, and I. Sharf, “Distributed model predictive control for UAVs collaborative payload transport,” in _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2020, pp. 11 666–11 672.
  * [17] P. Ru and K. Subbarao, “Nonlinear model predictive control for unmanned aerial vehicles,” _Aerospace_ , vol. 4, no. 2, p. 31, 2017.
  * [18] Q. Nguyen and K. Sreenath, “Exponential control barrier functions for enforcing high relative-degree safety-critical constraints,” in _2016 American Control Conference (ACC)_. IEEE, 2016, pp. 322–328.
  * [19] B. Houska, H. Ferreau, and M. Diehl, “ACADO Toolkit – An Open Source Framework for Automatic Control and Dynamic Optimization,” _Optimal Control Applications and Methods_ , vol. 32, no. 3, pp. 298–312, 2011.
  * [20] H. Ferreau, H. Bock, and M. Diehl, “An online active set strategy to overcome the limitations of explicit mpc,” _International Journal of Robust and Nonlinear Control_ , vol. 18, no. 8, pp. 816–830, 2008.
  * [21] H. Ferreau, C. Kirches, A. Potschka, H. Bock, and M. Diehl, “qpOASES: A parametric active-set algorithm for quadratic programming,” _Mathematical Programming Computation_ , vol. 6, no. 4, pp. 327–363, 2014\.
  * [22] S. Hert and S. Schirra, “3D convex hulls,” in _CGAL User and Reference Manual_ , 5.3.1 ed. CGAL Editorial Board, 2021. [Online]. Available: https://doc.cgal.org/5.3.1/Manual/packages.html#PkgConvexHull3
  * [23] The CGAL Project, _CGAL User and Reference Manual_ , 5.3.1 ed. CGAL Editorial Board, 2021. [Online]. Available: https://doc.cgal.org/5.3.1/Manual/packages.html
  * [24] N. Koenig and A. Howard, “Design and use paradigms for gazebo, an open-source multi-robot simulator,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , Sendai, Japan, Sep 2004, pp. 2149–2154.
  * [25] F. Furrer, M. Burri, M. Achtelik, and R. Siegwart, _Robot Operating System (ROS): The Complete Reference (Volume 1)_. Cham: Springer International Publishing, 2016, ch. RotorS—A Modular Gazebo MAV Simulator Framework, pp. 595–625. [Online]. Available: http://dx.doi.org/10.1007/978-3-319-26054-9_23
  * [26] C. Wang, Y. Li, Y. Meng, S. L. Smith, and J. Liu, “Learning control barrier functions with high relative degree for safety-critical control,” _arXiv preprint arXiv:2011.10721_ , 2020.
  * [27] A. Hegde and D. Ghose, “Multi-UAV collaborative transportation of payloads with obstacle avoidance,” _IEEE Control Systems Letters_ , 2021.
  * [28] L. Wang, _Model predictive control system design and implementation using MATLAB®_. Springer Science & Business Media, 2009.
  * [29] F. A. Goodarzi and T. Lee, “Stabilization of a rigid body payload with multiple cooperative quadrotors,” _Journal of Dynamic Systems, Measurement, and Control_ , vol. 138, no. 12, 2016.
  * [30] A. S. Aghdam, M. B. Menhaj, F. Barazandeh, and F. Abdollahi, “Cooperative load transport with movable load center of mass using multiple quadrotor UAVs,” in _2016 4th International Conference on Control, Instrumentation, and Automation (ICCIA)_. IEEE, 2016, pp. 23–27.