Nonlinear Model Predictive Control for Aerial Manipulation
Dario Lunni, Angel Santamaria-Navarro, Roberto Rossi, Paolo Rocco, Luca Bascetta and Juan Andrade-Cetto
Abstract— This paper presents a nonlinear model predictive
controller to follow desired 3D trajectories with the end effector
of an unmanned aerial manipulator (i.e., a multirotor with a
serial arm attached). To the knowledge of the authors, this is the
first time that such controller runs online and on board a limited
computational unit to drive a kinematically augmented aerial
vehicle. Besides the trajectory following target, we explore the
possibility of accomplishing other tasks during flight by taking
advantage of the system redundancy. We define several tasks
designed for aerial manipulators and show in simulation case
studies how they can be achieved by either a weighting strategy,
within a main optimization process, or a hierarchical approach
consisting on nested optimizations. Moreover, experiments are
presented to demonstrate the performance of such controller
in a real robot.
I. INTRODUCTION
Over the last few years, the application field of unmanned
aerial vehicles (UAVs) has been considerably expanded from
“passive” applications, like monitoring or surveillance, to
“active” operations which require interactions with the en-
vironment (e.g., manipulation). With the improvements of
brush-less motors and batteries, new sophisticated UAV pro-
totypes have been developed that can physically interact with
the environment. These high-performance vehicles, called
unmanned aerial manipulators (UAMs), usually consist on
a multirotor platform with a robot arm attached underneath
(e.g., the UAM shown in Fig. 1).
First studies on UAMs analyzed how load transportation
affects the vehicle dynamics [1], [2]. In most of those works,
controllers were designed to achieve trajectory tracking with
the aerial vehicle during the navigation phases. However,
when using a serial arm able to move with respect to
the platform, the models involved in the design of these
controllers are complicated due to dynamic coupling between
the parts of the system [3]. A common practice is to simplify
the controller design and consider the dynamics of each part
decoupled. In [4] a scheme of nested proportional-integral-
derivative controllers (PIDs) was designed for attitude sta-
bilization, vision-based navigation and a gripping maneuver.
Similar PID schemes are compared with an integral back-
stepping controller in [5]. Among the undesired dynamic
Dario Lunni, Roberto Rossi, Paolo Rocco and Luca Bascetta
are with Politecnico di Milano, Dipartimento di Elettronica,
Informazione e Bioingegneria, Piazza Leonardo da Vinci, 32, Italy,
E-mail: dario.lunni, roberto.rossi, paolo.rocco,
luca.bascetta @polimi.it
Angel Santamaria-Navarro and Juan Andrade-Cetto are
with the Institut de Rob
`
otica i Inform
`
atica Industrial, CSIC-
UPC, Llorens i Artigas 4-6, Barcelona 08028, Spain, E-mail:
asantamaria,cetto@iri.upc.edu. Their work was partially
funded by the EU project AEROARMS H2020-ICT-2014-1-644271,
and by the Spanish Ministry of Economy and Competitiveness project
ROBINSTRUCT TIN2014-58178-R.
Fig. 1. Aerial manipulator, used in the experiments, consisting on a 3DR-
X8 coaxial multirotor platform with a custom-built 3 degrees-of-freedom
serial arm attached below.
effects, there is the change of the center of mass during flight,
that can be solved designing a low-level attitude controller
such as a Cartesian impedance controller [6], or an adaptive
controller [7]. Moreover, a desired end effector pose might
require a non-horizontal robot configuration that the low level
controller would try to compensate, changing in turn the arm
end effector position. In this way, [8] designs a controller
exploiting the manipulator and multirotor models, however
flight stability is preserved by restricting the arm movements
to those not jeopardizing UAM integrity.
Recently, there has been growing interest in using numer-
ical optimal control for aerial vehicles. In [9] a method is
presented for 3D trajectory generation of a UAV limiting the
3 translational jerks and accelerations. The computed trajec-
tory is then fed to a closed-loop control, achieving a similar
performance as in the case of using model predictive control.
Similarly, [10] describes a system for real-time generation
of optimal trajectories with minimum snap, allowing large
excursions from the hover position during operations like
take-off and navigation. In both [9] and [10] the generated
trajectories are for nonholonomic multirotors, which at a
high level of control have 4 degrees of freedom (DoF).
In [11], this trajectory generation is optimized for a vehicle
with a cable-suspended load computing nominal trajectories
with various constraints regarding the load swing. All these
trajectory generation methods are actually considered part of
the planning module.
The application of optimal control to UAMs can be
seen in [12] where a nonlinear model predictive control
scheme (NMPC) is described using a direct multiple shooting
method, but results for a multirotor equipped with a serial
arm are only shown in a simulated environment. Similarly,
[13] presents an NMPC to achieve pick-and-place operations
with a real robot. However, both in [12] and [13] the
NMPCs are used for offline trajectory generation and online
performance is left for future work. Moreover, none of these
methods aims to accomplish several tasks simultaneously by
exploiting the redundancy of UAMs in terms of degrees-of-
freedom (DoFs).
The redundancy of UAM systems (i.e., 4 DoFs from the
multirotor and, at least, 3 DoFs from the arm system),
can be exploited not only to achieve a primary task (e.g.,
trajectory tracking) but also a lower priority stabilizing task
by optimizing some given quality indices (e.g., improving
manipulability or avoiding joint limits) as in [14], [15]. In
these works, as in most of recent approaches (e.g., [16],
[17], [18]) the problem is solved using hierarchical task
composition control but in almost all cases without using
optimal control. A close approach is [19], which presents
a trajectory generation method for UAMs through quadratic
programming, taking advantage of redundancy. However, the
hierarchy of tasks is softly imposed using a weighted sum
and conflicts with antagonistic tasks would arise.
In this paper, we combine both ideas of using optimal
control and accomplishing several tasks with kinematically
redundant UAMs. We present an NMPC controller for UAMs
able to work online and on board a limited computational
unit. We consider a task consisting on a 3D trajectory
tracking with the arm end effector, and define other tasks
to improve the arm manipulability and align the arm center
of gravity (CoG) with the platform gravitational vector.
Moreover, system bounds and constraints are set in the
optimization process to limit the UAM motion and avoid self-
collisions with the arm end effector. The use of these tasks
is shown in simulation case studies by using a weighting
strategy and a nested optimization scheme which allows us
to impose a hierarchy. Real robot experiments are presented
to show the performance of the NMPC.
The remainder of this article is structured as follows.
The robot kinematics is presented in the following Section.
Section III provides the NMPC details. Tasks and constraints
are defined in Sections IV and V, respectively. Section VI
presents the validation of the method throughout simulations
and real experiments. Finally, conclusions are given in Sec-
tion VII.
II. ROBOT MODEL
Multirotors are equipped with more than two aligned
coplanar propellers. Due to their symmetric design, motion
control is achieved by altering the rotation rate of one or
more of these propellers, thereby changing its torque load
and thrust lift characteristics. At a high level of control, a
multirotor is an underactuated vehicle with only 4 DoFs, i.e.,
the platform tilting (roll and pitch variables) is used by the
low level attitude controller to produce desired translational
velocities of the vehicle. Instead, attaching a robotic arm with
more than two degrees of freedom to a multirotor the whole
system becomes redundant.
In this paper, we consider a UAM composed of a free-
flying platform (i.e., a multirotor) with a serial arm attached,
allowing not only to accomplish tasks using the arm end
effector (e.g., end effector positioning) but also to exploit
the whole system redundancy to achieve internal motions
that ease the flight behaviour. The multirotor-arm system
considered has n DoFs, with n ≥ 7, defined as follows.
Let
i
p
b
and
i
φ
b
denote the absolute position and ori-
entation (i.e., the triple of ZYX yaw-pitch-roll angles) of
the vehicle body (b) expressed in a global inertial reference
frame (i). Let q be the arm joint positions. The complete
state of the UAM can be described by
ξ =
h
i
p
b
>
i
φ
b
>
q
>
i
>
. (1)
Considering the multirotor underactuation, it is worth to
separate the non controllable DoFs in the state, leading to
ξ =
µ
>
σ
>
>
, (2)
where µ is the vector of controlled variables (i.e., platform
position plus yaw angle and arm joint variables) and σ the
non controlled variables (i.e., platform roll and pitch).
III. NONLINEAR MODEL PREDICTIVE CONTROL
Model predictive control (MPC) is a control technique that
consists in numerically solving an optimization problem at
each time step. A model of the process is used to predict
the future evolution of the system in order to optimize
the control signal. This future time horizon, in which the
algorithm is computed, is called prediction horizon. NMPC
refers to particular MPC problems where the process model
is nonlinear, the cost functional is nonquadratic or general
nonlinear constraints are used.
A. Weighting strategy
Considering a generic dynamic system, which has a state
x and is controlled by the variables in u, the solution of the
optimal control problem at time t
k
= kT
s
(∀ 1 < k < N ),
where T
s
is the time step, over a finite prediction horizon of
N steps, is defined by
min
u
h(x
k
, u, t
k
)
s.t. x
k+1
= f (x
k
, u
k
)
y
min
≤ g(x
k
, u
k
) ≤ y
max
x
min
≤ x
k
≤ x
max
u
min
≤ u
k
≤ u
max
(3)
where u =
u
>
k
, u
>
k+1
, . . . , u
>
k+N −1
>
is a vector of control
variables, f is the model of the system, g represents a generic
constraint, and h is a generic cost function defined by
h(x
k
, u, t
k
) =
N−1
X
i=0
N
h
−1
X
j=0
h
j
+ ||u
k+i
− u
k+i−1
||
2
W
u
+ ||u
k+N
||
2
W
s
,
(4)
where h
j
= h
j
(x
k+i
, u
k+i
, t
k
) are N
h
generic positive
cost functions to be minimized, and W
u
and W
s
weights
on control actions and terminal cost respectively. Notice
how in (3) the optimization process is constrained by the
dynamic model of the system, x
k+1
= f (x
k
, u
k
), subject to
lower and upper bounds on state, control actions, and output
variables defined by x
min
and x
max
, u
min
and u
max
and
y
min
and y
max
, respectively.
B. Hierarchical approach
Instead of following a weighting strategy, we can impose a
hierarchy between the tasks (cost functions), similarly to [17]
and [18], but in this case using optimal control. The required
scheme is based on nested optimizations as explained in the
following.
Drawing inspiration from [20], we propose to compute
a cascade of optimizations for each time step in order to
minimize different cost functionals, each one related to a
different task. However and differently from [20], we do not
require to linearize the system. Initially, we solve the optimal
control problem considering the cost functional of a main
task (e.g., the end effector trajectory tracking described later
on in Section (IV-A)). From this minimization we are able to
compute the predicted values of the cost functional for the
N future steps. Then, the hierarchy is imposed by adding
a constraint in the secondary task resolutions. In particular,
the value of the cost function at higher priority is set as a
constraint to the next optimization iteration.
For a secondary task, the minimization of the cost functional
follows the same procedure as for the primary task, but this
time incorporating the constraint resulting from solving the
primary task (see (3)), defined by
g
1
(x
k
, u, t
k
) = 0 , ∀k. (5)
This method can be repeated as many times as wanted for
several subtasks imposing the hierarchy, depending on the
redundancy of the robot with respect to the task definitions,
e.g., the optimization solver will find a solution in the alloted
time only if the tasks higher in the hierarchy do not require
that DoF to be accomplished (i.e., if the optimization process
does not have a constraint for that DoF).
C. System model in the optimization problem
In this paper we consider that UAMs are not meant for
acrobatic maneuvers. Hence, we will not include the platform
tilt in the trajectory generation algorithm (platform roll and
pitch angles will be assumed negligible in our analysis).
Then, for the optimization problem we can consider the state
x = µ =
i
p
>
b
i
ψ
b
q
>
>
, (6)
while the control action is directly the state derivative
u =
˙
µ =
i
˙
p
>
b
i
˙
ψ
b
˙
q
>
>
. (7)
Formulating the control action in differential form is a well-
known method to obtain zero steady state error. The function
of the system model f is just an integrator of input u.
The non-linearity of the approach appears in the definition
of the cost function and in the constraints. The choice of
this reduced system model was made to guarantee online
computation, on board a platform with limited resources.
Hereafter, we assume that the inner control loop of
the system can perfectly track the computed references.
There are some works [21], [22] which show that adding
a manipulator introduces unwanted external disturbances to
the multirotor which, if not compensated for, will make it
unstable. Although, with slow arm movements (as in our
case) the inner-loop controller can already compensate most
of these effects, one has to specially consider from the
following tasks, the ones designed to improve stability of
the platform.
IV. UAM TASKS
We can consider several cost functions for UAMs (3), in
order to accomplish a given task or to preserve stability. In
this Section we present a few of such task functions.
A. End effector tracking
The main interaction task will be executed by the arm
end effector, it is thus important to be able to track a desired
end effector trajectory. The following cost function can be
defined
h
1
=
N−1
X
i=0
||e
e
(x
k+i
) ||
2
W
1
+ ||e
e
(x
k+N
) ||
2
W
s1
, (8)
where
e
e
(x
k+i
) =
i
p
e
(x
k+i
) −
i
p
d
e
(t
k+i
) (9)
is the end effector tracking error,
i
p
d
e
is the desired end
effector position over the predicted horizon, and W
1
and
W
s1
are the weight matrices for the task and the terminal
cost. Although h
1
encloses the end effector position error,
notice how this cost function can also consider its orientation
by augmenting with
i
φ
e
and
i
φ
d
e
the current and desired end
effector poses, respectively.
Notice that each cost function presented in the following
includes a terminal cost. In a number of works, collected
in [23], it is pointed out that terminal cost weights are a
key ingredient to achieve stability with NMPCs. However,
stability proof of the proposed NMPC is outside the scope
of the present work.
B. Robot center of gravity alignment
When the arm CoG is not aligned with the geometrical
center of the platform, undesired torques appear in the
vehicle’s base, which must be compensated with the action
of the propellers. In order to minimize this actuation effort
and avoid instability, it is beneficial to design a task to favor
this alignment, such as
h
2
=
N−1
X
i=0
||
b
p
Gxy
(x
k+i
)||
2
W
2
+ ||
b
p
Gxy
(x
k+N
) ||
2
W
s2
,
(10)
where
b
p
Gxy
(x
k+i
), is the vector describing the position of
the arm CoG projected onto the xy plane of the body ref-
erence frame during the prediction horizon. This expression
can be obtained as in [17], [18]. Notice that we are assum-
ing that the quadrotor is internally balanced. Otherwise, a
different equilibrium point should be assigned for the arm
CoG.
The formulation of (10) can effectively reduce the static
torques produced on the quadrotor. However, an additional
cost function can be considered in order to reduce the
disturbances produced by inertial forces on the quadrotor.
In fact, the following cost function accounts for the velocity
of the CoG:
h
3
=
N−1
X
i=0
||
b
v
Gxy
(k + i) ||
2
W
3
+ ||
b
v
Gxy
(k + N) ||
2
W
s3
,
(11)
with
b
v
Gxy
(k + i) =
b
J
Gxy
(x
k+i
) u
k+i
, (12)
and
b
J
Gxy
(x
k+i
) the arm CoG Jacobian. Penalizing the
motion of the CoG has the goal of reducing the inertial
forces resulting on the quadrotor plane, thus on axes
orthogonal to the thrust direction.
C. Arm manipulability
During a manipulation task, a useful objective function is
represented by the arm manipulability index. A first direct
way of expressing this cost function is
h
4
=
N−1
X
i=0
W
4
1
||D (x
k+i
) ||
2
+ W
s4
1
||D (x
k+N
) ||
2
, (13)
where
D (x
k+i
)= det
J
e,q
(x
k+i
) J
e,q
(x
k+i
)
>
. (14)
J
e,q
(x
k+i
) is the submatrix of the end effector Jacobian
J
e
(x
k+i
) composed by the columns corresponding to the
arm joints.
A second cost function useful to maximize the manipula-
bility of the arm is obtained by minimizing the conditioning
number of matrix J
e,q
J
>
e,q
:
h
5
=
N−1
X
i=0
W
5
||1−ρ
J
eq
(k + i)||
2
+W
s5
||1−ρ
J
eq
(k + N)||
2
,
(15)
with
ρ
J
eq
(k + i) = ρ
J
e,q
(x
k+i
) J
e,q
(x
k+i
)
>
. (16)
ρ(A) = λ
A,M
/λ
A,m
is the conditioning number of a ma-
trix A, with λ
A,M
and λ
A,m
the maximum and minimum
eigenvalues, respectively. Notice that, far from singularity
points, the matrix J
e,q
J
>
e,q
is symmetric positive definite,
then its eigenvalues are real and positive. Notice also that
the closer the cost function h
5
in (15) is to zero, the more
the manipulability ellipsoid is similar to a generalized sphere.
The weights W
4
, W
5
, W
s4
and W
s5
are scalar values.
A third alternative formulation of the cost function related
to manipulability can be obtained by applying the gradient
based method in [24]. It would be expressed with a cost
function on the control action u.
D. Cost function on the control action
The cost function h(x
k
, u, t
k
) includes the term
||u
k+i
− u
k+i−1
||
2
W
u
penalizing the control action. Notice
that the control action for our system consists in the deriva-
tives of UAM joint positions. Then, with the weight matrix
W
u
properly set, the motion can be arbitrarily distributed on
the UAM joints. For instance, it can be desirable to penalize
quadrotor motion in favor of arm DoFs during manipulation
tasks, and vice versa during navigation phases.
V. SPECIFIC NMPC CONSTRAINTS FOR UAMS
We introduce a first constraint consisting in avoiding self-
collisions. To do so, we impose a safety distance between
the joints and the aerial base with
b
z
i
(x
k
) ≥ 0.1 [m]
b
z
e
(x
k
) ≥ 0.1 [m] ,
(17)
where
b
z
e
is the position of the end effector in the z direction
of the body reference frame and, similarly,
b
z
i
is the distance
of the i-th joint. Notice that just a limited number of points
of the arm can potentially have a collision with the aerial
base, reducing the number of required constraints. These
constraints will be integrated in the optimization through the
term g (see (3)). Thanks to the flexibility of the method, a
generic constraint can be added to the problem.
A part from constraints, we also impose bounds to both
joint positions and velocities. Joint position bounds are ex-
pressed as state bounds, by imposing specific values to x
min
and x
max
. On the other hand, velocity bounds correspond
to control action constraints, thus they can be obtained by
setting values of u
min
and u
max
.
VI. VALIDATION AND EXPERIMENTAL RESULTS
A. Preliminary simulations using a reduced model with a
weighting task strategy
In order to validate the described controller, we first
present simulation case studies programmed in MATLAB
and considering a simplified model. In particular, the system
consists of a planar multirotor, thus described by three DoFs,
roll angle, y displacement and z altitude, and a 2 DoFs
robotic arm. As described in Section III, roll angle is not
considered in the state of the NMPC model. Therefore,
the model of the system has 4 DoFs. The aim of this
preliminary test is to show how several cost functions can
be integrated using different weights, and solved exploiting
the redundancy. The main cost function is the end effector
trajectory tracking h
1
. The end effector desired trajectory is
described by the two translational positions y
e
and z
e
. In
addition, the CoG cost functions h
2
and h
3
are integrated in
the model.
The low cardinality of the model state allows to include
the manipulability cost functions h
4
and h
5
too, which
is quite difficult for systems of higher order, due to the
required computational burden. This is only possible for
those optimization solvers able to obtain a solution even
0 2 4 6 8 10
Time [s]
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
EE Position [m]
y
z
y
des
Fig. 2. End effector trajectory compared with the reference signal. Notice
how the z reference is not reported because it is always null.
0 2 4 6 8 10
Time [s]
-0.3
-0.2
-0.1
0
0.1
0.2
0.3
UAM control action
u
y
[m/s]
u
z
[m/s]
u
q1
[rad/s]
u
q2
[rad/s]
Fig. 3. Control actions on the four degrees of freedom computed by the
NMPC. Blue and red lines represent multirotor translational DoFs, while
yellow and violet lines represent arm joint velocities.
without the analitical Jacobians of the cost functions (i.e.,
using numerical computations).
The desired end effector trajectory is in y direction and it
is divided in two phases. In the first phase a small sinusoidal
trajectory is required, while the second phase is characterized
by a bigger sinusoid.
The chosen optimization solver for the non linear problem
is the ACADO Toolkit [25]. The system can be set with
different weight ratios among the cost functions, depending
on the desired interactions between them. Hereafter, we
present the results of a test in which all the five cost functions
have weights different from zero.
In Fig. 2, the end effector trajectory is reported. The
reference trajectory is correctly tracked in both directions.
The z reference is not reported because it is always null. In
Fig. 3, the control actions, output of the NMPC algorithm,
are represented. Notice that bounds on maximum velocity
are respected. It is interesting to show how the motion is
distributed on different joints. In fact, analyzing the blue line,
corresponding to the control action on y, it is evident that the
small sinusoidal motion is mainly performed with the arm
joints, while the large sinusoid is performed using multirotor
DoFs.
Fig. 4 reports the behavior of the conditioning number
ρ
J
e,q
, defined in (16), by showing results of three exper-
iments, performed with different weights. The blue line
0 2 4 6 8 10
Time [s]
2
3
4
5
6
7
8
ρ(J J
T
)
Null
Medium
High
Fig. 4. Behavior of the conditioning number ρ
J
e,q
in three different
experiments, differing in the weight assigned to the conditioning number
cost function. The red line represents the experiment with larger assigned
weight, achieving a smaller ρ
J
e,q
, whereas the blue line is the result of the
experiment with zero weight assigned to the cost function, thus not reducing
ρ
J
e,q
.
represents an experiment when the cost function h
5
is not
included in the optimization process. The red line is a result
of an experiment in which a large weight was assigned to
this cost function, while the yellow line is the case of the
experiment presented in the previous Figures. The approach
can effectively reduce the conditioning number ρ
J
e,q
with a
proper choice of the associated weight.
A clear disadvantage of a weighting strategy is the lack
of good solutions when tasks are antagonistic (i.e., when the
accomplishment of a task requires the oposite solution of
an other task). Instead, when using a hierarchical solution,
we cannot guarantee the fulfillment of a secondary task, but
at least we can find reasonable solutions for tasks higher
in the hierarchy. In the following sections simulations and
experiments using a hierarchical NMPC are presented.
B. Simulations using a complete model and a hierarchical
task strategy
In this Section a simulation of a fully actuated UAM
consisting on a multirotor with 4 DoFs plus a 3-DoF serial
arm (UAM of 7 DoFs) is used to show how the secondary
tasks can also be executed with a different approach.
The effect of adding a secondary task hierarchically is
shown in Fig. 5. In this simulation the controller is set with
a prediction horizon of 8s. In both cases the optimal control
drives the end effector through the required waypoints. This
comparison is then extended to Fig. 6 with a different
trajectory. Here, it is reasonable to expect the improvement of
the flight behaviour when the whole CoG is vertically aligned
with the platform center of rotation. Moreover, considering
that most of the workspace of the arm exists below the
aerial platform (i.e., without colliding with the multirotor),
it is clearly shown how the multirotor (black line trajectory)
performs quite better when a secondary task to align the arm
CoG is present (Fig. 6(b)). In these particular simulations we
removed all motion bounds to show how, in the case where
only the tracking task is present (Fig. 6(a)), the platform and
end effector are moving in similar vertical positions, thus
with the arm fully extended in a forward position.
