# Differential Flatness and Flatness Inspired Control of Aerial Manipulators
based on Lagrangian Reduction

Skylar X. Wei1, Peder Hårderup2, and Joel W. Burdick1 1Engineering and Applied
Sciences, California Institute of Technology, Pasadena, CA, USA. {swei,
<EMAIL_ADDRESS>of Decision and Control Systems, KTH Royal
Institute of Technology, Stockholm, Sweden<EMAIL_ADDRESS>work is
partially funded by NASA Jet Propulsion Laboratory and California Institute of
Technology Graduate Student Fellowship.

###### Abstract

This paper shows that the dynamics of a general class of aerial manipulators,
consist of an underactuated multi-rotor base with an arbitrary k-linked
articulated manipulator, are differentially flat. Methods of Lagrangian
Reduction under broken symmetries produce reduced equations of motion whose
key variables: center-of-mass linear momentum, vehicle yaw angle, and
manipulator relative joint angles become the flat outputs. Utilizing flatness
theory and a second-order dynamic extension of the thrust input, we transform
the mechanics of aerial manipulators to their equivalent trivial form with a
valid relative degree. Using this flatness transformation, a quadratic
programming based controller is proposed within a Control Lyapunov Function
(CLF-QP) framework, and its performance is verified in simulation.

##  I INTRODUCTION

An aerial manipulator consists of an underactuated flying multi-rotor body
with a multi-link manipulator attached mainly, but not exclusively, at the
body’s geometric center. Aerial manipulation is of increasing interest [1]
since such systems inherit the high mobility of conventional multi-rotor
drones, with the added ability to interact with the environment via the robot
arm’s end-effector. Aerial manipulation has many practical applications, such
as delivering packages and payloads [2], inspection of physical infrastructure
using arm-mounted sensors [3][4], and tool operation [5].

Aerial manipulators present several challenges in trajectory planning and
control. First, they are typically underactuated. Moreover, movements of their
heavy arms or heavy payloads can cause complex shifts in the overall center-
of-mass (CoM), which can potentially induce instabilities.

This paper shows that a general class of aerial manipulators is differentially
flat. Flat systems are equivalent to a trivial system via an endogenous
transformation [6], which enables dynamic feedback linearization. Hence, our
results lead to nonlinear controllability results for these complex aircraft,
and to the design of locally exponentially stabilizing controllers. Multi-
rotors (without robot arms) are known to be differentially flat, and important
multi-rotor trajectory planning methods are based on this fact [7][8]. Our
result allows methods developed for conventional multi-rotors to be
generalized to aerial manipulators.

Prior efforts to prove the differential flatness of aerial manipulators have
required assumptions that are limiting in practice. For example, [9] showed
that an aerial manipulator with a 2-DoF arm is differentially flat, but
assumed that the CoM must be fixed in the end-effector frame, which
unrealistically implies a massless or motionless arm. This result was
generalized in [10] to manipulators with any number of links. But the results
assume that the CoM can only be affected by external forces - an assumption
invalid when a manipulator (with mass) is in motion. A planar aerial
manipulator with any number of rigid or elastic joints was proven to be flat
in [11], and such result was generalized to any number of protocentric
manipulators in [12]; however, the overall CoM of the system must be fixed,
else there are unaccounted Coriolis terms. In [13], valid flat outputs for
rotorcraft with cable-suspended loads were given. But this result does not
generalize to aerial manipulators, as passive cable dynamics cannot model
active dynamic coupling in manipulation. In contrast, our results allow for a
completely variable CoM, and arbitrary arm geometries.

We use recent results in Lagrangian Reduction to formulate reduced aerial
manipulator equations of motion (EoM). We have previously used reduction to
prove the small-time locally controllability of an aerial manipulator with a
planar arm [14]. This work extends the reduction process from a planar arm to
general $k$-linked arms. Further, we propose a new flatness proof for aerial
manipulators that crucially uses the reduced EoM. The flat outputs are: the
linear momentum of the system CoM in the inertial frame, the yaw angle, and
the manipulator joint angles. For completeness, the singularities and the
geometric significance of the flatness derivation are addressed. Inspired by
the flat system’s equivalence to a trivial Brunosky system, we suggest a
second order dynamic extension to the thrust input. This allows us to design a
locally exponentially stabilizing controller using a control Lyapunov function
based quadratic program [15]. The tracking performance is demonstrated in
simulation.

The paper is organized as follows. Section II defines an aerial manipulator.
Section III reviews concepts in Lagrangian Reduction, while Section IV
develops the reduced equations in a control-affine form. Section V reviews
differential flatness, and proves our main theorem. An exponential stabilizing
controller based on flatness and dynamic extension is given in Section VI and
simulated in Section VII.

##  II System Description

We analyze the following class of aerial manipulators.

Definition: A multi-rotor aerial platform with the following characteristics
is an Aerial Manipulator (AM) (see Fig. 1):

  * •

The multi-rotor includes $n$-pairs of identical rotors attached to a common
base, where $n\geq 2$. Each rotor pair consists of one clockwise and one
counterclockwise rotating rotor. All thrust axes point in a common direction,
denoted by unit vector $\hat{z}_{b}$.

  * •

A $k$-link fully-actuated manipulator is attached to the base’s geometric
center. All arm joints are revolute.

  * •

All system components are rigid, and complex-fluid structure interactions are
ignored.

Our model is derived using the following reference frames:

  * •

The earth-fixed inertial frame
$E=\\{O^{e},\hat{x}_{e},\hat{y}_{e},\hat{z}_{e}\\}$.

  * •

The aerial-base body frame
$B=\\{O^{b},\hat{x}_{b},\hat{y}_{b},\hat{z}_{b}\\}$.

  * •

Manipulator $i^{th}$ link frame
$L_{i}=\\{O^{L_{i}},\hat{x}_{L_{i}},\hat{y}_{L_{i}},\hat{z}_{L_{i}}\\}$.

Notationally, vector $\boldsymbol{s}_{ab}\in\mathbb{R}^{3}$ denotes the
position of frame B’s origin relative to frame A’s origin, and and
$\mathcal{R}_{ab}\in SO(3)$ denotes the orientation of frame $B$ relative to
frame $A$. The geometry of the robot arm is described using the Denavit-
Hartenberg (DH) convention. A link reference frame is attached to each link
according to the DH convention. Let $\mathcal{R}_{L_{i}}$, $\forall
i\in\\{1,\cdots,k\\}$ denote the set of rotation matrices that describe the
relative rotation of the $i^{th}$ link frame with respect to the $(i-1)^{st}$
link frame. Let
$\boldsymbol{\eta}\triangleq[\eta_{1},\cdots,\eta_{k}]^{T}\in\mathbb{S}^{k}$
denote the vector of robot arm joint angles, as defined in the DH convention.
The aerial-base body forms link $0$. For simplicity, our derivations assume
that $\mathcal{R}_{L_{1}}=I_{3\times 3}$, and that the manipulator’s first
link operates in the $\hat{x}_{b}-\hat{z}_{b}$ plane only. But this assumption
is easily generalized. The linear velocity of the aerial-base in the $B$ frame
is defined as
$\dot{\boldsymbol{s}}_{b}\triangleq\mathcal{R}_{eb}^{T}\dot{\boldsymbol{s}}_{eb}$.
Using standard roll, pitch, and yaw angles
$\boldsymbol{\xi}=[\phi,\theta,\psi]^{T}$, the angular velocity,
$\boldsymbol{\omega}_{b}$, of the aerial-base in the $B$ frame is:

$\boldsymbol{\omega}_{b}=\left[\begin{smallmatrix}1&0&-\sin(\theta)\\\
0&\cos(\phi)&\sin(\phi)\cos(\theta)\\\
0&-\sin(\phi)&\cos(\phi)\cos(\theta)\end{smallmatrix}\right]\left[\begin{smallmatrix}\dot{\phi}\\\
\dot{\theta}\\\
\dot{\psi}\end{smallmatrix}\right]=\Xi(\boldsymbol{\xi})\dot{\boldsymbol{\xi}}.$
(1)

Lastly,
$\dot{\mathcal{R}}_{eb}\triangleq\mathcal{R}_{eb}S(\boldsymbol{\omega}_{b})$
where $\mathcal{S}(\cdot)$ is the $3\times 3$ skew-symmetric matrix such that
$\mathcal{S}(\boldsymbol{\omega}_{b})\boldsymbol{\beta}=\boldsymbol{\omega}_{b}\times\boldsymbol{\beta},\forall\boldsymbol{\beta}\in\mathbb{R}^{3}$.

Figure 1: Geometry of an aerial manipulator system.

##  III Lagrangian Reduction Preliminary

A mechanical system is defined by the tuple $\Sigma=(Q,L,\mathcal{T})$, where
$Q$ is its finite-dimensional configuration space, assumed to be a smooth
manifold. Let $TQ$ denote the tangent bundle of $Q$, and $T_{q}Q$ the tangent
space to $Q$ at $q\in Q$. Let $L:TQ\to\mathbb{R}$ be the system Lagrangian and
let $\mathcal{T}(q,\dot{q})\in T^{*}Q$ represent the external forces acting on
$\Sigma$, where $T^{*}Q$ is the dual of $TQ$. A Lagrangian possesses a
symmetry if there is an action on its arguments that renders the Lagrangian
invariant. This symmetry allows the reduction of the dynamical system to a
lower dimensional phase space.

A mechanical system possess a symmetry with respect to Lie group $G$ if its
Lagrangian $L:TQ\to\mathbb{R}$ and external forces $\mathcal{T}(q,\dot{q})$
are Lie Group Invariant. The left action of Lie group $G$ on smooth manifold
$Q$ is the map $\Phi_{\mathtt{g}}:Q\times Q:\rightarrow Q:q\rightarrow gq$ for
any $q\in Q$. The configuration space of an aerial manipulator has the product
structure $Q=G\times\mathcal{B}$ where $G=SE(3)$ describes the rigid body
location of the multi-rotor base, and the shape space $\mathcal{B}$ models the
manipulator joint variables. The Lie group $SE(3)$ is a semidirect-product
group: $SE(3)=H\otimes\mathcal{V}$, where Lie subgroup $H=SO(3)$ has a left
action on $\mathcal{V}=\mathbb{R}^{3}$. Thus
$Q=SO(3)\otimes\mathbb{R}^{3}\times\mathcal{B}$.

Associated with a Lie group is its Lie algebra, $\mathfrak{g}$, a vector space
isomorphic to the tangent space at the group identity, i.e.
$\mathfrak{g}\simeq T_{e}G$. The Lie algebra of a semidirect-product group can
be written as $\mathfrak{g}=\mathfrak{h}\otimes
T\mathcal{V}\simeq\mathfrak{h}\otimes\mathcal{V}$ with elements
$(\xi_{\mathtt{h}},\xi_{\mathcal{V}})\in\mathfrak{g}$. In local/body
coordinates, $\xi_{\mathtt{h}}=\mathtt{h}^{-1}\dot{\mathtt{h}}\in\mathfrak{h}$
and $\xi_{\mathcal{V}}=\mathtt{h}^{-1}\dot{v}\in T\mathcal{V}$ where
$(\dot{\mathtt{h}},\dot{v})\in TG$ is an arbitrary tangent vector and
$\mathfrak{h}$ is the Lie Algebra of group $H$. Hence,
$(q,\dot{q})=(h,\dot{h},\nu,\dot{\nu},r,\dot{r})$ where $h,\dot{h}\\!\in\\!H$,
$\nu,\dot{\nu}\\!\in\\!\mathcal{V}$, and $r,\dot{r}\\!\in\\!\mathcal{B}$. For
more details, see [14].

During a manipulation task, the system’s CoM displaces as the arm moves,
breaking a symmetry in the systems’s potential energy. We use advected
parameters [16, 17] to formulate Lagrangian reduction under symmetry breaking
potential energy contributions. For mechanical systems, an advected parameter,
$\boldsymbol{\gamma}(t)$, is a vector expressed in a body-fixed reference
frame satisfying the differential equation:

$\left(\frac{d}{dt}+\mathtt{g}^{-1}(t)\dot{\mathtt{g}}(t)\right)\boldsymbol{\gamma}(t)=0.$
(2)

where $\mathtt{g}\in G$. For aerial manipulators, advected parameter
$\boldsymbol{\gamma}(t)\triangleq\mathcal{R}_{eb}^{T}\boldsymbol{e}_{3}\in\mathcal{V}$
models the direction of gravity (a symmetry-breaking term) in the body-fixed
coordinates. The dependency of the potential energy on the aerial base
position is another symmetry breaking term. With
$\boldsymbol{\zeta}\triangleq\mathcal{R}_{eb}^{T}\boldsymbol{s}_{eb}$, the
potential energy $V_{AM}$ can be expressed as
$V_{AM}(r,\boldsymbol{\gamma},\boldsymbol{\zeta})$, where $r$ denotes the
shape variables. Hence, this reformulated potential energy is $G$-invariant.

Define the Augmented Lagrangian
$\overline{L}:TG\times\mathcal{V}\to\mathbb{R}$ by augmenting the state with
advected parameter $\boldsymbol{\gamma}$ and base position
$\boldsymbol{\zeta}$. If the Augmented Lagrangian is
$\Phi_{\mathtt{h}}$-invariant, then it can be reduced to
$\mathfrak{h}\times\mathcal{V}\times TM$ [18] where $\mathfrak{h}$ is a Lie
algebra of $H$. It can be shown that the system’s reduced equation takes the
general form (see [18] Theorem 3.2.1):

$\displaystyle\dot{\varrho}=\dot{r}^{T}\\!\alpha(r,\boldsymbol{\gamma})\dot{r}$
$\displaystyle+\dot{r}^{T}\\!\beta(r,\boldsymbol{\gamma})\varrho+\varrho^{T}\\!\kappa(r,\boldsymbol{\gamma})\varrho+\tau_{\varrho}(q,\boldsymbol{\gamma}),$
(3) $\displaystyle M(r)\ddot{r}$
$\displaystyle=-C(r,\dot{r})+N(r,\dot{r},\varrho)+\tau_{r}(r,\boldsymbol{\gamma}),$
(4) $\displaystyle\dot{\boldsymbol{\gamma}}$
$\displaystyle=-\xi_{h}\boldsymbol{\gamma},$ (5)
$\displaystyle\dot{\boldsymbol{\zeta}}$
$\displaystyle=-\xi_{h}(\boldsymbol{\zeta}-A(r,\boldsymbol{\gamma})),$ (6)

where $\varrho\in\mathfrak{se}^{*}(3)$ are generalized momenta, defined as
$\varrho_{i}(\xi)=\left<b_{i},\xi\right>$ along the symmetry directions where
$\xi\in\mathfrak{g}$ and ${b_{i}}$ denote any basis of the tangent space to
the orbit at $q\in Q$. Also,
$\tau_{\varrho}(r,\boldsymbol{\gamma},\boldsymbol{\zeta})$ represents the
conservative forces and moments resulting from gravity projected along the
momenta directions. The functions $\alpha(r,\boldsymbol{\gamma})$,
$\beta(r,\boldsymbol{\gamma})$, and $\kappa(r,\boldsymbol{\gamma})$ are
smoothly dependent upon the shape variables $r$ and advected parameter
$\boldsymbol{\gamma}$. For the shape dynamics, the reduced mass-matrix,
Coriolis, potential terms, and actuation forces are denoted as $M(r)$,
$C(r,\dot{r})$, $N(r,\dot{r},\varrho)$, and $\tau_{r}(r,\boldsymbol{\gamma})$
respectively. Lastly, $A(r,\boldsymbol{\gamma})$ is an invertible matrix which
will arise in the reconstruction equation, defined later (7). The structure of
(3) and (4) follows from the reduced variational principle with an extended
base space consisting of the generalized momenta
$\varrho=(\rho_{1},\cdots,\rho_{n})$ and shape-space variables $(r,\dot{r})$.
Eq.s (3) - (6) are, respectively, the momentum equation, shape dynamics,
advection equation, and position dynamics of the system. Together, they form a
complete, reduced representation of the system dynamics.

To recover the spatial motion of the system, we employ a reconstruction
equation as known as connection. It defines a horizontal space of $T_{q}Q$ as
$H_{q}Q\triangleq\mbox{Ker}(\mathcal{A}(q))$, where $\mathcal{A}$ is a
principle connection form and describes motion along the fiber of $Q$ as the
flow of a left-invariant vector field. See [19] and [17]. The general form of
connection is:

$\xi_{{h}}={h}^{-1}\dot{{h}}=-A(r,\gamma)\dot{r}+\Gamma^{-1}(r,\gamma)\varrho,$
(7)

where $A(r,\gamma)$ and $\Gamma(r,\gamma)$ are mass and inertia liked matrix.

##  IV System Dynamics using Lagrangian Reduction and Reconstruction

This section explicitly derives the reduced EoM for the general class of
aerial manipulators defined above.

###  IV-A Kinematics and Dynamics:

Suppose $m_{b}$ and $\mathcal{I}_{b}$ are the mass and inertia tensor of the
multi-rotor, expressed in frame $B$. The kinetic energy of the multi-rotor is
$K_{b}=\frac{1}{2}m_{b}\dot{\boldsymbol{s}}_{eb}^{T}\dot{\boldsymbol{s}}_{eb}+\frac{1}{2}\boldsymbol{\omega}_{b}^{T}\mathcal{I}_{b}\boldsymbol{\omega}_{b}$,
and potential energy is
$V_{s}=m_{b}g\boldsymbol{e}_{3}^{T}\boldsymbol{s}_{eb}$, where vectors
$\boldsymbol{e}_{1}$, $\boldsymbol{e}_{2}$, and $\boldsymbol{e}_{3}$ are the
standard Cartesian basis vectors.

Let the mass of link $i=1,\cdots,k$, w.r.t. frame $L_{k}$ be $m_{i}$. The
$k$-link manipulator dynamics can be conveniently expressed using the
manipulator Jacobian matrix [20]. Similar to the multi-rotor base, the kinetic
and potential energy of each link can be calculated by summing the
translational and rotational contributions. Let
$\dot{\boldsymbol{x}}=[\dot{\boldsymbol{s}}_{b}^{T},\boldsymbol{\omega}_{b}^{T},\dot{\boldsymbol{\eta}}^{T}]^{T}\in\mathbb{R}^{6+k}$.
The total kinetic energy of the system can be rewritten as the following:

$\displaystyle
K_{tot}=\frac{1}{2}\dot{\boldsymbol{x}}^{T}\underbrace{\left[\begin{array}[]{c|c|c}\mathcal{M}_{\boldsymbol{p}}&\mathcal{M}_{\boldsymbol{p}\boldsymbol{\omega}}&\mathcal{M}_{\boldsymbol{p}\boldsymbol{l}}\\\
\hline\cr\mathcal{M}_{\boldsymbol{p}\boldsymbol{\omega}}^{T}&\mathcal{M}_{\boldsymbol{\omega}}&\mathcal{M}_{\boldsymbol{\omega}\boldsymbol{l}}\\\
\hline\cr\mathcal{M}_{\boldsymbol{p}\boldsymbol{l}}^{T}&\mathcal{M}_{\boldsymbol{\omega}\boldsymbol{l}}^{T}&\mathcal{M}_{\boldsymbol{l}}\end{array}\right]}_{\text{$\mathcal{M}(\boldsymbol{\eta})$}}\dot{\boldsymbol{x}},$

where $\mathcal{M}(\boldsymbol{\eta})\in\mathbb{R}^{(6+k)\times(6+k)}$ is the
overall system mass matrix, and its block partition diagonal matrices
$\mathcal{M}_{\boldsymbol{p}}\in\mathbb{R}^{3\times 3}$,
$\mathcal{M}_{\boldsymbol{\omega}}\in\mathbb{R}^{3\times 3}$, and
$\mathcal{M}_{\boldsymbol{l}}\in\mathbb{R}^{k\times k}$ are symmetric mass and
inertia matrices that describe the multi-rotor structure and the manipulator
with respect to $B$ frame. Matrices
$\mathcal{M}_{\boldsymbol{p}\boldsymbol{\omega}}\in\mathbb{R}^{3\times 3}$,
$\mathcal{M}_{\boldsymbol{p}\boldsymbol{l}}\in\mathbb{R}^{3\times k}$, and
$\mathcal{M}_{\boldsymbol{\omega}\boldsymbol{l}}\in\mathbb{R}^{3\times k}$
highlights the coupling effects. The total potential energy of the aerial
manipulator system w.r.t. frame $E$ is
$V_{AM}=g\boldsymbol{e}_{3}^{T}\left(m_{t}\boldsymbol{s}_{eb}+\mathcal{R}_{eb}\boldsymbol{\Delta}\right)$
where $m_{t}\triangleq m_{b}+\sum_{i=1}^{k}m_{i}$ is the total mass of the
system, and
$\boldsymbol{\Delta}(\boldsymbol{\eta})=\sum_{i=1}^{k}m_{i}\,s_{bl_{i}}(\eta_{1},\cdots,\eta_{i})$
is the manipulator CoM in the frame $B$ . It is important to note that the
vector $\boldsymbol{x}$ contains cyclic coordinates,
$\int\boldsymbol{\dot{s}}_{b}$, $\int\boldsymbol{\omega}_{b}$.

The non-conservative forces (thrust, $T$) and moments (roll, pitch, yaw
moment, $\tau_{\phi}$, $\tau_{\theta}$, and $\tau_{\psi}$) produced by the
motors are used as control inputs for the multi-rotor system. The torque input
at the revolute joint connecting link $(i-1)$ and link $i$ is denoted as
$\tau_{l_{i}}$, $\forall i\in\\{1,\cdots,k\\}$. As mentioned previously, the
manipulator is fully actuated with $k$ total inputs
$\boldsymbol{\tau}_{L}\triangleq[\tau_{l_{1}},\cdots,\tau_{l_{k}}]^{T}\in\mathcal{T}_{L}^{k}\subset\mathbb{R}^{k}$.

###  IV-B The Reduced Aerial Manipulator Dynamics

For the aerial manipulator system, the defined advected parameters
$\boldsymbol{\gamma}$ and $\boldsymbol{\zeta}$ satisfy the following advection
equations:

$\dot{\boldsymbol{\gamma}}=-\mathcal{S}(\boldsymbol{\omega}_{b})\boldsymbol{\gamma}\quad\quad\dot{\boldsymbol{\zeta}}=-\mathcal{S}(\boldsymbol{\omega}_{b})\boldsymbol{\zeta}+\dot{\boldsymbol{s}}_{b}.$
(8)

The conservative forces and momenta due to gravity can be derived using the
Lagrange-d’Alembert principle:

$\begin{array}[]{ll}-dV_{AM}=-\frac{\partial
V_{AM}}{\partial\boldsymbol{\gamma}}\dot{\boldsymbol{\gamma}}dt-\frac{\partial
V_{AM}}{\partial\boldsymbol{\zeta}}\dot{\boldsymbol{\zeta}}dt-\frac{\partial
V_{AM}}{\partial\boldsymbol{\eta}}\dot{\boldsymbol{\eta}}dt.\end{array}$ (9)

Explicitly, the force and torque of gravity
$\boldsymbol{\tau}_{\boldsymbol{p}}$ and $\boldsymbol{\tau}_{\boldsymbol{l}}$
are:

$\displaystyle\boldsymbol{\tau}_{\boldsymbol{p}}=-gm_{t}\boldsymbol{\gamma},\quad\boldsymbol{\tau}_{\boldsymbol{l}}=-g\mathcal{S}(\boldsymbol{\gamma})\mathbf{\Delta}-g\left(\frac{\partial\boldsymbol{\Delta}}{\partial\boldsymbol{\eta}}\right)^{T}\\!\boldsymbol{\gamma}.$
(10)

The Augmented Lagrangian, parametrized by $\boldsymbol{\gamma}$ and
$\boldsymbol{\zeta}$, is $SE(3)$-invariant by [18], Theorem 3.2.1. The
Augmented Lagrangian
$\overline{L}(\boldsymbol{\dot{s}}_{b},\boldsymbol{\omega}_{b},\boldsymbol{\eta},\boldsymbol{\gamma},\boldsymbol{\zeta})$
for the system is:

$\begin{array}[]{ll}\overline{L}\\!=\\!\frac{1}{2}\left[\begin{smallmatrix}\boldsymbol{\dot{s}}_{b}\\\
\boldsymbol{\omega}_{b}\\\
\boldsymbol{\eta}\end{smallmatrix}\right]^{T}\\!\\!\mathcal{M}(\mathbf{\boldsymbol{\eta}})\\!\\!\left[\begin{smallmatrix}\dot{\boldsymbol{s}}_{b}\\\
\boldsymbol{\omega}_{b}\\\
\boldsymbol{\eta}\end{smallmatrix}\right]-g(m_{t}\langle\boldsymbol{\gamma},\boldsymbol{\zeta}\rangle+\langle\boldsymbol{\gamma},\boldsymbol{\Delta}\rangle).\end{array}$
(11)

In the standard basis for $\mathfrak{se}(3)$, the Lie algebra of $SE(3)$, the
generalized linear and angular momenta take the form:
$\boldsymbol{p}\triangleq\frac{\partial\overline{L}}{\partial\dot{\boldsymbol{s}}_{b}}\in
T^{*}\mathbb{R}^{3}$ and
$\boldsymbol{l}\triangleq\frac{\partial\overline{L}}{\partial\boldsymbol{\omega}_{b}}\in
T^{*}SO(3)$ which are:

$\begin{bmatrix}\boldsymbol{p}\\\
\boldsymbol{l}\end{bmatrix}=\underbrace{\begin{bmatrix}\mathcal{M}_{\boldsymbol{p}}&\mathcal{M}_{\boldsymbol{p}\boldsymbol{\omega}}\\\
\mathcal{M}_{\boldsymbol{p}\boldsymbol{\omega}}^{T}&\mathcal{M}_{\boldsymbol{\omega}}\end{bmatrix}}_{\text{$\triangleq\mathcal{M}_{s}$}}\begin{bmatrix}\dot{\boldsymbol{s}}_{b}\\\
\boldsymbol{\omega}_{b}\end{bmatrix}+\underbrace{\begin{bmatrix}\mathcal{M}_{\boldsymbol{p}\boldsymbol{l}}\\\
\mathcal{M}_{\boldsymbol{\omega}\boldsymbol{l}}\end{bmatrix}}_{\text{$\triangleq\mathcal{M}_{sl}$}}\dot{\boldsymbol{\eta}}.$
(12)

The aerial manipulator system’s connection (which reconstructs the AM’s motion
from the momentum equations) is simply:

$\begin{bmatrix}\dot{\boldsymbol{s}}_{b}\\\
\boldsymbol{\omega}_{b}\end{bmatrix}=\mathcal{M}_{s}^{-1}\left(\begin{bmatrix}\boldsymbol{p}\\\
\boldsymbol{l}\end{bmatrix}-\mathcal{M}_{sl}\dot{\boldsymbol{\eta}}\right).$
(13)

Following the work in [21], the non-conservative forces and moments can be
projected along the acting momentum directions. Since the symmetry directions
are a simple basis of $\mathfrak{se}$(3), the momentum equation including
conservative forces $-dV_{AM}$ and non-conservative forces and moments
becomes:

$\begin{array}[]{ll}\dot{\boldsymbol{p}}&=\boldsymbol{p}\times\boldsymbol{\omega}_{b}+\boldsymbol{\tau}_{\boldsymbol{p}}+T\boldsymbol{e}_{3},\\\
\dot{\boldsymbol{l}}&=\boldsymbol{p}\times\dot{\boldsymbol{s}}_{b}+\boldsymbol{l}\times\boldsymbol{\omega}_{b}+\boldsymbol{\tau}_{\boldsymbol{l}}+\begin{bmatrix}\tau_{\phi}&\tau_{\theta}&\tau_{\psi}\end{bmatrix}^{T}.\end{array}$
(14)

Further, the shape dynamics of the manipulator is derived based on the Euler-
Lagrange equation:

$\ddot{\boldsymbol{\eta}}\\!=\\!\mathcal{X}\\!\left(\\!-\mathcal{C}(\boldsymbol{x},\dot{\boldsymbol{x}})\dot{\boldsymbol{x}}\\!-\\!\boldsymbol{D}(\boldsymbol{x})\\!+\\!\left[\begin{smallmatrix}\mathbf{0}_{1\times
2}&T&\tau_{\phi}&\tau_{\theta}&\tau_{\psi}&\boldsymbol{\tau}_{L}^{T}\end{smallmatrix}\right]^{T}\right),$
(15)

where matrix $\mathcal{X}\triangleq\left[\begin{smallmatrix}0_{k\times
6}&I_{k\times k}\end{smallmatrix}\right]\mathcal{M}^{-1}(\boldsymbol{\eta})$,
and $\mathcal{C}(\boldsymbol{x},\dot{\boldsymbol{x}})$ is the Coriolis matrix
of the shape variable which can be calculated as the following [20]:

$\mathcal{C}_{(p,j)}\\!=\\!\sum_{i=1}^{12+2k}\\!\frac{1}{2}\left(\frac{\partial\mathcal{M}_{(p,j)}}{\partial
x_{i}}+\frac{\partial\mathcal{M}_{(p,i)}}{\partial
x_{j}}-\frac{\partial\mathcal{M}_{(i,j)}}{\partial x_{p}}\right),$ (16)

and $\boldsymbol{D}(\boldsymbol{x})$ is the potential term where
$\boldsymbol{D}(\boldsymbol{x})=\frac{\partial
V_{AM}}{\partial\boldsymbol{x}}$.

###  IV-C Control Affine Form of the AM’s reduced dynamics

We choose the following state parametrization for the reduced dynamical
equations:

$\mathbf{q}\triangleq[\boldsymbol{p}^{T},\boldsymbol{l}^{T},\phi,\theta,\psi,\boldsymbol{\eta}^{T},\dot{\boldsymbol{\eta}}^{T}]^{T}\in\mathbb{R}^{9+2k},$

where Euler angles are used, instead of the advected parameter
$\boldsymbol{\gamma}$, to parametrize multi-rotor orientation. With
$\mathbf{q}$, the reduced AM dynamics take the control affine form:

$\displaystyle\dot{\mathbf{q}}$
$\displaystyle=\mathbf{f}(\mathbf{q})+\mathcal{G}(\mathbf{q})\mathbf{u},$ (17)
$\displaystyle\mathbf{u}$
$\displaystyle=[\tau_{l_{1}},\cdots,\tau_{l_{k}},T,\tau_{\phi},\tau_{\theta},\tau_{\psi}]^{T}\in\mathcal{U}\subset\mathbb{R}^{4+k},$
(18)

where $\mathcal{U}$ is the space of all control inputs. Further, the drift
term and input vector fields are

$\displaystyle\mathbf{f}(\mathbf{q})$
$\displaystyle=\begin{bmatrix}\boldsymbol{p}\times\boldsymbol{\omega}_{b}-m_{t}g\boldsymbol{\gamma}\\\
\boldsymbol{p}\times\dot{\boldsymbol{s}}_{b}+\boldsymbol{l}\times\boldsymbol{\omega}_{b}-g\mathcal{S}(\boldsymbol{\gamma})\mathbf{\Delta}-g\left(\frac{\partial\boldsymbol{\Delta}}{\partial\boldsymbol{\eta}}\right)^{T}\boldsymbol{\gamma}\\\
\Xi^{-1}(\boldsymbol{\xi})\boldsymbol{\omega}_{b}(\boldsymbol{p},\boldsymbol{l},\boldsymbol{\eta},\boldsymbol{\dot{\eta}})\\\
\dot{\boldsymbol{\eta}}\\\
\mathcal{X}\left(-\mathcal{C}(\boldsymbol{x},\dot{\boldsymbol{x}})\dot{\boldsymbol{x}}-\boldsymbol{D}(\boldsymbol{x})\right)\end{bmatrix},$
$\displaystyle\mathcal{G}(\mathbf{q})$
$\displaystyle=\begin{bmatrix}0_{3\times k}&\boldsymbol{e}_{3}&0_{3\times
3}\\\ 0_{3\times k}&0_{3\times 1}&I_{3\times 3}\\\ 0_{3\times k}&0_{3\times
1}&0_{3\times 3}\\\ 0_{2\times k}&0_{2\times 1}&0_{2\times 3}\\\
\mathcal{X}\left[\begin{smallmatrix}0_{6\times k}\\\ I_{k\times
k}\end{smallmatrix}\right]&\mathcal{X}\left[\begin{smallmatrix}0_{2\times
1}\\\ 1\\\ 0_{(3+2k)\times
1}\end{smallmatrix}\right]&\mathcal{X}\left[\begin{smallmatrix}0_{3\times
3}\\\ I_{3\times 3}\\\ 0_{k\times 3}\end{smallmatrix}\right]\ \end{bmatrix}.$

It is important to note, states $\boldsymbol{\omega}_{b}$ and
$\dot{\boldsymbol{s}}_{b}$ can be expressed in terms of $\mathbf{q}$ using the
connection (13).

##  V Differential Flatness

Flatness, first defined by Fliess et al. [6] and originating from differential
algebra, transforms systems as a differential field generated by a set of
states and inputs. A flat system has well characterized nonlinear structures
which can be exploited in designing control algorithms for planning,
trajectory generation, and stabilization [22]. We here adopted the Lie-
Bäcklund framework to approach to flatness and system equivalence. Consider
two systems ($\mathcal{A}$, $X$) and ($\mathcal{B}$, $Y$) and a smooth mapping
$\Phi:\mathcal{A}\to\mathcal{B}$. The pair $(\mathcal{A},X)$ is a system of
differential equation where $\mathcal{A}$ is an open set of $\mathbb{R}^{n}$
and $X$ is a smooth vector field on $\mathcal{A}$.

######  Definition.

(Equivalent System) [22] Systems ($\mathcal{A}$, $X$) and ($\mathcal{B}$, $Y$)
are equivalent at $(a,b)\in\mathcal{A}\times\mathcal{B}$ if there exist a
smooth mapping $\Phi$ from a neighborhood of $a$ to a neighborhood of
$b=\Phi(a)$ which is an endogenous transformation at $(a,b)$.

An endogenous transformation is an invertible transformation that "exchanges"
the trajectory between two systems. See [22] for the rigorous definition. This
leads to the formal definition of differential flatness:

######  Definition.

(Differentially Flat System) The control system $(\mathcal{A},X)$ is
differentially flat around $a$ if and only if it is equivalent to a trivial
system in a neighborhood of $a$.

The trivial system referred to in the above definition is the system
$(\mathbb{R}^{\infty}_{a},X_{a})$ with coordinates
$(\boldsymbol{y},\dot{\boldsymbol{y}},\ddot{\boldsymbol{y}},\ldots)$ and
vector field
$X_{a}(\boldsymbol{y},\dot{\boldsymbol{y}},\ddot{\boldsymbol{y}},\ldots)=(\dot{\boldsymbol{y}},\ddot{\boldsymbol{y}},\dddot{\boldsymbol{y}},\ldots)$.
Casually speaking, the trivial system composes of chain of integrators.
Further, the set $\boldsymbol{y}=\\{y_{j}\text{ s.t. }j=1,\cdots,a\\}$ is
called a flat output of $\mathcal{A}$. This is equivalent to the more
familiarizing yet informal definition. Given the nonlinear system

$\dot{\boldsymbol{x}}=f(\boldsymbol{x}(t),\boldsymbol{u}(t));\;\boldsymbol{x}\in\mathbb{R}^{n},\boldsymbol{u}\in\mathbb{R}^{m},$
(19)

where $\boldsymbol{x}$ are the $n$ states and $\boldsymbol{u}$ are the $m$
inputs, $\boldsymbol{y}\in\mathbb{R}^{m}$ is said to be a flat output if:

  * •

$y_{i}=g_{i}(\boldsymbol{x},\boldsymbol{u},\dot{\boldsymbol{u}},\ldots,\boldsymbol{u}^{(j_{i})}),\;j_{i}\in\mathbb{N},i=1,2,\ldots,m$.

  * •

$x_{i}=h_{i}(\boldsymbol{y},\dot{\boldsymbol{y}},\ldots,\boldsymbol{y}^{(k_{i})}),\;k_{i}\in\mathbb{N},i=1,2,\ldots,n$,
$u_{i}=\tilde{h}_{i}(\boldsymbol{y},\dot{\boldsymbol{y}},\ldots,\boldsymbol{y}^{(l_{i})}),\;l_{i}\in\mathbb{N},i=1,2,\ldots,m$.

  * •

All components of $\boldsymbol{y}$ are differentially independent, i.e.
$\boldsymbol{y}$ satisfies no differential equation
$\Phi(\boldsymbol{y},\dot{\boldsymbol{y}},\ldots,\boldsymbol{y}^{(k)})=0,\;k\in\mathbb{N}$.

###  V-A Main Theorem and Proof

######  Theorem 1.

$\boldsymbol{\sigma}=[(\mathcal{R}_{eb}\boldsymbol{p})^{T},\psi,\boldsymbol{\eta}^{T}]^{T}=[\sigma_{1},\cdots,\sigma_{4+k}]^{T}\in\mathbb{R}^{4+k}$
is set of differentially flat outputs for the defined class of aerial
manipulators except at singularities which are
$\phi,\theta=\frac{\kappa\pi}{2}$, $\forall\kappa\in\mathbb{Z}$ and $T=0$.

###### Proof.

For ease of notation, we define
$\mathcal{R}_{eb}\boldsymbol{p}\triangleq\boldsymbol{p}_{e}$. Physically
speaking, the flat outputs consists of $\boldsymbol{p}_{e}$, the linear
momentum of the CoM in $E$ frame, $\psi$, yaw position angle, and
$\boldsymbol{\eta}$, relative joint angles. We will state without proving that
these outputs are differentially independent except at singularity points
addressed at the end of this section.

Starting with extracting the only external force, thrust $T$, we can
differentiate $\boldsymbol{p}_{e}$ that unfolds the following relationship:

$\frac{d}{dt}\boldsymbol{p}_{e}=\mathcal{R}_{eb}\left(\dot{\boldsymbol{p}}-\boldsymbol{p}\times\boldsymbol{\omega}_{b}\right)=-m_{t}g\boldsymbol{e}_{3}+\mathcal{R}_{eb}\boldsymbol{e}_{3}T.$
(20)

Algebraically, we can use (20) to extract the forces and orientation
(represented using Euler angles):

$T(\dot{\boldsymbol{p}}_{e})=\|\dot{\boldsymbol{p}}_{e}+m_{t}g\boldsymbol{e}_{3}\|_{2},$
(21)
$\phi(\dot{\boldsymbol{p}}_{e},\psi)=\sin^{-1}\left(\frac{\boldsymbol{e}_{1}^{T}\dot{\boldsymbol{p}}_{e}\sin(\psi)-\boldsymbol{e}_{2}^{T}\dot{\boldsymbol{p}}_{e}\cos\psi}{T}\right),$
(22)
$\theta(\dot{\boldsymbol{p}}_{e},\psi)=\tan^{-1}\left(\frac{\boldsymbol{e}_{1}^{T}\dot{\boldsymbol{p}}_{e}\cos(\psi)+\boldsymbol{e}_{2}^{T}\dot{\boldsymbol{p}}_{e}\sin\psi}{\boldsymbol{e}_{3}^{T}\dot{\boldsymbol{p}}_{e}+m_{t}g}\right).$
(23)

Therefore,
$\mathcal{R}_{eb}(\boldsymbol{\xi})=\mathcal{R}_{eb}(\dot{\boldsymbol{p}}_{e},\psi)$.

Using the roll-pitch-yaw dynamics (1), the body rates can be computed as
$\boldsymbol{\omega}_{b}=\Xi(\boldsymbol{\xi})\dot{\boldsymbol{\xi}}=\boldsymbol{\omega}_{b}(\dot{\boldsymbol{p}}_{e},\ddot{\boldsymbol{p}}_{e},\psi,\dot{\psi})$.
We can also recover the body frame general linear momenta $\boldsymbol{p}$ as
a function as the flat outputs and their derivatives once $\mathcal{R}_{eb}$
is known,
$\boldsymbol{p}(\boldsymbol{p}_{e},\dot{\boldsymbol{p}}_{e},\psi)=\mathcal{R}_{eb}^{T}\boldsymbol{p}_{e}$.
Using the connection (13), we can obtain the multi-rotor linear velocity in
frame $B$:

$\dot{\boldsymbol{s}}_{b}\left(\boldsymbol{p}_{e},\dot{\boldsymbol{p}}_{e},\ddot{\boldsymbol{p}}_{e},\psi,\dot{\psi},\boldsymbol{\eta},\dot{\boldsymbol{\eta}}\right)=\frac{1}{m_{t}}\left(\boldsymbol{p}-\mathcal{M}_{\boldsymbol{{p\omega}}}\boldsymbol{\omega}_{b}-\mathcal{M}_{\boldsymbol{pl}}\dot{\boldsymbol{\eta}}\right).$

From the definition of generalized linear momentum (12), the overall CoM
angular momenta, $\boldsymbol{l}$, and its derivative, $\dot{\boldsymbol{l}}$,
are also functions of the flat outputs and their time derivatives:

$\boldsymbol{l}(\boldsymbol{p}_{e},\dot{\boldsymbol{p}}_{e},\ddot{\boldsymbol{p}}_{e},\psi,\dot{\psi},\boldsymbol{\eta},\dot{\boldsymbol{\eta}})=\mathcal{M}_{\boldsymbol{p}\boldsymbol{\omega}}^{T}\dot{\boldsymbol{s}}_{b}+\mathcal{M}_{\boldsymbol{\omega}}\boldsymbol{\omega}_{b}+\mathcal{M}_{\boldsymbol{\omega}\boldsymbol{l}}\dot{\boldsymbol{\eta}}.$

Compactly,
$\dot{\boldsymbol{l}}=\dot{\boldsymbol{l}}(\boldsymbol{p}_{e},\dot{\boldsymbol{p}}_{e},\ddot{\boldsymbol{p}}_{e},\dddot{\boldsymbol{p}}_{e},\psi,\dot{\psi},\ddot{\psi},\boldsymbol{\eta},\dot{\boldsymbol{\eta}},\ddot{\boldsymbol{\eta}})$.
Lastly, we have 3 equations and 3 unknowns to algebraically solve for the
roll-pitch-yaw torque $\tau_{\phi},\tau_{\theta}$ and $\tau_{\psi}$:

$\begin{bmatrix}\tau_{\phi}\\\ \tau_{\theta}\\\
\tau_{\psi}\end{bmatrix}\left(\begin{smallmatrix}\boldsymbol{p}_{e},\dot{\boldsymbol{p}}_{e},\ddot{\boldsymbol{p}}_{e},\\\
\dddot{\boldsymbol{p}}_{e},\psi,\dot{\psi},\\\
\ddot{\psi},\boldsymbol{\eta},\dot{\boldsymbol{\eta}},\ddot{\boldsymbol{\eta}}\end{smallmatrix}\right)=\dot{\boldsymbol{l}}-\boldsymbol{l}\times\omega_{b}-\boldsymbol{\tau}_{\boldsymbol{l}}-\boldsymbol{p}\times\dot{\boldsymbol{s}}_{b}.$
(24)

The torque input for the manipulator can be calculated by substituting the
respective variables into (15):

$\boldsymbol{\tau}_{\boldsymbol{L}}=\left[\begin{smallmatrix}0_{k\times
6}&I_{k\times
k}\end{smallmatrix}\right]\left(\mathcal{M}(\boldsymbol{\eta})\ddot{\boldsymbol{x}}+\mathcal{C}(\boldsymbol{x},\dot{\boldsymbol{x}})\dot{\boldsymbol{x}}+\boldsymbol{D}(\boldsymbol{x})\right),$
(25)

where
$\ddot{\boldsymbol{x}}=\begin{bmatrix}\ddot{\boldsymbol{s}}_{b}^{T}&\dot{\boldsymbol{\omega}}_{b}^{T}&\ddot{\boldsymbol{\eta}}\end{bmatrix}^{T}$.
By differentiation, $\ddot{\boldsymbol{s}}_{b}$ and
$\ddot{\boldsymbol{\omega}}_{b}$ are also functions of the flat outputs and
their derivatives up to $\dddot{\boldsymbol{p}}_{e}$, $\ddot{\psi}$, and
$\ddot{\boldsymbol{\eta}}$. As a result, one can show that
$\boldsymbol{\tau}_{L}=\boldsymbol{\tau}_{L}(\boldsymbol{p}_{e},\dot{\boldsymbol{p}}_{e},\ddot{\boldsymbol{p}}_{e},\dddot{\boldsymbol{p}}_{e},\psi,\dot{\psi},\ddot{\psi},\boldsymbol{\eta},\dot{\boldsymbol{\eta}},\ddot{\boldsymbol{\eta}})$.
Therefore, all states $\boldsymbol{q}$ and inputs $\boldsymbol{u}$ are
algebraic functions of the flat outputs $\boldsymbol{\sigma}$ and their time
derivatives. ∎

The only singularities arising in the proof are when the system at free fall,
i.e. $\boldsymbol{e}_{3}^{T}\dot{\boldsymbol{p}}_{e}+m_{t}g=0$, or
equivalently $T\cos\phi\cos\theta=0$. Therefore, we confine the domain of the
attitude angles to avoid singularity, i.e.
$\phi\in(-\frac{\pi}{2},\frac{\pi}{2})$,
$\theta\in(-\frac{\pi}{2},\frac{\pi}{2})$, and thrust to be strictly positive,
$T>0$.

###  V-B Equivalence to trivial system:

Similar to the multi-rotor case, with a second order dynamic extension of the
total thrust, we can obtain a valid relative degree using the flat output as
the desired output [23]. We extend the state variable to include the thrust
dynamics:
$\mathbf{q}_{de}\triangleq\begin{bmatrix}\mathbf{q}^{T},T,\dot{T}\end{bmatrix}^{T}$.
Consequently, the control inputs become
$\mathbf{u}_{de}=\begin{bmatrix}\boldsymbol{\tau}_{L}^{T},\ddot{T},\tau_{\phi},\tau_{\theta},\tau_{\psi}\end{bmatrix}^{T}\in\mathcal{U}_{de}$.
We here restate the control-affine form with extended states and inputs.

$\displaystyle\dot{\mathbf{q}}_{de}=\mathbf{f}_{de}(\mathbf{q}_{de})$
$\displaystyle+\mathcal{G}_{de}(\mathbf{q}_{de})\mathbf{u}_{de},$ (26)
$\displaystyle\mathbf{f}_{de}\\!=\\!\\!\left[\begin{smallmatrix}\\!\mathbf{f}(\mathbf{q})+\left[\begin{smallmatrix}\boldsymbol{e}_{3}T\\\
0_{8\times 1}\\\ \mathcal{X}\left[\begin{smallmatrix}0_{2\times 1}\\\ T\\\
0_{(3+2k)\times 1}\end{smallmatrix}\right]\end{smallmatrix}\right]\\\
\dot{T}\\\ 0\end{smallmatrix}\\!\right]$
$\displaystyle\\!,\,\mathcal{G}_{de}\\!=\\!\\!\left[\begin{smallmatrix}0_{1\times(k+4)}\\\
\mathbf{\mathcal{G}(\mathbf{q})}_{(2:11,:)}\\\
\\!\mathcal{X}\left[\begin{smallmatrix}0_{3\times k}&0_{3\times 1}&0_{3\times
3}\\\ 0_{3\times k}&0_{3\times 1}&I_{3\times 3}\\\ I_{k\times k}&0_{k\times
1}&0_{k\times 3}\\\ \end{smallmatrix}\right]\\\ 0_{1\times(k+4)}\\\
[0_{1\times k}\quad 1\quad 0_{1\times 3}]\end{smallmatrix}\\!\right]\\!\\!.$

Since the original inputs $\mathbf{u}$ are functions of the flat output up to
their $3^{rd}$ time derivatives, we propose an auxiliary control input
$\mathbf{v}=\left[\begin{smallmatrix}\left(\boldsymbol{p}^{T}_{e}\right)^{(3)}&\psi^{(2)}&\left(\boldsymbol{\eta}^{T}\right)^{(2)}\end{smallmatrix}\right]^{T}$,
where the (⋅) denotes the number of time differentiation, which can expressed
as the following:

$\mathbf{v}=\mathbf{f}_{v}(\boldsymbol{\sigma},\dot{\boldsymbol{\sigma}},\ddot{\boldsymbol{\sigma}},T,\dot{T})+\mathcal{G}_{v}(\boldsymbol{\sigma},\dot{\boldsymbol{\sigma}},\ddot{\boldsymbol{\sigma}},T,\dot{T})\mathbf{u_{de}},$
(27)

where $\mathbf{f}_{v}\in\mathbb{R}^{4+k}$ and
$\mathcal{G}_{v}\in\mathbb{R}^{(4+k)\times(4+k)}$ are the drift vector field
and actuation matrix, respectively. Using the extended dynamics (26) and
connection (12), one can show $\mathbf{f}_{v}$ and $\mathcal{G}_{v}$ can be
expressed as a function of $\mathbf{q}_{de}$ by differentiating the flat
outputs until the control inputs surfaces.

By the system equivalence argument, one can obtain the following Brunovsky’s
canonical description of (17):

$\frac{d}{dt}\\!\\!\\!\left[\\!\begin{smallmatrix}\boldsymbol{p}_{e}\\\
\psi\\\ \boldsymbol{\eta}\\\ \dot{\boldsymbol{p}}_{e}\\\ \dot{\psi}\\\
\dot{\boldsymbol{\eta}}\\\
\ddot{\boldsymbol{p}}_{e}\end{smallmatrix}\\!\right]\\!\\!=\\!\underbrace{\left[\\!\begin{smallmatrix}0_{(7\\!+\\!k)\times(4\\!+\\!k)}&I_{(7\\!+\\!k)\\!\times\\!(7\\!+\\!k)}\\\
0_{(4\\!+\\!k)\\!\times\\!(4\\!+\\!k)}&0_{(4\\!+\\!k)\\!\times\\!(7\\!+\\!k)}\end{smallmatrix}\\!\right]}_{\text{$\mathcal{F}_{B}$}}\\!\\!\left[\\!\begin{smallmatrix}\boldsymbol{p}_{e}\\\
\psi\\\ \boldsymbol{\eta}\\\ \dot{\boldsymbol{p}}_{e}\\\ \dot{\psi}\\\
\dot{\boldsymbol{\eta}}\\\
\ddot{\boldsymbol{p}}_{e}\end{smallmatrix}\\!\right]\\!+\\!\underbrace{\left[\\!\begin{smallmatrix}0_{(7\\!+\\!k)\\!\times\\!(4\\!+\\!k)}\\\
I_{(4\\!+\\!k)\\!\times\\!(4\\!+\\!k)}\end{smallmatrix}\\!\right]}_{\text{$\mathcal{G}_{B}$}}\\!\mathbf{v}.$
(28)

(a) Desired Output Tracking Performance and Min-Norm control inputs using CLF-
QP controller from Simulation. $\mathcal{Q}$ is a $15$ by $15$ identity matrix
to maximize radius of attraction. An initial condition disturbance of is
introduced to demonstrate robustness. Linear momentum $\sigma_{1}$,
$\sigma_{2}$, and $\sigma_{3}$ are in $kg\cdot m/s$ and angles $\psi$,
$\eta_{1}$, and $\eta_{2}$ are in $rad$. ((b)) CLF-QP and theoretical control
input are compared where the theoretical ones are obtained from flatness
derivation: (24), (25), extend thrust using given desired trajectory. Torque
inputs $\tau_{l_{1}}$, $\tau_{l_{2}}$, $\tau_{\phi}$, $\tau_{\theta}$, and
$\tau_{\psi}$ are in $N\cdot m$, and $\ddot{T}$ has the unit of $N/s^{2}$.
((c)) Visualization of aerial manipulator follows a desired path and
stabilizes while the arm is manipulating. Figure 2: Simulation result of an
aerial manipulator with a 2-link manipulator.

###  V-C Results Discussion

Our general class of aerial manipulators are strongly and nonlinearly
accessible and controllable because of flatness [6]. The flatness proof is
rather algebraic, which leads to efficient implementations, but masks its
geometric significance. One can observe similarities between the AMs analyzes
in this paper and a the well known proof of multi-rotor flatness [7]. Our
method of determining the rotation matrix $\mathcal{R}_{eb}$ is identical to
the multi-rotor case, but uses an Euler representation of $SO(3)$
representation. Inverting the yaw rotation by $\psi$, the directional vector
parallel to gravity in body frame gives us the roll and pitch angle as shown
in (22) and (23). A key difference arises in our use of the reconstruction
equation (13), which models the coupling between the arm dynamics and multi-
rotor base dynamics in symbolically compact way.

The dynamic extension is non-conventional, but practically motivated. It can
be realize by considering a standard DC motor model. Let $\Omega_{j}$ be the
RPM of the $j^{th}$ motor. The thrust and resistive torque generated by the
$j^{th}$ motor can be modeled as $c_{T}\Omega_{j}^{2}$ and $\pm
c_{Q}\Omega_{j}^{2}$, respectively, where $c_{T}$ and $c_{Q}$ are coefficients
that depends on rotor geometry. Further, the rotor acceleration per minute,
$\dot{\Omega}_{j}$, is proportional to the motor axial torque as well as the
armature current as $i(t)=\frac{\dot{\Omega}_{j}}{K_{m_{j}}}$. Using a
standard RLC circuit model, we can express $\ddot{T}_{j}$ as:

$\displaystyle\ddot{T}_{j}=2c_{T}\frac{i^{2}(t)}{K^{2}_{m_{j}}}+\frac{2c_{T}}{K_{m_{j}}L_{j}}\Omega_{j}\left(U_{j}-K_{\mathcal{V}_{j}}\Omega_{j}-R_{j}\,i(t)\right),$

where $U_{j}$, $R_{j}$, $L_{j}$, $K_{m_{j}}$ and $K_{\mathcal{V}_{j}}$ are the
voltage, internal resistance, inductance, torque constant, and back-emf
constant of brushless motor $j$, respectively. Thus, $\ddot{T}$ can be
modulated by regulating the electrical power feed to the motors.

##  VI Exponentially Tracking Controllers

Like multi-rotors, AMs are underactuated. Further, their CoMs can shift
signficantly during arm manipulation. These characteristics offer challenges
to the stabilization and trajectory tracking problems of AMs. Suppose we are
given a desired trajectory
$\boldsymbol{\sigma}_{d}(t)=[\boldsymbol{p}_{e,d}^{T}(t),\psi_{d}(t),\boldsymbol{\eta}_{d}^{T}(t)]^{T}$,
as a function of time. We assume the path be at least three times continuously
differentiable and dynamically feasible (i.e., desired behavior can be
realized within the range of control inputs). We can exploit the Brunovsky’s
equivalent system, a readily fully controllable normal form, as in [23] and
[24]. Hence, we use this dynamic feedback linearized form (17) to implement an
optimization based controller that guarantees local exponential tracking.

Starting with defining the tracking error between the actual output and the
time-dependent desired trajectory:

$\mathbf{e}(\mathbf{q}_{de})\triangleq\begin{bmatrix}\boldsymbol{p}_{e}(\mathbf{q}_{ed})\\\
\hline\cr\psi(\mathbf{q}_{ed})\\\
\boldsymbol{\eta}(\mathbf{q}_{ed})\end{bmatrix}-\begin{bmatrix}\boldsymbol{p}_{e,d}(t)\\\
\hline\cr\psi_{d}(t)\\\
\boldsymbol{\eta}_{d}(t)\end{bmatrix}=\begin{bmatrix}\mathbf{e}_{1}(\mathbf{q}_{de}(t))\\\
\hline\cr\\\ \mathbf{e}_{2}(\mathbf{q}_{de}(t))\\\ \end{bmatrix}.$ (29)

One can easily verify that the output tracking error
$\mathbf{e}(\mathbf{q}_{de})$ has a vector relative degree
$\mathbf{r}=\left[\begin{smallmatrix}3&3&3&2&2&\cdots&2\end{smallmatrix}\right]$.
Explicitly,

$\mathbf{e}^{(\mathbf{r})}\\!(\mathbf{q}_{de})=\begin{bmatrix}\mathbf{e}_{1}^{(3)}(\mathbf{q}_{de})\\\
\mathbf{e}_{2}^{(2)}(\mathbf{q}_{de})\end{bmatrix}=\mathbf{f}_{\mathbf{v}}(\mathbf{q}_{de})-\boldsymbol{\sigma}_{d}^{\mathbf{(r)}}+\mathcal{G}_{\mathbf{v}}\mathbf{u}_{de}.$
(30)

The rank of the decoupling matrix $\mathcal{G}_{\mathbf{v}}$ is verified
symbolically, indicating that matrix $\mathcal{G}_{\mathbf{v}}$ is invertible
except at the defined singularities. We leverage the well-established CLF-QP
formulation [15],[25],[26] to drive the error, $\mathbf{e}(\mathbf{q_{de})}$,
to zero exponentially. Let
$\mathbf{h}(\mathbf{q_{de},t})=\begin{bmatrix}\mathbf{e}_{1}^{T}&\mathbf{e}_{2}^{T}&\dot{\mathbf{e}}_{1}^{T}&\dot{\mathbf{e}}_{2}^{T}&\ddot{\mathbf{e}}_{1}^{T}\end{bmatrix}^{T}$,
$\dot{\mathbf{h}}=\mathcal{F}_{B}\mathbf{h}+\mathcal{G}_{B}\mathbf{v}$ where
$\mathcal{F}_{B}$, $\mathcal{G}_{B}$, and $\mathbf{v}$ are stated in (28). We
construct the CLF, $V(\mathbf{h})$, using the solution
$\mathcal{P}=\mathcal{P}^{T}\succ 0$ of the Continuous-Time Algebraic Riccati
Equation (CARE):

$\displaystyle\\!V(\mathbf{h})=\mathbf{h}^{T}\mathcal{P}\mathbf{h},\;\;\mathcal{F}_{B}^{T}\mathcal{P}+\mathcal{P}^{T}\mathcal{F}_{B}-\mathcal{P}\mathcal{G}_{B}\mathcal{G}_{B}^{T}\mathcal{P}=-\mathcal{Q},$
(31)

where $\mathcal{Q}=\mathcal{Q}^{T}\succ 0$. Inspired by [25], we propose the
following task space QP controller:

$\displaystyle\begin{split}\mathbf{u}_{de}^{*}&=\operatorname*{arg\,min}_{\mathbf{u}_{de}\in\mathcal{U}_{de}\subset\mathbb{R}^{4+k}}\|\mathcal{G}_{v}(\mathbf{q}_{de})\mathbf{u}_{de}+\mathbf{f}_{v}(\mathbf{q}_{de})\|^{2},\\\
\mbox{s.t.}\\!\quad
L_{\mathcal{F}_{B}}\\!&V(\mathbf{q}_{de},t)+L_{\mathcal{G}_{B}}\\!V(\mathbf{q}_{de},t)(\mathcal{G}_{de}(\mathbf{q}_{de})\mathbf{u}_{de}+\mathbf{f}_{\mathbf{v}}(\mathbf{q}_{de}))\\\
&-L_{\mathcal{G}_{B}}\\!V(\mathbf{q}_{de},t)\boldsymbol{\sigma}_{d}^{\mathbf{(r)}}\leq-\frac{\lambda_{min}(\mathcal{Q})}{\lambda_{max}(\mathcal{P})}V(\mathbf{q}_{de},t),\end{split}$
(32)

where $L_{\mathcal{F}_{B}}\\!V$ and $L_{\mathcal{G}_{B}}\\!V$ are the
following

$\displaystyle\dot{V}(\mathbf{h})=\underbrace{\mathbf{h}^{T}(\mathcal{F}_{B}^{T}\mathcal{P}+\mathcal{P}\mathcal{F}_{B})\mathbf{h}}_{L_{\mathcal{F}_{B}}V}+\underbrace{2\mathbf{h}^{T}\mathcal{P}\mathcal{G}_{B}}_{L_{\mathcal{G}_{B}}V}\mathbf{v}<0,\quad\forall\mathbf{h},$

One key advantage of the exponentially stabilizing CLF-QP controller (32) is
its ability to incorporate nonholonomic constraints. E.g., it can include a
friction cone constraint to model the contact between the arm’s end-effector
and a surface, or the act of AM perching. On the other hand, if input
constraints are not incorporated in the trajectory planning process, they can
be added into the QP formulation at the cost of a lower exponential
convergence rate.

##  VII Simulation Result

We verify the tracking performance of the proposed control strategy in
simulation. The desired trajectory is designed to highlight the effects of CoM
shift: it consists of two piece-wise continuous and differentiable segments.
The first segment highlights the multi-rotor’s ability to follow a specified
path under the controller. The manipulator deploys during the second path
segment, thereby testing the system’s ability to track the desired path with
rapid change in overall CoM. In the simulation, an aerial manipulator with
2-link arm is modeled with the base vehicle mass being $2.7\,kg$, and the
manipulator masses of the first and second path segments is $0.5\,kg$ and
$1.0\,kg$, respectively, which mimics the delivery of a payload. The first and
second link manipulator lengths are set to be $0.25\,m$ and $0.2\,m$.
Moreover, the 2-link manipulator forms a planar manipulator with
$\mathcal{R}_{L_{2}}=\mathcal{R}_{L_{1}}=I_{3\times 3}$, i.e. the appended arm
is restricted to manipulate in $\hat{x}_{b}-\hat{z}_{b}$ plane. The control
frequency is set to be 1 kHz, and a zero-order-hold (ZOH) is used between each
controller update. In Fig. 2((a)), the controller provides promising tracking
performance despite an initial disturbance. A comparison of control effort
between the QP-based controller and flatness generated control action is given
in Fig. 2((b)). By deviating from the theoretical flatness control action as
needed, the CLF-QP controller can reject initial state error and exponentially
track the desired trajectory.

##  VIII Conclusion

In summary, the EoM of a general class of broken symmetry aerial manipulators
is derived using Lagrangian Reduction with advected parameters. Inspired by
the dynamical coupling in the reduced equations, we theorize and prove that
the outputs consisting of $\boldsymbol{p}_{e}$, overall CoM linear momentum,
$\psi$, yaw position angles, and $\boldsymbol{\eta}$, manipulator relative
joint angles, are differentially flat. The flat output parameterization of the
control inputs necessitated a second-order dynamic extension of thrust input
to allow a valid vector relative degree and dynamic feedback linearization.
Using this extension, we introduced a CLF-QP-based exponentially stabilizing
controller that guarantees local exponential tracking of the desired flat
outputs.

In future work, we plan to demonstrate the controller’s performance on a
hardware system, integrated with flatness-based trajectory planning
algorithms. Moreover, we seek to include the contact constraints that arise in
perching-like behavior and physical contact and design analogous controllers
that can accommodate such constraints.

## References

  * [1] X. Ding, P. Guo, K. Xu, and Y. Yu, “A review of aerial manipulation of small-scale rotorcraft unmanned robotic systems,” Chinese Journal of Aeronautics, vol. 32, no. 1, pp. 200–214, 2019.
  * [2] H. Sayyaadi and A. Soltani, “Modeling and control for cooperative transport of a slung fluid container using quadrotors,” Chinese Journal of Aeronautics, vol. 31, no. 2, pp. 262–272, 2018.
  * [3] S. Hamaza, I. Georgilas, G. Heredia, A. Ollero, and T. Richardson, “Design, modeling, and control of an aerial manipulator for placement and retrieval of sensors in the environment,” Journal of Field Robotics, vol. 37, no. 7, pp. 1224–1245, 2020.
  * [4] A. Jimenez-Cano, G. Heredia, and A. Ollero, “Aerial manipulator with a compliant arm for bridge inspection,” in 2017 Intern. Conf. on Unmanned Aircraft Systems, pp. 1217–1222, 2017.
  * [5] H.-N. Nguyen, C. Ha, and D. Lee, “Mechanics, control and internal dynamics of quadrotor tool operation,” Automatica, vol. 61, pp. 289–301, 2015.
  * [6] M. Fliess, J. Lévine, P. Martin, and P. Rouchon, “Differential flatness and defect: An overview,” Banach Center Pub.s, vol. 32, 01 1995.
  * [7] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in IEEE Int. Conf. Robotics and Automation, pp. 2520–2525, 2011.
  * [8] S. Formentin and M. Lovera, “Flatness-based control of a quadrotor helicopter via feedforward linearization,” in IEEE Conf. on Decision and Control and European Control Conf., pp. 6171–6176, 2011.
  * [9] J. Welde and V. Kumar, “Coordinate-free dynamics and differential flatness of a class of 6DOF aerial manipulators,” in 2020 IEEE Intern. Conf. on Robotics and Automation, pp. 4307–4313, 2020.
  * [10] J. Welde, J. Paulos, and V. Kumar, “Dynamically feasible task space planning for underactuated aerial manipulators,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3232–3239, 2021.
  * [11] B. Yüksel, N. Staub, and A. Franchi, “Aerial robots with rigid/elastic-joint arms: Single-joint controllability study and preliminary experiments,” in 2016 IEEE/RSJ Intern. Conf. on Intelligent Robots and Systems, pp. 1667–1672, 2016.
  * [12] B. Yüksel, G. Buondonno, and A. Franchi, “Differential flatness and control of protocentric aerial manipulators with any number of arms and mixed rigid-/elastic-joints,” in 2016 IEEE/RSJ Intern. Conf. on Intelligent Robots and Systems, pp. 561–566, 2016.
  * [13] K. Sreenath, T. Lee, and V. Kumar, “Geometric control and differential flatness of a quadrotor UAV with a cable-suspended load,” in 52nd IEEE Conf. on Decision and Control, pp. 2269–2274, 2013.
  * [14] S. X. Wei, M. R. Burkhardt, and J. Burdick, “Nonlinear controllability assessment of aerial manipulator systems using lagrangian reduction,” in 7th IFAC Workshop on Lagrangian and Hamiltonian Methods for Nonlinear Control, (Berlin, Germany), pp. 255–260, 2021.
  * [15] E. D. Sontag, “A ‘universal’ construction of artstein’s theorem on nonlinear stabilization,” Systems and Control Letters, vol. 13, no. 2, pp. 117–123, 1989.
  * [16] D. D. Holm, J. E. Marsden, and T. S. Ratiu, “The euler–poincaré equations and semidirect products with applications to continuum theories,” Advances in Mathematics, vol. 137, no. 1, pp. 1–81, 1998.
  * [17] J. P. Ostrowski, The mechanics and control of undulatory robotic locomotion.  PhD thesis, California Inst. of Tech., 1996.
  * [18] M. R. Burkhardt, Dynamic Modeling and Control of Spherical Robots.  PhD thesis, California Inst. of Tech., 2018.
  * [19] A. Bloch, P. Krishnaprasad, J. Marsden, and R. Murray, “Nonholonomic mechanical systems with symmetry,” Archive for Rational Mechanics and Analysis, vol. 136, pp. 21–99, 1996.
  * [20] R. M. Murray, S. S. Sastry, and L. Zexiang, A Mathematical Introduction to Robotic Manipulation.  CRC Press, Inc., 1st ed., 1994.
  * [21] J. Ostrowski, “Reduced equations for nonholonomic mechanical systems with dissipative forces,” Reports on Mathematical Physics, vol. 42, no. 1, pp. 185–209, 1998.
  * [22] P. Martin, R. Murray, and P. Rouchon, “Flat systems,” Plenary Lectures and Mini-Courses 4th European Control Conf., 01 1997.
  * [23] J. O. d. A. Limaverde Filho, T. S. Lourenço, E. Fortaleza, A. Murilo, and R. V. Lopes, “Trajectory tracking for a quadrotor system: A flatness-based nonlinear predictive control approach,” in 2016 IEEE Conf. on Control Applications, pp. 1380–1385, 2016.
  * [24] J. Hauser, S. Sastry, and G. Meyer, “Nonlinear control design for slightly non-minimum phase systems: Application to v/stol aircraft,” Automatica, vol. 28, no. 4, pp. 665–679, 1992.
  * [25] A. D. Ames, K. Galloway, K. Sreenath, and J. W. Grizzle, “Rapidly exponentially stabilizing control lyapunov functions and hybrid zero dynamics,” IEEE Trans. Automatic Control, vol. 59, no. 4, pp. 876–891, 2014.
  * [26] J. W. Grizzle, C. Chevallereau, R. W. Sinnet, and A. D. Ames, “Models, feedback control, and open problems of 3d bipedal robotic walking,” Automatica, vol. 50, no. 8, pp. 1955–1988, 2014.