1.5pt

# Motion Planning for Aerial Pick-and-Place based on Geometric Feasibility
Constraints

Huazi Cao  Jiahao Shen  Cunjia Liu  Bo Zhu  Shiyu Zhao H. Cao, J. Shen, and S.
Zhao are with the School of Engineering at Westlake University, Hangzhou,
China<EMAIL_ADDRESS>Liu is with the
Department of Aeronautical and Automotive Engineering at Loughborough
University, Loughborough, UK<EMAIL_ADDRESS>Zhu is with the School of
Aeronautics and Astronautics at Sun Yat-sen University, Guangzhou, China.
<EMAIL_ADDRESS>
###### Abstract

This paper studies the motion planning problem of the pick-and-place of an
aerial manipulator that consists of a quadcopter flying base and a Delta arm.
We propose a novel partially decoupled motion planning framework to solve this
problem. Compared to the state-of-the-art approaches, the proposed one has two
novel features. First, it does not suffer from increased computation in high-
dimensional configuration spaces. That is because it calculates the
trajectories of the quadcopter base and the end-effector separately in the
Cartesian space based on proposed geometric feasibility constraints. The
geometric feasibility constraints can ensure the resulting trajectories
satisfy the aerial manipulator’s geometry. Second, collision avoidance for the
Delta arm is achieved through an iterative approach based on a pinhole mapping
method, so that the feasible trajectory can be found in an efficient manner.
The proposed approach is verified by three experiments on a real aerial
manipulation platform. The experimental results show the effectiveness of the
proposed method for the aerial pick-and-place task.

###### Note to Practitioners

Aerial manipulators have attracted increasing research interest in recent
years due to their potential applications in various domains. In this paper,
we particularly focus on the motion planning problem of the pick-and-place of
aerial manipulators. We propose a novel partially decoupled motion planning
framework, which calculates the trajectories of the quadcopter base and the
end-effector in Cartesian space, respectively. Geometric feasibility
constraints are proposed to coordinate the trajectories to ensure successful
execution. Three experiments on a real aerial manipulator platform demonstrate
the effectiveness of the approach. In future research, we will address the
motion planning problem of aerial manipulators in complex environments.

###### Index Terms:

Aerial manipulator, Delta arm, Aerial pick-and-place, Motion planning,
Collision avoidance

##  I Introduction

An aerial manipulator is a novel type of flying robot that consists of a
multirotor and a robotic arm. Due to their ability to move quickly and operate
precisely in high-altitude and complex workspaces, the aerial manipulator has
potential applications in various domains, including transportation,
inspection, and maintenance (see [1, 2, 3, 4] for recent surveys).

Aerial manipulation has been studied from various aspects such as platform
design [5, 6, 7], motion control [8, 9, 10], motion planning [11, 12, 13] and
visual servoing [14, 15, 16, 17] up to now. Our work focuses on the motion
planning problem of aerial pick-and-place tasks, where the aerial manipulator
is required to grasp and move objects in the environment (see Fig. 1). It is
noted that safety and high efficiency are important to the aerial pick-and-
place task. This motivates our study to focus on an effective motion planning
scheme that ensures collision-free trajectories.

Figure 1: Aerial pick-and-place by an aerial manipulator. The experimental
video is available at https://youtu.be/q7O9v7l2Oho.

Different from the motion planning of multirotors, the motion planning of an
aerial manipulator is more challenging since the aerial manipulator has more
degrees of freedom and is required to manipulate the objects. Different from
the motion planning of a ground mobile manipulator, the motion planning of an
aerial manipulator is more challenging since the aerial manipulator flies in a
3D environment rather than a 2D environment. In addition, the robotic arm and
the multirotor base are dynamically coupled, which means their movements
mutually affect each other. Existing approaches for motion planning for aerial
manipulation can be classified into two categories based on the space in which
planners calculate trajectories.

The first category is to plan the motion of the aerial manipulator in the
configuration space. In early works, the RRT* method has been used to plan the
path of the aerial manipulator in the configuration space without considering
the dynamics [11, 12]. As a consequence, the resulting trajectory may not be
executable for the aerial manipulator when its movement is fast. To address
this issue, the dynamics of the aerial manipulator must be considered in
motion planning. The existing methods that consider the dynamics in motion
planning can be classified into three types.

The first type uses a kinematics controller as a local planner in the
sampling-based global planner [13]. It guarantees the feasibility of the
trajectory for the real system and also enables searching for a solution
directly in the reduced and more relevant task space. However, collision
avoidance is not inherently embedded in the local planning, which may cause
its result is not collision-free. The second type uses the differential
flatness principle to ensure the dynamical feasibility [18]. In particular,
motion planning methods for a special long-reach aerial manipulator have been
proposed in [19, 20] based on this point of view. The platform in these works
consists of a multirotor with a long bar extension that incorporates a
lightweight dual arm in the tip. Since the dynamical feasibility constraints
represented by the differential flatness are nonlinear, this type of method
may be computationally expensive. The third type uses trajectory generation to
ensure the dynamical feasibility [21]. In the trajectory generation, the
trajectories are represented by spline curves. The dynamical feasibility
constraints are considered in the trajectory generation problem by utilizing
the derivative property of the spline curves. Planning in the configuration
space, however, suffers high computation costs when the dimension of the space
is high [22]. Unfortunately, aerial manipulators generally have high degrees
of freedom (DoFs), which therefore motivates researchers to study other
approaches to solve the motion planning problem.

The second category of approaches directly plans the trajectory of the end-
effector in the Cartesian space. The motion planning of the whole aerial
manipulator is often solved practically by decoupling the flying base and the
manipulator [23]. Firstly, the trajectory of the flying base approaching the
manipulation position is calculated. Then, the motion of the end-effector is
planned by assuming that the flying base stays in the same pose during
manipulation. However, this method is conservative and inefficient in terms of
energy and execution time [1]. To address this issue, the dynamic feasibility
constraint must be considered in the trajectory planning of the end-effector.
Therefore, a dynamically feasible task space planning method for underactuated
aerial manipulators based on the differential flatness principle has been
proposed in [24]. However, this method does not consider obstacle avoidance
which is generally required in real scenarios.

Figure 2: Coordinates, revised workspace, and shape polyhedron of the aerial
manipulator.

The above analysis reveals the limitations of the existing motion planning
approaches for aerial manipulators. Planning in the configuration space incurs
high computational costs due to the high DoF of aerial manipulators, while the
existing methods of planning in Cartesian space do not consider obstacle
avoidance, a crucial factor in real-world scenarios. To address these
limitations, this paper proposes a novel framework that integrates the motion
planning of both the flying base and the manipulator in a constrained
workspace. The proposed algorithm is designed for an aerial manipulator
consisting of a quadcopter and a Delta arm. The novelty of our approach is
outlined below:

1) We propose a novel partially decoupled motion planning method for the
aerial pick-and-place task. This method calculates the dynamically feasible
and collision-free trajectories of the flying base and the manipulator in
Cartesian space, respectively. The resulting trajectories are coordinated for
successful execution. By solving the motion planning problem in Cartesian
space, the high DoF of the aerial manipulator can be handled more efficiently
than planning in the configuration space with a much lower computational load.
Compared with the existing methods that plan trajectories in the configuration
space, this method does not suffer from the problem of increased computation
in high-dimensional configuration spaces. Compared with the existing methods
that plan trajectories in Cartesian space, the proposed method ensures that
the trajectories are collision-free.

2) We propose novel geometric feasibility constraints to ensure the
trajectories of the quadcopter and the end-effector can be successfully
executed. Our proposed constraints are linearly represented by the positions
of the quadcopter and the end-effector, whereas the original geometry
constraints are nonlinearly represented by the configuration of the aerial
manipulator. By using the constraints, our method ensures that the resulting
trajectories satisfy the geometry of the aerial manipulator. This is
particularly important for motion planning of the aerial manipulator in
Cartesian space.

3) Collision avoidance for the Delta arm is achieved through an efficient
iterative approach based on a pinhole mapping method. At each iteration, a
quadratic programming (QP) problem is solved to determine the collision-free
trajectory for the end-effector. A collision avoidance term, designed based on
the pinhole mapping method and collision check results, is formulated into the
QP problem, so that the aerial manipulator is driven away from the obstacles
in the local environment. Compared to collision avoidance in the configuration
space [21, 18], the proposed iterative approach is faster as it is computed in
Cartesian space.

The proposed algorithms are verified by three experiments on an aerial
manipulator platform in the real aerial pick-and-place task. Unlike the
traditional Delta arm, the Delta arm used in this paper drives the joint
angles by three four-bar linkages to magnify the control forces [25].
Experiments including collision avoidance, aerial retrieval, and aerial
transport are conducted to validate the novelties.

The rest of this paper is structured as follows. The problem statement and
preliminaries are given in Section II. Kinematics and geometric feasibility
constraints of the aerial manipulator are presented in Section III. The motion
planning of the quadcopter base is proposed in Section IV. Section V gives the
motion planning of the Delta arm. Then, the experimental verification is given
in Section VI. Conclusions are drawn in Section VII.

##  II Problem Statement and Preliminaries

###  II-A Problem statement

The platform is an aerial manipulator that consists of a quadcopter and a
Delta arm (see Fig. 2(a)). The base of the Delta arm is attached underneath
the quadcopter. The end-effector used in this paper is a gripper and it is
mounted on the end of the Delta arm, whose position can be controlled by the
three actuators attached to the base of the Delta arm. The orientation of the
end-effector is set the same as the orientation of the quadcopter [26].

The aerial manipulator has three reference frames: the inertial frame
$\varSigma_{I}$, the quadcopter body-fixed frame $\varSigma_{B}$, and the
Delta arm frame $\varSigma_{D}$ (see Fig. 2(a)). $\varSigma_{I}$ is an
inertial frame where the $z$-axis is in the direction of the gravity vector.
$\varSigma_{B}$ is rigidly attached to the quadcopter base. Its origin
coincides with the center of gravity of the quadcopter. $\varSigma_{D}$ is
rigidly attached to the Delta arm base at its geometric center $\bm{p}_{C}$.

Let $\bm{p}_{B}\in\mathbb{R}^{3}$ and $\bm{R}_{B}\in SO(3)$ denote the
position of the quadcopter in $\varSigma_{I}$ and the rotation matrix from
$\varSigma_{B}$ to $\varSigma_{I}$, respectively. Let
$\bm{p}_{E}\in\mathbb{R}^{3}$ denote the position of the end-effector in
$\varSigma_{I}$. Then, the geometric relationship between $\bm{p}_{E}$ and
$\bm{p}_{B}$ can be represented as

$\bm{p}_{E}-\bm{p}_{B}=\bm{R}_{B}\bm{p}_{E}^{B},$ (1)

where $\bm{p}_{E}^{B}\in\mathbb{R}^{3}$ is a function of the Delta arm’s
actuated joint angles $q_{1},q_{2},q_{3}$.

For a pick-and-place task, denote $\bm{p}_{O}\in\mathbb{R}^{3}$ and
$\psi_{O}\in\mathbb{R}$ as the position and the orientation of the target
object in $\varSigma_{I}$, respectively, whereas $\psi_{E}\in\mathbb{R}$
denotes the orientation angle of the end-effector. Let $t_{G}$ denote the time
that $\bm{p}_{E}$ arrives at $\bm{p}_{O}$ and $t_{\text{grip}}$ the closing
time of the gripper.

The goal of the motion planning for the aerial pick-and-place is to calculate
the collision-free trajectories for the quadcopter and the Delta arm to move
from a starting position to a feasible grasping configuration and from that
grasping configuration to the end position. Given the geometric relationship,
dynamical feasibility constraints, obstacles in the environment, the start
position $\bm{p}_{B,\text{start}}$, and the end position
$\bm{p}_{B,\text{end}}$, the resulting trajectories must be collision-free and
satisfy that $\bm{p}_{E}(t)=\bm{p}_{O}$ and $\psi_{E}(t)=\psi_{O}$ when
$t\in[t_{G},t_{G}+t_{\text{grip}}]$.

###  II-B Overview of the proposed motion planning method

Figure 3: Structure of the proposed motion planning method for aerial pick-
and-place.

The proposed motion planning method is partially decoupled, which calculates
the trajectories of the quadcopter base $\bm{p}_{B}(t)$ and the end-effector
$\bm{p}_{E}(t)$ in Cartesian space, respectively. The geometric feasibility
constraints are proposed to coordinate the trajectories to ensure successful
execution (see Section III-B for details). The overall architecture of the
motion planning and control system is shown in Fig. 3. The system is
decomposed into three components.

1) The first component is the motion planning of the quadcopter base. Its
inputs are the positions of the object and the obstacles. Its output is the
trajectory of the quadcopter base $\bm{p}_{B,\text{ref}}(t)$. The motion
planning of the quadcopter base can be further decomposed into four steps. The
first step is feasible grasping position calculation. Its role is to find a
suitable position for the quadcopter base to allow the aerial manipulator to
grasp the object. The details of this step can be seen in Section IV-A. The
second step is path planning. Its role is to find a path for the quadcopter
base to move from a given starting position to a feasible grasping position
and from that grasping position to a given end position. In this paper, we use
the A* method to calculate the path [27, Section 12.1.1]. The third step is
flight corridor generation. Its role is to generate a safe flight corridor for
the quadcopter base which constrains the motion of the quadcopter base to
avoid collisions. The details of this step can be seen in Section IV-B. The
fourth step is trajectory generation. Its role is to calculate the trajectory
of the quadcopter base based on the piecewise Bézier curve. We use the method
proposed in [28] to ensure the resulting trajectory satisfies the safety, the
dynamical feasibility, and the waypoints constraints. Compared with the
existing methods of aerial manipulators (e.g., [21]), the proposed method
calculates the trajectory of the quadcopter base in Cartesian space. Compared
with the existing methods of the standard quadcopter (e.g., [28]), the
proposed method guarantees the aerial manipulator arrives at the feasible
grasping configuration without collisions.

2) The second component is the motion planning of the Delta arm. Its inputs
are the position of the object $\bm{p}_{O}$ and $\bm{p}_{B,\text{ref}}(t)$.
Its output is the trajectory of the end-effector $\bm{p}_{E,\text{ref}}(t)$.
This motion planning method can be further decomposed into three steps. The
first step is the initial condition calculation. Its role is to calculate the
position, velocity, and acceleration of the end-effector at the beginning of
the manipulation stage. The second step is the optimal trajectory planning of
the end-effector based on the Bézier curve. Its role is to calculate the
trajectory of the end-effector from the initial position to the object with
several constraints. The trajectory planning of the end-effector is
represented as a QP problem form. In particular, we propose geometric
feasibility constraints of the aerial manipulator and encode this constraint
into the QP problem to ensure the trajectories satisfy the geometry of the
aerial manipulator. The third step is collision avoidance. It is important and
its role is to ensure the trajectory of the end-effector is collision-free. In
this step, the collisions between the aerial manipulator and the obstacles in
a local map are detected based on the GJK method. The second and third steps
are run iteratively. If there is a collision, then the objective function of
the QP problem in the second step is updated by a pinhole mapping method.
Repeat the second and third steps until no collision occurs. All the
corresponding sections introducing these steps are listed in Fig. 3. Compared
with the existing methods [21, 29], the proposed method requests lower
computational power since the collision avoidance of the Delta arm is achieved
by an iterative approach in Cartesian space.

3) The third component is the controller of the aerial manipulator. Its inputs
are the trajectories of the quadcopter base and the end-effector. Its outputs
are total force $f\in\mathbb{R}$ of the rotors, torque vector
$\bm{\tau}\in\mathbb{R}^{3}$ of the rotors, torque $\tau_{G}\in\mathbb{R}$ of
the gripper, and the torque vector that each actuator should generate
$\bm{\tau}_{M}\in\mathbb{R}^{3}$. The controller consists of three
subcomponents. The first step is an extended state observer (ESO) -based
flight controller. It was proposed in our previous work [30] and uses ESOs to
estimate dynamic coupling between the aerial manipulator and the Delta arm.
Its role is to generate the force $f$ and torque vector $\bm{\tau}$ for the
quadcopter base so that the trajectory of the quadcopter can be tracked. The
second step is the end-effector controller. Its role is to control the gripper
to grasp or release objects. The third step is the Delta arm controller. Its
role is to generate the torque vector $\bm{\tau}_{M}$ for the Delta arm so
that the trajectory of the end-effector can be tracked. The details of the
Delta arm controller can be seen in our previous work [30].

The steps can be classified into offboard processes and onboard processes. In
Fig. 3, steps in the small grey rectangle are done on an offboard computer,
while processes in the white rectangle run onboard the aerial manipulator
during flights.

###  II-C preliminaries to Bézier curves

A $n$-th degree Bézier curve is defined by a set of control points and
Bernstein polynomial bases. Let
$\bm{c}_{i}\in\mathbb{R}^{3},b_{i,n}(\tau)\in\mathbb{R}$ denote the $i$-th
control point and Bernstein polynomial basis, respectively. Then, the $n$-th
degree 3D Bézier curve is written as
$\bm{B}(\tau)=\sum_{i=0}^{i=n}\bm{c}_{i}^{T}b_{i,n}(\tau)$, where

$b_{i,n}(\tau)=\left(\begin{array}[]{c}n\\\ i\\\
\end{array}\right)\tau^{i}(1-\tau)^{n-i},$ (2)

where $\tau\in[0,1]$, $\left(\begin{smallmatrix}n\\\ i\\\
\end{smallmatrix}\right)$ is the binomial coefficient. According to [31,
Section 2.4], the derivative of the Bézier curve can be obtained by Lemma 1.
In addition, the Bézie curve B(t) is entirely confined within the convex hull
defined by all these control points, which is referred to as the convex hull
property (see Lemma 2).

######  Lemma 1 (Derivative [31])

Let $\bm{B}^{(k)}(\tau)=\sum_{i=0}^{n-j}\bm{c}_{i}^{(k)}b_{i,n-j}(\tau)$
denote the $k$-th derivative of $\bm{B}(\tau)$, then the control points of
$\bm{B}^{(k)}(\tau)$ can be calculated iteratively by
$\bm{c}_{i}^{(k)}=(n-k+1)(\bm{c}_{i+1}^{(k-1)}-\bm{c}_{i}^{(k-1)})$, where
$i=0,1,\cdots,n-j$.

######  Lemma 2 (Convex hull property [31])

Let
$\mathbb{H}=\\{a_{0}\bm{c}_{0}+a_{1}\bm{c}_{1}+\cdots+a_{n}\bm{c}_{n}|a_{0}+a_{1}+\cdots+a_{n}=1,a_{i}\geq
0\\}$ denote the convex hull defined by all the control points, then
$\bm{B}(\tau)\in\mathbb{H}$ for all $\tau\in[0,1]$.

##  III Kinematics and Geometric Feasibility Constraints

This section proposes the kinematics and geometric feasibility constraints of
the aerial manipulator.

###  III-A Kinematics of the aerial manipulator

According to (1), the time derivative of $\bm{p}_{E}$ is

$\begin{split}\dot{\bm{p}}_{E}=&\dot{\bm{p}}+\dot{\bm{R}}_{B}\bm{p}_{E}^{B}+\bm{R}\dot{\bm{p}}_{E}^{B}\\\
=&\dot{\bm{p}}+\bm{R}_{B}\bm{R}_{D}^{B}\dot{\bm{p}}_{E}^{D}-[\bm{R}_{B}\bm{p}_{E}^{B}]_{\times}\bm{\omega},\\\
\end{split}$ (3)

where $\bm{\omega}\in\mathbb{R}^{3}$ is the angular velocity vector of the
quadcopter expressed in $\varSigma_{B}$, and $[\cdot]_{\times}$ denotes the
skew-symmetric matrix.

Let $\bm{p}_{C}^{B}\in\mathbb{R}^{3}$ denote the position of the center of the
base in $\varSigma_{B}$. Let $\bm{p}_{E}^{B}\in\mathbb{R}^{3}$ and
$\bm{p}_{E}^{D}\in\mathbb{R}^{3}$ denote the positions of the end-effector in
$\varSigma_{B}$ and $\varSigma_{D}$, respectively. The relationship between
$\bm{p}_{E}^{B}$ and $\bm{p}_{E}^{D}$ is

$\bm{p}_{E}^{B}=\bm{R}_{D}^{B}\bm{p}_{E}^{D}+\bm{p}_{C}^{B},$ (4)

where $\bm{R}_{D}^{B}\in SO(3)$ is the rotation matrix from $\varSigma_{D}$ to
$\varSigma_{B}$.

The lengths for the upper and lower arms are represented by $l_{U}$ and
$l_{L}$ as illustrated in Fig. 2(a). Circumradius of the top base and the
bottom end-effector base are, respectively, defined as $r_{F}$ and $r_{M}$.
The length of the gripper is denoted as $l_{g}$. The relationship between the
end-effector position $\bm{p}_{E}^{D}$ and the joint vector
$\bm{q}=[q_{1},q_{2},q_{3}]^{T}\in\mathbb{R}^{3}$ is

$\left\|\bm{p}_{E}^{D}+\bm{l}_{G}-\bm{h}_{i}\right\|^{2}=l_{L}^{2},\quad
i=1,2,3,$ (5)

where $\bm{l}_{G}=[0,0,l_{g}]^{T}$, and

$\bm{h}_{i}=\left[\begin{array}[]{c}-(r_{F}-r_{M}+l_{U}\cos
q_{i})\cos[(i-1)\pi/3]\\\ (r_{F}-r_{M}+l_{U}\cos q_{i})\sin[(i-1)\pi/3]\\\
l_{U}\sin q_{i}\\\ \end{array}\right].$ (6)

On the one hand, given a joint vector $\bm{q}$, the position $\bm{p}_{E}^{D}$
can be solved from (5) based on the forward kinematics. On the other hand,
given a position $\bm{p}_{E}^{D}$, the joint vector $\bm{q}$ can be solved
from (5) by the inverse kinematics. Details can be found in [32, 33].

As can be seen from Fig. 2(a), the joint angles of the Delta arm are driven by
planar four-bar linkages. The relationship between the joint angles and the
crank position angles can be calculated by the kinematics of the planar four-
bar linkage [34, Section 3.6].

###  III-B Geometric feasibility constraints

Combining (1) and (4), the geometric relationship between the end-effector and
the quadcopter is

$\bm{p}_{E}-\bm{p}_{B}=\bm{R}_{B}(\bm{R}_{D}^{B}\bm{p}_{E}^{D}+\bm{p}_{C}^{B}),\quad\bm{p}_{E}^{D}\in\mathbb{W},$
(7)

where $\mathbb{W}$ is the workspace of the Delta and can be calculated by the
forward kinematics of the Delta arm. The workspace is approximated as a convex
polyhedron [35]. Therefore, the expression of the workspace is

$\mathbb{W}=\\{\bm{p}|\bm{A}_{D}\bm{p}\leq\bm{b}_{D}\\}.$ (8)

According to (7), the range of $\bm{p}_{E}-\bm{p}_{B}$ is determined by
$\mathbb{W}$ and $\bm{R}_{B}$. We define
$\bm{R}_{B}=\bm{R}_{\psi}\bm{R}_{\theta,\phi}$, where $\bm{R}_{\psi}$ is the
rotation matrix determined by the yaw angle $\psi$, $\bm{R}_{\theta,\phi}$ are
the rotation matrix determined by the pitch angle $\theta$ and roll angle
$\phi$. The yaw angle of the quadcopter is constant, i.e., $\psi=\psi_{O}$,
when the aerial manipulator is grasping or placing an object. Then, (7) is
rewritten as

$\bm{R}_{\psi_{O}}^{T}(\bm{p}_{E}-\bm{p}_{B})=\bm{R}_{\theta,\phi}(\bm{R}_{D}^{B}\bm{p}_{E}^{D}+\bm{p}_{C}^{B}),\quad\bm{p}_{E}^{D}\in\mathbb{W}.$
(9)

To make the above equation more concise, we define

$\mathbb{W}_{\theta,\phi}=\\{\bm{R}_{\theta,\phi}(\bm{R}_{D}^{B}\bm{p}_{E}^{D}+\bm{p}_{C}^{B})|\bm{p}_{E}^{D}\in\mathbb{W}\\}.$
(10)

Therefore, (9) is rewritten as
$\bm{R}_{\psi_{O}}^{T}(\bm{p}_{E}-\bm{p}_{B})\in\mathbb{W}_{\theta,\phi}$.

To linearize the geometric relationship (9), we define
$\mathbb{W}_{R}=\\{\bm{p}|\bm{w}_{\min}\leq\bm{p}\leq\bm{w}_{\max}\\}$ as the
revised workspace, and it satisfy that
$\mathbb{W}_{R}\subset\mathbb{W}_{\theta,\phi}$. Since the roll and pitch
angles of the quadcopter are small when the aerial manipulator is
manipulating, the bounds of $\theta$ and $\phi$ can be determined with several
experiments. Let $\theta_{\min},\theta_{\max}$ denote the minimum and maximum
of $\theta$. Let $\phi_{\min},\phi_{\max}$ denote the minimum and maximum of
$\phi$. We calculate $\mathbb{W}_{R}$ by two steps.

The first step is calculating boundaries of $\mathbb{W}_{\theta,\phi}$.
Combining (8) and (10), the expression of $\mathbb{W}_{\theta,\phi}$ can be
rewritten as

$\mathbb{W}_{\theta,\phi}=\\{\bm{p}|\bm{A}_{D}(\bm{R}_{\theta,\phi}\bm{R}_{D}^{B})^{T}\bm{p}\leq\bm{b}_{D}+\bm{A}_{D}\bm{R}_{D}^{BT}\bm{p}_{C}^{B}\\}.$
(11)

According to (11), we obtain the boundaries
$\mathbb{W}_{\theta=\theta_{\min},\phi=0}$,
$\mathbb{W}_{\theta=\theta_{\max},\phi=0}$,
$\mathbb{W}_{\theta=0,\phi=\phi_{\min}}$,
$\mathbb{W}_{\theta=0,\phi=\phi_{\max}}$.

The second step is calculating the intersection of these sets
$\mathbb{W}_{I}$. According to the definition of the intersection, we have

$\begin{split}\mathbb{W}_{I}=\\{\bm{p}|\bm{A}_{D}(\bm{R}_{\theta_{\min},0}\bm{R}_{D}^{B})^{T}\bm{p}&\leq\bm{b}_{D}+\bm{A}_{D}\bm{R}_{D}^{BT}\bm{p}_{C}^{B},\\\
\bm{A}_{D}(\bm{R}_{\theta_{\max},0}\bm{R}_{D}^{B})^{T}\bm{p}&\leq\bm{b}_{D}+\bm{A}_{D}\bm{R}_{D}^{BT}\bm{p}_{C}^{B},\\\
\bm{A}_{D}(\bm{R}_{0,\phi_{\min}}\bm{R}_{D}^{B})^{T}\bm{p}&\leq\bm{b}_{D}+\bm{A}_{D}\bm{R}_{D}^{BT}\bm{p}_{C}^{B},\\\
\bm{A}_{D}(\bm{R}_{0,\phi_{\max}}\bm{R}_{D}^{B})^{T}\bm{p}&\leq\bm{b}_{D}+\bm{A}_{D}\bm{R}_{D}^{BT}\bm{p}_{C}^{B}\\}.\end{split}$
(12)

Since the expression of the intersection (12) is complicated, it may be
inconvenient when applied to real systems. We let the largest cuboid that can
be inscribed within the intersection as $\mathbb{W}_{R}$. The cuboid can be
calculated by the method proposed in [36]. Then,
$\bm{w}_{\min}=[{w}_{x,\min},{w}_{y,\min},{w}_{z,\min}]^{T}$ and
$\bm{w}_{\max}=[{w}_{x,\max},{w}_{y,\max},{w}_{z,\max}]^{T}$ are subsequently
determined by the size of the cuboid. Fig. 2(b) gives an illustration for
calculating the revised workspace in the pitch direction. Then, the geometric
feasibility constraints are

$\bm{w}_{\min}\leq\bm{R}_{\psi_{O}}^{T}(\bm{p}_{E}-\bm{p}_{B})\leq\bm{w}_{\max},$
(13)

where

$\bm{R}_{\psi_{O}}=\left[\begin{array}[]{ccc}\cos\psi_{O}&-\sin\psi_{O}&0\\\
\sin\psi_{O}&\cos\psi_{O}&0\\\ 0&0&1\\\ \end{array}\right].$ (14)

##  IV Motion Planning for the Quadcopter base

This section presents a method to generate the trajectory of the quadcopter
for the aerial pick-and-place task. This method consists of four steps:
feasible grasping position, path planning, flight corridor generation, and
Bézier curve-based trajectory generation. The purposes and relationships of
these steps are given in Section II-B. In the algorithm, the path planning can
be achieved by the existing method. In our work, we use the A* method to
obtain the path in the 3D grid map which is used to represent the environment
of the task. The Bézier curve-based trajectory generation is achieved by an
existing method proposed in [28]. It bounds positions and higher order
dynamics of the trajectory entirely within safe regions by using Bernstein
polynomial basis and formulating the trajectory generation problem as typical
convex programs.

Compared to the existing methods for standard quadcopters [28], the proposed
method for the quadcopter base of the aerial manipulator has two novelties.
First, the feasible grasping position is calculated to ensure the aerial
manipulator can manipulate the object. Second, the volume of the aerial
manipulator changes with the movement of the Delta arm. To address this issue,
the aerial pick-and-place task is divided into two stages: moving and
manipulation stages. The flight corridors in the two stages are obtained,
respectively. The details are shown as follows.

###  IV-A Feasible grasping position

To grasp the object, the position of the end-effector must arrive at
$\bm{p}_{O}$ with an orientation angle of $\psi_{O}$. The feasible grasping
position of the quadcopter is constrained by the geometric shape of the aerial
manipulator. Let $\bm{p}_{B,f}\in\mathbb{R}^{3}$ denote the feasible grasping
position. Let $\bm{R}_{B,f}\in SO(3)$ denote the desired rotation matrix of
the quadcopter base at the feasible grasping position. According to (1), the
feasible grasping position of the quadcopter is

$\bm{p}_{B,f}=\bm{p}_{O}-\bm{R}_{B,f}\bm{p}_{E}^{B},$ (15)

According to (15), one can conclude that $\bm{R}_{B,f}$ and $\bm{p}_{E}^{B}$
need to be determine before calculating $\bm{p}_{B,f}$.

In the manipulation stage, the yaw angle of the quadcopter is set as
$\psi_{O}$ to satisfy the grasp angle constraint of the end-effector. We
assume that the roll and pitch angles of the quadcopter are small when the
quadcopter base is around $\bm{p}_{B,f}$. This assumption is reasonable since
the motion of the quadcopter is conservative. According to the assumption, we
have $\bm{R}_{B,f}=\bm{R}_{\psi_{O}}$. To ensure the manipulability of the
Delta arm, we let the end-effector stay at the center of $\mathbb{W}_{R}$ when
the aerial manipulator picks up the object. Then, (15) is rewritten as

$\bm{p}_{B,f}=\bm{p}_{O}-0.5\bm{R}_{\psi_{O}}(\bm{w}_{\min}+\bm{w}_{\max}).$
(16)

For an aerial pick-and-place task, we already have the start position
$\bm{p}_{B,\text{start}}$, feasible grasping position $\bm{p}_{B,f}$, and the
end position $\bm{p}_{B,\text{start}}$. Then, the path of the quadcopter can
be obtained by the A* method.

###  IV-B Flight corridor generation

The flight corridor is a collection of convex overlapping polyhedra that
models free space and provides a connected corridor containing the resulting
path. A convex decomposition method proposed in [37] is adopted to generate
the flight corridor by inflating the resulting path. However, this method was
originally designed for a traditional quadcopter with a fixed volume, while
the volume of the aerial manipulator changes with the movement of the Delta
arm. Therefore, the method cannot be directly used for the aerial manipulator.
To address this issue, we calculate the flight corridors in the moving and the
manipulation stages, respectively. In the moving stage, the position of the
end-effector is set to stay at the top point
$\bm{p}_{\text{top}}^{B}\in\mathbb{R}^{3}$ of the Delta arm’s workspace
$\mathbb{W}_{R}$. From the definition of $\mathbb{W}_{R}$, we have

$\bm{p}_{\text{top}}^{B}=\left[\Array{c}0.5(w_{x,\min}+w_{x,\max})\\\
0.5(w_{y,\min}+w_{y,\max})\\\ w_{z,\min}\right].$ (17)

The shape of the aerial manipulator now can be approximated as a sphere with a
radius $r_{S}$. Then, we can use the convex decomposition method to generate
the flight corridor in the moving stage.

Figure 4: An illustration for the motion planning of the aerial pick-and-
place.

In the manipulation stage, we use a designed polyhedron as the flight corridor
to ensure the object is reachable for the aerial manipulator (see Fig. 4). The
designed polyhedron is designed based on the geometric feasibility constraints
(13) and it is represented as

$\bm{w}_{\min}\leq\bm{R}_{\psi_{O}}(\bm{p}-\bm{p}_{B,d})\leq\bm{w}_{\max}.$
(18)

The duration time in this polyhedron is determined by the mechanical behavior
of the gripper. We set the duration time as the closing time of the gripper
$t_{\text{grip}}$.

##  V Motion Planning for The Delta Arm

In this section, we calculate the collision-free trajectory of the Delta arm
in the Cartesian coordinate. The proposed method for the Delta arm utilizes
the resulting trajectory of the quadcopter. In the moving stage, the Delta arm
stays at an initial state and its end-effector stays at a fixed position
$\bm{p}_{\text{top}}^{B}$ relative to the quadcopter base. The position
$\bm{p}_{\text{top}}^{B}$ can be calculated by (17). Therefore, the Delta arm
does not require additional motion planning calculations in the moving stage.

Let $t_{B}$ denote the time at which the quadcopter base enters the designed
polyhedron. Let $t_{B}$ denote the beginning time of the manipulation stage.
The time $t_{B}$ can be determined by $t_{B}=t_{G}-2v_{E,\max}/a_{E,\max}$,
where $v_{E,\max}$ and $a_{E,\max}$ are maximum velocity and acceleration of
the end-effector, respectively. The procedure of the manipulation stage is
given here. From $t_{B}$ to $t_{G}$, the end-effector moves to the object.
Then, the aerial manipulator keeps the position of the end-effector for the
duration time $t_{\text{grip}}$ to pick up or place the object. After picking
up or placing the object, the Delta arm returns to its initial state.
According to the procedure, one can find that the trajectory of the end-
effector from $t_{B}$ to $t_{G}$ needs to be calculated. The details to
calculate the trajectory from $t_{B}$ to $t_{G}$ are shown as follows.

###  V-A Initial condition

The initial condition for the end-effector consists of the initial position
$\bm{p}_{E,t_{B}}$, the initial velocity $\dot{\bm{p}}_{E,t_{B}}$, and the
initial acceleration $\ddot{\bm{p}}_{E,t_{B}}$. They are calculated as
follows.

####  V-A1 Initial position

According to (1), the initial position $\bm{p}_{E,t_{B}}$ is calculated by

$\bm{p}_{E,t_{B}}=\bm{p}_{B,t_{B}}+\bm{R}_{B,t_{B}}\bm{p}_{\text{top}}^{B},$
(19)

where
$\bm{p}_{B,t_{B}},\bm{R}_{B,t_{B}}=[\bm{r}_{1,t_{B}},\bm{r}_{2,t_{B}},\bm{r}_{3,t_{B}}]$
denote $\bm{p}_{B},\bm{R}_{B}$ at the time $t_{B}$, respectively. According to
(19), we calculate $\bm{p}_{B,t_{B}}$ and $\bm{R}_{B,t_{B}}$ to obtain
$\bm{p}_{E,t_{B}}$. $\bm{p}_{B,t_{B}}$ can be directly obtained by the
trajectory of the quadcopter.

The matrix $\bm{R}_{B,t_{B}}$ is calculated based on the differential flatness
of the quadcopter. At the time $t_{B}$, the yaw angle of the quadcopter base
is $\psi_{O}$ to ensure the orientation angle of the end-effector equals
$\psi_{O}$. The unit orientation vector in the ground plane is
$\bm{r}_{g}=[\cos\psi_{O},\sin\psi_{O},0]^{T}$. According to [38], we have

$\bm{r}_{3,t_{B}}=\frac{\ddot{\bm{p}}_{t_{k}}+g\bm{e_{3}}}{\|\ddot{\bm{p}}_{t_{B}}+g\bm{e_{3}}\|},$
(20)

and vectors $\bm{r}_{1,t_{B}}$ and $\bm{r}_{2,t_{B}}$ can be determined by

$\bm{r}_{2,t_{B}}=\frac{\bm{r}_{3,t_{B}}\times\bm{r}_{g}}{\|\bm{r}_{3,t_{B}}\times\bm{r}_{g}\|},\bm{r}_{1,t_{B}}=\bm{r}_{2,t_{B}}\times\bm{r}_{3,t_{B}}.$
(21)

####  V-A2 Initial velocity and acceleration

Let $\delta_{I}$ denote a small time step. We can calculate
$\bm{p}_{E,t_{B}-\delta_{I}}$ and $\bm{p}_{E,t_{B}+\delta_{I}}$ according to
the above method. Then, the derivatives are approximated by the differences.
The initial velocity and acceleration are

$\begin{split}\dot{\bm{p}}_{E,t_{B}}&=(\bm{p}_{E,t_{B}}-\bm{p}_{E,t_{B}-\delta_{I}})/\delta_{I},\\\
\ddot{\bm{p}}_{E,t_{B}}&=(\bm{p}_{E,t_{B}+\delta_{I}}-2\bm{p}_{E,t_{B}}+\bm{p}_{E,t_{B}-\delta_{I}})/\delta_{I}^{2}.\end{split}$
(22)

###  V-B Optimal trajectory planning

The trajectory of the end-effector is calculated by an iterative approach. The
trajectory planning of the end-effector is formulated as a QP problem. At each
iteration, the objective function of the QP problem is updated and the QP
problem is solved to calculate the collision-free trajectory of the end-
effector. Let $\bm{p}_{E,\text{ref}}(t),t\in[t_{B},t_{G}]$ denote the
trajectory. A $n_{E}$-th order Bézier curve is adopted to represent the
trajectory and it is

$\bm{p}_{E,\text{ref}}(\tau_{M})=\sum_{i=0}^{n_{E}}\bm{c}_{E,i}b_{i,n_{E}}(\tau_{M}),$
(23)

where
$\bm{c}_{E,i}=[{c}_{E,x,i},{c}_{E,y,i},{c}_{E,z,i}]^{T}\in\mathbb{R}^{3}$ and
${b}_{i,n_{E}}(\tau_{M})$ are the $i$-th control point and Bernstein
polynomial basis of the Bézier curve, respectively, and
$\tau_{M}=(t-t_{B})/(t_{G}-t_{B})$. We denote the parameter vector of the
trajectory as
$\underline{\bm{c}}_{E}=[{c}_{E,x,0},\ldots,{c}_{E,x,n_{E}},{c}_{E,y,0},\ldots,{c}_{E,y,n_{E}},{c}_{E,z,0},\ldots,{c}_{E,z,n_{E}}]^{T}$.
The QP problem is then formulated as

$\begin{split}\min\quad&J=\underline{\bm{c}}_{E}^{T}\bm{Q}_{O,E}\underline{\bm{c}}_{E}+\bm{q}_{O,E}^{T}\underline{\bm{c}}_{E}\\\
\text{s.t.}\quad&\bm{A}_{E,eq}\underline{\bm{c}}_{E}=\bm{b}_{E,eq},\\\
&\bm{A}_{E,ie}\underline{\bm{c}}_{E}\leq\bm{b}_{E,ie},\\\ \end{split}$ (24)

where $\bm{Q}_{O,E}\in\mathbb{R}^{3(n_{E}+1)\times 3(n_{E}+1)}$ is the Hessian
matrix of the objective function and it is semidefinite,
$\bm{q}_{O,E}\in\mathbb{R}^{3(n_{E}+1)}$ is a vector,
$\bm{A}_{E,eq}\in\mathbb{R}^{18\times 3(n_{E}+1)}$ and
$\bm{A}_{E,ie}\in\mathbb{R}^{(9n_{E}+4)\times 3(n_{E}+1)}$ are constraint
matrices, $\bm{b}_{eq}\in\mathbb{R}^{18}$ and
$\bm{b}_{ie}\in\mathbb{R}^{9n_{E}+4}$ are constraint vectors. The linear
equality constraint ($\bm{A}_{E,eq}=\bm{b}_{E,eq}$) is endpoint constraints.
The linear inequality constraint ($\bm{A}_{ie}=\bm{b}_{ie}$) consists of
dynamical feasibility, geometric feasibility, and grasp constraints. These
constraints are adopted to ensure the solution of the problem (24) is
collision-free and can be executed successfully. Definitions and roles of the
objective and the constraints are as follows.

####  V-B1 Objective function

The objective function is denoted as $J=J_{J}+J_{O}$, where $J_{J}$ is the
cost function to minimize the jerk along the trajectory, and $J_{O}$ is a
penalty function for the collision. The details of the two terms are

$J_{J}=\sum_{i=1}^{m}\int_{T_{i-1}}^{T_{i-1}}(j_{x}^{2}(t)+j_{y}^{2}(t)+j_{z}^{2}(t))dt,$
(25)
$J_{O}=\sum_{k=1}^{n_{O}}\lambda_{k}\sum_{i=0}^{n_{E}}(c_{i,x}-x_{M,k})^{2}+(c_{i,y}-y_{M,k})^{2}+(c_{i,z}-z_{M,k})^{2},$
(26)

where $j_{x},j_{y},j_{z}$ denote the jerks of the trajectory in the
corresponding three dimensions, respectively, $\lambda_{k}$ is a changing
weighting factor, $x_{M,k},y_{M,k},z_{M,k}$ are corresponding elements of the
obstacle mirror position $\bm{p}_{M,k}$. We define the obstacle mirror set as
$\mathbb{O}_{M}=\\{\bm{p}_{M,1},\bm{p}_{M,2},\ldots,\bm{p}_{M,n_{O}}\\}$,
where $n_{O}$ is the number of the obstacles that collide with the aerial
manipulator during the whole iteration process. The obstacle mirror position
$\bm{p}_{M,k}$ can be obtained through pinhole mapping of the corresponding
obstacle position. As the iterations progress, the algorithm can guide the
trajectory of the end-effector towards the obstacle mirror positions while
ensuring that it is collision-free for the obstacles. See Section V-C for
details of calculating the changing weighting factors
$\lambda_{1},\lambda_{2},\ldots,\lambda_{n_{O}}$ and the obstacle mirror set
$\mathbb{O}_{M}$. By using the Lemma 1 into (25), we can obtain
$J=\underline{\bm{c}}_{E}^{T}\bm{Q}_{o,E}\underline{\bm{c}}_{E}+\bm{q}_{o,E}^{T}\underline{\bm{c}}_{E}$.
We leave the details of the $\bm{Q}_{o,E}$ and $\bm{q}_{o,E}$ for brevity.

####  V-B2 Constraints

The constraints for the trajectory planning problem of the end-effector
consist of endpoint, dynamical feasibility, geometric feasibility, and grasp
constraints. The details of the constraints are given as follows:

The endpoint constraints are introduced to ensure the trajectory of the end-
effector starts at $\bm{p}_{E,t_{B}}$ and ends at $\bm{p}_{O}$ with desired
velocities and accelerations. The endpoint constraints are given as

$\begin{split}c_{0,\mu,E}&=\mu_{E,t_{B}},s_{E}^{-1}c_{0,\mu,E}^{(1)}=\dot{\mu}_{E,t_{B}},s_{E}^{-2}c_{0,\mu,E}^{(2)}=\ddot{\mu}_{E,t_{B}},\\\
c_{n_{E},\mu,E}&=\mu_{O},s_{E}^{-1}c_{n_{E},\mu,E}^{(1)}=\dot{\mu}_{E,t_{G}},s_{E}^{-2}c_{n_{E},\mu,E}^{(2)}=\ddot{\mu}_{E,t_{G}},\\\
\end{split}$ (27)

where $c_{i,\mu,E}^{(k)}$ denotes the $i$-th control point of
${d^{k}f_{\mu}(\tau)}/{d\tau^{k}}$ and can be calculated by Lemma 1,
$\mu\in[x,y,z]$, $s_{E}=t_{G}-t_{B}$,
$\bm{p}_{E,t_{B}},\dot{\bm{p}}_{E,t_{B}},\ddot{\bm{p}}_{E,t_{B}}$ can be
obtained from the Section V-A and
$\dot{\bm{p}}_{E,t_{B}}=0,\ddot{\bm{p}}_{E,t_{B}}=0$

The dynamical feasibility constraints consist of velocity and acceleration
constraints to ensure the generated trajectory is dynamically feasible. The
dynamical feasibility constraints are

$\begin{split}\dot{\mu}_{\min}\leq&s_{E}^{-1}c_{i,\mu,E}^{(1)}\leq\dot{\mu}_{\max},i=0,1,\ldots,n_{E}-1,\\\
\ddot{\mu}_{\min}\leq&s_{E}^{-2}c_{i,\mu,E}^{(2)}\leq\ddot{\mu}_{\max},i=0,1,\ldots,n_{E}-2,\\\
\end{split}$ (28)

where $\mu\in[x,y,z]$, the subscript ${\min}$ denotes the lower bound of the
corresponding variable, and the subscript ${\max}$ denotes the upper bound of
the corresponding variable.

The geometric feasibility constraints are introduced to ensure the
trajectories of the quadcopter and the end-effector are geometrically feasible
for the Delta arm. According to (13), it can be describe as

$\bm{R}_{\psi_{O}}\bm{w}_{\min}\leq\bm{p}_{E,\text{ref}}(t)-\bm{p}_{B,\text{ref}}(t)=\bm{R}_{\psi_{O}}\bm{w}_{\max}.$
(29)

As stated above, the trajectory of the end-effector is represented by a
$n_{E}$-th order Bézier curve. The trajectory of the quadcopter is part of a
$n_{B}$-th order Bézier curve.

To reveal the geometric feasibility constraints on the parameters, we use a
$n_{E}$-th order Bézier curve to fit the trajectory of the quadcopter from
$t_{B}$ to $t_{G}$. Then, the geometric feasibility constraints on the
parameters can be formulated as linear algebraic equations. Let
$\bm{p}_{B,0},\bm{p}_{B,1},\ldots,\bm{p}_{B,n_{E}}$ denote $n_{E}+1$ points of
the trajectory $\bm{p}_{B,\text{ref}}(t),t\in[t_{B},t_{G}]$. These points
divide the trajectory into $n_{E}$ segments. The time interval between each
two adjacent points is the same. The $n_{E}$-th order Bézier curve is denoted
as $\bm{h}(t)=\sum_{i=0}^{n_{E}}\bm{c}_{B,i}b_{i,n_{E}}(\tau_{M})$, where
$\bm{c}_{B,i}=[{c}_{B,x,i},{c}_{B,y,i},{c}_{B,z,i}]^{T}$ is the $i$-th control
point of $\bm{h}(t)$. The control points can be obtained by fitting
$\bm{h}(t)$ to the points $\bm{p}_{B,0},\bm{p}_{B,1},\ldots,\bm{p}_{B,n_{E}}$.
Then, the geometric feasibility constraints can be rewritten as

$\bm{R}_{\psi_{O}}\bm{w}_{\min}\leq\sum_{i=0}^{n_{E}}(\bm{c}_{E,i}-\bm{c}_{B,i})b_{i,n_{E}}(\tau_{M})\leq\bm{R}_{\psi_{O}}\bm{w}_{\max},\\\
$ (30)

According to the convex hull property (see Lemma 2), the geometric feasibility
constraints on the parameters is

$\begin{split}w_{r,\mu,\min}+c_{B,\mu,i}\leq&c_{E,\mu,i}\leq
w_{r,\mu,\max}+c_{B,\mu,i},\\\ &i=0,1,\ldots,n_{E},\end{split}$ (31)

where $w_{r,\mu,\min}$ is the element of $\bm{R}_{\psi_{O}}\bm{w}_{\min}$,
$w_{r,\mu,\max}$ is the element of $\bm{R}_{\psi_{O}}\bm{w}_{\max}$, and
$\mu\in\\{x,y,z\\}$.

The grasp constraints are introduced to ensure the gripper does not collide
with the object. To avoid such collision, we let the end of the trajectory in
a cone. Let $t_{C}$ denote the time to enter the cone. Let
$\bm{p}_{E,\text{ref}}(t_{C})=[x_{E,t_{C}},y_{E,t_{C}},z_{E,t_{C}}]^{T}$
denote the position of the end-effector at the time $t_{c}$. Then, we have

$\begin{split}-\tan\gamma\leq\frac{x_{E,t_{C}}-x_{O}}{z_{E,t_{C}}-z_{O}}&\leq\tan\gamma,\\\
-\tan\gamma\leq\frac{y_{E,t_{C}}-y_{O}}{z_{E,t_{C}}-z_{O}}&\leq\tan\gamma,\\\
\end{split}$ (32)

where $\gamma$ is the angle of the cone. By substituting the defined (23) into
(32), the grasp constraints (32) can be rewritten as a linear form

$\begin{split}\sum_{i=0}^{n_{E}}({c}_{E,x,i}+{c}_{E,z,i}\tan\gamma)b_{i,n_{E}}(\tau_{C})\leq&x_{O}+z_{O}\tan\gamma,\\\
\sum_{i=0}^{n_{E}}({c}_{E,x,i}-{c}_{E,z,i}\tan\gamma)b_{i,n_{E}}(\tau_{C})\geq&x_{O}-z_{O}\tan\gamma,\\\
\sum_{i=0}^{n_{E}}({c}_{E,y,i}+{c}_{E,z,i}\tan\gamma)b_{i,n_{E}}(\tau_{C})\leq&y_{O}+z_{O}\tan\gamma,\\\
\sum_{i=0}^{n_{E}}({c}_{E,y,i}-{c}_{E,z,i}\tan\gamma)b_{i,n_{E}}(\tau_{C})\geq&y_{O}-z_{O}\tan\gamma,\\\
\end{split}$ (33)

where $\tau_{C}=(t_{C}-t_{B})/(t_{G}-t_{B})$.

Figure 5: Results of the collision avoidance experiment. Figure 6: Results of
the aerial retrieval experiment. Figure 7: Results of the aerial transport
experiment.

###  V-C Collision avoidance

The subsection proposes a method to detect collisions and calculate the
changing weighting factors $\lambda_{1},\lambda_{2},\ldots,\lambda_{n_{O}}$
and the obstacle mirror set $\mathbb{O}_{M}$ in (26). Before the iteration
process, $\mathbb{O}_{M}$ is set as an empty set, i.e.,
$\mathbb{O}_{M}=\emptyset$, and $n_{O}$ is set as zero. At each iteration, the
collision is detected using the solution of the QP problem (24). If the
solution is collision-free, the iteration process is terminated and the
collision-free solution is outputted as the trajectory of the end-effector. If
there are collisions between the aerial manipulator and the obstacles in the
environment with the solution, we calculate the changing weighting factors and
$\mathbb{O}_{M}$ for the next iteration calculation.

The collision detection method is proposed to detect if the solution of the QP
problem (24) is collision-free. The method considers the collision of the
Delta arm and the end-effector. The trajectory of the quadcopter is collision-
free, which is ensured by the flight corridor. Therefore, we do not consider
the collision of the quadcopter. The proposed collision detection method
consists of three steps.

We first use a shape polyhedron to represent the Delta arm and the end-
effector in collision detection. The vertices of the shape polyhedron are
$\bm{p}_{U,i},\bm{p}_{L,i},i=1,2,3$ (see the blue points in Fig.2(c)). Then,
we have

$\bm{p}_{U,i}=\bm{p}_{B}+\bm{R}_{\psi}\bm{R}_{D}^{B}\tilde{\bm{p}}_{U,i},i=1,2,3,$
(34)

where

$\tilde{\bm{p}}_{U,i}=\left[\Array{c}r_{S}\cos(\frac{1+2i}{3}\pi)\\\
r_{S}\sin(\frac{1+2i}{3}\pi)\\\ 0\\\ \right]$ (35)

In addition, the lower vertices can be calculated by

$\bm{p}_{L,i}=\bm{P}_{E}+\bm{R}_{\psi}\bm{R}_{D}^{B}\tilde{\bm{p}}_{L,i},i=1,2,3,$
(36)

where

$\tilde{\bm{p}}_{L,i}=\left[\Array{c}l_{C}\cos(\frac{1+2i}{6}\pi)\\\
l_{C}\sin(\frac{1+2i}{6}\pi)\\\ l_{C}\\\ \right]$ (37)

where $l_{C}$ is a constant parameter for safety and is determined by the size
of the gripper when the gripper is open.

Second, we introduce a local map to reduce the computational cost of the
collision detection. This is because detecting collisions in the entire
environment can be computationally expensive, especially if the environment is
large or if there are many obstacles. We decrease the number of obstacles to
be calculated by adding a box around the end-effector and the object and thus
only detecting collisions inside it. The size of the box is determined by
$\bm{p}_{B,t_{B}}$ and $\bm{p}_{O}$. We let
$\mathbb{M}_{\text{local}}=\\{\bm{p}\in\mathbb{R}^{3}|\bm{l}_{\min}\leq\bm{p}\leq\bm{l}_{\max}\\}$
denote the box. In addition, $\bm{l}_{\min}$ and $\bm{l}_{\max}$ can be
calculated by

$\begin{split}l_{\mu,\min}=\min\\{\mu_{B,t_{B}},\mu_{O}\\}-l_{s},\\\
l_{\mu,\max}=\max\\{\mu_{B,t_{B}},\mu_{O}\\}+l_{s},\\\ \end{split}$ (38)

where $\mu\in\\{x,y,z\\}$, $l_{\mu,\min},l_{\mu,\max},\mu_{B,t_{B}},\mu_{O}$
are corresponding element of
$\bm{l}_{\min},\bm{l}_{\min},\bm{p}_{B,t_{B}},\bm{p}_{O}$, respectively,
$l_{s}$ is a constant parameter for safety.

Third, we use the GJK method proposed in [39] to detect collisions. If the QP
problem solution reveals a collision between the aerial manipulator and
obstacle $i$, then the two endpoints $\bm{T}_{i,L}$ and $\bm{T}_{i,R}$ of the
solution within the collision area with respect to the obstacle can be
determined (see Fig. 4). Let $\bm{O}_{i}$ denote the center of the obstacle
$i$. Then, we calculate the obstacle mirror position $\bm{p}_{M,i}$ by a
pinhole mapping method. The position of the pinhole is
$\bm{p}_{P,i}=0.5(\bm{T}_{i,L}+\bm{T}_{i,R})$. And, we have

$\bm{p}_{M,i}=2\bm{p}_{P,i}-\bm{O}_{i}.$ (39)

The values of the weighting factors are updated by
$\lambda_{i}=\lambda_{i}+\alpha\Delta\lambda_{i}$, where $i=1,2,\ldots,n_{O}$,
and $\alpha>0$ is a constant gain. The parameter $\Delta\lambda_{i}$ is the
step size used for updating the $i$-th weighting factor and is a critical
factor that affects the computation time of the method. The expression for
$\Delta\lambda_{i}$ is given as

$\Delta\lambda_{i}=\|\bm{T}_{i,L}-\bm{T}_{i,R}\|.$ (40)

##  VI Experimental Verification

This section presents experimental results to verify the effectiveness of the
proposed motion planning algorithms. The experimental video is available at
https://youtu.be/q7O9v7l2Oho.

First of all, we describe the experimental setup. The aerial manipulator
platform used in the experiments consists of a quadcopter and a Delta arm. The
wheelbase of the quadcopter is $0.65$ m. The mass of the quadcopter (including
a battery) is $3.60$ kg. The Delta arm consists of a mounting base ($0.56$
kg), a movable robotic arm ($0.44$ kg), and a gripper ($0.32$ kg). A flight
controller proposed in our previous work [30] runs on a Pixhawk 4 autopilot.
This controller uses extended state observers (ESOs) to estimate dynamic
coupling between the aerial manipulator and the Delta arm. The proposed motion
planning method runs on an onboard Intel NUC i7 computer with ROS (an open-
source robotics middleware suite). The experiments are conducted in a Vicon
system, which provides accurate position measurements of the quadcopter base
and the end-effector. The measurement data of the Vicon system is sent to a
ground control station through an ethernet switch. Then, the ground control
station sends the measurement data to the aerial manipulator with a frequency
of 100 Hz through a 5 GHz wireless router.

The perception of the aerial manipulator is not surveyed in this paper. We
assume that the obstacles in the environment are already known. In particular,
the locations of the obstacles and the object can be obtained by the Vicon
system. Then, the environment can be previously built as a grid map which
consists of a set of cubes. The size of each cube is set as 0.1 m. This map is
used for the path planning of the quadcopter base. The description of the
controllers is provided in Section II-B.

In all the examples, we use the same set of parameters of the motion planner:
$\alpha=3.0$, $r_{S}=0.50$ m, $l_{C}=0.06$ m, $l_{S}=0.20$ m. The velocity and
the acceleration constraints for the quadcopter base are set as 0.5 m/s and
1.0 m/s2, respectively. The velocity and the acceleration constraints for the
end-effector are set as 0.5 m/s and 2.0 m/s2, respectively. The bounds of the
geometric feasibility constraints are set as
$\bm{w}_{\min}=[-0.06,-0.06,-0.60]^{T}$ and
$\bm{w}_{\max}=[0.06,0.06,-0.40]^{T}$.

###  VI-A Example 1: Collision avoidance

We validate the effectiveness of the proposed method in the collision
avoidance task. The environment of this example is illustrated in Fig. 5.
There are two types of obstacles in the environment. The first type of
obstacles restrict the motion of the quadcopter base and the size of the
flight corridor. The collision avoidance for this type of obstacles is
achieved by the flight corridor. The second type of obstacles restrict the
motion of the Delta arm and must be avoided through the motion planning of the
Delta arm. In Section V-C, we propose an iterative collision avoidance method
to avoid the second type of obstacles. In order to show its effectiveness, the
result of the motion planning with the collision avoidance method is
calculated.

Fig. 5 shows the results of the motion planning with the collision avoidance
method and without the collision avoidance method. The generated flight
corridor is shown in Fig. 5(a). As shown in Fig. 5(b), there are four
obstacles near the object. In particular, the aerial manipulator collides with
one of these obstacles in the resulting trajectory without the collision
avoidance method. The collision area is shown as a red dot line in Fig. 5 and
its length is 0.26 m. The resulting trajectory with the collision avoidance
method is shown in Fig. 5 (see the blue line). As can be seen, the result of
the proposed method is collision-free. The total computational time for
calculating the path, flight corridor, and trajectory of the quadcopter in the
collision avoidance task is 46.3 ms, while the computational time for
calculating the trajectory of the end-effector is 36.4 ms.

###  VI-B Example 2: Aerial retrieval

The goal of this experiment is to retrieve an object by the aerial
manipulator. In the task, the aerial manipulator moves to and picks up the
object. Then, the aerial manipulator returns to the start position. The start
position is set as $[0,0,-2.00]$. The position and the orientation angle of
the object are set as $[0,-2.00,-1.24]$ and $0^{\circ}$ (see Fig. 6(a)),
respectively. As shown in Fig. 6(b), screens are in the flight environment,
which needs to be avoided by the aerial manipulator in the moving stage. The
aerial manipulator is set to fly around the screens. As shown in Fig. 6(a),
there are three obstacles near the object. The aerial manipulator has to avoid
colliding with these obstacles.

Fig. 6(b)-(d) shows the result of the aerial retrieval experiment. The
generated flight corridor is shown in Fig. 6(c). The span time of the
experiment is 58 s. The mean tracking error of the quadcopter base in the
moving stage is 0.05 m, while that in the manipulation stage is 0.01 m. The
quadcopter flies faster in the moving stage than in the manipulation stage.
However, the higher velocity also causes a larger tracking error. The
computational time for calculating the path, flight corridor, and trajectory
of the quadcopter in the aerial retrieval task is 43.9 ms, while the
computational time for calculating the trajectory of the end-effector is 25.7
ms.

###  VI-C Example 3: Aerial transport

The goal of this experiment is to grasp an object and put it on the target
location. In the task, the aerial manipulator first flies to and picks up the
object. Then, the aerial manipulator flies to the target location and puts the
object on the target location. Finally, the aerial manipulator returns to the
start position. The start position is set as $[0,0,-2.00]$. The position and
the orientation angle of the object are set as $[0,2.00,-1.22]$ and
$0^{\circ}$, respectively. The position of the target location is set as
$[0,-2.00,-1.24]$. The whole process of the experiment is shown in Fig. 7(c).
In the experiment, the aerial manipulator is also set to fly around the
screens to utilize the experiment field. As shown in Fig. 7(a) and (b), there
are two obstacles near the object and three obstacles near the target
location. The aerial manipulator has to avoid colliding with these obstacles.

Fig. 7(c)-(e) show the result of the aerial transport experiment. The
generated flight corridor is shown in Fig. 7(d). The span time of the
experiment is 88 s. The mean tracking error of the quadcopter base in the
moving stage is 0.06 m, while that in the manipulation stage is 0.01 m. The
computational time for calculating the trajectory of the quadcopter in the
aerial transport task is 45.6 ms, while the computational time for calculating
the trajectory of the end-effector is 32.8 ms. The experiment result validates
the effectiveness of the proposed motion planning method in the aerial
transport task.

##  VII Conclusion

This paper proposed a novel partially decoupled motion planning method of the
aerial manipulator for the aerial pick-and-place task. This method calculates
the dynamically feasible and collision-free trajectories of the flying base
and the manipulator respectively in Cartesian space. The proposed geometric
feasibility constraints can ensure the resulting trajectories are coordinated
to complete tasks. The proposed method is verified by three experimental
results. It is verified by these experiments that the proposed geometric
feasibility constraints can ensure the trajectories of the quadcopter base and
the end-effector satisfy the geometry of the aerial manipulator. The results
also illustrate the ability of the proposed method to avoid obstacles. This
ability is limited by the partially decoupled structure since the obstacles
nearby the object are avoided by the Delta arm rather than the whole aerial
manipulator. However, in order to avoid large obstacles nearby the object,
both the quadcopter base and the Delta arm must be used. This will be one
important research direction for future research.

## References

  * [1] A. Ollero, M. Tognon, A. Suarez, D. Lee, and A. Franchi, “Past, present, and future of aerial robotic manipulators,” IEEE Transactions on Robotics, vol. 38, no. 1, pp. 626–645, 2021.
  * [2] D. Xilun, G. Pin, X. Kun, and Y. Yushu, “A review of aerial manipulation of small-scale rotorcraft unmanned robotic systems,” Chinese Journal of Aeronautics, vol. 32, no. 1, pp. 200–214, 2019.
  * [3] A. Mohiuddin, T. Tarek, Y. Zweiri, and D. Gan, “A survey of single and multi-uav aerial manipulation,” Unmanned Systems, vol. 8, no. 02, pp. 119–147, 2020.
  * [4] F. Ruggiero, V. Lippiello, and A. Ollero, “Aerial manipulation: A literature review,” IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 1957–1964, 2018.
  * [5] 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, 2020.
  * [6] K. Zhang, P. Chermprayong, F. Xiao, D. Tzoumanikas, B. Dams, S. Kay, B. B. Kocer, A. Burns, L. Orr, C. Choi, et al., “Aerial additive manufacturing with multiple autonomous robots,” Nature, vol. 609, no. 7928, pp. 709–717, 2022.
  * [7] H.-N. Nguyen, S. Park, J. Park, and D. Lee, “A novel robotic platform for aerial manipulation using quadrotors as rotating thrust generators,” IEEE Transactions on Robotics, vol. 34, no. 2, pp. 353–369, 2018.
  * [8] D. Mellinger, Q. Lindsey, M. Shomin, and V. Kumar, “Design, modeling, estimation and control for aerial grasping and manipulation,” in 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2668–2673, 2011.
  * [9] Y. Chen, J. Liang, Y. Wu, Z. Miao, H. Zhang, and Y. Wang, “Adaptive sliding-mode disturbance observer-based finite-time control for unmanned aerial manipulator with prescribed performance,” IEEE Transactions on Cybernetics, 2022.
  * [10] T. G. Chen, K. A. Hoffmann, J. E. Low, K. Nagami, D. Lentink, and M. R. Cutkosky, “Aerial grasping and the velocity sufficiency region,” IEEE Robotics and Automation Letters, vol. 7, no. 4, pp. 10009–10016, 2022.
  * [11] H. Lee, H. Kim, and H. J. Kim, “Planning and control for collision-free cooperative aerial transportation,” IEEE Transactions on Automation Science and Engineering, vol. 15, no. 1, pp. 189–201, 2016.
  * [12] K. Alexis, G. Darivianakis, M. Burri, and R. Siegwart, “Aerial robotic contact-based inspection: planning and control,” Autonomous Robots, vol. 40, pp. 631–655, 2016.
  * [13] M. Tognon, E. Cataldi, H. A. T. Chavez, G. Antonelli, J. Cortés, and A. Franchi, “Control-aware motion planning for task-constrained aerial manipulation,” IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 2478–2484, 2018.
  * [14] J. Thomas, G. Loianno, K. Sreenath, and V. Kumar, “Toward image based visual servoing for aerial grasping and perching,” in 2014 IEEE International Conference on Robotics and Automation (ICRA), pp. 2113–2118, 2014.
  * [15] H. Seo, S. Kim, and H. J. Kim, “Aerial grasping of cylindrical object using visual servoing based on stochastic model predictive control,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 6362–6368, 2017.
  * [16] Y. Chen, Y. Wu, Z. Zhang, Z. Miao, H. Zhong, H. Zhang, and Y. Wang, “Image-based visual servoing of unmanned aerial manipulators for tracking and grasping a moving target,” IEEE Transactions on Industrial Informatics, 2022.
  * [17] P. Ramon-Soria, B. C. Arrue, and A. Ollero, “Grasp planning and visual servoing for an outdoors aerial dual manipulator,” Engineering, vol. 6, no. 1, pp. 77–88, 2020.
  * [18] A. Ivanovic, M. Car, M. Orsag, and S. Bogdan, “Exploiting null space in aerial manipulation through model-in-the-loop motion planning,” in 2020 International Conference on Unmanned Aircraft Systems (ICUAS), pp. 686–693, 2020\.
  * [19] A. Caballero, A. Suarez, F. Real, V. M. Vega, M. Bejar, A. Rodriguez-Castaño, and A. Ollero, “First experimental results on motion planning for transportation in aerial long-reach manipulators with two arms,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 8471–8477, 2018.
  * [20] A. Caballero, P. J. Sanchez-Cuevas, M. Bejar, G. Heredia, M. A. Trujillo, and A. Ollero, “An aerodynamic extension for motion planning with dynamics awareness in aerial long-reach manipulators,” International Journal of Aerospace Engineering, vol. 2020, pp. 1–17, 2020.
  * [21] H. Kim, H. Seo, J. Kim, and H. J. Kim, “Sampling-based motion planning for aerial pick-and-place,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 7402–7408, 2019.
  * [22] S. R. Lindemann and S. M. LaValle, “Current issues in sampling-based motion planning,” in Robotics Research. The Eleventh International Symposium, pp. 36–54, 2005.
  * [23] G. Garimella, M. Sheckells, S. Kim, G. Baraban, and M. Kobilarov, “Improving the reliability of pick-and-place with aerial vehicles through fault-tolerant software and a custom magnetic end-effector,” IEEE Robotics and Automation Letters, vol. 6, no. 4, pp. 7501–7508, 2021.
  * [24] J. Welde, J. Paulos, and V. Kumar, “Dynamically feasible task space planning for underactuated aerial manipulators,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3232–3239, 2021.
  * [25] M. Muller, “A novel classification of planar four-bar linkages and its application to the mechanical analysis of animal systems,” Philosophical Transactions of the Royal Society of London. Series B: Biological Sciences, vol. 351, no. 1340, pp. 689–720, 1996.
  * [26] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: modelling, planning and control.  Springer Science & Business Media, 2010.
  * [27] N. J. Nilsson, The quest for artificial intelligence.  Cambridge University Press, 2009.
  * [28] F. Gao, W. Wu, Y. Lin, and S. Shen, “Online safe trajectory generation for quadrotors using fast marching method and bernstein basis polynomial,” in 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 344–351, 2018.
  * [29] A. Caballero, M. Bejar, A. Rodriguez-Castano, and A. Ollero, “Motion planning with dynamics awareness for long reach manipulation in aerial robotic systems with two arms,” International Journal of Advanced Robotic Systems, vol. 15, no. 3, p. 1729881418770525, 2018.
  * [30] H. Cao, Y. Li, C. Liu, and S. Zhao, “ESO-based robust and high-precision tracking control for aerial manipulation,” IEEE Transactions on Automation Science and Engineering, 2023.
  * [31] H. Prautzsch, W. Boehm, and M. Paluszny, Bézier and B-spline techniques, vol. 6.  Springer, 2002.
  * [32] M. López, E. Castillo, G. García, and A. Bashir, “Delta robot: inverse, direct, and intermediate Jacobians,” Proceedings of the Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science, vol. 220, no. 1, pp. 103–109, 2006.
  * [33] X. Yang, Z. Feng, C. Liu, and X. Ren, “A geometric method for kinematics of Delta robot and its path tracking control,” in 2014 14th International Conference on Control, Automation and Systems (ICCAS 2014), pp. 509–514, 2014.
  * [34] J. E. Shigley, C. R. Mischke, and T. H. Brown Jr, Standard handbook of machine design.  McGraw-Hill Education, 2004.
  * [35] M. Laribi, L. Romdhane, and S. Zeghloul, “Analysis and dimensional synthesis of the delta robot for a prescribed workspace,” Mechanism and machine theory, vol. 42, no. 7, pp. 859–870, 2007.
  * [36] S. Mondal, A. Biswas, and A. Sarkar, “Finding the largest empty cuboid inside a 3d digital object,” Multimedia Tools and Applications, pp. 1–21, 2021\.
  * [37] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, “Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3D complex environments,” IEEE Robotics and Automation Letters, vol. 2, no. 3, pp. 1688–1695, 2017.
  * [38] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in 2011 IEEE International Conference on Robotics and Automation, pp. 2520–2525, 2011.
  * [39] C. J. Ong and E. G. Gilbert, “Fast versions of the gilbert-johnson-keerthi distance algorithm: Additional results and comparisons,” IEEE transactions on robotics and automation, vol. 17, no. 4, pp. 531–539, 2001.