control of quadrotor aerial vehicles equipped with a ... of quadrotor... · control of quadrotor...

Control of Quadrotor Aerial Vehicles Equipped with a Robotic Arm

G. Arleo, F. Caccavale, G. Muscio and F. Pierri

Abstract— In this paper a novel hierarchical motion controlscheme for quadrotor aerial vehicles equipped with a manipu-lator is proposed. The controller is organized into two layers:in the top layer, an inverse kinematics algorithm computes themotion references for the actuated variables; in the bottomlayer, a motion control algorithm is in charge of tracking themotion references computed by the top layer. A simulationcase study is developed to demonstrate the effectiveness ofthe approach in the presence of disturbances and unmodeleddynamics.

I. INTRODUCTION

Unmanned Aerial Vehicles (UAVs) are being widely usedin a number of applications, mostly military but also civilian,involving surveillance of indoor or outdoor environments,remote inspection and monitoring of hostile environments.Among UAVs, quadrotor helicopters are emerging as a pop-ular platform, due to their larger payload capability and theirhigher manoeuvrability with respect to single-rotor vehicles.

Motion control of quadrotors is a widely investigatedbut still challenging issue, since the quadrotor is an under-actuated system and, often, is equipped with limited sensingdevices. Conventional approaches to UAVs control havebeen based on linear controller design. These approachesinclude proportional integral derivative (PID) controllers[1], [2], proportional derivative controllers (PD) [3], linearquadratic regulator (LQR) controllers [4], [5] and robustH∞ controllers, requiring model linearization about a set ofequilibrium conditions [6]. Linear controllers are character-ized by satisfactory performance near the design conditionsand in hovering conditions, but performance degradation isusually observed when the aircraft moves away from theseconditions. To overcome these drawbacks, many nonlinearcontrollers have been proposed, such as model predictivecontrol [7], adaptive control [8], backstepping and slidingmode techniques [9], [10]. In [11] the problem of actuatorsaturation has been taken into account via a nested saturation-based controller. Since it is possible to make a conceptualseparation between position and orientation of a quadrotor,hierarchical controllers, based on an inner-outer loop, havebeen successfully proposed, for example, in [12], [13].Finally, a number of vision based controllers have beenadopted, see e.g. the results in [14], [15].

Recently, UAVs have been proposed for tasks as graspingand manipulation, this is a challenging issue since thevehicle is characterized by an unstable dynamics and thepresence of the object causes nontrivial coupling effects [16].Moreover, the capability to perform complex tasks requires

School of Engineering, University of Basilicata, 85100 Potenza, Italy.Corresponding author e-mail:[email protected]

suitable mechanical structure: in [17], [18] properly designedgrippers for aerial grasping processes are proposed, while[19] presents an UAV equipped with a robotic arm to executedexterous manipulation tasks. In detail, in [19] the dynamicmodel of the whole system, UAV plus manipulator, is devisedand a Cartesian impedance control is developed in such away to cope with contact forces and external disturbances.

In this paper, the problem of motion control of the end-effector of a robot manipulator mounted on a quadrotor heli-copter is tackled through a hierarchical control architecture.Namely, in the top layer, an inverse kinematics algorithmcomputes the motion references for the actuated variables,i.e., position and yaw angle of the quadrotor vehicle andjoint variables for the manipulator. In the bottom layer, amotion control algorithm is in charge of tracking the motionreferences. The proposed motion controller in the bottomlayer is an extension of the approach proposed in [12] tovehicle-manipulator systems: a vehicle position controllercomputes the thrust force and the reference values for pitchand roll angles, then an attitude controller, on the basisof these references, computes the moments acting on thequadrotor, while a manipulator controller computes the jointtorques.

A simulation case study is developed to demonstrate theeffectiveness of the approach in the presence of disturbancesand unmodeled dynamics.

II. MODELING

A quadrotor is a rotorcraft with four rotors; the propellerof each rotor generates a force and the total thrust isgiven by the sum of the individual forces. Therefore, thequadrotor is an under-actuated vehicle with four input forcesand six degrees of freedom (DOFs), describing the positionand orientation of the vehicle center of mass. Two of therotors rotate in clockwise direction, while the other two arecounterclockwise.

The system considered in this paper, depicted in Fig. 1,is composed by a quadrotor vehicle equipped with an-DOFrobotic arm.

A. Kinematics

Let Σb denotes the vehicle body-fixed reference framewith origin at the vehicle center of mass; its position withrespect to the world fixed inertial reference frame,Σ, is givenby the (3×1) vectorpb, while its orientation is given by therotation matrixRb

Rb(φb)=

cψcθ cψsθsϕ − sψcϕ cψsθcϕ + sψsϕsψcθ sψsθsϕ + cψcϕ sψsθcϕ − cψsϕ−sθ cθsϕ cθcϕ

, (1)

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 21st Mediterranean Conference on Control and Automation.Received February 19, 2013.

Fig. 1. Quadrotor and robotic arm system with the corresponding frames.

whereφb = [ψ θ ϕ]T is the triple ofZYX yaw-pitch-roll

angles andcγ andsγ denote, respectively,cos γ and sin γ.Let us consider the frameΣe attached to the end effector

of the manipulator. The position ofΣe with respect toΣ isgiven by

pe = pb +Rbpbeb , (2)

where the vectorpbeb describes the position ofΣe withrespect toΣb. The linear velocitype of Σe in the fixed frameis obtained by differentiating (2)

pe = pb − S(Rbpbeb)ωb +Rbp

beb , (3)

whereS(·) is the (3 × 3) skew-symmetric matrix operatorperforming the cross product [20]. The orientation ofΣe canbe described by the rotation matrix

Re = RbRbe , (4)

whereRbe describes the orientation ofΣe with respect toΣb.

By differentiating (4), the angular velocityωe of Σe is givenby

ωe = ωb +Rbωbeb , (5)

whereωbeb = RTb (ωe − ωb) is the relative angular velocity

between the end effector and the frameΣb, expressed in theframeΣb.

Let q be the (n × 1) vector of joint coordinates of themanipulator. Then,pbeb(q) and Rb

e(q) represent the usualdirect kinematics equations of a ground-fixed manipulatorwith respect to its base frame,Σb. The (6× 1) vector of thegeneralized velocity of the end-effector with respect toΣb,vbeb =

[

pbTeb ωbTeb

]

T, can be expressed in terms of the joint

velocitiesq via the manipulator JacobianJbeb, i.e.,

vbeb = Jbeb(q)q. (6)

On the basis of (3) and (5), the generalized end-effectorvelocity, ve =

[

pTe ωT

e

]T, can be expressed as

ve = Jb(q,Rb)vb + Jeb(q,Rb)q, (7)

wherevb =[

pTb ωT

b

]Tand

Jb =

[

I3 −S(Rbpbeb)

O3 I3

]

, Jeb =

[

Rb O3

O3 Rb

]

Jbeb ,

while Im andOm denote (m×m) identity and null matrices,respectively.

If the attitude of the vehicle is expressed in term of yaw-pitch-roll angles, equation (7) becomes

ve = Jb(q,φb)TA(φb)xb + Jeb(q,Rb)q , (8)

with

xb =

[

pbφb

]

, TA(φb) =

[

I3 O3

O3 T (φb)

]

, (9)

whereT (φb) is the transformation matrix between the an-gular velocityωb and the time derivative of the Euler anglesφb

T (φb) =

0 −sψ cψcθ0 cψ sψcθ1 0 −sθ

.

Since the vehicle is an under-actuated system, i.e., only4independent control inputs are available against the6 degreesof freedom, the position and the yaw angle are usually thecontrolled variables, while pitch and roll angles are used asintermediate control inputs for position control. Hence, it isworth rewriting the vectorxb as follows

xb =

[

ηbσb

]

, ηb =

[

pbψ

]

, σb =

[

θϕ

]

.

Thus, the differential kinematics (8) becomes

ve = Jη(q,φb)ηb + Jσ(q,φb)σb + Jeb(q,φb)q

= Jζ(σb, ζ)ζ + Jσ(σb, ζ)σb ,(10)

whereζ =[

ηbT qT

]

T is the vector of controlled variables,Jη is composed by the first4 columns ofJbTA(φb), Jσis composed by the last2 columns ofJbTA(φb) andJζ =[Jη Jeb].

If the end-effector orientation is expressed via a triple ofEuler angles (e.g., yaw-pitch-roll angles),φe, the differentialkinematics (10) has to be rewritten in terms of the vectorxe = [pe

T φeT]T. Indeed, sincexe = T−1

A (φe)ve, thedifferential kinematics becomes

xe = T−1A (φe)

[

Jζ(σb, ζ)ζ + Jσ(σb, ζ)σb

]

. (11)

B. Dynamics

The dynamical model of the vehicle-manipulator systemcan be derived, assuming negligible aerodynamical effectsand low speed displacements, by using, e.g., the Euler-Lagrange formulation [19]

M(ξ)ξ +C(ξ, ξ)ξ + g(ξ) = u , (12)

where ξ = [xbT qT]T ∈ IR(6+n×1), M represents the

symmetric and positive definite inertia matrix of the system,



C is the matrix of Coriolis and centrifugal terms,g is thevector of gravity forces,u is the vector of inputs

u =

ufuµuτ

=

Rb(φb)fbb

RTb (φb)T (φb)µ

bb

τ

, (13)

τ is the (n×1) vector of the manipulator joint torques, whilethe (3 × 1) vectorsf bb andµbb are, respectively, the forcesand the torques generated by the4 motors of the quadrotorhelicopter, expressed in the frameΣb.

The matrices introduced in (12) can be detailed by con-sidering the expressions derived in [19]. In particular, theinertia matrix can be viewed as a block matrix

M(ξ) =

Mpp Mpφ Mpq

MpφT Mφφ Mφq

MpqT Mφq

T M qq

,

where Mpp ∈ IR3×3, Mpφ ∈ IR3×3, Mpq ∈ IR3×n,Mφφ ∈ IR3×3, Mφq ∈ IR3×n and M qq ∈ IRn×n. Thedetailed expressions of the blocks are given in [19].

Similarly, matrixC and vectorg in (12) can be detailedas

C(ξ, ξ) =

Cp

Cφ

Cq

, g(ξ) =

gpgφgq

,

with Cp ∈ IR3×(6+n), Cφ ∈ IR3×(6+n), Cq ∈ IRn×(6+n)

andgp ∈ IR3, gφ ∈ IR3 andgq ∈ IRn.The input vectorsf bb andµbb can be expressed as

f bb =

00fz

, µbb =

µψµθµϕ

, (14)

wherefz is the total thrust applied by the rotors along thezbaxis. Bothfz andµbb are related to the four actuation forcesoutput by the quadrotor motorsf via the following relation[21]

[

fzµbb

]

=

1 1 1 10 l 0 −l−l 0 l 0c −c c −c

f1f2f3f4

= Nf , (15)

where l > 0 is the distance from each motor to the vehiclecenter of mass,c = γd/γt, andγd, γt are the drag and thrustcoefficient, respectively.

III. CONTROL SCHEME

A task for the vehicle-manipulator system modeled in theprevious section is usually specified in terms of a desired tra-jectory for the end-effector position,pe,d(t), and orientation,φe,d(t); also, the corresponding desired linear and angularvelocities, pe,d(t) and φe,d(t), are assigned. The proposedhierarchical control strategy is composed by two layer. Inthe top layer, an inverse kinematics algorithm computes themotion referencesηb,r(t), ηb,r(t), qr(t) andqr(t). Then, inthe bottom layer, a motion control algorithm is designed insuch a way thatηb and q track the corresponding desiredtrajectories output by the top layer. The whole scheme is

summarized in Fig. 2. It assumed thatdim{ζ} = n + 4 ≥dim{ve} = 6, i.e., the number of DOFs characterizingthe vehicle-manipulator system is, at least, equal to thedimension of the assigned task.

Quad. Position

Controller

Eqs. (23)--(27)

Inverse

Kinematics

Eq. (16)

Quad. Attitude

Controller

Eqs. (30)-(31)

Quadrotor

Inputs

Eq. (32)

Manipulator

Controller

Eq. (33)

fpe,d pe,d

φe,d φe,d

ζr ζr θr ϕr

σb, σb

fz

µbb

ζ, ζ,σb, σb

τ

Fig. 2. Block scheme of the control architecture.

A. Inverse Kinematics

The differential kinematics (11) is considered to derivea closed-loop inverse kinematics algorithm [20], in chargeof computing the motion references for the motion controlloops placed at the bottom layer

ζr = J†ζ(σb, ζr)TA(φe,r) (xe,d +Ke)−

J†ζ(σb, ζr)Jσ(σb, ζr)σb ,

(16)

whereJ†ζ = Jζ

T(

JζJζT)

is a pseudoinverse ofJζ , K isa symmetric positive definite gain matrix,e is the kinematicinversion error, given bye = xe,d − xe,r, andxe,r is theend-effector pose computed on the basis ofζr andσb.

Notice that all the direct kinematics quantities are com-puted via (2) and (4) by usingζr (i.e., the output of thealgorithm) and the measured value ofσb, which is notcomputed by the algorithm.

If n + 4 = 6, the vehicle-manipulator system is non-redundant, namelyJζ is square and the pseudo-inverse canbe replaced by the inverse. On the other hand, ifn+ 4 > 6the vehicle-manipulator system is kinematically redundantand the redundant DOFs can be exploited to fulfill secondarytasks by resorting to a task-priority approach [22], [23].

Remark 1: If the motion control algorithms need the de-sired values of the accelerations (qr andηr), a second-orderinverse kinematics algorithm should be used [24], althougha numerical differentiation of the outputs of algorithm (16)could be adopted in the presence of limited noise and suitablysmall sampling time. Moreover, if the desired end-effectororientation is assigned in terms of the rotation matrix,Re,d(t), an inverse kinematics algorithm similar to (16) canbe adopted, where the kinematic inversion error is computedin terms of the unit quaternion [25].

B. Motion Control

Onceζr and its derivatives are computed by the inversekinematics algorithm, they are delivered to the motion con-trol loops in charge of achieving the desired motion. Thecontrol strategy proposed here is a generalization of thecontrol laws proposed in [12] for quadrotor vehicles (withoutthe manipulator) to vehicle-manipulator systems. This is,inturn, a hierarchical inner-outer loop control scheme, based



on two steps. The outer control loop is designed to trackthe assigned position and yaw angle for the vehicle, byintroducing a virtual control vector aimed at linearizing theposition dynamics. Then, by using the relation between theforce vectoruf and the quadrotor attitude, a reference valuefor the roll and pitch angles is devised and used to feed anattitude controller (inner loop). The whole design is possiblesince the quadrotor position and attitude dynamics can bewritten as a cascade system in which the attitude subsystemdoes not depend on the position.

In order to globally linearize the closed-loop dynamics,the following control inputu can be considered

u = M(ξ)α+C(ξ, ξ)ξ + g(ξ) , (17)

where the auxiliary inputα can be partitioned, according to(13), i.e.,α =

[

αpT αφ

T αqT]

T, with αφ = [αψ αθ αϕ]T.

Hence, the globally linearized closed-loop dynamics is givenby ξ = α, i.e.

pb = αp ,

φb = αφ ,

q = αq .

(18)

In turn, the auxiliary control inputs for the manipulator,the quadrotor position and the quadrotor yaw angle can bechosen, respectively, as

αq = qr +Kq,V (qr − q) +Kq,P (qr − q) , (19)

αp = pr +Kp,V (pr − p) +Kp,P (pr − p) , (20)

and

αψ = ψr + kψ,V (ψr − ψ) + kψ,P (ψr − ψ) , (21)

whereK∗,V , K∗,P with ∗ = {q, p} are symmetric positivedefinite gain matrices andkψ,V , kψ,P are positive scalargains. If the reference accelerations are not provided by theinverse kinematics algorithms, the acceleration feedforwardterms in (20)–(21) could be either omitted or computednumerically (see Remark 1).

1) Quadrotor position controller:On the basis of eq. (17),the following expression ofuf can be derived

uf = Mppαp +Mpφαφ +Mpqαq +Cpξ + gp. (22)

This expression does not allow to computeuf , since itincludes the controlαφ, whose computation requires ref-erences values for roll and pitch angles, not available atthis stage. To overcome this problem,uf is computed ina slightly different way with respect to (22), i.e.,

uf = Mppαp +m(1)pφαψ +Mpqαq +Cpξ + gp, (23)

wherem(1)pφ denotes the first column of the matrixMpφ.

It can be noting that, since the manipulator links are muchlighter than the vehicle body, the elements of matrixMpφ

are often negligible with respect to those ofMpp. Therefore,in practice,uf in (23) is very close to the ideal control inputcomputed in (22). The validity of this choice will be checkedin the simulation case study.

In view of (13), uf depends on the attitude of thequadrotor via the relation

uf =h(fz,σb) ⇒

uf,xuf,yuf,z

=

(cψsθcϕ + sψsϕ) fz(sψsθcϕ − cψsϕ) fz

cθcϕfz

. (24)

Therefore, the total thrust,fz, and reference trajectories forthe roll and pitch angles to be fed to the inner loop can becomputed as

fz = ‖uf‖, (25)

θr = arctan

(

uf,xcψ + uf,ysψuf,z

)

, (26)

ϕr = arcsin

(

uf,xsψ − uf,ycψ‖uf‖

)

. (27)

2) Quadrotor attitude control:Once the reference valuefor roll and pitch angles have been computed, the controlinputs αθ and αϕ can be realized via simple PD controllaws

αθ = −θ + kθ,P (θr − θ) , (28)

αϕ = −ϕ+ kϕ,P (ϕr − ϕ) , (29)

wherekθ,P andkϕ,P are positive scalar gains.Finally, uµ can be computed as

uµ = MpφTαp +Mφφαφ +Mφqαq +Cφξ + gφ, (30)

and, from (13), the vehicle torques as

µbb = T−1(φb)Rb(φb)uµ. (31)

Remark 2:As for αθ andαϕ laws similar to (20) and (19)can be used, requiring a numerical differentiation ofθr andϕr, which are likely to be affected by noise.

3) Computation of quadrotor inputs:Once fz and µbbhave been computed, the four actuation forces of the rotorsof the vehicle can be easily obtained via (15)

f = N−1

[

fzµbb

]

. (32)

4) Manipulator control: Finally, the torques acting on themanipulator joints can be computed as

uq = MTpφαp +MT

φqαφ +M qqαq +Cqξ + gq. (33)

IV. SIMULATION RESULTS

The proposed algorithm has been tested in simulation byconsidering the dynamic model of an ASCTEC PELICANquadrotor, characterized by massmb = 2kg and inertiamatrix Hb = diag([1.24, 1.24, 2.48]) [19]. The distancefrom each motor to the vehicle center of mass, the dragcoefficient and the thrust coefficient in (15) have been setto l = 0.25 m, γd = 7.5 · 10−7 Nm s2 and γt = 3 · 10−5

N s2, respectively.The quadrotor is equipped with a 5-DOF robotic manip-

ulator characterized by 5 revolute joints, disposed in sucha way that the first two joint axes intersect. The Denavit-Hartenberg parameters [20] of the arm are reported in TableI while Fig. 3 shows the joint axes. The mass and inertia of



Fig. 3. Robot manipulator.

the manipulator links have been estimated via a CAD model:the masses arem1 = 80g, m2 = 10g, m3 = 6g, m4 = 2gandm5 = 1.5g, while the moments of inertia of each linkthrough its center of gravity are given byIl1 = 1.04 · 10−4,Il2 = 2.27 · 10−4, Il3 = 4.56 · 10−5, Il4 = 7.18 · 10−5 andIl5 = 2.90 · 10−6.

TABLE I

DENAVIT-HARTEMBERG PARAMETERS FOR THE5 DOF MANIPULATOR

Joint d [mm] ϑ a [mm] α [rad]

1 0 ϑ1 0 −π/22 0 ϑ2 150 π/23 0 ϑ3 88 04 0 ϑ4 0 −π/25 18 ϑ5 0 0

In order to simulate a realistic scenario, only the mea-surements of position and orientation of the vehicle andthe manipulator joint positions have been assumed to beavailable, while the velocity measurements have been ob-tained via a first-order filter with a time constant of0.03 s.The quadrotor position can be obtained by resorting to GPS(few centimeters accuracy at a rate up to 10 Hz), externallocalization devices, such as VICON motion capture system(very high accuracy at about 200 Hz) or on-board camera-based systems (few centimeters accuracy at about 20-50Hz) [26]. As for the attitude measurements, the ASCTECPELICAN has an inbuilt IMU (Inertial Measurement Unit)equipped with 3 axis gyroscopes, providing the measure-ments of the attitude rates, 3 axis accelerometers, providingthe measurements of the accelerations, and a magnetometer,providing the absolute heading. Then, a filter within the IMUprovides an absolute attitude measurement at a rate of1 Khz[26]. Therefore, in order to simulate the measured data, thefollowing assumptions have been made:

• a normally distributed measurement noise has beenadded to the position signals; in detail for vehicleposition the noise has mean of 10−3 m and standarddeviation of5 ·10−3 m, for vehicle orientation the meanis 10−3 rad and the standard deviation is10−3 rad,and for the joint position the mean is10−4 rad andthe standard deviation is5 · 10−4 rad;

−2 −1 0 1 2

−2−1

01

2−1

0

1

2

3

4

x [m]y [m]

z [m

]

ActualDesired

Fig. 4. 3D trajectory of the end-effector.

• vehicle position and attitude are available at a frequencyof 25 Hz;

• manipulator joint position are available at a frequencyof 250 Hz.

The controller outputs are computed at a rate of 250 Hz.Finally, in order to test the robustness to the model

uncertainties, a nominal estimate of the inertia matrix hasbeen considered, whose elements are assumed to be equal to0.9 times their true values (i.e., a10% error).

The simulation model has been built in Matlab/Simulinkc©

environment. The controller parameters are summarized inTable II.

TABLE II

CONTROLLER PARAMETERS

Parameter Value Parameter Value

Kp,P 15 · I3 Kp,V 2.5 · I3

Kq,P 20 · I5 Kq,V 5 · I5

kψ,P 10 kψ,V 5

kθ,P 10 kθ,V 5

kφ,P 12.5 kφ,V 5

K 50 · I6

The end-effector is tasked to follow a circular helix withradius of 1.5 m and pitch ofπ/2 (Fig. 4) and, then, anhovering phase of5 s is commanded. Moreover, rotations ofthe end-effector ofπ/4 rad along roll, pitch and yaw axis isrequired.

Figs. 5–9 report the simulation results. In detail, Fig.5 shows the norm of end-effector position and orientationerrors between the desired and the planned values. The noiseaffecting the data are due to the termσb in (16). As forcontroller performance, Fig. 6 reports both the desired pathof the manipulator end-effector and the actual one, expressedin the fixed frame. The position and orientation errors areshown in Fig. 7, it can be seen that a good trajectory trackingis achieved: during the transient the maximum error is about4 ·10−2 m and4 ·10−2 rad, while at steady state the residualerrors are of the same order of magnitude of the measurementnoise mean. Finally, Fig. 8 and Fig. 9 show the controlefforts. In detail, Fig. 8 shows the thrust of the single rotorsof the vehicle, while Fig. 9 shows the joint torques of therobotic arm.



0 6 12 18 24 30 360

1

2

3

x 10−3

Time [s]

[m]

(a) position error

0 6 12 18 24 30 360

3

6

9

x 10−3

Time [s]

[rad

]

(b) orientation error

Fig. 5. Norm of inverse kinematics end-effector error.

0 6 12 18 24 30 36−2

−1

0

1

2

Time [s]

x [m

]

(a)

0 6 12 18 24 30 36−2

−1

0

1

2

Time [s]

y [m

]

(b)

0 6 12 18 24 30 360

1

2

3

4

Time [s]

z [m

]

DesiredActual

(c)

0 6 12 18 24 30 36

0

0.2

0.4

0.6

0.8

Time [s]

ψ [r

ad]

(d)

0 6 12 18 24 30 36

0

0.2

0.4

0.6

0.8

Time [s]

θ [r

ad]

(e)

0 6 12 18 24 30 36

0

0,2

0,4

0,6

0,8

Time [s]

φ[ra

d]

(f)

Fig. 6. Desired and actual trajectory for position (a–c) andorientation(d–f) of the end-effector.

Finally, the validity of the assumption done in Section III-B.1 is checked. To this aim, the norm of the inertia matrixM and the norm of the matrixM0, obtained by setting tozero the second and third column of the sub-matrixMpφ inM , have been compared during the simulation. The results,both in terms of absolute values and differences, have beenreported in Figs. 10(a) and 10(b), respectively. It can be seenthat the norms are very close, therefore it can be arguedthat the use ofM0 for control design in lieu ofM is anacceptable assumption.

Remark 3:The proposed controller, has been tested alsoin ideal conditions, i.e., in the absence of uncertainties andassuming a frequency of 1 KHz for all the measured data. Inthis case the error peaks are one order of magnitude lowerand of the same order of magnitude of the noise. The figuresreferred to this simulation are not reported for the sake ofbrevity.

V. CONCLUSIONS AND FUTURE WORK

In this paper, a novel hierarchical motion control schemehas been proposed for a quadrotor aerial vehicle equippedwith a robotic arm. First, on the basis of the desired poseof the arm end-effector, an inverse kinematics algorithmcomputes the references for the vehicle position, the vehicleyaw angle and for the manipulator joints. Then, a cascadecontroller ensures the tracking of the desired trajectories.A simulation case study proves the effectiveness of theapproach also in the presence of nonidealities and modeluncertainties. Future work will be focused on a rigorousstability analysis and on the experimental validation of thepresented results.

0 6 12 18 24 30 36−0.04

−0.02

0

0.02

0.04

Time [s]

[m]

pe,dx

−pex

pe,dy

−pey

pe,dz

−pez

(a) position error

0 6 12 18 24 30 36−0.04

−0.02

0

0.02

0.04

Time [s]

[rad

]

ψe,d

−ψe

θe,d

−θe

ψe,d

−ψe

(b) orientation error

Fig. 7. Position (a) and orientation (b) tracking error of the end-effector.

0 6 12 18 24 30 36

−20

0

20

40

Time [s]

[N]

(a) thrust rotor n. 1

0 6 12 18 24 30 36

−20

0

20

40

Time [s]

[N]

(b) thrust rotor n. 2

0 6 12 18 24 30 36−30

−20

−10

0

10

20

30

40

Time [s]

[N]

(c) thrust rotor n. 3

0 6 12 18 24 30 36−30

−20

−10

0

10

20

30

40

Time [s]

[N]

(d) thrust rotor n. 4

Fig. 8. Thrust of quadrotor.

ACKNOWLEDGMENTS

The research leading to these results has received fund-ing from the European Community’s Seventh FrameworkProgramme under grant agreement n. 287617 (collaborativeproject ARCAS).

The research activity of Giuseppe Muscio is supported byASI (Agenzia Spaziale Italiana).

REFERENCES

[1] S. Bouabdallah, A. Noth, R. Siegwart, “PID vs LQ control techniquesapplied to an indoor micro quadrotor,”Proc. of the IEEE/RSJInterna-tional Conf. on Intelligent Robots and Systems, pp. 2451-2456, 2004.

[2] G.M. Hoffmann, H. Huang, S.L. Waslander, C.J. Tomlin, ”Quadrotorhelicopter flight dynamics and control: Theory and experiment,” Proc.of the AIAA Guidance, Navigation, and Control Conf. and Exhibit,2007.

[3] S. Bouabdallah, P. Murrieri, R. Siegwart, ”Towards autonomous indoormicro VTOL”, Autonomous Robots, 18(2), pp. 171-183, 2005.

[4] J. Shin, D. Fujiwara, K. Nonami, K. Hazawa, ”Model-based optimalattitude and positioning control of small-scale unmanned helicopter,”Robotica, 23, pp. 51-63, 2005.

[5] J.P. How, B. Bethke, A. Frank, D. Dale, J. Vian, ”Real-timeindoor au-tonomous vehicle test environment,”IEEE Control Systems Magazine,28(2), pp. 5164, 2008.

[6] M. La Civita, G. Papageorgiou, W.C. Messner, T. Kanade, ”Design andflight testing of an H∞ controller for a robotic helicopter,”Journal ofGuidance, Control, and Dynamics, 29(2), pp. 485-494, 2006.

[7] H.J. Kim, D.H. Shim, ”A flight control system for aerial robots:Algorithms and experiments”,Control Engineering Practice, 11(2),pp. 1389-1400, 2003.

[8] E. Johnson, S. Kannan, ” Adaptive trajectory control forautonomoushelicopters,”Journal of Guidance, Control, and Dynamics, 28(3), pp.524-538, 2005.



0 6 12 18 24 30 36

−0.01

0

0.01

0.02

0.03

Time [s]

[Nm

]

τ1

τ2

τ3

(a) torques of joints 1–3

0 6 12 18 24 30 36−0,002

−0,001

0

0,001

0,002

Time [s]

[Nm

]

τ4

τ5

(b) torques of joints 4–5

Fig. 9. Joint torques for the robotic arm.

0 6 12 18 24 30 36

2,4839

2,484

2,4841

Time [s]

||M||||M

0||

(a) absolute values

0 6 12 18 24 30 36

0

1

2

3

4x 10−5

Time [s]

(b) ‖M‖ − ‖M0‖

Fig. 10. Comparison between the norm of the inertia matrix and the normof the matrixM0 used for control design.

[9] S. Bouabdallah, R. Siegwart, ”Backstepping and Sliding-mode Tech-niques Applied to an Indoor Micro Quadrotor”,Proc. of The IEEEInternational Conf. on Robotics and Automation (ICRA), pp. 2259-2264, 2005

[10] T. Madani, A. Benallegue, ”Backstepping sliding-mode control appliedto a miniature quadrotor flying robot”,Proc. of the 32nd Annual Conf.of the IEEE Industrial Electronics Society, pp. 700-705, 2006.

[11] F. Kendoul, D. Lara, I. Fantoni, R. Lozano, ”Real-time nonlinearembedded control for an autonomous quadrotor helicopter”,Journalof Guidance, Control, and Dynamics, 30(4), pp. 1049-1061, 2007.

[12] F. Kendoul, I. Fantoni, R. Lozano, “Asymptotic stability of hierarchicalinner-outer loop-based flight controllers,”Proceedings of the 17thIFAC World Congress, Seoul. KR, pp. 1741–1746, 2008.

[13] F. Kendoul, Y. Zhenyu, K. Nonami, ”Guidance and Nonlinear ControlSystem for Autonomous Flight of Minirotorcraft Unmanned AerialVehicles”, Journal of Field Robotics, 27(3), pp. 311-334, 2010.

[14] F. Kendoul, K. Nonami, I. Fantoni, R. Lozano, ”An adaptive vision-based autopilot for mini flying machines guidance, navigationandcontrol”, Autonomous Robots, 27(3), pp. 165-188, 2009.

[15] M. Bl osch, S. Weiss, D. Scaramuzza, R. Siegwart, ”Vision based MAVnavigation in unknown and unstructured environments”,Proc. of 2010IEEE International Conf. on Robotics and Automation (ICRA) pp. 21-28, 2010.

[16] P. Pounds, D. Bersak, A. Dollar, ”Grasping from the air:Hoveringcapture and load stability”,Proc. of 2011 IEEE International Conf.on Robotics and Automation, pp. 2491-2498, 2011.

[17] V. Ghadiok, J. Goldin, W. Ren, ”Autonomous indoor aerialgrippingusing a quadrotor”,Proc. of 2011 IEEE/RSJ International Conf. onIntelligent Robots and Systems, pp. 4645-4651, 2011.

[18] D. Mellinger, Q. Lindsey, M. Shomin, V. Kumar, ”Design, modelling,estimation and control for aerial grasping and manipulation”, Proc.of 2011 IEEE/RSJ International Conf. on Intelligent Robots and Sys-tems, pp. 2668-2673, 2011.

[19] V. Lippiello, F. Ruggiero, “Cartesian Impedance Control of UAV witha Robotic Arm,”10th International IFAC Symposium on Robot Control(SYROCO), 2012.

[20] B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo,Robotics – Modelling,Planning and Control, Springer, London, UK, 2009.

[21] K. Nonami, F. Kendoul, S. Suzuki, W. Wang,Atonomous Fly-ing Robots, Unmanned Aerial Vehicles and Micro Aerial Vehicles,Springer, 2010.

[22] S. Chiaverini, ”Singularity-robust task-priority redundancy resolutionfor real-time kinematic control of robot manipulators,”IEEE Trans-actions on Robotics and Automation, Vol. 13, pp. 398–410, 1997.

[23] G. Antonelli, “Stability analysis for prioritized closed-loop inverse

kinematic algorithms for redundant robotic systems”,IEEE Transac-tion on Robotics, vol. 25, pp. 985–994, 2009.

[24] F. Caccavale, S. Chiaverini, B. Siciliano, “Second-order kinematic con-trol of robot manipulators with jacobian damped least-squares inverse:Theory and experiments,”IEEE/ASME Transactions on Mechatronics,vol. 2, pp. 188–194, 1997

[25] S. Chiaverini and B. Siciliano, “The unit quaternion: Auseful tool forinverse kinematics of robot manipulators,”Systems Analysis ModellingSimulation, vol. 35, pp. 45–60, 1999.

[26] G.H. Lee, M. Achtelik, F. Fraundorfer, M. Pollefeys, R.Siegwart,”A Benchmarking Tool for MAV Visual Pose Estimation”,Proc.of the 11th Int. Conf. on Control, Automation, Robotics and Vision(ICARCV), pp. 1541-1546, 2010.



control of quadrotor aerial vehicles equipped with a ... of quadrotor... · control of quadrotor...

Documents