IEEE Robotics & Automation Magazine - December 2018 - 61

as the reference input. In order, these controllers are a proportional controller on the position, velocity, and attitude
errors and a proportional-integral-derivative controller on
the angular rate error. Due to the cascaded structure, subsequent control stages acted as damping on the previous stages. Additional integral and derivative actions were added to
the rate-control loop to increase the tracking performance
of the angular rates.
Furthermore, gravity compensation was taken into
account, and a static center-of-mass correction was applied.
The latter was required because the additional weight of
the manipulator and end effector affected the center of
mass of the total system. Given the structure of the manipulator we used, the variations in the center of mass were
considered negligible. As such, the center-of-mass correction yielded a constant compensation torque about yt b .
The value was found empirically by readjusting the compensation torque until the set-point and actual position in
free flight match.
Contact Controller
The contact controller, based on the work presented in [2], is
specifically designed for use when the system is in contact
with the environment. We demonstrated that stability in the
contact phase can be maintained while simultaneously applying a substantial contact force. This result was achieved by
actively exploiting the coupling between the roll and yaw state
during the contact phase, while retaining the regular controllers for the pitch and altitude. For this article, a modified version of the controller presented in [2] was derived specifically
for the contact phase. This version of the controller relies solely on angular state measurements to maintain its orientation
relative to the end effector and is therefore fully position independent. This allows the multirotor to automatically track
end effector movements without requiring active adjustments
to its set-points.
In this derivation, we considered the end effector to be
fixed to the surface due to friction and normal force.
Meanwhile, we considered the manipulator to be connected to the end effector by a spherical joint in W e . Furthermore, we assumed that the static vertical surface is
oriented so that xt e is perpendicular to it and points
inwards. Applying screw theory [16] and ignoring frictional effects, the equations of motion expressed in W b can
be described as follows:
T

T

I b To bb, e = ad Tbb,e I b T bb, e + Ad H bg ^W g hT + ^W bhT + Ad TH eb ^W ehT.
(1)
b, e
b

Here, I b To is the change in momentum of the multirotor
with respect to W e, with I b representing the inertia of the
multirotor and T bb, e the relative twist of W b to W e expressed in
T
W b. The fictitious forces are accounted for by ad Tbb,e I b T bb, e
^ W b is not an inertial frame). W g , W b, and W e are the gravity,
j
input, and contact wrenches, respectively. H i ! SE (3) is the
homogeneous transformation matrix from W i to W j . The

T

matrix Ad H ij describes the transformation of a given wrench
from frame j to frame i. W g is the gravitational frame, which
coincides with W b and is oriented as W w . Note that H eb
depends on n sp and L m .
Assuming quasistatic conditions, the reaction wrench of
the environment can be found using the balance of forces:
R
V
03 # 1
S
W
0
0 W
S
T
^W eh = S e
- R b > 0 H + > 0 HW ,
SS
W
Fu
mu g W
T
X

(2)

where Fu and m u are the thrust and mass of the multirotor,
respectively. Because I b is invertible, by combining and rearranging (1) and (2), the dynamics of the constrained system
can be described as
To bb, e = f (T bb, e , H eb, U, n sp) ,

(3)

with U : = 6x x, x y, x z, Fu@T being the input torques and thrust
generated by the multirotor.
Due to the constraints imposed on the system, the multirotor can be stabilized by stabilizing its rotational dynamics.
For this, we used a state-feedback controller. To apply such a
controller, (3) is linearized around the equilibrium state given
by the following:
= 0; zo b = 0; i b = i sp;
io b = 0; } b = 0; }o b = 0;
x x, y, z = 0; Fu = Feq; n c = i sp,
zb

(4)

where the pitch set-point i sp is given as an input and Feq is
the thrust needed for the system to remain in equilibrium,
given by the following:
Feq =

mu g
.
cos (i sp)

(5)

Note that, in the equilibrium configuration, the reader
could estimate the normal force FN applied by the system on
the environment by using the following relation:
FN =

mu g
.
tan (i sp)

(6)

We represent the linearized rotational dynamics as follows:
Xo = A (i sp) (X - X eq) + B (i sp) (U - U eq) ,

(7)

with X = 6z b zo b i b io b } b }o b@T describing the angular
state. X eq and U eq are filled with the equilibrium values
of (4).
The linear quadratic regulator method, combined with
gain-scheduling, can be applied to (7) to find stabilizing control gains K (i sp) for each i sp so that
U = K (i sp) (X sp - X) + U eq
December 2018

*

(8)

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

61
IEEE Robotics & Automation Magazine - December 2018

Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - December 2018
