# Aerial Manipulator Force Control Using Control Barrier Functions

Dimitris Chaikalis1, Vinicius Gonçalves2, Anthony Tzes2,3 and Farshad
Khorrami1,2 1New York University, Electrical & Computer Engineering, Brooklyn,
NY 11201, USA.2NYUAD Center for Artificial Intelligence and Robotics, UAE3New
York University Abu Dhabi (NYUAD), Electrical Engineering, Abu Dhabi 129188,
UAECorresponding author’s email<EMAIL_ADDRESS>
###### Abstract

This article studies the problem of applying normal forces on a surface, using
an underactuated aerial vehicle equipped with a dexterous robotic arm. A
force-motion high-level controller is designed based on a Lyapunov function
encompassing alignment and exerted force errors. This controller is coupled
with a Control Barrier Function constraint under an optimization scheme using
Quadratic Programming. This aims to enforce a prescribed relationship between
the approaching motion for the end-effector and its alignment with the
surface, thus ensuring safe operation. An adaptive low-level controller is
devised for the aerial vehicle, capable of tracking velocity commands
generated by the high-level controller. Simulations are presented to
demonstrate the force exertion stability and safety of the controller in cases
of large disturbances.

##  I Introduction

Aerial platforms capable of autonomous task execution, safe operation and
interaction with their environment, occasionally referred to as aerial workers
[1], promise to increase efficiency and safety in tasks such as inspection
[2], and transportation [3]. Typical multirotor vehicles equipped with planar
motors are extensively used due to their availability in various sizes and
development of high-fidelity sensors enabling operation in diverse indoor or
outdoor settings and being robust to surrounding environments

Underactuation due to the planar placement of rotors leads to a coupling of
the vehicle’s altitude to its attitude. A common approach to re-instating the
degrees of freedom lost due to underactuation, while also providing the
vehicle with a method for interacting with its environment, is the attachment
of robotic arms on the aerial vehicle. Such systems are sometimes referred to
as aerial manipulators [4].

Aerial manipulators are capable of interacting with other entities via
exchanging forces. Force control techniques can be used to solve a variety of
problems, such as multi-robot cooperation by regulating either physical [4] or
virtual forces [5] being exchanged. Likewise, the force control problem of
regulating force exchanged with the environment can be used to solve problems
such as automated push-pull and robotic grasping.

The problem of force exertion using fixed-base robotic manipulators has been
thoroughly investigated[6], and can be extended to aerial manipulators using
floating-base platforms [7].

Impedance control techniques control the interaction of aerial manipulators
with the environment [8] in standalone or coordinated cases [9, 10]. This area
has been addressed in space robotics while interacting with floating objects
[11].

Along these lines, this article studies the problem of exerting prescribed
normal forces on planar surfaces, with a multirotor aerial vehicle coupled
with a dexterous robotic arm. The problem of sustained force exertion has
applicability in tasks such as contact inspection, autonomous pushing, or
clearing an area of obstacles, and in space robotics [12, 13].

In [14], a system is presented for force exertion applicable only for
horizontal forces. The system consists of an aerial vehicle and a single DoF
tool, with an additional slider at the end-effector, aimed at allowing
friction-less sliding along one unconstrained axis of the surface.

An application on the peg-in-hole problem [15] is handled with a dual-arm
manipulator for increased manipulability and simple controllers. In [16] the
authors use a similar platform, with an eye-in-hand camera for achieving
contact on a surface enriched with fixed visual features, enabling the control
to take place in the image space. In [17] a fully-actuated vehicle is used,
allowing for increased mobility during interaction, while the distance between
end-effector and environment is used to aid the controller in achieving
compliance. The model predictive control architecture is employed in [18, 19]
for achieving force exertion with the environment, demanding accurate
knowledge of the underlying dynamics.

In this work, it is desired for the force exertion to be perpetrated on a
selected location of a surface and exclusively along its normal axis. With the
interest of ensuring safety during the interaction task, an optimization-based
controller is developed, incorporating control barrier functions (CBF) in its
design [20]. The driving controller utilises kinematics of the vehicle and the
robotic arm and outputs system-level velocity commands, allowing the developed
scheme to be applicable to any type of multirotor platform and robot
manipulator. The CBF is coupled with the driving controller via an
optimization problem (using Quadratic Programming (QP)), aiming to guarantee
that a desired relation between distance to the surface and end-effector
alignment is maintained at all times. This ensures that contact with the
environment will occur only under strict performance requirements, albeit in a
continuous control manner.

This paper is structured as follows. Section II presents the model of the
aerial manipulator and the environment, while also describing the task
definition. Section III outlines the overall proposed optimization-based
control design with the low-level controller for the aerial vehicle’s
stability in Section IV. Simulation studies are presented in Section V
validating the approach, while concluding remarks appear in Section VI.

##  II Problem Statement & System Model

The studied Unmanned Aerial Manipulator (UAM) system consists of an
underactuated multirotor aerial vehicle coupled with a robotic arm with
revolute joints, shown in Figure 1.

Figure 1: Example Unmanned Aerial Manipulator (UAM) system.

There exists a stationary planar surface, on which it is desired to exert
normal forces. The relevant frames for the studied problem are: $\mathcal{U}$
the frame fixed on the center of the aerial vehicle, $\mathcal{E}$ the frame
of the force sensor attached on the robotic arm end-effector, $\mathcal{W}$
the world-fixed inertial frame and $\mathcal{P}$ the inertial frame attached
on the wall. It is assumed that the $\widecheck{z}$ vector of frame
$\mathcal{P}$ is normal to the wall, directed outside the wall with its center
on the surface of the wall. Furthermore, it is assumed that the end-effector
tool that will perform the force-exertion is aligned with the $\widecheck{z}$
vector of the frame $\mathcal{E}$ (see Figure 2).

Figure 2: Coordinate frames of a UAM system

The position ${p_{\mathcal{U}}^{\mathcal{W}}}\in\mathbb{R}^{3}$ of the center
of the aerial vehicle is measured in $\mathcal{W}$, while the attitude of the
vehicle frame $\mathcal{U}$ in $\mathcal{W}$ is dictated by the roll $\phi$,
pitch $\theta$ and yaw $\psi$ angles, or by the rotation matrix
${R_{\mathcal{U}}^{\mathcal{W}}}\triangleq
R_{z}(\psi)R_{y}(\theta)R_{x}(\phi)$. The configuration of the robotic arm is
represented by the vector of joint angles ${q_{m}}\in\mathbb{R}^{n}$, for a
$n$-joint robot. All vectors in this paper are column vectors. Assuming known
fixed transformation
$\left({p_{\mathcal{P}}^{\mathcal{W}}},{R_{\mathcal{P}}^{\mathcal{W}}}\right)$
between $\mathcal{W}$ and $\mathcal{P}$, the complete pose of the end-effector
can be computed relative to the wall frame $\mathcal{P}$.

The goal is for the end-effector position and orientation to appropriately
align with $\mathcal{P}$ and subsequently exert forces along the
$\widecheck{z}$ axis of $\mathcal{P}$. This force should be exerted at the
center of $\mathcal{P}$. Let the position of the end-effector along the
$\widecheck{z}$ axis of the wall frame be $Z$ and $F_{d}<0$ be the desired
force. The following reaction force model, as a function of the insertion into
the wall, $Z$, is assumed 111$Z<0$ implies an insertion of the tool into the
wall, whereas $Z>0$ means that no contact between the wall and the tool was
established.: a function $F:\mathbb{R}\rightarrow\mathbb{R}$ that is
continuous, non-decreasing and such that there exist a unique $Z_{d}<0$ such
that $F(Z_{d})=F_{d}$. A typical example satisfying the above assumptions is
the spring-action force model:

$F(Z)=\left\\{\begin{array}[]{lll}\kappa Z&\mbox{if }Z\leq 0\\\ 0&\mbox{if
}Z>0&\end{array}\right..$

in which $Z_{d}=F_{d}/\kappa$. The joint motor controllers accept joint
velocity references, while the aerial vehicle flight controller handles the
low-level attitude control, allowing attitude and thrust setpoints to be
transmitted to the autopilot. Autopilots typically accept as input the
normalized thrust $\frac{T_{f}}{T_{m}}$, where $T_{m}$ is the maximum value of
$T_{f}$. Since the thrust relies on the employed brushless motor, propeller
and battery characteristics, it can be introduced in the vehicle translational
dynamics as an additional unknown constant.

The aerial manipulator’s dynamic model is:

${\frac{d}{dt}v_{\mathcal{U}}^{\mathcal{W}}}=\frac{1}{c_{m}}\left({R_{\mathcal{U}}^{\mathcal{W}}}{\widecheck{e}_{z}}T_{f}+{F_{e}}\right)-{g}\
,\ {\frac{d}{dt}q_{m}}={u_{m}},$ (1)

where
${v_{\mathcal{U}}^{\mathcal{W}}}\triangleq{\frac{d}{dt}\left(p_{\mathcal{U}}^{\mathcal{W}}\right)}$,
and ${F_{e}}$ is the external force vector attributed to forces exchanged with
the environment and disturbances from the robotic arm. It is assumed to be
upper bounded in norm by a value $\Delta$ and also to be slow-varying
${\dot{F}_{e}}\simeq 0$ (as in [21]). Furthermore, ${g}\triangleq
9.81{\widecheck{e}_{z}}$ is the gravity acceleration vector, $q_{m}$ the
manipulator joint position vector and ${u_{m}}$ is the robotic arm joint
velocity vector, while
${\widecheck{e}_{x}},{\widecheck{e}_{y}},{\widecheck{e}_{z}}$ are the columns
of the $3\times 3$ identity matrix respectively.

In (1), the unknown constant $c_{m}=\frac{m}{T_{m}}$ is related to the overall
system mass $m$, while the rotation matrix ${R_{\mathcal{U}}^{\mathcal{W}}}$
can be expressed in its corresponding Euler angles. Let the control input
vector be

${\tau}={[T_{f}\ \ \phi_{d}\ \ \theta_{d}\ \ \omega_{z}\ \
u_{m}^{\top}]^{\top}},$ (2)

where $\omega_{z}$ is the angular velocity about the vehicle’s
${\widecheck{z}}$-axis. The controller shoud be designed designed so as to
arrive at final control values ${\tau}$, while exerting a force at a selected
wall-location. The controller framework is split into: a) the _high level
controller_ that generates the (i) UAM’s linear velocity, (ii) yaw rate and
(iii) the manipulator joint velocity. The latter two are sent directly as low-
level commands, but the linear velocity is sent to: b) a _low level
controller_ that generates the appropriate remaining low level control
signals.

##  III High level controller

###  III-A Basic definitions

In order to ensure safety during task execution, it is desired for the aerial
manipulator to achieve proper alignment prior to attempting force interaction,
thus avoiding undesirable collisions of parts of the system.

For this reason, a controller is sought, capable of guaranteeing end-effector
alignment and force convergence. This controller is then combined with an
appropriate control barrier function (CBF) under an optimization scheme, in
order to ensure that a desirable relation between end-effector distance from
the wall and end-effector alignment is maintained at all times. The devised
controller outputs linear velocity and yaw angular rate commands for the
aerial vehicle and joint angle velocity commands for the robotic arm, which
are then transmitted to the lower-level controllers.

For the sake of simplicity, the variables in this section are expressed in the
wall frame. Thus, let $x,y,z$ the vehicle positions in $\mathcal{P}$ (center
of $\mathcal{U}$ measured in $\mathcal{P}$). As far as the high-level
controller is concerned, the following _configuration vector_ is defined

$q\triangleq[x\ \ y\ \ z\ \ \psi\ \ q_{m}^{\top}]^{\top}.$ (3)

The output command of the controller will be $\dot{q}$ and the high-level
controller assumes the dynamics $\dot{q}=u$, i.e..It is also assumed
implicitly that the UAM’s roll($\phi$) and pitch($\theta$) rates are zero, as
far as the high-level controller is concerned.

Let $X(q),Y(q),Z(q)$ be the coordinates of the position of the end-effector
frame and $\widecheck{x}(q),\widecheck{y}(q),\widecheck{z}(q)$ the orthonormal
vectors of the orientation of the end-effector frame. These elements form the
end-effector frame, measured in wall frame, and can be obtained using forward
kinematics. Using forward differential kinematics, the linear and angular
velocity geometric Jacobians $J_{v}(q),J_{\omega}(q)$ are also defined for the
end-effector frame measured in the wall frame.

According to Section II, the goal is to control the UAM to have $X(q)=Y(q)=0$,
$\widecheck{z}(q)=-\widecheck{e}_{z}$ and to exert a force $F(Z(q))=F_{d}$.
The task can be separated into: a) alignment and b) force exertion, which are
to be implemented using scalar potential functions.

###  III-B The alignment potential function

Let the positive-definite alignment function be

$A(q)\triangleq\frac{k_{x}}{h}\lvert X(q)\lvert^{h}+\frac{k_{y}}{h}\lvert
Y(q)\lvert^{h}+\frac{k_{o}}{g}\lvert
1+{\widecheck{e}_{z}}^{\top}\widecheck{z}(q)\lvert^{g},$

for constants $h>1,g>1,k_{x},k_{y},k_{o}>0$. Note that $A(q)=0$ if and only if
$X(q)=Y(q)=0$ and $\widecheck{z}(q)=-\widecheck{e}_{z}$. Define the symbol
$[x]^{s}=\mbox{sign}(x)\lvert x\lvert^{s}$. The gradient of $A$ in $q$ will be
necessary. It is given by

$\displaystyle\nabla A(q)=\ $ $\displaystyle
k_{x}[X(q)]^{h-1}J_{v}(q)^{\top}{\widecheck{e}_{x}}+k_{y}[Y(q)]^{h-1}J_{v}(q)^{\top}{\widecheck{e}_{y}}+$
$\displaystyle
k_{o}[1+{\widecheck{e}_{z}}^{\top}\widecheck{z}(q)]^{g-1}J_{\omega}(q)^{\top}\left({\widecheck{e}_{z}}\times\widecheck{z}(q)\right).$

###  III-C Force exertion potential function

Consider a continuous and non-decreasing function
$\gamma:\mathbb{R}\rightarrow\mathbb{R}$ with $\gamma(s)=0$ if and only if
$s=0$. Let:

$U(q)\ {\triangleq}\ \int_{Z_{d}}^{Z(q)}\gamma\Big{(}F(\xi)-F_{d}\Big{)}d\xi.$

It can be shown that $U(q)\geq 0$ and zero if and only if $q$ is such that
$Z(q)=Z_{d}$. Furthermore, $U(q)$ is a differentiable function on $q$, with
$\nabla
U(q)=\gamma\Big{(}F(Z(q))-F_{d}\Big{)}J_{v}(q)^{\top}{\widecheck{e}_{z}}.$

###  III-D The task Lyapunov function

Let the _high-level Lyapunov function_ can be defined be

$V_{H}(q)\triangleq A(q)+U(q).$

Clearly, $V_{H}(q)$ is differentiable and $V_{H}(q)=0$ if and only if
$X(q)=Y(q)=0$, $\widecheck{z}(q)=-{\widecheck{e}_{z}}$ and $F=F_{d}$ (i.e.,
$Z(q)=Z_{d}$).

###  III-E The distance-alignment error diagram

Given the Lyapunov function, one could consider a controller $u=-K_{q}\nabla
V_{H}(q)$ for a positive scalar $K_{q}$. However, this can be unsafe, because
this controller does not consider that the UAM’s manipulator can collide with
the wall. To solve this issue, a _distance-alignment error_ relationship will
be enforced. Essentially, the distance between the end-effector and the wall
$D(q)$ and the alignment error function $A(q)$ should follow a relationship in
which there is a minimal distance $D$ for each alignment error $A$, and thus
the approach will happen in a safe way. Geometrically, this can be visualized
by plotting $A$ and $D$ in a diagram, as shown in Fig. 3 and enforcing the
points along the trajectory to be above a certain curve that contains the
point $A=0,D=0$.

Figure 3: Alignment error (A) and distance (D)-diagram versus the shaping
function $\Omega(s)=\sqrt{s}$.

For this paper, the end-effector wall-distance function is considered to be
simply the distance from the end-effector point to the wall, $D(q)\triangleq
Z(q)$. More complex distance functions, taking into the consideration the
geometry of the robot, can be used if necessary. Regardless, consider a
function $\Omega$ that is differentiable, increasing and such that
$\Omega(0)=0$. This function will shape the distance-alignment relationship.
To enforce this relationship, the _barrier distance-alignment relation
function_ $B(q)\triangleq D(q)-\Omega(A(q))$ is defined, so the desired
relationship can be written as $B(q)\geq 0$. Since that $B$ is differentiable
in $q$, this inequality is suitable to be enforced through CBFs with a
differential inequality of the form $\nabla B(q)^{\top}\dot{q}\geq-K_{B}B(q)$
for a positive $K_{B}$.

###  III-F Joint and configuration velocities limits

Limits for $\dot{q}$ and also joint limits for the manipulator will also be
enforced. Consider joint limits for the manipulator $\underline{q}_{m}\leq
q_{m}\leq\overline{q}_{m}$ and configuration velocity limits
$\underline{u}\leq\dot{q}\leq\overline{u}$. The joint limits can be enforced
through CBFs, and the corresponding differential inequalities can be merged
into the configuration velocity limits. Split $\underline{u}$ and
$\overline{u}$ in its first four entries $\underline{u}_{D}$,
$\overline{u}_{D}$ and remaining entries $\underline{u}_{m}$,
$\overline{u}_{m}$. Define

$\displaystyle\underline{b}(q)\triangleq[\underline{u}_{D}^{\top}\ \
\max(\underline{u}_{m},K_{L}(\underline{q}_{m}-q_{m}))^{\top}]^{\top}$
$\displaystyle\overline{b}(q)\triangleq[\overline{u}_{D}^{\top}\ \
\min(\overline{u}_{m},K_{L}(q_{m}-\overline{q}_{m}))^{\top}]^{\top}$ (4)

in which $K_{L}$ is a positive scalar and the maximum and minimum are taken
component-wise. Then $\underline{b}(q)\leq\dot{q}\leq\overline{b}(q)$
implements the desired constraint.

###  III-G The optimization problem

The QP-controller can be computed for the high-level input $u_{H}$. Define the
steering controller $u_{d}(q)\triangleq-K_{q}\nabla V_{H}(q)$. The control
input $u_{H}$ is:

$\displaystyle u_{H}(q)$ $\displaystyle\triangleq$
$\displaystyle\mbox{arg}\min_{\mu}\ \ \ \lvert\lvert\mu-
u_{d}(q)\lvert\lvert^{2}~{}\mbox{such that}$ (5) $\displaystyle\nabla
B(q)^{\top}\mu\geq-K_{B}B(q)$
$\displaystyle\underline{b}(q)\leq\mu\leq\overline{b}(q).$

Note that $u_{H}$ is indeed a function only of $q$, since the reaction force
is modelled to be configuration-dependent through the function $F(Z(q))$.
However, it should be noted that _it is not necessary_ to know the force-
reaction model function $F$ to compute this controller. Indeed, the force
model only appears through the quantity $F(Z(q))$, that is part of the
steering controller. This quantity can be obtained without any information
about the force model provided that there is a force sensor that measures it.
The mention to the force model is only necessary for the sake of the
mathematical analysis.

The following results can then be shown about this controller:

###### Proposition 1

The set

$\mathbb{P}\triangleq\left\\{q\ \lvert\ B(q)>0\ ,\
\underline{q}_{m}<q_{m}<\overline{q}_{m}\right\\}$ (6)

is positive invariant.

###### Proof:

The CBF constraint in (4) guarantees strict bounds
$\underline{q}_{m},\overline{q}_{m}$ for $q_{m}$, while the first constraint
of (5) guarantees $\dot{B}(q)$ is non-decreasing as $B(q)\rightarrow 0$, thus
ensuring positive invariance of $\mathbb{P}$. ∎

###### Proposition 2

If the initial configuration $q_{0}\in\mathbb{P}$, then the optimization
problem is always feasible. In particular, $\mu=0$ is always a strictly
feasible solution.

###### Proof:

Application of $\mu=0$ in (5), results in

$-K_{B}B(q)<0,~{}~{}~{}\underline{b}(q)<0<\overline{b}(q)$

Both of the above inequalities are already guaranteed from the positive
invariance of $\mathbb{P}$, thus $\mu=0$ is a strictly feasible solution,
because the $\geq$ conditions are met with $>$. ∎

###### Proposition 3

The solution to the optimization problem is unique.

###### Proof:

From convex function theory, a strictly convex function minimized over a
convex set, has a unique optimal solution (if any). In (5), the constraint set
is comprised of linear inequalities and is thus convex. The quadratic
optimization problem objective function of (5) can be written in the form

$\min_{\mu}~{}~{}\frac{1}{2}\mu^{T}H\mu+f^{T}\mu,$

with $H=2I$, where $I$ the identity matrix and $f=-2u_{d}(q)$. Since $H$ is
positive definite, the objective function is strictly convex. ∎

###### Proposition 4

For $q\in\mathbb{P}$, $u_{H}(q)$ is a continuous function of $q$.

###### Proof:

The constraint functions are continuous in $q$. Objective function matrices
$H,f$ are also continuous in $q$. From [22], it can be shown that the above
conditions, along with $\mu=0$ being a strictly feasible solution (from
Proposition 2), are sufficient to prove continuity of the optimal solution
$u_{H}(q)$ in $q$. ∎

###### Proposition 5

If the initial configuration $q_{0}\in\mathbb{P}$, then for all $t\geq 0$ the
closed loop system using (5), $\dot{q}=u_{H}(q)$, is Lyapunov stable to the
set $\mathbb{V}_{H}\triangleq\left\\{q\ \lvert\ V_{H}(q)=0\right\\}$, i.e.,
$\dot{V}_{H}=\nabla V_{H}(q(t))^{\top}u_{H}(q(t))\leq 0$ for all $t\geq 0$.

###### Proof:

Let $G(\mu,q)$ be the objective function of (5), then by definition, the
solution $\mu=u_{H}(q)$ is the unique minimizer of $G$, or $G(u_{H}(q),q)\leq
G(\mu,q)$ for any feasible solution $\mu$.

Given $\mu=0$ is a feasible solution, from Proposition 2, $G(u_{H}(q),q)\leq
G(0,q)$ or

$2K_{q}\nabla V_{H}(q)^{T}u_{H}(q)\leq-\lvert\lvert
u_{H}(q)\lvert\lvert^{2}\leq 0,$

which from the definition of $\dot{V}_{H}(q)$ results in

$\dot{V}_{H}(q)\leq 0.$

∎

##  IV Low level controller

The high level controller provides the low-level variables $\omega_{z}$ and
$u_{m}$ directly from $u_{H}(q)$. More precisely, $\omega_{z}$ comes from the
$\psi$ component of $u_{H}(q)$ ($4_{th}$ element), and $u_{m}$ from the
$q_{m}$ component of $u_{H}(q)$ (last $n$ elements). The linear velocity
component of $u_{H}(q)$ (the first three components), however, needs to be
converted into low-level variables, which is the role of the _low level
controller_. The aerial vehicle controller module needs to guarantee
convergence to the desired linear velocities commanded by the CBF force
controller, despite effects due to forces exchanged with the environment, as
well as uncertainty in assigning vehicle thrust, as explained in Section II.

Clearly, the UAV velocity commands in $u_{H}$ generated by the high-level
controller in the wall frame can be directly transformed to desired velocities
in the world frame, henceforth denoted by ${v_{\mathcal{U},d}^{\mathcal{W}}}$.
A control law to track this linear velocity will be shown, and its Lyapunov
stability will be proved. For this purpose, some definitions are necessary.
Define the velocity error, $e_{v}\triangleq
v_{\mathcal{U}}^{\mathcal{W}}-v_{\mathcal{U},d}^{\mathcal{W}}$. Let the
variables ${\widehat{F}_{e}},\widehat{c}_{m}$ evolve according to the
following dynamics

$\displaystyle{\dot{\widehat{F}}_{e}}=\sigma_{p}{e_{v}}-\sigma_{r}\sigma_{p}{\widehat{F}_{e}},$
(7)
$\displaystyle\dot{\widehat{c}}_{m}=\sigma_{m}~{}{e_{v}}^{\top}\left(K_{v}{e_{v}}-{g}\right)$
(8)

with arbitrary initial conditions, in which $\gamma,\sigma_{p}>0$ and
$\sigma_{r}\geq 0$. Furthermore, define

${F_{f}}\triangleq\widehat{c}_{m}\left({g}-K_{v}{e_{v}}\right)-{\widehat{F}_{e}},$
(9)

for a positive definite matrix $K_{v}\in\mathbb{R}^{3\times 3}$. Finally,
define ${\widecheck{F}_{R}}\triangleq
R_{z}(\psi)^{\top}\frac{{F_{f}}}{\lvert\lvert{F_{f}}\lvert\lvert}$.

###### Proposition 6

If the control inputs $T_{f}$, $\phi_{d}$ and $\theta_{d}$ are:

$\displaystyle
T_{f}=\lvert\lvert{F_{f}}\lvert\lvert,\phi_{d}=\text{asin}\left({\widecheck{e}_{y}}^{\top}{\widecheck{F}_{R}}\right),\theta_{d}=\text{atan2}\left({\widecheck{e}_{x}}^{\top}{\widecheck{F}_{R}},{\widecheck{e}_{z}}^{\top}{\widecheck{F}_{R}}\right)$
(10)

and assuming slow external disturbances ${\dot{F}_{e}}\simeq 0$ (as in [21])
then $e_{v}$ and $\widetilde{F}_{e}$ remain ultimately uniformly bounded.

###### Proof:

Let $\widetilde{c}_{m}\ {\triangleq}\
c_{m}-\widehat{c}_{m},{\widetilde{F}_{e}}\ {\triangleq}\
{F_{e}}-{\widehat{F}_{e}}$, where $c_{m}>0$ and constant by definition. Define
the low-level Lyapunov function
$V_{L}\triangleq\frac{1}{2}{e_{v}}^{\top}{e_{v}}+\frac{1}{2\sigma_{m}c_{m}}\widetilde{c}_{m}^{2}+\frac{1}{2c_{m}\sigma_{p}}{\widetilde{F}_{e}}^{\top}{\widetilde{F}_{e}}$.

Note that with the choices in (10), the term
$R_{\mathcal{U}}^{\mathcal{W}}\widecheck{e}_{z}T_{f}$ in (1) becomes $F_{f}$.
Using (1) and (9) the Lyapunov function derivative becomes

$\displaystyle\dot{{V_{L}}}=$
$\displaystyle-{e_{v}}^{\top}K_{v}{e_{v}}+\frac{\widetilde{c}_{m}}{c_{m}}\left({e_{v}}^{\top}K_{v}{e_{v}}-{e_{v}}^{\top}{g}-\frac{\dot{\widehat{c}}_{m}}{\sigma_{m}}\right)+$
$\displaystyle\frac{1}{c_{m}}\left({e_{v}}-\frac{{\dot{\widehat{F}}_{e}}}{\sigma_{p}}\right)^{\top}{\widetilde{F}_{e}},$
(11)

where $\dot{c}_{m}=0$, and ${\dot{F}_{e}}\simeq 0$ was used. The substitution
of the adaptation evolution laws (7), (8) in (11) results in

$\dot{{V_{L}}}=-{e_{v}}^{\top}K_{v}{e_{v}}-\frac{\sigma_{r}}{2c_{m}}\left(\lvert\lvert{\widetilde{F}_{e}}\lvert\lvert^{2}+\lvert\lvert{\widehat{F}_{e}}\lvert\lvert^{2}-\lvert\lvert{F_{e}}\lvert\lvert^{2}\right).$
(12)

If the gain $\sigma_{r}=0$ then $\dot{V_{L}}<0$ with respect to state error
variables, proving convergence of $e_{v}\rightarrow 0$. However, in the
presence of unmodelled excitations [23], such adaptive controllers are prone
to parameter drift, which can cause the estimates to grow unbounded despite
guarantees for the tracking of error variables.

The $\sigma$-modification rule (i.e., $\sigma_{r}>0$) is introduced to
alleviate this instability in adaptations[24] by essentially regulating
$\widehat{F}_{e}$ towards $0$ as the error $e_{v}$ converges to $0$. Looking
at (12), with $\Delta$ upper bound of the bounded force ${\lvert\lvert
F_{e}\lvert\lvert}$ as stated in Section II then

$\dot{{V_{L}}}\leq-\lambda_{\text{min}}(K_{v})\lvert\lvert{e_{v}}\lvert\lvert^{2}-\frac{\sigma_{r}}{2c_{m}}\lvert\lvert{\widetilde{F}_{e}}\lvert\lvert^{2}+{\frac{\sigma_{r}}{2c_{m}}\Delta^{2}},$
(13)

in which $\lambda_{\text{min}}(K_{v})$ is the smallest eigenvalue of $K_{v}$.
Thus, the error signal $e_{v}$ and estimation error $\widetilde{F}_{e}$ are
uniformly ultimately bounded in the set

$\mathbb{O}\triangleq\left\\{e_{v},\ {\widetilde{F}_{e}}\ \Big{|}\
\lvert\lvert{e_{v}}\lvert\lvert\leq\sqrt{\frac{\sigma_{r}}{2c_{m}\lambda_{\text{min}}(K_{v})}}\Delta,\
\lvert\lvert{\widetilde{F}_{e}}\lvert\lvert\leq{\Delta}\right\\}.$ (14)

∎

Regarding initial adaptation estimates, it is straightforward for
$\widehat{F}(0)=0$ since no information about external disturbances exists at
initialization. The selection of $\widehat{c}_{m}(0)$ is more nuanced.

Since $c_{m}$ represents the thrust-to-weight ratio of the aerial vehicle
(albeit in $Kg/N$), a large value of $\widehat{c}_{m}$ signifies an initial
assumption that the vehicle has a small thrust-to-weight capacity and thus the
controller will start with large thrust commands. In the case where the
assumption is wrong, this behavior could result in dangerously large
accelerations during take-off.

Conversely, a small value of $\widehat{c}_{m}$ assumes a large thrust-to-
weight ratio, meaning initial thrust commands will be small. If the assumption
is wrong, in this case the vehicle will simply slowly increase the thrust
level as $\widehat{c}_{m}$ builds up and take off when the estimate becomes
close enough to the real ratio. Thus, the authors suggest selecting a value of
$\widehat{c}_{m}(0)\approx 0.02$ for any size of UAV, as it corresponds to
assumed hovering thrust at just $20\%$ of capacity, which is a significantly
better ratio than any commercial type of UAV.

The overall control architecture can be found in Figure 4.

Figure 4: Overall UAM-control block diagram.

##  V Simulation Studies

The Gazebo physics engine was employed for simulating the system along with
the proposed controllers under ROS Noetic. The DJI Matrice 100 quadcopter was
simulated, with the ArduCopter autopilot running in a Software-In-The-Loop
configuration tasked with handling the low-level attitude control of the
vehicle. A $2$-DoF robotic arm is attached underneath the aerial vehicle. A
screenshot from the simulator can be seen in Figure 1.

The total mass of the system is $c_{m}=2.5$ Kg. The two links of the arm have
lengths $l_{1}=0.27$ m and $l_{2}=0.87$ m respectively, while the first joint
is at $0.13$ m below the center of the vehicle. The CBF gains are selected as
$k_{x,y}=2,k_{o}=1,h=g=1.5,K_{q}=0.3$, and

Robot joint limits are $\pm 105^{o}$ for each joint, with $K_{L}=0.35$.
Configuration velocity limits:
$\overline{u}=-\underline{u}=[0.45,0.45,0.35,0.35,0.35,0.35]^{\top}$. The
force control function used is $\gamma(F)=0.5[F-F_{d}]^{0.5}$, where $F$ the
force measured at the sensor and the alignment-barrier function
$\Omega(s)=\frac{4}{\pi}\mbox{atan}(1.5\sqrt{s})$. The UAV controller gains
used were
$K_{v}=\mbox{diag}(0.4,0.4,0.6),\sigma_{m}=0.005,\sigma_{p}=0.01,\sigma_{r}=10^{-3}$,
with initial adaptation estimates $\widehat{F}_{e}=0_{3\times
1},\widehat{c}_{m}=0.02$. The employed units comply with SI (meters for
lengths, Newtons for forces, seconds for time and rad for angles).

###  V-A Force Exertion Scenario

For the first simulation, the performance of the devised controller is
validated while exerting force on a planar surface of arbitrary placement.
Specifically, the orientation of axis $\widecheck{z}$ of $\mathcal{P}$ is
$[-0.664,-0.664,-0.342]^{\top}$ when expressed in $\mathcal{W}$. The goal is
to exert $F_{d}=-3$N of normal force.

Figure 5: Force measurement and CBF barrier constraint.

As can be seen in Figure 5 (left), the controller manages to achieve the
desired force exertion. Figure 5 (right) depicts the evolution of end-effector
distance from the surface $(D(q))$ over the barrier-alignment function
$(\Omega(A(q)))$. Clearly the CBF constraint manages to enforce the desired
relationship between the two values (plotted in blue), ensuring safe force
exertion.

In Figure 6 the velocity commands for the aerial vehicle generated by the
optimization-based controller are plotted alongside their measured
counterparts, to visualize the performance of the low-level controller of the
UAV.

Figure 6: Commanded and measured vehicle velocities during force exertion.

###  V-B Evaluation of Performance under Disturbances

In the next scenario, an external disturbance affects the yaw of the vehicle
while exerting force, causing it to slip along the wall and lose contact and
alignment with the environment. This is an important test in order to evaluate
the capability of the designed CBF controller in overcoming such disturbances
and maintaining safety during task execution.

####  V-B1 CBF Disabled

For the first simulation, the CBF constraint in the optimization function is
disabled. Without this safety-oriented action, the force controller attempts
to simultaneously exert forces and correct the end-effector alignment whilst
in contact with the wall, resulting in undesirable interaction with the
environment and eventual loss of stability.

The force measured at the end-effector is plotted in the top of Figure 7,
clearly showing failure in maintaining desired force exertion, while the
bottom shows the evolution of $D(q),\Omega(A(q))$, with a rather large
alignment error.

Figure 7: Force, distance and alignment without the CBF constraint.

####  V-B2 CBF Enabled

In contrast, Figure 8 shows that the proposed CBF controller manages to
maintain a distance to the wall while recovering the end-effector’s alignment,
allowing for the force exertion to quickly resume safely.

Figure 8: Forces, distance and alignment with the CBF constraint.

Figure 9 shows the evolution of distance over alignment for the same scenario.
The disturbance clearly causes the barrier function to be violated and it can
be seen how the controller drives the system back to the desired curve.

Figure 9: Evolution of distance over alignment barrier

##  VI Conclusions

This paper presents a control architecture for force exertion on planar
surfaces, with a platform consisting of an underactuated multirotor aerial
vehicle and an attached robotic arm. An optimization-based control scheme is
designed, combining force measurements and a Lyapunov controller with control
barrier functions, ensuring the system is capable of achieving force exertion
in a prescribed, safety-oriented manner. An adaptive controller is devised for
the velocity control of the aerial vehicle, guaranteeing bounded-error command
tracking. The performance of the controller is validated via simulation
studies.

## References

  * [1] T. Bartelds, A. Capra, S. Hamaza, S. Stramigioli, and M. Fumagalli, “Compliant aerial manipulators: Toward a new generation of aerial robotic workers,” _IEEE Robotics and Automation Letters_ , vol. 1, no. 1, pp. 477–483, 2016.
  * [2] A. Jimenez-Cano, J. Braga, G. Heredia, and A. Ollero, “Aerial manipulator for structure inspection by contact from the underside,” in _2015 IEEE/RSJ international conference on intelligent robots and systems (IROS)_. IEEE, 2015, pp. 1879–1884.
  * [3] N. Michael, J. Fink, and V. Kumar, “Cooperative manipulation and transportation with aerial robots,” _Autonomous Robots_ , vol. 30, pp. 73–86, 2011.
  * [4] D. Chaikalis, F. Khorrami, and A. Tzes, “Adaptive control approaches for an unmanned aerial manipulation system,” in _2020 International Conference on Unmanned Aircraft Systems (ICUAS)_. IEEE, 2020, pp. 498–503.
  * [5] N. Evangeliou, D. Chaikalis, A. Tsoukalas, and A. Tzes, “Visual collaboration leader-follower uav-formation for indoor exploration,” _Frontiers in Robotics and AI_ , vol. 8, p. 777535, 2022.
  * [6] S. Jung, T. Hsia, and R. Bonitz, “Force tracking impedance control of robot manipulators under unknown environment,” _IEEE Transactions on Control Systems Technology_ , vol. 12, no. 3, pp. 474–483, 2004.
  * [7] K. Gkountas, D. Chaikalis, and A. Tzes, “Force control design for a robot manipulator attached to a UAV,” _IFAC-PapersOnLine_ , vol. 51, no. 30, pp. 548–553, 2018.
  * [8] F. Forte, R. Naldi, A. Macchelli, and L. Marconi, “Impedance control of an aerial manipulator,” in _2012 American Control Conference (ACC)_ , 2012, pp. 3839–3844.
  * [9] G. Muscio, F. Pierri, M. A. Trujillo, E. Cataldi, G. Antonelli, F. Caccavale, A. Viguria, S. Chiaverini, and A. Ollero, “Coordinated control of aerial robotic manipulators: Theory and experiments,” _IEEE Transactions on Control Systems Technology_ , vol. 26, no. 4, pp. 1406–1413, 2017.
  * [10] D. Chaikalis, N. Evangeliou, A. Tzes, and F. Khorrami, “Decentralized leader-follower visual cooperative package transportation using unmanned aerial manipulators,” in _2023 European Control Conference (ECC)_ , 2023, pp. 1–5.
  * [11] S. Sharma, M. Suomalainen, and V. Kyrki, “Compliant manipulation of free-floating objects,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_ , 2018, pp. 865–872.
  * [12] D. Smrcka, T. Baca, T. Nascimento, and M. Saska, “Admittance force-based UAV-wall stabilization and press exertion for documentation and inspection of historical buildings,” in _2021 International Conference on Unmanned Aircraft Systems (ICUAS)_. IEEE, 2021, pp. 552–559.
  * [13] E. Papadopoulos, F. Aghili, O. Ma, and R. Lampariello, “Robotic manipulation and capture in space: A survey,” _Frontiers in Robotics and AI_ , p. 228, 2021.
  * [14] X. Meng, Y. He, and J. Han, “Hybrid force/motion control and implementation of an aerial manipulator towards sustained contact operations,” in _2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2019, pp. 3678–3683.
  * [15] M. Car, A. Ivanovic, M. Orsag, and S. Bogdan, “Impedance based force control for aerial robot peg-in-hole insertion tasks,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2018, pp. 6734–6739.
  * [16] M. Xu, A. Hu, and H. Wang, “Image-based visual impedance force control for contact aerial manipulation,” _IEEE Transactions on Automation Science and Engineering_ , vol. 20, no. 1, pp. 518–527, 2023.
  * [17] K. Bodie, M. Brunner, M. Pantic, S. Walser, P. Pfändler, U. Angst, R. Siegwart, and J. Nieto, “Active interaction force control for contact-based inspection with a fully actuated aerial vehicle,” _IEEE Transactions on Robotics_ , vol. 37, no. 3, pp. 709–722, 2021.
  * [18] L. Peric, M. Brunner, K. Bodie, M. Tognon, and R. Siegwart, “Direct Force and Pose NMPC with Multiple Interaction Modes for Aerial Push-and-Slide Operations,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_ , 2021, pp. 131–137.
  * [19] G. Nava, Q. Sablé, M. Tognon, D. Pucci, and A. Franchi, “Direct force feedback control and online multi-task optimization for aerial manipulators,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 2, pp. 331–338, 2020.
  * [20] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and P. Tabuada, “Control barrier functions: Theory and applications,” in _2019 18th European Control Conference_ , 2019, pp. 3420–3431.
  * [21] S. Kim, H. Seo, S. Choi, and H. J. Kim, “Vision-guided aerial manipulation using a multirotor with a robotic arm,” _IEEE/ASME Transactions On Mechatronics_ , vol. 21, no. 4, pp. 1912–1923, 2016.
  * [22] B. Morris, M. J. Powell, and A. D. Ames, “Sufficient conditions for the lipschitz continuity of qp-based multi-objective control of humanoid robots,” in _52nd IEEE Conference on Decision and Control_ , 2013, pp. 2920–2926.
  * [23] P. A. Ioannou and P. V. Kokotovic, “Instability analysis and improvement of robustness of adaptive control,” _Automatica_ , vol. 20, no. 5, pp. 583–594, 1984.
  * [24] E. Lavretsky and K. A. Wise, “Robust adaptive control,” in _Robust and adaptive control: With aerospace applications_. Springer, 2012, pp. 317–353.