\section{INTRODUCTION}

\par Aerial Manipulators (AMs) have gained much attention in recent years \cite{AM literature review}. The Unmanned Aerial Vehicle (UAV) acts as a floating base for the manipulator enabling it to conduct active operations in the 3D space, see Fig. \ref{fig1}. This combination of a UAV and a manipulator provides the system with enough capabilities to perform a  range of complex operations where human access is limited (e.g., in disaster-affected zones). The other industrial and commercial applications of AMs are in the maintenance of power grids, the inspection of bridges, and canopy sampling \cite{AM literature review}.

Such applications involve trajectory tracking maneuvers by the AM near static objects like bridges, trees, and buildings. It is unarguable that any system should be designed to be safe. In fact, safety has been a major hurdle in deploying such AM systems in these applications \cite{Safety issues}.
Three major issues are faced with said close maneuvers. Firstly, hovering close to such objects leads to ground, ceiling, and wall effects, causing immeasurable turbulent disturbances
\cite{Ground effect}. Secondly, the AM encounters disturbances in the form of forces and torques due to the highly coupled dynamics of the UAV, and manipulator \cite{Robotica: Modeling and control}. These disturbances can lead to instability of the AM and cause a collision with the obstacles. E.g., external factors in the form of wind disturbances can lead to instability \cite{Wind disturbances}. Thirdly, it is only sometimes possible to accurately model the obstacles around a trajectory due to their irregular shape, lack of visibility, or uncertainty associated with obstacle locations. This can occur when the AM maneuvers through a dark or uneven tunnel incapacitating it to determine a bound across the obstacles.
Keeping this in mind, the AM must operate while keeping a safe distance from obstacles and maintaining stability. The AM movement can be bound in a desired workspace around the desired trajectory. This will prevent any possible collisions with obstacles.

\noindent \textbf{Related Work}

\noindent While the literature is sufficiently populated with novel design approaches of AMs \cite{Design 1}-\cite{Design 2}, prior work involving safe control of the AM has been sparse.
 Adaptive controller\cite{Adaptive control} tackles torques due to the highly coupled dynamics of the AM by using an outer loop adaptive control over the proportional–derivative (PD) inner loop of the UAV. Though it provides computational efficiency, it is incapable of incorporating constraints to avoid obstacles.
Model Predictive Control (MPC) \cite{NMPC} significantly reduces the abruptness in control inputs and tracks the desired trajectory while anticipating future dynamic interactions of the AM. PID and traditional adaptive controllers lack this predictive ability. MPC is applied to open a hinged door \cite{MPC Door open}. Considering the coupled dynamics of an AM and a hinged door, an MPC in the framework of a Linear Quadratic Regulator is designed.

\begin{figure}
    \centering
    \includegraphics[width=0.35\textwidth]{AM_angles.png}
    \caption{Aerial Manipulator as in RRC, IIIT Hyderabad}
    \label{fig1}
\end{figure}

Barrier Lyapunov Function (BLF) \cite{CBF Intro} is used as a tool to enforce the safety of non-linear dynamical systems. Barrier certificates \cite{ZCBF} are established considering a safe region of operation defined as $\zeta$. While guaranteeing forward invariance of $\zeta$,  safety is ensured.
BLF-based MPC for a non-linear system described in \cite{MPC BLF NL} proposes a stabilizing controller to ensure avoidance of a set of states associated with the unsafe region for a chemical process.
MPC combined with constraints using BLF \cite{P Mali} is used for distributive multi-UAV avoidance.
An MPC scheme for safety planning \cite{MPC safety} using BLF demonstrates the trade-off between the safety and performance of a UAV.

\noindent \textbf{Contributions}

\noindent The key contributions of this paper are the following:

\begin{enumerate}
    \item To the best of the author's knowledge, this is the first attempt to incorporate safe operation for AM maneuvers amidst unknown disturbances and boundary conditions.
    \item We introduce BLF-based constraints over an MPC controller to include obstacle avoidance and achieve tangible performance gain in trajectory tracking for the end-effector of the AM in comparison to prior MPC controllers \cite{NMPC}.
    \item We exploit BLF to create a novel constraint for bounding the AM inside a defined boundary, contrary to it's collision avoidance utility.
    \item A disturbance resistivity term is introduced in the BLF for guaranteeing safety under bounded random disturbances.

\end{enumerate}

\noindent The paper is structured as follows, Section II provides the dynamics model of the AM and BLF forward invariance constraint for collision avoidance. Section III provides the problem statement, while Section IV proposes a control architecture for the safe operation of AM. Section V discusses the simulation and benchmark results.

\section{PRELIMINARIES}

\subsection{Mathematical model of aerial manipulator}
In this section, we present the dynamics model of an AM \cite{2DOF design} in Newton-Euler Form. Here, the center of the UAV coincides with the center of gravity of the AM. We denote the Inertial frame $I = [I_x,I_y,I_z]$ with the centre at $O_I$, the Body frame fixed to the UAV is represented as $B = [B_x, B_y, B_z]$ centred at $O_B$. The manipulator link frames are denoted as $M_l = [M_{x_{l}}, M_{y_{l}}, M_{z_{l}}]$ centred at $O_l$. The position of the center of the UAV in the inertial frame is $\mathbf{p}_I = [x_I, y_I, z_I]^T$ and orientation in Euler angle convention is $\Phi = [\phi, \theta, \psi]^T$. Similarly, the manipulator joint angles are defined by $\Theta = [\theta_1,\theta_2,...,\theta_j]^T$ where $j$ is the number of joints, refer to Fig. \ref{fig1}. The acceleration vector due to gravitational forces is denoted by $\mathbf{g} = [0,0,g_z]$ where $g_z$=9.81 $m/s^2$. The mathematical models of UAV and manipulator are presented first, followed by the combined dynamics of AM.

\subsubsection{UAV Dynamics}
\par The translational dynamics of UAV is given in (\ref{eq1}).
\begin{equation}
\!
\begin{aligned}
\ddot{\mathbf{p}}_I &= \mathbf{g} + {^{I}R_B}\mathbf{T}_B/m_B
\end{aligned}
\label{eq1}
\end{equation}
where $m_B$ is the mass of UAV, $T_B$ is the thrust vector acting on the UAV, defined in the body frame. $^{F_1}R_{F_2}$ denotes standard rotation matrix in 3D for transformation from frame $F_2$ to frame $F_1$ \cite{Rotation matrix}.

Denoting angular acceleration in the body frame as ${\mathbf{\omega}}_B$, the rotational dynamics is given in (\ref{eq2}).
\begin{equation}
\dot{\mathbf{\omega}}_B = \mathbf{I}_B^{-1}(\mathbf{\tau}_B - \dot{\mathbf{\omega}}_B \times \mathbf{I}_B\dot{\mathbf{\omega}}_B)
\label{eq2}
\end{equation}

where $\tau_B$ and $\mathbf{I}_B$ are respectively the torque acting on the UAV and inertia matrix, defined in the body frame.

\par Hence, combining (\ref{eq1}) and (\ref{eq2}) in matrix addition form, the UAV dynamics is shown below.

\begin{equation}
\label{eq3}
\resizebox{\hsize}{!}{$
\begin{bmatrix}
m_B\mathbf{I}_{3\times3} && \mathbf{0}_{3\times3} \\
\mathbf{0}_{3\times3} && \mathbf{I}_B \\
\end{bmatrix}
\begin{bmatrix}
\ddot{\mathbf{p}}_I \\
\dot{\mathbf{\omega}}_B \\
\end{bmatrix} +
\begin{bmatrix}
\mathbf{0}_{3\times3} \\
\mathbf{\omega}_B \times \mathbf{I}_B\mathbf{\omega}_B \\
\end{bmatrix}
=
\begin{bmatrix}
m_B\mathbf{g} + {^{I}R_B}T_B \\
\mathbf{\tau}_B \\
\end{bmatrix}$}
\end{equation}

where $\mathbf{I}_{3\times3}$ is a 3$\times$3 identity matrix, $\mathbf{0}_{3\times3}$ is a $3\times3$ null matrix.
\subsubsection{Dynamics of Floating Base Manipulator}

\par We derive the equation of motion for an individual link based on Newton-Euler formulation using standard Denavit-Hartenberg (DH) convention \cite{DH parameters}. Forward recursion starting from base link to the end-effector is used as
\begin{equation}
\begin{aligned}
\omega_{i} &= {^{i}\mathbf{R}_{i-1}}\omega_{i-1} + {^{i}\mathbf{R}_0}z_{i-1}\dot \theta_i\\
\alpha_{i} &= {^{i}\mathbf{R}_{i-1}}\alpha_{i-1} + {^{i}\mathbf{R}_0}z_{i-1}\ddot \theta_i + \omega_{i} \times {^{i}\mathbf{R}_0}z_{i-1}\dot \theta_{i}\\
\mathbf{a}_{e,i} &= {^{i}\mathbf{R}_{i-1}}\mathbf{a}_{e,i-1} + \dot \omega_i \times \mathbf{r}_{i,i+1} + \omega_i \times (\omega_i \times \mathbf{r}_{i,i+1})\\
\mathbf{a}_{c,i} &= {^{i}\mathbf{R}_{i-1}}\mathbf{a}_{e,i-1} + \dot \omega_i \times \mathbf{r}_{i,ci} + \omega_i \times (\omega_i \times \mathbf{r}_{i,ci})\\
\mathbf{v}_{e,i} &= {^{i}\mathbf{R}_{i-1}}\mathbf{v}_{e,i-1} + \mathbf{r}_{i-1,i} \times {^{i}\mathbf{R}_0}z_{i-1}\dot \theta_i\\
\mathbf{p}_{e,i} &= {^{i}\mathbf{R}_{i-1}}\mathbf{p}_{e,i-1} + \mathbf{r}_{i-1,i} \times {^{i}\mathbf{R}_0}z_{i-1}\theta_i
\end{aligned}
\label{eq4}
\end{equation}
where $\theta_i$ is the joint angle of joint i, $l_i$, $m_i$, $I_i$ are the link length, mass and inertia of link i respectively, $\mathbf{r}_{j,ci}$ is the vector from joint j to the CoM of link i, $\mathbf{r}_{j,l}$ is the vector from joint j to joint l, $\mathbf{a}_{c,i}$ is the acceleration of the center of mass of link i, $\omega_i$, $\alpha_i$ are the angular velocity and angular acceleration of frame i w.r.t. frame i, $z_{i-1}$ is the axis of rotation of joint i w.r.t. frame 0.

\par Backward recursion shows force and torque on link i as
\begin{equation}
\begin{aligned}
\mathbf{f}_i &= {^{i}\mathbf{R}_{i+1}}\mathbf{f}_{i+1} + m_i\mathbf{a}_{c,i} - m_i\mathbf{g}_i\\
\mathbf{\tau}_i &= {^{i}\mathbf{R}_{i+1}}\mathbf{\tau}_{i+1} - \mathbf{f}_i \times \mathbf{r}_{i,ci} + {^{i}\mathbf{R}_{i+1}}\mathbf{f}_{i+1} \times \mathbf{r}_{i+1,ci}\\& + I_i\alpha_i + \omega_i \times (I_i \times \omega_i)
\label{eq5}
\end{aligned}
\end{equation}

where $\mathbf{f}_i$, $\tau_i$ are the force and torque respectively exerted by link i-1 on link i with the terminal conditions as $\mathbf{f}_{n+1}$ = 0 and $\tau_{n+1}$ = 0.
\subsubsection{Dynamics of the Aerial Manipulator}
\par AM dynamics are written by adding the forces and torques from the manipulator given in (\ref{eq5}) to the UAV dynamics given in (\ref{eq4}) and utilizing the mass $m_{am}$ and moment of inertia $\mathbf{I}_{am}$ of the combined system. The coupled equation of motion for the UAV with the manipulator is given in (\ref{eq6}).
\begin{equation}
\resizebox{\hsize}{!}{$
\begin{bmatrix}
m_{am}\mathbf{I}_{3\times3} && \mathbf{0}_{3\times3}\\
\mathbf{0}_{3\times3} && \mathbf{I}_{am} \\
\end{bmatrix}
\begin{bmatrix}
\ddot{\mathbf{p}}_I\\
\dot{\mathbf{\omega}}_B \\
\end{bmatrix} +
\begin{bmatrix}
\mathbf{0}_{3\times3}\\
\omega_B \times \mathbf{I}_{am}\omega_B \\
\end{bmatrix} =
\begin{bmatrix}
m_B\mathbf{g} + {^I\mathbf{R}_B}T_B+\mathbf{f}_0 \\
\mathbf{\tau}_B+\mathbf{\tau}_0\\
\end{bmatrix} $}
\label{eq6}
\end{equation}

\subsection{Barrier Lyapanov Function}

A BLF for the avoidance of a point obstacle by a UAV is presented here. The control affine form
 of the UAV dynamics  given in (\ref{eq3})  is shown  below.

\begin{equation}
     \mathbf{\dot x} = f(\mathbf{x}) + g(\mathbf{x}) \mathbf{u}
     \label{eq7}
\end{equation}
where $f(\mathbf{x})$ and $g(\mathbf{x})$ are functions of the state $\mathbf{x}$ and $\mathbf{u}$ is the control input at time $t$.

$h(\mathbf{x})$ is a valid BLF if it is continuously differentiable and the following conditions given in (\ref{eq8}) are satisfied.
\begin{equation}
  \left\{\begin{array}{@{}l@{}}
    h(\mathbf{x}) \geq  0 , \ \forall \ \mathbf{x}  \in  \zeta\\
    h(\mathbf{x}) <  0 , \  \forall  \ \mathbf{x}  \notin  \zeta\\
  \end{array}\right.\,
  \label{eq8}
\end{equation}

where $\zeta$ denotes the set of all states in the safe region of operation and is defined as $\zeta = \{\mathbf{x} \in  \mathbb{R} ^{n} ~|~ h(\mathbf{x}) \geq 0\}$.

If initially, the UAV resides in the safe region and the condition $\dot h (\mathbf{x}) \geq 0$ implies that $\zeta$ is forward invariant. This ensures that $h(\mathbf{x})$ remains in the desired safe region. The forward invariance condition $\dot h (\mathbf{x}) \geq 0$ can be relaxed to $\dot h (\mathbf{x}) \geq - \gamma h^{z}(\mathbf{x})$ leading to asymptotic convergence of $h(x)$ to 0.  Considering the state-space model in (\ref{eq7}), the forward invariance condition can be written as given below.

\begin{equation}
    \frac{\partial h(\mathbf{x})}{\partial x} (f(\mathbf{x}) + g(\mathbf{x}) \mathbf{u}) + \gamma h^{z}(\mathbf{x}) \geq 0
    \label{eq10}
\end{equation}
where $\gamma > 0 $ and $z > 0 $ are tunable parameters. BLF ($h_0$) for point obstacle avoidance satisfying the conditions given in (\ref{eq8}) and (\ref{eq10}) can be selected as shown below.

\begin{equation}
    h_0(\mathbf{x}) = \sqrt{2 \alpha _{max} (||\vec{\mathbf{p}}|| - d_s)} + \frac{\vec{\mathbf{p}}^T}{||\vec{\mathbf{p}}||} \vec{\mathbf{v}}
    \label{eq11}
\end{equation}

where $\alpha _{max}$ is the maximum acceleration attainable by the AM, $d_s$ is the desired safe distance, $\vec{\mathbf{p}}$ is the range vector from point obstacle and $\vec{\mathbf{v}}$ is the velocity of the robot at time $t$.  Differentiating $h_0(\mathbf{x})$ in (\ref{eq11}) and substituting in (\ref{eq10}) gives forward invariance condition on the controller as given in (\ref{eq12}).

\begin{equation}
\begin{aligned}
    \frac{\alpha_{max}~ \vec{\mathbf{v}}^T \vec{\mathbf{p}}}{\sqrt{2 \alpha _{max} (||\vec{\mathbf{p}}|| - d_s)}} - {\left (\frac{\vec{\mathbf{p}}^T}{||\vec{\mathbf{p}}||} \vec{\mathbf{v}} \right )}^{2} + ||\vec{\mathbf{v}}||^2  + \vec{\mathbf{p}}^T \mathbf{u} \\ +  \gamma h_0^z(\mathbf{x}) ||\vec{\mathbf{p}}|| ~\geq ~0
\end{aligned}
\label{eq12}
\end{equation}

\section{PROBLEM FORMULATION}

The primary aim of the paper is to design a controller to follow the desired trajectory ($\mathbf{p}^{d}$) for the end-effector of the AM i.e. to minimize at any time $t$ the trajectory error ($e_e(\mathbf{x})$)
\begin{equation}
     \underset{\mathbf{u}} {\min} \ e_{e}(\mathbf{x}) =  ||\mathbf{p}_{e}(\mathbf{x})- \mathbf{p}^{d}|| \  \forall \ t > 0
     \label{eq13}
\end{equation}
\vspace{-0.2em}
where $\mathbf{p}_{e}(\mathbf{x})$ is the position of the end-effector and $\mathbf{p}^{d}$ is the desired position of the end-effector at time $t$. This trajectory is defined for the end-effector, while the trajectory of the UAV is left to be determined by the controller, such as to reduce $\mathbf{e}_{e}(\mathbf{x})$ given in (\ref{eq13}).

\begin{figure}[]
\centering
\includegraphics[width=0.38\textwidth]{Workspace_with_circle.png}
\caption{\small{Inverse Workspace of a floating base manipulator wrt to a fixed end-effector point (0,0). This is a 2-D representation of a 3-D workspace. The 3-D workspace can be visualized by rotating 360\textdegree around line x = 0. The trajectory bound shown as a circle is a sphere $||p_r - O|| = r$ where $p_r$ is a point on the sphere, $O$ is the center of the sphere in 3D space and $r$ is the radius of the sphere.}}
\label{fig2}
\end{figure}

There are multiple ways to reach a desired point on the trajectory by placing the UAV in the inverse workspace of the floating base manipulator. This inverse workspace, Fig. \ref{fig2}, is defined by taking the inverse kinematics of the floating base manipulator considering it's end-effector to be fixed at the desired trajectory point. E.g., Fig. \ref{fig2} shows the inverse workspace of a 2R manipulator in which manipulator joint angles are constrained.

\begin{figure}[t]
\subfigure[] {\includegraphics[width=0.22\textwidth]{Drone_Problem_Formulation.png}}~~~
\subfigure[] {\includegraphics[width=0.25\textwidth]{Bounding.png}}
\centering
\caption{\small{\textbf{Setup for AM simulation:} (a) Barrier avoidance: Shows close operation of an AM with rectangular walls on the three sides with a known position. (b) Free space workspace tracking: Shows bounded maneuver of the AM in the desired workspace within an allowed free space of the operation (here, the obstacle position information is not available).} }
\label{fig3}
\end{figure}

Two cases are considered here, \textbf{Case I}, the location of the obstacle is known, and \textbf{Case II}, the allowed free space for operation is given without specifying the location of the obstacle. For \textbf{Case I} shown in Fig. \ref{fig3}(a), the end-effector of the AM has to follow the desired trajectory close to static obstacles. The condition for safety is given in (\ref{eq14}).
\begin{equation}
\begin{aligned}
    ||\vec{\mathbf{s}}_j ( \mathbf{x}_i )|| \geq s^{min}_j \ \forall \ j \in \{1,...,C\}\\
    \vec{\mathbf{s}}_j \times [a,b,c]^T = 0
    \label{eq14}
\end{aligned}
\end{equation}
\vspace{-0.5em}

where $\vec{\mathbf{s}}_j$ is the perpendicular distance vector of the critical point $j$ from the wall and $s^{min}_j$ is the minimum safety distance from the wall. The number of critical points is denoted by $C$.
In Fig. \ref{fig3}(a), critical points on the AM are (A) the base of the manipulator with safety radius ${s}_1$, (B) joint 2 of the manipulator with safety radius ${s}_2$ and (C) end-effector of the manipulator with safety radius ${s}_3$. Equation of the wall is given by $ax + by + cz + d = 0$ where $a$,$b$,$c$,$d$ are parameters of the plane and $x$, $y$, $z$ are points on the plane.

For \textbf{Case II}, Fig. \ref{fig3}(b), a safe free space of operation around the trajectory is provided, and we provide a guarantee that the AM will stay in that safe bounded region. We choose a spherical differentiable part Fig. \ref{fig2} of the non-differentiable inverse workspace with radius $r$. The condition for bounding in the desired workspace is given in (\ref{eq15}).

\begin{equation}
\begin{aligned}
    ||\vec{\mathbf{d}} (\mathbf{x} )|| \leq r,\,\,\,
    \vec{\mathbf{d}} (\mathbf{x}_i) = \mathbf{p}_{I}(\mathbf{x} )- (\mathbf{p}^{d} - d_{iw})
    \label{eq15}
\end{aligned}
\end{equation}
where $r$ is the radius of the desired workspace and $d_{iw}$ is the deviation of the workspace center from the desired trajectory point. As the bound for the allowable free space is within the inverse workspace, the end-effector would be able to reach the desired point in at least one orientation of the AM.

\begin{figure*}[ht]
\includegraphics[scale=0.44]{MPC_BLF_Formulation.png}
\caption{\small{\textbf{Control Architecture of the AM.} Outer loop controller denotes the MPC optimization problem while the inner loop controller denotes the on-board PID control of the AM. Constraints on the optimizer are denoted by input constraints ($\mathbf{u}$) an Constraints.}}
\label{fig4}
\end{figure*}

\section{PROPOSED CONTROLLER}
The architecture of the proposed controller is shown in Fig. \ref{fig4}. It consists of an outer loop MPC  with BLF constraints. The inner loop is driven by a conventional PID controller. The outer loop provides the desired acceleration and yaw angle references to the inner loop controller.
\subsection{MPC (Outer loop control)}
The state vector of the AM is defined as $\mathbf{x} = [\mathbf{p}_I, \mathbf{\dot{p}}_I, \psi, \dot\psi, \Theta, \dot\Theta]$, where $\mathbf{p}_I$ and $\mathbf{\dot{p}}_I$ is the position and velocity of the center of mass of the UAV in inertial frame respectively, $\psi$ and $\dot\psi$ is the yaw and yaw rate, and $\Theta = [\theta_1, \theta_2]$ and $\dot\Theta = [\dot\theta_1, \dot\theta_2]$ is the joint angle and angular rate of the manipulator's joints.
The control input for MPC is $\mathbf{u}= [\mathbf{a}_d, \ddot\psi, \ddot\Theta_d]$ where $\mathbf{a}_d = [a_x, a_y, a_z]$ is the acceleration of the UAV center in the inertial frame, $\ddot\psi$ is the yaw angular acceleration and $\ddot\Theta$ is the angular acceleration of manipulator's joints. The state space model used within the MPC is formulated as given in (\ref{eq15}).

\begin{equation}
\begin{aligned}
\mathbf{\dot x} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} \\
\end{aligned}
\label{eq15}
\end{equation}

\begin{equation}
\mathbf{A} = \begin{bmatrix}
0_{3x3} && I_{3x3} && 0_{3x6}\\
0_{3x3} && 0_{3x3} && 0_{3x6}\\
0_{1x7} && 1 && 0_{1x4}\\
0_{1x3} && 0_{1x3} && 0_{1x6}\\
0_{2x3} && 0_{2x7} && I_{2x2}\\
0_{2x3} && 0_{2x3} && 0_{2x6}\\
\end{bmatrix}
\mathbf{B} = \begin{bmatrix}
0_{3x3} && 0 && 0_{3x2}\\
I_{3x3} && 0 && 0_{3x2}\\
0_{1x3} && 0 && 0_{1x2}\\
0_{1x3} && 1 && 0_{1x2}\\
0_{1x3} && 0 && 0_{1x2}\\
0_{1x3} && 0 && I_{1x2}
\end{bmatrix}\\\\
\label{eq16}
\end{equation}

In (\ref{eq16}), ${I}$ is a diagonal matrix such that, $I(i,j) = 1$, $\forall$ $i=j$ and $I(i,j) = 0$, $\forall$ $i \neq j$ and ${0}$ is zero matrix such that, $0(i,j) = 0$, $\forall$ $i,j$ where $i,j\in\mathbb{N}$.

For Aerial Manipulator dynamics with state variables as $\mathbf{x}$ and control variables as $\mathbf{u}$,
the optimal control problems at every time instant $t_k$ where $t_k = kT$ and $T$ is a time step is given in (\ref{eq17}).
\begin{subequations}
\label{eq:Cost_function}
\begin{align}
\underset{\mathbf{u}} {\min} \ l( \mathbf{x_k,u_k}, t_{k})\\
\textrm{s.t.} \  \mathbf{x}_{k+1} = A \mathbf{x}_{k} + B \mathbf{u}_{k}\\
\mathbf{u}_{min} \leq \mathbf{u}_k \leq \mathbf{u}_{max}\\
\mathbf{x}_{min} \leq \mathbf{x}_k \leq \mathbf{x}_{max}
\end{align}
\label{eq17}
\end{subequations}

where $\mathbf{u} = [{\mathbf{u}_k}^\top, {\mathbf{u}_{k+1}}^\top, ...., {\mathbf{u}_{k+N-1}}^\top]^\top $ denotes the vector of control variables and $f$ is the dynamic model of the system. $\mathbf{u}_{min}$ and $\mathbf{u}_{max}$ represent the bounds on $\mathbf{u}$ and $\mathbf{x}_{min}$ and $\mathbf{x}_{max}$ represent the bounds on $\mathbf{x}$. $\mathbf{x}_{k}$ denotes the state of the system at $k^{th}$ time step, similarly $\mathbf{u}_{k}$ denotes the control input of the system at $k^{th}$ time step. The cost $l$ is the summation of multiple cost functions $l = \sum_{i = 1}^{N_l} l_i$ explained in the section \textit{Weighing strategy}.

\subsubsection{Weighing strategy}

The $N_{l}$ costs are chosen to follow the trajectory and enhance stability. These are defined in (\ref{eq18}), (\ref{eq20}) and (\ref{eq21}).

\paragraph{Tracking error for the End-Effector}

The primary task is the tracking of the end effector over a given trajectory. This is achieved by penalizing the difference between the current and desired position as given in (\ref{eq18})

\begin{equation}
\begin{aligned}
l_{1} = \ \sum_{i = 0}^{n-1} (  \left|\left|   \mathbf{e}_{e}(\mathbf{x}_{k+i})   \right|\right|^2_{W_{1}} ) + \left|\left|  \mathbf{e}_{e}( \mathbf{x}_{k+N})  \right|\right|^2_{W_{s_{1}}}
\end{aligned}
\label{eq18}
 \end{equation}

 where $\mathbf{e}_{e}(\mathbf{x}_{k+i}) =  \ \mathbf{p}_{e}(\mathbf{x}_{k+i} )- \mathbf{p}^{d}_{k+i}$.

is the end-effector tracking error calculated at each time step of the prediction horizon. To decrease error from the desired trajectory, we penalize higher velocities of the manipulator end-effector as given in (\ref{eq20}).

\begin{equation}
\begin{aligned}
l_{2} = \ \sum_{i = 0}^{n-1} (  \left|\left|   \mathbf{v}_{e}(\mathbf{x}_{k+i})   \right|\right|^2_{W_{2}} ) + \left|\left|  \mathbf{v}_{e}( \mathbf{x}_{k+N})  \right|\right|^2_{W_{s_{2}}}
\end{aligned}
\label{eq20}
\end{equation}

where $\mathbf{v}_{e}$ is the velocity of the End-Effector of the Manipulator.

\paragraph{COG Allignment Error}

As the manipulator moves in the $\mathbf{B_x B_z}$ plane of the UAV (refer to Fig. \ref{fig1}) while the UAV changes its attitude $(\theta, \phi, \psi)$, the Center of Gravity of the system moves in the $\mathbf{B_x B_y}$ direction of the UAV due to which undesirable torques appear which destabilize the UAV. The following cost given in (\ref{eq21}) is introduced considering this factor.
\begin{equation}
\begin{aligned}
l_{3} = \ \sum_{i = 0}^{n-1} (  \left|\left|   \mathbf{p}_{{G}_{XY}}(\mathbf{x}_{k+i})   \right|\right|^2_{W_{3}} ) + \left|\left|  \mathbf{p}_{{G}_{XY}}( \mathbf{x}_{k+N})  \right|\right|^2_{W_{s_{3}}}
\end{aligned}
\label{eq21}
\end{equation}

where $\mathbf{p}_{{G}_{XY}}$ is the Center of Gravity of the Manipulator in the $\mathbf{B_xB_y}$
plane. \\

MPC to reach desired points using this approach was used in \cite{NMPC}, and we would call this technique \textbf{Naive MPC}.

\subsection{PID (Inner Loop Control)}

From the control input ($\mathbf{u}_{k}^{min}$) obtained from the outer loop controller, the desired values of $\theta$, $\phi$ are estimated using small angle approximation (refer to Fig. \ref{fig4}) and along with $\psi_d$ are tracked by the PID controller namely PID Attitude. Similarly, PID Height regulates the height, and PID Manipulator angle controls the joint angles $\theta_1$ and $\theta_2$ \cite{Adaptive control}.

\subsection{Safe operation near known barriers}

For \textbf{Case I}, the priority for safety is the highest. The AM has to avoid collisions for both the UAV and the manipulator simultaneously while tracking the desired trajectory.
The critical points are chosen such that if we can guarantee collision avoidance for these points, the entire system can safely perform desired maneuvers with collision.
BLF used for collision avoidance along the radial direction of the Wall is given in (\ref{eq22}).

\begin{equation}
\noindent h_1(\mathbf{x}_k) = \sqrt{\frac{2 \vec{\mathbf{s}}_j(\mathbf{x}_{k+i})^T \alpha_{max}}{||\vec{\mathbf{s}}_j(\mathbf{x}_{k+i})||}  (||\vec{\mathbf{s}}_j(\mathbf{x}_{k+i})|| - s^{min}_j)} + \frac{\vec{\mathbf{s}}_j(\mathbf{x}_{k+i})^T}{||\vec{\mathbf{s}}_j(\mathbf{x}_{k+i})||} \vec{\mathbf{v}}_{k+i}
\label{eq22}
\end{equation}

\subsection{Bounded Trajectory Tracking }

\begin{figure}[t]
\includegraphics[scale=0.4]{Workspace.png}
\centering
\caption{\small{Desired workspace for the UAV (refer to the circle in Fig. \ref{fig2}). X denotes the Center of the UAV, $\vec{p}$ denotes vector from X to O, $\vec{v}$ denotes instantaneous velocity of X and $\vec{v}_k$ denotes instantaneous velocity of desired trajectory of the end-effector}}
\label{fig5}
\end{figure}

\noindent For \textbf{Case II}, one of our major contributions in this section is the usage of BLF for the bounding of the UAV in the desired workspace shown in Fig. \ref{fig5}. We exploit the property that a particle can exit a sphere only through its motion in the radially outwards direction. Hence, the UAV can only escape the boundary if it is provided with a high velocity in the radial direction (along $\vec{\mathbf{p}}$ or $-\vec{\mathbf{p}}$ ).
Due to this restricted movement in the radial directions on both sides, the UAV does not leave the workspace at that particular instant $k$. BLF ($h_{2_a}$) along $\vec{\mathbf{p}}$ is given in (\ref{eq23}) and a similar BLF ($h_{2_b}$) along $-\vec{\mathbf{p}}$ can be found by replacing $\vec{\mathbf{p}}$ with $-\vec{\mathbf{p}}$.

\begin{equation}
h_{2_{a}}(\mathbf{x}_k) = \sqrt{\frac{2 (\vec{\mathbf{p}}_{k+i})^T \alpha_{max}}{||\vec{\mathbf{p}}_{k+i}||}  (||\vec{\mathbf{p}}_{k+i}|| - r)} + \frac{\vec{\mathbf{p}}_{k+i}^T}{||\vec{\mathbf{p}}_{k+i}||} (\vec{\mathbf{v}}_{k+i} - \vec{\mathbf{v}}^t_{k+i})
\label{eq23}
\end{equation}

\subsection{BLF with Disturbance Rejection}

Another contribution of this paper involves handling unseen disturbances, such as constant wind or impulses acting at random intervals in the framework of BLF. The condition in (\ref{eq10}) need not be satisfied for the given $h(\mathbf{x})$ in the presence of unmeasured disturbances. If the AM encounters a sudden disturbance, it might result in a collision with obstacles. The condition  $\dot h (\mathbf{x}) \geq - \gamma h^{z}(\mathbf{x})$ is tightened while maintaining relaxation for smooth operation as given in (\ref{eq25}) with $\lambda>0$.
\begin{equation}
    \dot{h}_i(\mathbf{x}) + \gamma({h}^z_i(\mathbf{x}) - \lambda) \geq 0
    \label{eq25}
\end{equation}

\noindent The BLF functions $h_1$, $h_{2_a}$ and $h_{2_b}$ are put in (\ref{eq25}) and are added to the MPC optimizer as constraints. The resultant controller is termed as \textbf{MPC-BLF}. To the best of the author's knowledge, there have been no attempts to handle disturbance forces or torques for an AM using a BLF.

\section{SIMULATION RESULTS}

The algorithm is implemented on Python 3 on an Intel® Core™ i7-8550U CPU PC running at 1.80 GHz. We use the 'SLSQP' method from scipy \cite{scipy} as the non-linear optimizer for MPC. The AM used for simulation is a mathematical replica of a laboratory scale AM shown in Fig. \ref{fig1} with the following specifications given in Table \ref{table1}.

\begin{table}[h]
\label{table_example}
\centering
\resizebox{\columnwidth}{!}{%
\begin{tabular}{|>{\centering}p{0.17\textwidth}|>{\centering\arraybackslash}p{0.26\textwidth}|}
\hline
\textbf{Parameter} &  \textbf{Value} \\
\hline
Mass &   $3.5~kg$ \\
\hline
Arm length & $0.3~m$\\
\hline
Propeller Diameter & $0.33~m$ \\
\hline
Moment of Inertia - UAV&  $I_x = 0.3~kg~m^2$, $I_y = 0.3~kg~m^2$, $I_z = 0.6~kg~m^2$\\
\hline
Manipulator length & $link 1 = 0.15~m$, $link 2 = 0.15~m$  \\

\hline
Moment of Inertia - Manipulator&  $I_1 = 4.256\times10^{-5}~kg~m^2$, $I_2 = 8.321\times10^{-5}~kg~m^2$ \\
\hline

Moment of Inertia - Manipulator&  $I_1 = 4.256\times10^{-5}~kg~m^2$, $I_2 = 8.321\times10^{-5}~kg~m^2$ \\
\hline
UAV attitude constraints & $|\theta| \leq \pi/10~ rad$ , $|\phi| \leq \pi/10~ rad$\\
\hline
Manipulator joint angle constraints & $|\theta_1| \leq \pi/3~ rad $, $|\theta_1 + \theta_2| \leq \pi/2~ rad$ \\
\hline
\end{tabular}}
\caption{Specifications of the Aerial Manipulator}
\label{table1}

\end{table}

We perform simulations separately for two cases.

\textbf{Case I}: Avoiding Wall on three sides of the AM (The position of the obstacle is known).

\textbf{Case II}: Free space for the operation of AM is given (The position of obstacles is unknown, a  desired spherical workspace  for the UAV is created.)

For both of the simulations, the task is to follow a desired trajectory by the end-effector. A uniformly random disturbance with an amplitude of $d_m=0.8\,m/s^2$ is used to evaluate the performance in the presence of external disturbances.

\subsection{Modified Naive MPC for comparison}

The Naive MPC is modified as given below  to compare the performance with the proposed controller.

\subsubsection{\textbf{Hard Constraint (MPC-HC)}}

\noindent For \textbf{Case I}, a hard constraint on the critical points to avoid collision with the Wall is enforced as in (\ref{eq14}) $\forall~i~\in [0,n]$.
For \textbf{Case II}, a hard constraint on the relative position of UAV center (Fig. \ref{fig5}) as in (\ref{eq15}) $\forall~i~\in [0,n]$ is added. Similar to \textbf{Case I}, this is the intuitive constraint for bounding.
These additional constraints are put on the MPC optimizer in (\ref{eq17}).

\subsubsection{\textbf{Soft Constraint (MPC-SC)}}

\noindent For \textbf{Case I}, a cost function for safe operation near walls is given in (\ref{eq29}). This cost increases as the Wall is approached, penalizing the critical points going near  the Wall.
\begin{equation}
\begin{aligned}
l_{4} = \ \sum_{i = 0}^{n-1}   \frac{1}{(||\vec{\mathbf{p}}_j ( \mathbf{x}_{k+i} )|| - s_j)^2_{W_{4}}}  +    \frac{1}{(||\vec{\mathbf{p}}_j ( \mathbf{x}_{k+n} )|| - s_j)^2_{W_{s_{4}}}}
\end{aligned}
\label{eq29}
 \end{equation}

For \textbf{Case II}, a cost function that penalizes the UAV movement outside the workspace is given in (\ref{eq30}) and is added to the MPC cost.
\begin{equation}
\begin{aligned}
l_{5} = \ \sum_{i = 0}^{n-1}   \left|\left|   \mathbf{p}(\mathbf{x}_{k+i})   \right|\right|^2_{W_{5}}  + \left|\left|  \mathbf{p}( \mathbf{x}_{k+n})  \right|\right|^2_{W_{s_{5}}} \ \forall \ |\mathbf{p}(\mathbf{x}_{k+i})| \geq r
\end{aligned}
\label{eq30}
 \end{equation}
Parameters and weights for the different MPC versions are given in Table \ref{table2}.

\begin{table}[h]
\begin{center}
{\begin{tabular}{|>{\centering}p{0.15\textwidth}|>{\centering\arraybackslash}p{0.28\textwidth}|}
\hline
\textbf{Parameter} &  \textbf{Value} \\
\hline
MPC Weights &   $w_1 = 10\times \mathbf{I}_{3\times3}$, $w_{s_1} = 50\times \mathbf{I}_{3\times3}$, $w_2 = 2\times \mathbf{I}_{3\times3}$, $w_{s_2} = 10\times \mathbf{I}_{3\times3}$, $w_3 = \mathbf{I}_{3\times3}$, $w_{s_3} = 5\times \mathbf{I}_{3\times3}$, $w_4 = 5\times \mathbf{I}_{3\times3}$, $w_{s_4} = 20\times \mathbf{I}_{3\times3}$, $w_5 = 5\times \mathbf{I}_{3\times3}$, $w_{s_5} = 20\times \mathbf{I}_{3\times3}$ \\
\hline
$s_j^{min}$ & $0.1$ m\\
\hline
$u_k$ Initialization & $\mathbf{0}_{1 \times 6n}$ \\
\hline
$d_{iw}$ &  $0.225$ $m$\\
\hline
$\gamma$ & $3$  \\
\hline
Sampling time ($t_s$) & $0.1$ s \\
\hline
Total time ($t$) & $120$ s \\
\hline
Max disturbance amplitude & $d_m  = 0.8\, m/s^2$ \\
\hline

\hline
\end{tabular}
\caption{Weights and Parameters for MPC}
\label{table2}
\end{center}
\end{table}
\subsection{Metrics for performance comparison}

\noindent The performance of the controller is evaluated by the following metrics for  $N$ time steps ($N = t_f/t_s$). Here, $t_f$ is the final time, and $t_s$ is the sampling time in seconds.

\begin{itemize}
    \item Maneuver completion without collision or escaping the desired workspace
    \item Manipulator end-effector root mean square error from the desired trajectory, TE = $\sqrt{\frac{1}{N} \sum_{k = 0}^{N-1} (\mathbf{e}_{e}(\mathbf{x}_{k}))^2}$
    \item Control effort, $c_e = \sum_{k = 0}^{N-1}{||\mathbf{u}_k||^2}$
    \item Control Smoothness, $c_s = \sum_{k = 0}^{N-1}{|\Delta\mathbf{u}_k|}$
\end{itemize}

\subsection{Performance comparison for \textbf{Case II}}
Selection of the prediction horizon ($n$) and the parameter $\lambda$ determines the computational load and disturbance rejection capabilities, respectively.
An analysis was conducted by varying n and $\lambda$ and the results are given in Table \ref{table3}. We chose n = 5 and $\lambda = 5$ as it shows low TE while maintaining the workspace bound. These values are used for all subsequent simulations. $n = 10$ does not show significant improvement (<5\%) in TE compared to $n=5$ and is rejected because of a very high computational time compared to $n=5$ (>600\%). $\lambda = 5$ shows high disturbance resistivity given its low TE and low $c_s$. $T_C$ denotes the computational time for one step.

\begin{table}[htbp]
\centering
\resizebox{\columnwidth}{!}{%
{\begin{tabular}{|>{\centering}p{0.085\textwidth}|>{\centering}p{0.047\textwidth}|>{\centering}p{0.047\textwidth}|>{\centering}p{0.047\textwidth}|>{\centering}p{0.047\textwidth}|>{\centering}p{0.047\textwidth}|>{\centering\arraybackslash}p{0.047\textwidth}|}
\hline
Parameter & \multicolumn{2}{c|}{n = 1} & \multicolumn{2}{c|}{n = 5} & \multicolumn{2}{c|}{n = 10}\\

\cline{2-3} \cline{4-5} \cline{6-7}
        &  $\lambda = 1$ & $\lambda = 5$ & $\lambda = 1$ & $\lambda = 5$
	& $\lambda = 1$ & $\lambda = 5$  \\    \hline

\hline
$T_C$ (s) & 0.0313  & 0.0309  & 0.1712 & 0.1603 & 1.370 & 1.232 \\
\hline
TE (m) & 0.1943 & 0.1839 & 0.0738 & 0.0658 & 0.0701 & 0.0623\\
\hline
$c_s$  & 0.1593 & 0.1603 & 0.0746 & 0.0772 & 0.0716 & 0.0694 \\
\hline
$c_e$ & 0.1432 & 0.1248 & 0.0801 & 0.0788 & 0.0939 & 0.0942 \\
\hline
\end{tabular}}}
\caption {Ablation showing the effect of parameters on the proposed MPC-BLF method (units mentioned in brackets)}
\label{table3}
\end{table}

\begin{figure}[htbp]
    \subfigure[] {\includegraphics[width=0.93\columnwidth]{safe_region_bounding_naive_MPC.png}}\vspace{-1em}
    \subfigure[] {\includegraphics[width=0.93\columnwidth]{safe_region_bounding_hard_constraint.png}}\vspace{-1em}
    \subfigure[] {\includegraphics[width=0.93\columnwidth]{safe_region_bounding_soft_constraint.png}}\vspace{-1em}
    \subfigure[] {\includegraphics[width=0.93\columnwidth]{safe_region_bounding_my_algo.png}}\vspace{-1em}
    \caption{\textbf{AM position for Case II with disturbances:} (a) Naive MPC , (b) MPC-HC, (c) MPC-SC, (d) MPC- BLF. <green> -> Desired trajectory, <blue> -> UAV Center, <orange> -> End-effector position}
    \label{fig6}
\end{figure}

\begin{figure}[htbp]
    \subfigure[] {\includegraphics[width=0.48\columnwidth]{safe_region_bounding_naive_MPC_workspace.png}}\vspace{-0.4em}
    \subfigure[] {\includegraphics[width=0.48\columnwidth]{safe_region_bounding_hard_constraint_workspace.png}}\vspace{-0.4em}
    \subfigure[] {\includegraphics[width=0.48\columnwidth]{safe_region_bounding_soft_constraint_workspace.png}}\vspace{-0.4em}
    \subfigure[] {\includegraphics[width=0.48\columnwidth]{safe_region_bounding_my_algo_workspace.png}}\vspace{-0.4em}
    \caption{{Relative position of the UAV center w.r.t. the workspace bound (\textbf{Case II}) during the maneuvers in Fig. \ref{fig6}: } (a) Naive MPC , (b) MPC-HC, (c) MPC-SC, (d) MPC- BLF. <lightgreen> -> desired workspace boundary, <blue> -> UAV center. UAV Center should not move out of the workspace boundary.}
    \label{fig7}
\end{figure}

 The results given in Table \ref{table4} show that none among the Naive MPC, MPC-HC, and MPC-SC is able to restrict the UAV in the desired workspace, either in the presence or absence of external disturbances. MPC-HC has no penalization for the velocity of the UAV; hence when a large control input is provided, it exits the bounds. MPC-SC has to trade-off between TE and bounding cost, hence compromising on one of the factors. The proposed MPC-BLF is able to restrict the UAV within the desired workspace with the lowest control effort, highest control smoothness, and lowest tracking error. The inference is more evident in the presence of external disturbances. The trajectory of the AM is shown in Fig. \ref{fig6} for all four methods in the presence of external disturbances. The proposed MPC-BLF method is the only one to confine the UAV within the safe boundary (Fig. \ref{fig7}(d)).

\begin{figure}[htbp]
    \subfigure[] {\includegraphics[width=0.9\columnwidth]{Wall_x_naive_MPC.png}}\vspace{-1em}
    \subfigure[] {\includegraphics[width=0.89\columnwidth]{Wall_x_hard_constraint.png}}\vspace{-1em}
    \subfigure[] {\includegraphics[width=0.9\columnwidth]{Wall_x_soft_constraint.png}}\vspace{-1em}
    \subfigure[] {\includegraphics[width=0.9\columnwidth]{Wall_x_my_algo.png}}\vspace{-1em}
    \caption{\textbf{AM position for Case I with disturbances in $I_x$ direction:} Red circle depicts collision with any wall. (a) Naive MPC: End-effector collides with the Wall (b) MPC - HC : UAV collides with Wall (c) MPC - SC : End-effector collides with the Wall (d) MPC-BLF: Maneuver completion.
    <green> -> Desired trajectory, <blue> -> UAV Center, <orange> -> End-effector position, <Red> -> Wall.}
    \label{fig8}
\end{figure}

\subsection{Performance comparison for \textbf{Case I}}

As shown in Fig. \ref{fig8}, \textbf{Case I} shows similar behaviour as \textbf{Case II}. Fig. \ref{fig8} shows trajectory tracking in only $I_x$, a similar result is obtained in $I_y$, $I_z$. As shown in Table \ref{table4}, MPC-SC shows a very high TE compared to MPC-BLF even in the absence of disturbances. MPC-BLF is able to avoid the walls contrary to any other method in the presence of disturbances.

\begin{table*}[h]
\centering

\resizebox{\hsize}{!}{%
\fbox{\begin{tabular}{|p{0.09\textwidth}|>{\centering}p{0.09\textwidth}||>{\centering}p{0.085\textwidth}|>{\centering}p{0.085\textwidth}||>{\centering}p{0.085\textwidth}|>{\centering}p{0.085\textwidth}||>{\centering}p{0.085\textwidth}|>{\centering}p{0.085\textwidth}||>{\centering}p{0.085\textwidth}|>{\centering\arraybackslash}p{0.085\textwidth}|}

\hline
  &  & \multicolumn{2}{c||}{Naive MPC}& \multicolumn{2}{c||}{MPC - HC}&
 \multicolumn{2}{c||}{MPC- SC} &   \multicolumn{2}{c|}{MPC- BLF}\\
\cline{3-4} \cline{5-6} \cline{7-8} \cline{9-10}
        &  &   \textbf{Case I}& \textbf{Case II} &  \textbf{Case I} & \textbf{Case II}
	&  \textbf{Case I} & \textbf{Case II}  & \textbf{Case I} & \textbf{Case II} \\
 \hline

\multirow{5}{*}{Without Disturbance} &  & $\times$ & $\times$ & $\times$ &$\times$ & $\checkmark$ & $\times$ & $\checkmark$ & $\checkmark$ \\
\cline {2-10}
& TE (m) '$\downarrow$' & $0.02432*$ & $0.02429$ & $0.02657*$ & $0.02386$ & $0.04482$ & $0.09731$ & $\mathbf{0.02629}$ & $\mathbf{0.04750}$  \\
\cline {2-10}
& $c_s$ '$\downarrow$'& $0.00557*$ & $0.00612$ & $0.02880*$ & $0.04981$ & $\mathbf{0.00443}$ & $0.00698$ & $0.00732$ & $\mathbf{0.03212}$ \\
\cline {2-10}
& $c_e$ '$\downarrow$'& $0.01427*$ & $0.01489$ & $0.04787*$ & $0.06572$ & $0.02326$ & $0.02229$ & $\mathbf{0.022771}$ & $\mathbf{0.04531}$ \\
\cline {2-10}
& & & & & & & & & \\
\hline

\multirow{5}{*}{With Disturbance} &   & $\times$ & $\times$ & $\times$ & $\times$ & $\times$ & $\times$ & $\checkmark$ & $\checkmark$ \\
\cline {2-10}
& TE (m) '$\downarrow$'& $0.07366*$ & $0.06735$ & $0.05430*$ &  $0.05490*$ & $0.07708*$ & $0.11384$ & $\mathbf{0.07164}$ & $\mathbf{0.06589}$ \\
\cline {2-10}
& $c_s$ '$\downarrow$'& $0.08933*$ & $0.08942$ & $0.10546*$ & $0.11304*$ & $0.08933*$ & $0.07915$ & $\mathbf{0.15592}$ & $\mathbf{0.07720}$ \\
\cline {2-10}
& $c_e$ '$\downarrow$'& $0.17720*$ & $0.20924$ & $0.18921*$ & $0.18301*$ & $0.21039*$ & $0.08328$ & $\mathbf{0.23040}$ & $\mathbf{0.07887}$ \\
\cline {2-10}
& & & & & & & & & \\
\hline
\end{tabular}}}

\caption { \textbf{Algorithm Benchmarking for simulations: } For Wall Avoidance (\textbf{Case I}), '$\times$' signifies that the AM collided with the wall. For workspace bound (\textbf{Case II}), '$\times$' signifies that the UAV was unable to maintain its position inside the desired workspace. '*' denotes that the maneuver was incomplete due to collision with the Wall or the inability of the MPC optimizer to find a valid control input to satisfy safe operation conditions. '$\downarrow$' denotes that a lower value of the performance metric is desired.}
\label{table4}
\end{table*}

\section{CONCLUSION}
This paper presented a BLF-based Model predictive controller with a primary objective of safe operation in the proximity of static objects.
Our approach shows how BLF, formulated for barrier avoidance and free space tracking objectives, shows robust behavior for the safe operation of an Aerial Manipulator in the presence of external disturbances.
A state-of-the-art MPC-based method is modified using two types of constraints and compared with the proposed method, which shows significant improvement in two cases i.e with and without external disturbances.
This is validated in simulation using parameters from a real laboratory scale AM.


\section{Introduction}
\begin{figure*}
\begin{center}
\includegraphics[width=0.8\textwidth]{Fig1.png}
\end{center}
\caption{The workflow employed in this work: the energies of a few hundred structures are computed with DFT; this database is divided into training and validation sets; ten LRPs are fitted; Monte Carlo simulations are carried out for each potential; the final composition of the surface is obtained by averaging the results of the ten simulations. Snapshots from the Monte Carlo simulations may be added to the DFT database for reinforcement.}
\label{workflow}
\end{figure*}

\begin{figure}[b]
\begin{center}
\includegraphics[width=0.3\textwidth]{Fig2.png}
\end{center}
\caption{Schematics of the fcc(111) slab used in the Monte Carlo simulations. The vacuum is treated as an additional species, here in white, so that the supercell maintains a fcc structure. Two possible Monte Carlo moves are pictured by arrows. Opaque atoms indicate the swapping atoms and their nearest neighbors.}
\label{structure}
\end{figure}

Compositionally complex alloys (CCAs), with three or more metals in high concentration, could be  game changers in heterogeneous catalysis, showing much higher catalytic activities than standard rule-of-mixtures would predict \cite{yao2018carbothermal,loffler2018discovery,loffler2019toward,batchelor2019high,sun2021high}. Compositional complexity, however, makes it hard to understand the catalytic processes of these alloys in terms of atomistic mechanisms. In fact, the kinetics of a chemical reaction strongly depend on the composition of a catalyst's surface, which, for multicomponent alloys, may be very different from the bulk: one or more elements may have a low surface energy and/or a weak binding to the other elements (high chemical potential) and hence segregate to the surface \cite{wynblatt2019modeling,ferrari2020surface,ferrari2020frontiers,ferrari2021design,ruban2021segregation,chatain2021surface}. As a result, the catalytic activity of an alloy is ultimately determined by surface segregation, rather than directly by the nominal composition.

Surface segregation can be understood and controlled with atomistic simulations. If segregation enthalpies $\Delta H_\text{seg}$ are known, the surface composition may be predicted with mean-field models, such as the Langmuir-McLean equation \cite{mclean1957grain}.
Such mean-field models could be extended to multicomponent alloys \cite{ruban2021segregation}, but may fail when the bonding between certain pairs of elements is much  stronger than others and a significant degree of short-range ordering is present.

Monte Carlo simulations could directly incorporate eventual ordering effects. These simulations though require an accurate description of the interatomic interactions, which is difficult to obtain for multicomponent alloys. The most popular methods to parameterize these interactions for binary and ternary alloys are cluster expansion \cite{sanchez1984generalized,drautz2001spontaneous} and classical potentials \cite{daw1984embedded,baskes1992modified,foiles1987calculation}, both of which are, however, often impractical or not sufficiently accurate for alloys with more than three components because of the large number of parameters to be fitted. Effective interactions in slabs have also been computed with a perturbative approach based on the Coherent Potential Approximation \cite{soven1967coherent, gyorffy1972coherent}, called Generalized Perturbation Method (GPM) \cite{ducastelle1976generalized, schonfeld2019local,ruban2007theoretical}, but these calculations do not distinguish different local chemical environments and cannot incorporate relaxation energies.

In contrast, machine learning interatomic potentials have recently been shown to accurately describe the complex interactions occurring in bulk CCAs \cite{kostiuchenko2019impact,grabowski2019ab,jafary2019applying,kostiuchenko2020short,kormann2021b2,gubaev2021finite}. Among other formalisms \cite{hart2021-review-ML-for-alloys}, low-rank potentials (LRPs) \cite{shapeev2017accurate,kostiuchenko2019impact}, a type of \textit{on lattice} interaction model, have shone in terms of accuracy and efficiency in the description of local ordering effects in CCAs. LRPs essentially approximate the $m \times m \times ... \times m$, $n$-dimensional tensor that describes the interaction among $n$ atoms of species $1, ..., m$ with a tensor with a low rank $\bar{r}$, thereby reducing the number of free parameters from $m^n$ to $\text{O}\big(mn\bar{r}^2\big)$. LRPs are defined by specifying only two hyperparameters: the maximum rank $\bar{r}$ and the number of interacting neighbors $n$.

In this work we show that LRPs can accurately describe surface segregation mechanisms in CCAs. After benchmarking our approach against  literature data for the well-studied AgPd alloy, we compute the surface composition of the  $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ alloy. This alloy was experimentally shown to catalyze very efficiently the $\text{NH}_3$ decomposition reaction \cite{xie2019highly}, which may become key for the emerging hydrogen economy. Previous Monte Carlo calculations \cite{xie2019highly} on $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ nanoparticles with a Modified Embedded Atom Method (MEAM) potential \cite{baskes1992modified} showed no significant surface segregation. This appears surprising given the heterogeneity of the constituents in the alloy and may be due to the short annealing time employed in those calculations to simulate kinetic hindering. Based on the accurate machine learning interatomic potential, our simulations point out a significant Co segregation instead. This finding is corroborated by a simple $d$-band model.

We focused on the fcc(111) surfaces of AgPd and $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ because these are the most closely packed. To determine the temperature dependence of the surface concentration $c(T)$ of an element,  we employed the workflow in Fig.~\ref{workflow}. We first calculated the total energy of a few hundred slabs with Density Functional Theory (DFT) including magnetism and atomic relaxations. We independently fitted ten LRPs on the training set (80\% of the slabs) by minimizing the mean-square error on the energy and then validated our fits on the remaining 20\% of the slabs. The training and validation sets were shuffled for each potential. With the parametrized LRPs, we performed Monte Carlo simulations to determine the composition of the surface. We compared the outcomes for the ten potentials and took the average and standard deviation of the results. If the standard deviation was too large or if the potentials gave inconsistent results, snapshots were extracted from the Monte Carlo simulations and calculated with DFT to reinforce the fitting. For the LRPs, we fixed the interaction range to include only the nearest neighbors ($n=13$) and tested different maximum ranks $\bar{r}$. Independent calculations for the bulk $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ were performed using the same methodology.
\textcolor{black}{For each alloy a different potential has been fitted. We note that the methodology itself does not have a limitation with regards to the compositional range – a machine learning potential can be trained on any compositional range.  However, a particular instance of a trained machine learning potential is typically limited to the compositions it was trained on and the extrapolation error is \textit{a priori} not known.}

For the Monte Carlo simulations we adopted the setup in Fig.~\ref{structure}. To maintain the symmetry of an fcc lattice, we treated vacuum as an additional species. The PdAg and $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ then effectively became a ternary ($m=3$) and a senary ($m=6$) alloy, respectively. We considered nearest-neighbor swaps as our Monte Carlo moves, but we excluded swaps involving the ``vacuum atoms". For bulk calculations discussed later, a similar setup was chosen. For the technical details of the DFT and Monte Carlo simulations we refer to Sec.~\ref{methods}.  Our simulations fully account for relaxations effects at the surface and in the bulk.
We did not include atomic vibrations and volume changes in our Monte Carlo simulations because the differences of the vibrational and volume contributions to the free energy are usually negligible compared to segregation energy differences \cite{ferrari2020surface,ferrari2021design}.

\section{Results}
\subsection{Benchmark for AgPd}
\begin{figure}[h]
\begin{center}
\includegraphics[width=0.5\textwidth]{Fig3.png}
\end{center}
\caption{Excess concentration of Ag on the surface layer of AgPd as a function of temperature. Points are from literature: MF = Mean Field model with segregation energies calculated with DFT; GPM - MC = Monte Carlo with interaction energies from the Generalized Perurbation Method; EAM - MC = Monte Carlo with an Embedded Atom Method potential; Exp. = experimental (Auger Electron Spectroscopy). The inset shows a snapshot from a Monte Carlo simulation at 100 K, where the precipitation of the $\text{L}1_1$ phase is evident.}
\label{PdAg}
\end{figure}

\begin{table}[h]
    \centering
    \begin{tabular}{cc}
    \hline
         & Surface Energy (mJ/m$^2$) \\
    \hline
        Ag & 760 \\
        Pd & 1360 \\
    \hline
        Cu & 1340  \\
        Ni & 1920  \\
        Co & 2110  \\
        Fe & 2450  \\
        Mo & 2780  \\
    \hline
    \end{tabular}
    \caption{Formation energies of surfaces for the pure elements from the Materials Project database \cite{Jain2013}. The surface with the lowest formation energy for the most stable structure was considered for each element. }
    \label{surface_energies}
\end{table}

For the AgPd system we reached training and validation errors as low as 0.4 meV/at.~for a set of LRPs with maximum rank $\bar{r}=3$. The resulting surface composition can be deduced from Fig.~\ref{PdAg}, which shows the excess concentration of Ag on the surface layer with respect to the bulk averaged over our ensemble of ten LRPs. Surface segregation is very prominent in this case, since there is Ag enrichment even at a temperature as high as 3000 K, way beyond the melting temperature (if it were possible to keep the alloy from melting; note that the utilized on-lattice approach obviously cannot capture the solid-liquid transition). The segregation is mainly driven by the much lower surface energy of Ag with respect to Pd (see Tab.~\ref{surface_energies}).

The obtained results compare very well to previous investigations, where the Ag concentration was estimated with a mean-field model
\cite{ropo2005segregation,ropo2006chemical} as well as with Monte Carlo simulations based on interactions determined with the Generalized Perturbation Method \cite{ruban2007theoretical} or with an Embedded Atom Method potential \cite{foiles1987calculation}. Good agreement is also found with the experimental data of Ref.~\onlinecite{reniers1994surface} refined by Ref.~\onlinecite{ropo2005segregation}.

In agreement with other works \cite{muller2001first,sluiter2006ab,ruban2007theoretical} we observed the emergence of long-range ordering at low temperature, with Ag and Pd layers alternating along the $[111]$ direction (see inset of Fig.~\ref{PdAg}). This is due to the known formation of the $\text{L}1_1$ phase in this alloy. Note that this phase was not part of the training set of our LRPs, but it was nevertheless correctly identified as ground state.

\subsection{The $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ alloy}
\begin{table}[h]
    \begin{center}
    \begin{tabular}{c|cc|cc}
    \hline
         & \multicolumn{2}{c|}{Train Err. (meV/at.)} & \multicolumn{2}{c}{Valid.~Err. (meV/at.)} \\
         $\bar{r}$& Surf. & Bulk & Surf. & Bulk \\
         \hline
        3 & 4.2 & 3.5 & 4.5 & 4.9\\
        4 & 3.5 & 2.7 & 3.9 & 4.3\\
        5 & 3.0 & 2.1 & 4.1 & 4.2\\
    \hline
    \end{tabular}
    \end{center}
    \caption{Average fitting and validation root-mean-squared errors for the LRPs for the $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ system for the surface segregation and bulk simulations.}
    \label{error_LRP}
\end{table}

\begin{figure}
\begin{center}
\includegraphics[width=0.5\textwidth]{Fig4.png}
\end{center}
\caption{Excess concentration of the component elements on the surface layer of $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ as a function of temperature. The shaded areas indicate an interval of one standard deviation obtained over ten fitted potentials. Dashed lines correspond to the nominal bulk concentrations. Points are from a simple $d$-band model (see Sec.~\ref{dmodel}).}
\label{CoCuFeMoNi}
\end{figure}

For the $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ system we tested different values of the maximum rank $\bar{r}$, as reported in Tab.~\ref{error_LRP}. We found that optimal values for this alloy are $\bar{r}=4$ or $\bar{r}=5$, yielding training and validation errors around 3--4 meV/at.; in the following we report the results obtained with $\bar{r}=5$.

The excess concentrations of the component elements on the surface of $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ with respect to the bulk are displayed as solid lines in Fig.~\ref{CoCuFeMoNi} as a function of temperature. The shaded areas indicate an interval of one standard deviation obtained over ten fitted potentials. In contrast to Ref.~\onlinecite{xie2019highly}, we see a large Co segregation in this system that, similarly as for the AgPd system, persists up to very high temperature.

\section{Discussion}
\subsection{Why does Co segregate?}
\begin{figure*}
\begin{center}
\includegraphics[width=1.0\textwidth]{Fig5.png}
\end{center}
\caption{a) Bulk short-range order parameters for the first neighbor shells for the element pairs involving Co as a function of temperature. A negative value indicates favoured pairs. The shaded areas denote an interval of one standard deviation obtained over ten fitted potentials. b) A snapshot of the bulk $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ at 1000 K. Co atoms are highlighted in blue.}
\label{SRO}
\end{figure*}

The segregation of Co is at first quite unexpected, since it is not the element with the lowest surface energy among the five components (see Tab.~\ref{surface_energies}). In fact, this phenomenon originates in the bulk: we observed that in this alloy Co has a low affinity for the other elements and at low temperature it even shows a marked tendency for demixing. The surface acts therefore as a sink for the Co that escapes from the bulk.

To analyze in detail this aspect, we fitted another set of LRPs on bulk fcc structures for $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$, with a procedure analogous to that used for the (111) surface. The average training and validation errors for this new set of LRPs are reported in Tab.~\ref{error_LRP}. With the fitted potentials, we studied the bonding and eventual ordering modes in the bulk. The tendency of Co demixing in the bulk is confirmed by the analysis of the short-range order (SRO) parameter between nearest neighbor pairs, computed as
\begin{equation}
    \text{SRO}_{ij}=1-\frac{p_{ij}}{c_ic_j},
\end{equation}
where $p_{ij}$ is the observed probability of finding atom $j$ in the nearest neighborhood of $i$, and $c_i$ and $c_j$ are the bulk concentrations.
As shown in Fig.~\ref{SRO}a), the SRO parameter for the Co-Co pair is strongly negative, indicating overall attraction, whereas that for the other pairs is positive, indicating repulsion. According to our ensembles of potentials, the preferential bonding of Co to itself triggers even the precipitation of Co-rich domains at low temperature, as seen in Fig.~\ref{SRO}b). In this system, the segregation is therefore not driven by the lowest surface energy, but by the highest chemical potential (weakest bonding to the other elements).

\subsection{Co segregation emerges from a $d$-band effect}
\label{dmodel}
The mechanism that causes the segregation of Co descends from a purely electronic effect and can be understood in terms of  the so-called Friedel model \cite{sutton1993electronic,ruban1999surface}, that assumes a rectangular density of $d$-states with bandwidth $w$. According to this model, the binding energy of an alloy with $d$-band filling $f$ is
\begin{equation}
    E_\text{bind}=-5wf(1-f).
    \label{friedel}
\end{equation}

To demonstrate that for the $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ alloy the segregation of Co emerges from this simple $d$-band effect, we performed the same Monte Carlo calculations as before but with a simple pairwise interatomic potential that  depends only on the $d$-band fillings and $d$-bandwidths of the elements. We consider nearest-neighbor interactions only of type
\begin{equation}
    V_{ij}=-\frac{5}{6}w f_{ij} (1-f_{ij}),
    \label{d-band-model1}
\end{equation}
where
\begin{equation}
    f_{ij}=\frac{1-\sqrt{|1-2(f_i+f_j-2f_if_j)|}}{2},
    \label{d-band-model2}
\end{equation}
with $f_i$ and $f_j$ the band fillings of the pure elements and $w$ an average bandwidth (see Sec.~\ref{methods} for the details). Eqs.~\ref{d-band-model1} and \ref{d-band-model2} were derived so that the binding energy of a binary alloy reduces to Eq.~\eqref{friedel}.

The excess concentrations of the component elements on the surface of $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ obtained from this simple $d$-band model are shown with points in Fig.~\ref{CoCuFeMoNi}. The segregation trend observed with this model is qualitatively similar to that obtained with the LRP simulations, i.e.~there is a clear Co enrichment on the surface. For this simplified model we also notice a hint of a possible Fe segregation at low temperature.

Additional calculations with the $d$-band model, detailed in the Supplementary Material, show that the Co demixing and segregation depend strongly on the nominal composition of the bulk and that additions of Ni or Cu cause the segregation of these elements instead of Co. These results explain the segregation trend obtained with the LRPs with a $d$-band filling effect and fully validate our predictions in terms of fundamental transition-metals chemistry.

\bigskip

In summary, we investigated surface segregation mechanisms in a binary and a quinary alloy with machine-learning interatomic potentials. We observed a very good agreement with the literature for the binary alloy, but a previously unmentioned Co segregation in the quinary alloy. We motivated this result with a high bulk chemical potential of this element and justified it with a simple, parameter-free $d$-band model. The present work highlights the potential of  machine-learning interatomic potentials for simulating ordering and surface segregation  in CCAs.

\section{Methods}
\label{methods}

\subsection{DFT database}
To fit the LRPs for AgPd we employed a total of 200 slabs with size $4\times4\times7$, of which 100 are random configurations and 100 were Monte Carlo snapshots added later for reinforcement. For $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ we instead employed a total of 591 slabs,  of which 507 with size $2\times2\times10$ and 84 with size $4\times4\times10$ (we mixed different supercell sizes to avoid possible spurious periodic interactions). The $4\times4\times10$ slabs and 100 of the $2\times2\times10$ slabs were random, the rest were extracted from Monte Carlo and added later to the fitting database. We also included ordered configurations where each element completely segregates. To fit the LRPs for the bulk $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ we employed 349 structures with size $5\times4\times4$, of which 138 were random and 211 were extracted from Monte Carlo. For the structures and energies used to fit the potentials see \url{https://github.com/AlbertoFerrari8/Segregation_LRP}.

For the DFT calculations we employed the {\sc vasp} 5.4 package \cite{kresse1993ab,kresse1996efficiency,kresse1996efficient} with Projector Augmented Wave potentials \cite{blochl1994projector,kresse1999ultrasoft}. We treated the exchange-correlation functional with the PBE approximation \cite{perdew1996generalized}. The energy cutoff was set to 400 eV and the width of the Methfessel-Paxton function \cite{methfessel1989high} for the electronic smearing to 0.1 eV. The $k$-point meshes were centered around $\Gamma$ and the points were distributed according to the Monkhorst-Pack grids \cite{baldereschi1973mean,monkhorst1976special} with a linear density of 0.1 $2\pi/\text{\AA}$. The calculations for $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ were spin-polarized. The initial magnetic moments were set to 2.0, 0.0, 3.0, 0.0, and 1.0 $\mu_\text{B}$. Around 20 \AA~of vacuum prevented the interaction between periodic slabs. For all supercells, we relaxed the atomic positions until the interatomic forces were less than 0.05 eV/\AA. The lattice parameter was fixed to 4.033 \AA ~or AgPd (the equilibrium value for a random alloy at 0 K) and to 3.776 \AA~for $\text{Co}_{25}\text{Cu}_{10}\text{Fe}_{10}\text{Mo}_{45}\text{Ni}_{10}$ (as in Ref.~\cite{xie2019highly}).

\subsection{Monte Carlo simulations}

The surface composition was obtained as an average of the two surfaces of a $10\times10\times40$ slab, whereas for the bulk simulations we employed a $20\times20\times20$ supercell. The Monte Carlo simulations were performed in the canonical ensemble. A Monte Carlo move consisted in the swap of two nearest neighbor atoms. Starting from a random configuration, we first performed $1\cdot10^{2}$ steps/atom for equilibration at 5000 K and then began averaging, first at 5000 K and then decreasing the temperature by 100 K at each cycle. Averages were taken on 100 snapshots $1\cdot10^{2}$ steps/atom apart, so that $1\cdot10^{4}$ steps/atom were taken in total for each temperature. Only two vacuum layers were considered in the Monte Carlo simulations for efficiency.

\subsection{$d$-band model}
The $d$-band fillings for the pure elements were computed by integrating the $d$-density of states in the fcc structure up to the Fermi level. These calculations were carried out with the exact muffin-tin orbital method \cite{vitos2000application, vitos2001total, vitos2007computational}. The fillings take the following values: Co: 0.76, Mo: 0.47, Fe: 0.65, Ni: 0.87, Cu: 0.96. We took the value of 4.2 eV for the bandwidth $w$, estimated as a weighted average of the bandwidths of Co, Fe, Ni, and Cu in the fcc structure at the lattice parameter of 3.776 \AA.

\section*{Acknowledgements}

AF and FK were supported by the Nederlandse Organisatie voor Wetenschappelijk Onderzoek (NWO) [VIDI Grant No.~15707]. Part of the calculations were conducted on the Dutch national e-infrastructure with the support of SURF Cooperative.
VS and AVS were supported by the Russian Science Foundation (Grant No 18-13-00479, \url{https://rscf.ru/project/18-13-00479/}).

\section*{Data Availability}

Derived data supporting the findings of this study are available from the corresponding author upon request.

\section*{Author Contributions}
AF and FK designed the research and performed the DFT calculations. VS and AVS developed the code for the LRP parametrization and Monte Carlo simulations. AF performed the fitting, the Monte Carlo simulations, and the post-processing. All authors discussed the results and wrote the article.

\section*{Competing Interests}
The authors declare no competing interests.