Aerial Robots with Rigid/Elastic-joint Arms: Single-joint Controllability Study and Preliminary Experiments

HAL Id: hal-01348545

https://hal.archives-ouvertes.fr/hal-01348545

Submitted on 25 Jul 2016

HAL is a multi-disciplinary open access archive for the deposit and dissemination of sci- entific research documents, whether they are pub- lished or not. The documents may come from teaching and research institutions in France or abroad, or from public or private research centers.

L’archive ouverte pluridisciplinaire HAL, est destinée au dépôt et à la diffusion de documents scientifiques de niveau recherche, publiés ou non, émanant des établissements d’enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.

Aerial Robots with Rigid/Elastic-joint Arms:

Single-joint Controllability Study and Preliminary Experiments

Burak Yüksel, Nicolas Staub, Antonio Franchi

To cite this version:

Burak Yüksel, Nicolas Staub, Antonio Franchi. Aerial Robots with Rigid/Elastic-joint Arms: Single-

joint Controllability Study and Preliminary Experiments. 2016 IEEE/RSJ International Conference

on Intelligent Robots and Systems (IROS 2016), Oct 2016, Daejeon, South Korea. �hal-01348545�

Preprint version, final version at http://ieeexplore.ieee.org/ 2016 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, Daejeon, South Korea

Aerial Robots with Rigid/Elastic-joint Arms:

Single-joint Controllability Study and Preliminary Experiments

Burak Y¨uksel ¹ , Nicolas Staub ² and Antonio Franchi ²

Abstract— We present the dynamic modeling, analysis, and control design of a Planar-Vertical Take-Off and Landing (PVTOL) underactuated aerial vehicle equipped either with a rigid- or an elastic-joint arm. We prove that in both cases the system is exactly linearizable with a dynamic feedback and differentially flat for the same set of outputs (but different controllers). We compare the two cases with extensive and realistic simulations, which show that the rigid-joint case outperforms the elastic-joint case for aerial grasping tasks while the converse holds for link-velocity amplification tasks. We present preliminary experimental results using a actuated joint with variable stiffness (VSA) on a quadrotor platform.

I. I NTRODUCTION

The scientific interest on aerial robotics is increasing everyday with technological developments done over the last two decades. Vertical Take-Off and Landing (VTOL) vehicles, such as multi-rotors and ducted fan platforms, are able to track complex trajectories [1], and have been mainly used as flying sensors for several fields, such as surveil- lance, search and rescue, civil monitoring, agriculture, and multi-vehicle human collaboration with haptic interfaces [2]–

[7]. More recently, another application area of the flying robots became a great interest of the researchers; Aerial Physical Interaction (APhI). In such application, a flying robot is required to exert certain forces and torques to the environment, while maintaining a stable flight [8]. Examples of APhI are various, such as, e.g., tool operation [9], [10], surface inspection [11], object transportation [12], [13] and manipulation [14], and tethered flight [15], [16].

Aerial grasping is one important example of APhI, where flying robot grasps a moving or a stationary object during flight. This was firstly studied in [17], where the aerial robot is equipped with a rigid tool that is not moving w.r.t.

the aerial robot’s body-fixed frame. In [18] an aerial robot equipped with a rigid-joint arm system has been presented, and the flatness property is used for aerial grasping task.

In almost all the current designs, aerial manipulators are equipped with rigid-joint arms. On the other hand, compliant- joint manipulators are widely used in ground robots like humanoids and manipulators physically interacting with hu- mans [19]. They are also effective tools for fast motion tasks, exploiting the elasticity of the joint such as throwing an object or hammering on a surface, which requires high veloc- ities that rigid-joint arm cannot provide [20]. Yet, their usage

1

Max Planck Institute for Biological Cybernetics, Spemanstr. 38, 72076, T¨ubingen, Germany. [email protected]

2

LAAS-CNRS, Universit´e de Toulouse, CNRS, Toulouse, France, [email protected] , [email protected]

This work is partially funded by 1) the Eiffel Excellence Scholarship Programme by the French Ministry of Foreign Affairs and International Development, and by 2) the European Union’s Horizon 2020 research and innovation programme under grant agreement No 644271 AEROARMS.

in aerial robotics and APhI is not fully investigated, except presumably for [21], which shows the passive compliance of an elastic-joint arm attached to a flying quadrotor.

Driven by these facts, the goal of this paper is to rigorously study the tracking capabilities of aerial robots equipped with either a rigid- or an elastic-joint. Our main contributions are: i) to show that both rigid- and elastic-joint systems are exactly linearizable via a dynamic feedback and therefore are differentially flat ¹ ; ii) to present two nonlinear con- trollers (one for the rigid-joint case and one for the elastic- joint case) for the independent control of the flat outputs, namely the position of the aerial platform CoM and the absolute orientation of the link; iii) to use realistic dynamical simulations (including accurate dynamical modeling of all the inertial parts, parameter uncertainty, quantization, and noise) to validate the proposed controllers and to compare the rigid-joint case and the elastic-joint case, showing that the former is more suited for pure tracking tasks such as the aerial grasping while the latter is better shaped for tasks involving velocity maximization such as throwing and hammering (these results are given mainly as a technical report of this paper in [22], due to the page limitation); iv) a set of preliminary experiments, which show what we believe is the very first flying quadrotor plus single-joint arm with a Variable Stiffness Actuator (VSA) setup in the literature.

The interested reader is encouraged to read also [23] for interesting preliminary results on multi-link arms.

The paper is organized as follows. In Sec. II we present the kinematics of both systems. Section III presents the dynamic model in the rigid-joint case and shows its exact feedback linearization and flatness properties. Section IV does the same for the elastic-joint case. In Sec. V we present a possible outer-loop controller. In Sec. VI-A we explain the experimental setup of our flying robot, consisting of a quadrotor and Qbmove ² . Full and realistic simulation results that show the capabilities of the controllers and compare the two different cases are shown in Sec. II of [22].

II. S YSTEM M ODEL

In this section we model the kinematics and dynamics of a robot composed of an aerial platform (VTOL) equipped with a rigid- or elastic-joint arm. Similar to previous studies (see, e.g., [18], [24]) we consider here the case of a (vertical) planar-VTOL (PVTOL) aerial platform. This reduced system still captures the nonlinear features and the underactuation

1

We depart from [18] in two main aspects: i) we consider a more generic model where the center of thrust actuation is not forced to coincide with the center of mass (CoM) of the vehicle, which covers a larger class of realistic designs. ii) we consider also the case of elastic-joint arm (absent in [18]).

2

http://www.qbrobotics.com/ , and http://www.naturalmotioninitiative.org

− u _t z 1 P _G m ₁

d_G

P _W x _W z _W

x ₁

z ₁

¯ g:gravity

P _C

₁

p c

1

p c p c

2

m _m τ u _r

z _W z 1

z m z 2

θ₁

θ_m θ_e

θ2

θ₁₂ θ1m

p e

d2 ^x2

m ₂

z₂ d_e

P _E z m

x m

P _C

₂

P _E P _C

P _C

₂

P _C

₁

P _C

Fig. 1: Left: a sketch of the PVTOL equipped with an actuated link. Notice the offset between the CoM of the PVTOL (P C

₁

, red point) and the center of actuation (P G , green point). The rigid link is attached to P _C

₁

, around which the motor rotates and a either rigid or elastic joint is placed, i.e. P C

₁

= P M .

Right-up: relative and absolute angles of the rigid bodies, where the length of the z axes are made different just for better illustration.

Right-down: locations of important points on the arm.

of a 3D system, and allows to generalize the obtained results in a later stage. Furthermore, many practical aerial problems are, fundamentally, 2D problems immersed in a 3D world.

The PVTOL with the attached arm is depicted in Fig. 1.

We denote with F W : { P W , x _W , z _W } and F 1 : { P C

₁

, x ₁ , z ₁ } , the world (inertial) frame and the frame attached to the PVTOL, respectively, where P _C

₁

is the CoM of the PVTOL (without the arm). Both the motor and the joint of the arm rotate about an axis parallel to z _W × x _W and passing through P _C

₁

(i.e. P _C

₁

= P _M ). We then define the motor frame as F M : { P _M , x ₁ , z ₁ } that is rigidly attached to the motor output shaft. The joint can be either rigid or elastic, as explained later, therefore we consider also a link frame F 2 : { P _C

₂

, x ₂ ,z ₂ } , where P _C

₂

is the CoM of the link. Finally we denote with P E the center point of end-effector.

Given an angle θ _∗ ∈ R between the z-axes of two frames (all the angles are given in Fig. 1) the usual rotation matrix definition R _∗ ∈ SO(2) holds. Therefore, the orientations of F 1 in F W , F M in F 1 , F 2 in F 1 , and F 2 in F M are expressed by the rotation matrixes R ₁ , R _m , R ₂ , and R _e , respectively. Finally, the absolute motor angle is θ 1m = θ 1 +θ _m and absolute link angle is θ 12 = θ 1 +θ ₂ , as depicted in Fig. 1 (right). Notice that θ _e = θ ₂ − θ _m = θ ₁₂ − θ _1m remains zero if the joint is rigid and can be any if the link is elastic.

The constant position of P _C

₁

in F 2 is denoted with − d ₂ = [ − d 2

_x

− d 2

_z

] ^T ∈ R ² . The vector d _e = [d _e

_x

d e

_z

] ^T ∈ R ² denotes the constant position of the end-effector P _E in F 2 . The (time- varying) positions of P _C (CoM of the overall system), P _C

₁

, P _C

₂

and P _E in F W are denoted with p _c = [x _c z c ] ^T ∈ R ² , p _c

₁

= [x ₁ z ₁ ] ^T ∈ R ² , p _c

₂

= [x ₂ z ₂ ] ^T ∈ R ² , and p _e = [x _e z _e ] ^T ∈ R ² , respectively. The mass and moment of inertia of PVTOL, motor, and link are denoted with m ₁ ∈ R >0 , J ₁ ∈ R >0 ; m _m ∈

R >0 , J _m ∈ R >0 ; m ₂ ∈ R >0 , J ₂ ∈ R >0 , respectively. We use

the symbol ¯ g ∈ R ⁺ to denote the the gravitational constant.

The PVTOL is actuated by means of: i) a total thrust force − u _t z ₁ ∈ R ² applied at a point P _G , where u _t ∈ R is its magnitude, and ii) a total torque (moment) u _r (z ₁ × x ₁ ) ∈ R ² applied at P _G , where u _r ∈ R is the torque intensity. ³

3

For a planar birotor, P

_G

is the center of two coplanar propellers, u

_t

the sum of the propeller thrusts and u

_r

their difference times the distance to P

_G

.

Furthermore, a motor is attached to the PVTOL and applies a torque τ(z ₁ × x ₁ ) ∈ R ² at P _C

₁

to the joint, where τ ∈ R is its intensity. The inputs of the system are gathered in the vector u = [u _t u _r τ] ^T ∈ R ³ and shortly denoted in the following as thrust, PVTOL torque and motor torque. The constant position of P _G in F 1 is denoted with d _G = [d _G

_x

d _G

_z

] ^T ∈ R ² . Remark 1. We extend the literature in two directions: first we assume that P _C

₁

6≡ P _G (i.e., d _G is any , contrarily to what is typically assumed in the literature, see e.g., [18] where d _G = [0 0] ^T ); and second we also consider, for the first time, the case in which the joint can be elastic and not only rigid.

Two cases are then investigated, briefly referred to as Case- R (rigid case) and Case-E (elastic case). In both cases the system dynamics is written using the Lagrange equation as

¨

q = M ^{− 1} (q)(G(q)u − c(q, q) ˙ − g(q) + f _E (q) + f _ext ) (1) where q ∈ R ⁿ are the considered generalized coordinates (n = 4 for Case-R whereas n = 5 for the Case-E), M ∈ R ^{n × n} is the generalized inertia matrix, G ∈ R ^{n × 3} is the control input matrix, c ∈ R ⁿ is the centrifugal/Coriolis forces, g ∈ R ⁿ represents the gravitational forces, and f _E ∈ R ⁿ represents the forces due to the potential energy stored in the elastic joint (in the rigid-joint case f _E = 0). Finally, f _ext ∈ R ⁿ represents the wrench applied to the system from external environment.

If y ∈ R ³ is a flat output of (1), then denoting the higher- order derivatives of y with ¯ y, one can write the following

¯

y = ¯ f(¯ x) + G(¯ ¯ x)¯ u, (2a)

¯

u = G ¯ ^{− 1} (v − ¯ f), (2b)

¯

y = v, (2c)

where ¯ x ∈ R ^{n ¯} is the augmented state of dimension ¯ n, ¯ G is an invertible decoupling matrix, and v is a virtual input. Then 2b describes the linearizing control law that brings the system in the form of 2c, which is linear and controllable, as long as G ¯ is invertible, and moreover no zero dynamics appear if the total relative degree matches with ¯ n. We shall use this useful property throughout this paper. For the interested reader, we provide an informal review of exact feedback linearization and differential flatness in Sec. I of [22].

III. C ASE R: PVTOL WITH A R IGID -J OINT A RM

In this section we show that [p ^T _c

1

θ ₁₂ ] ^T is an exactly linearizing (i.e., flat) output for the system of Case-R. In order to prove that, let us choose as generalized coordinates q = [p ^T _c

1

θ ₁ θ ₁₂ ] ^T ∈ R ⁴ . Then let us compactly write R ¯ _∗ =

∂R

_∗

∂ θ

_∗

. With respect to those coordinates, the terms in (1) are the following (computations are omitted for brevity):

M =





m _s I ₂ ∗ ∗ 0 _1×2 m _A ∗ β

β

β ^T (θ ₁₂ ) 0 m B



 = M ^T ∈ R ^{4 × 4} , where (3) m _s = m ₁ + m _m +m ₂ , m _A = J ₁ ,

m _B = m ₂ k d ₂ k ² 2 +J ₂ + J _m , β β β (θ ₁₂ ) = m ₂ R ¯ ₁₂ d ₂ ∈ R ² ,

Preprint version, final version at http://ieeexplore.ieee.org/ 2 2016 IEEE/RSJ IROS

c(q, q) = ˙









β 1 (θ ₁₂ ) θ ˙ ₁₂ ² β 2 (θ ₁₂ ) θ ˙ ₁₂ ²

0 0









, g(q) =





 0

− m _s g ¯ 0 g ₄ (θ ₁₂ )





 , (4)

β 1 = − m ₂ (d ₂

_x

cos(θ ₁₂ ) + d ₂

_z

sin(θ ₁₂ )), β 2 = − m ₂ (d ₂

_z

cos(θ ₁₂ ) − d ₂

_x

sin(θ ₁₂ )), g ₄ = m ₂ g(d ¯ ₂

_x

cos(θ ₁₂ ) + d ₂

_z

sin(θ ₁₂ )), G(q) =







− sin(θ ₁ ) 0 0

− cos(θ ₁ ) 0 0 d _G

_x

1 − 1

0 0 1





 , f _E = 0 _{4 × 1} . (5) The explicit functional dependency of ¨ q then becomes

¨

x ₁ = f ₁ (θ ₁ , θ 12 , θ ˙ 12 , u _t , τ), z ¨ ₁ = f ₂ (θ ₁ , θ 12 , θ ˙ 12 , u _t , τ), θ ¨ ₁ = f ₃ (u _t , u _r ,τ), θ ¨ ₁₂ = f ₄ (θ ₁ , θ ₁₂ , θ ˙ ₁₂ , u _t , τ). (6) Now choose y = [p ^T _c

1

θ ₁₂ ] ^T ∈ R ³ as output. From (6), it is

¨

y = fff 1 (θ ₁ ,θ ₁₂ , θ ˙ 12 , u t , τ). In this case, the relative degree of the system is r = 2 + 2 +2 = 6 and the total number of states is ¯ n = 2n = 8. As explained in Sec. I of [22], we aim at equate these two values. This is achieved by considering as new control inputs ¯u = [ u ¨ _t u _r τ] ¨ ^T ∈ R ³ , and augmented state

¯

x = [q ^T q ˙ ^T u _t u ˙ _t τ τ] ˙ ^T ∈ R ¹² , thus ¯ n = 12. Taking the time derivative of ¨ y twice and substituting ¨ θ 12 and ¨ θ 1 from (6):

y ⁽⁴⁾ = fff ₃ (θ ₁ , θ ₁₂ , θ ˙ ₁ , θ ˙ ₁₂ ,u _t , u ˙ _t ,τ, τ, ˙ u ¨ _t , u _r , τ), ¨ (7) which implies that r = 4 +4 + 4 = 12 and ¯ y = y ⁽⁴⁾ . Therefore it is now worth checking the invertibility of ¯ G. In fact it is possible to analytically compute the determinant of ¯ G

det( G) = ¯ − ^u

^t

J

1

m

s

(J

₂

+J

_m

)m

_s

+m

₂

(m

₁

+m

_m

) k d

₂

k

²2

. (8) Therefore ¯ G is always invertible, as long as u _t 6 = 0. Being G(¯ ¯ x) invertible, and knowing that ¯ y = y ⁽⁴⁾ and ¯ f(¯ x) = y ¯ − G(¯ ¯ x)¯ u from (2a), the controller in the form of (2b) is exactly linearizing, i.e., it brings the system to the linear controllable form (2c). We have then just proved the following statement:

Proposition 1. The vector [p ^T _c

1

θ 12 ] ^T is an exactly linearizing output via dynamic feedback for the model in Case-R, as long as u _t 6 = 0. As a consequence, it is also a flat output.

Proposition 1 states the differential flatness indirectly. A direct proof using the algebraic map from the outputs to the states and inputs is provided in Sec. I-A of [22].

IV. C ASE -E: PVTOL WITH AN E LASTIC -J OINT A RM

In this section we show that for Case-E the output y = [p ^T _c

1

θ 12 ] ^T is also an exactly linearizing (i.e., flat) output. In order to prove it, let us consider as generalized coordinates q = [p ^T _c

1

θ 1 θ 12 θ 1m ] ^T ∈ R ⁵ , where an additional coordinate θ _1m entered to the configuration because of the elastic-joint.

In order to fix the ideas, an idealized elastic connection is sketched in Fig. 2, where θ 2 = θ m + θ e . In this case the matrices of the dynamical model (1) are, after some algebra,

M =







m s I 2 ∗ ∗ ∗

0 _{1 × 2} J ₁ ∗ ∗

β β

β ^T (θ ₁₂ ) 0 m _B − J m ∗

0 1 × 2 0 0 J m





 = M ^T ∈ R ^{5 × 5} , (9)

θ ₁ θ _m θ _e

θ ₂ z _W z ₁ pvtol

motor link

z _m z ₂

Fig. 2: An ideal example of elastic joint between the motor output shaft and link. Proportions are distorted for illustration purposes.

The innermost circle, fixed to F 1 , represents the PVTOL. The middle circle, fixed to F M , represents the actuator (or motor).

The outermost circle is connected to the middle circle via elastic components, and it is rigidly connected to the link (fixed to F 2 .)

c(q, q) = ˙









β 1 (θ 12 ) θ ˙ ₁₂ ² β 2 (θ 12 ) θ ˙ ₁₂ ²

0 0 0









, g(q) =







 0

− m s g ¯ 0 g 4 (θ 12 )

0







 ,

G(q) =











− sin(θ ₁ ) 0 0

− cos(θ 1 ) 0 0 d _G

_x

1 − 1

0 0 0

0 0 1











, f E (q) =









 0 0 0 f l (θ 1m ,θ 12 ) f m (θ _1m ,θ ₁₂ )









 . (10)

Notice that f _l (θ _1m , θ 12 ) is the elastic force acting on the link side, and f _m (θ _1m ,θ ₁₂ ) is the elastic force acting on the motor side. These forces can be nonlinear functions of θ 1m and θ 12 . In the linear spring case (or in the nonlinear case but for small deviations) we have that f _l (θ _1m , θ ₁₂ ) = k _e (θ _1m − θ 12 ) and f _m (θ _1m , θ 12 ) = k _e (θ ₁₂ − θ 1m ), where k _e > 0 is the stiffness (local, for the nonlinear case) of the elastic element.

Replacing M, c, g, G and f _E in (1) we can derive the explicit dependency of each entry of ¨ q, here summarized:

¨

x ₁ = ξ ₁ (θ ₁ ,θ ₁₂ , θ ˙ ₁₂ , θ _1m , u _t ), z ¨ ₁ = ξ ₂ (θ ₁ , θ ₁₂ , θ ˙ ₁₂ , θ _1m , u _t ), θ ¨ 1 = ξ 3 (u _t , u _r , τ), θ ¨ 12 = ξ 4 (θ ₁ , θ 12 , θ ˙ 12 , θ 1m , u _t ), θ ¨ 1m = ξ 5 (θ _1m ,θ ₁₂ , τ). (11)

Considering, as before, y = [p ^T _c

1

θ 12 ] ^T , from (11) we have

¨

y = ξ ξ ξ ₁ (θ ₁ , θ ₁₂ , θ ˙ ₁₂ ,θ _1m ,u _t ). (12) The total relative degree is r = 2 + 2 + 2 = 6 and the state dimension is ¯ n = 2n = 10. As explained in Sec. I of [22], we aim at equate these two values. This is achieved by consider- ing as new control inputs ¯u = [ u ¨ t u r τ] ^T ∈ R ³ , and augmented state ¯ x = [q ^T q ˙ ^T u _t u ˙ _t ] ^T ∈ R ¹² . In fact, differentiating twice w.r.t. time and substituting ¨ θ 12 , ¨ θ 1 , ¨ θ 1m from (11) we have

y ⁽⁴⁾ = ξ ξ ξ 3 (θ ₁ , θ 12 , θ 1m , θ ˙ 1 , θ ˙ 12 , θ ˙ 1m , u _t , u ˙ _t , u ¨ _t , u _r , τ), which means that r = 4 + 4 + 4 = 12 and ¯ n = 12, as wanted.

Therefore it is now worth to check if the matrix ¯ G(¯ x) is invertible. The analytical expression of its determinant is

det( G) = ¯ − ^u

^t

^k

^e

J

1

J

m

m

s

(J

₂

m

s

+m

₂

(m

₁

+m

_m

) k d

₂

k

²2

. (13) Therefore ¯ G is always invertible, as long as u _t 6 = 0 and k _e 6 = 0.

Being ¯ G(¯ x) invertible, and knowing that ¯ y = y ⁽⁴⁾ and ¯ f(¯ x) =

Configuration Variables Linearizing (Flat) Outputs Relative Degree Augmented States New Inputs Case R: q = [p

^T_c

1

θ

1

θ

12

]

^T

∈ R

⁴

y = [p

^T_c

1

θ

12

]

^T

∈ R

³

y ¯ = [p

⁽⁴⁾c₁ T

θ

₁₂⁽⁴⁾

]

^T

x ¯ = [q

^T

q ˙

^T

u

_t

u ˙

_t

τ τ] ˙

^T

∈ R

¹²

¯u = [ u ¨

_t

u

_r

τ] ¨

^T

∈ R

³

Rigid-joint r = 12 n ¯ = 12

Case E: q = [p

^T_c1

θ

1

θ

12

, θ

1m

]

^T

∈ R

⁵

y = [p

^T_c1

θ

12

]

^T

∈ R

³

y ¯ = [p

⁽⁴⁾c₁ T

θ

₁₂⁽⁴⁾

]

^T

x ¯ = [q

^T

q ˙

^T

u

_t

u ˙

_t

]

^T

∈ R

¹²

¯u = [ u ¨

_t

u

_r

τ]

^T

∈ R

³

Elastic joint r = 12 n ¯ = 12

TABLE I: First row: summary for the Case-R (rigid-joint arm). Second row: summary for the Case-E (elastic-joint arm). In both cases the total number of states ¯ n matches with the total relative degree r, which implies exact linearization and absence of internal dynamics.

¯

y − G(¯ ¯ x) u ¯ from (2a), the controller in the form of (2b) is exactly linearizing, i.e., it brings the system to the linear controllable form (2c). We have then just proved that:

Proposition 2. The vector [p ^T _c

1

θ 12 ] ^T is an exactly linearizing output via dynamic feedback for the model with elastic- joint arm (Case-E), as long as u _t 6 = 0 and k _e 6 = 0. As a consequence, they is is also a flat output.

Proposition 2 states the differential flatness indirectly. A direct proof using the algebraic map from the outputs to the states and inputs is provided in Sec. I-B of [22].

Remark 2. In Case-R both u _t and τ have to be ‘delayed’

twice, while for Case-E this is needed only for u _t . This happens because the spring in Case-E is a natural double integrator, so further delaying for τ is not needed.

Remark 3. Contrarily to the grounded manipulator case [25], where the flat outputs are the relative orientation of the consecutive links and motors, in the aerial case one has to consider the absolute link and motor orientations. This is due to the underactuation of the flying platform that we are using as the base of the rigid/elastic-joint arm.

Corollary 1. For both Case-R and Case-E, y = [p ^T _e θ 12 ] ^T is also a flat (and exactly linearizing) output.

Proof. The end-effector position p _e is a function of the sole y = [p ^T _c

1

θ ₁₂ ] ^T , in fact p _e = p _c

₁

+ R ₁₂ (θ ₁₂ )(d ₂ + d _e ).

Table I summarizes the results of Sections III and IV.

V. O UTERMOST C ONTROL L OOP

We have presented the feedback linearizing (nonlinear) controllers in Sec. III and Sec. IV, where the flat outputs are (in both cases) y = [p _c

₁

θ 12 ] ^T . The nonlinear inner loop (2b) is first used to bring the system in the decoupled and controllable linear form, as explained in Sec. II, and also in Sec. I of [22]. Then, given any 3-ple of desired trajectories of class C ³ , x ^d ₁ (t ), z ^d ₁ (t), θ ₁₂ ^d (t) for x 1 , z _m , and θ 12 , respectively, many outer control methods can be used as, e.g.,

v = y ¯ ^d + K[e ^T e ˙ ^T e ¨ ^T ...

e ^T ] ^T ∈ R ³ , (14) where e = [e _x e _z e _θ ] ^T and e _x = x ^d ₁ − x 1 , e _z = z ^d ₁ − z 1 , e _θ = θ ₁₂ ^d − θ 12 , and with K being the juxtaposition of 3 × 3 positive definite gain matrixes with properly chosen elements. To compensate the errors due to uncertainties, an integral term K _{i ∗} ^R _t ^t

^f

0

e _∗ dt is added in the outer loop of each channel, where

∗ := { x, z, θ } and K i ∗ ∈ R >0 . A scheme of whole control loop

for the Case-E can be found in Fig. 1 of [22].

Remark 4. Notice that this control strategy needs only the measurements of q and q, since the derivatives of the outputs ˙

present in (14) are computed as algebraic functions of q and

˙

q, thanks to the model (1) and its analytical derivatives.

VI. P RELIMINARY E XPERIMENTS

In this section we present our experimental setup, which consists of a quadrotor equipped with a rigid link that is actuated via a VSA, and present our preliminary exper- imental results. Extensive and realistic simulation results (with parametric uncertanties, measurement noises, sampling errors and actuation limits) are provided in [22] and in the video attachment of this paper.

A. Preliminary Experiments

Table II presents the main components of the experimental setup. We implemented all the controller parts sketched in Fig. 1 of [22] as C/C++ libraries and ROS nodes, similarly to what done in [27] and [28].

1) Preparation of the Qbmove: We decided to use a variable stiffness actuator for its wide range of stiffness preset capabilities which allow the user to choose between, e.g., high and low stiffness values, depending on the task of the robot (see also Sec. II of [22]). We opted for the Qbmove, an agonistic/antagonistic servo-VSA, whose spec- ifications are available ² . The meticulous reader will see that no gripper is featured in our setup, this is mainly due to safety considerations on the maximal deflection supported by the Qbmove. In order for our controller to work with Qbmove, several extra steps have to be conducted. First of all, a parametric identification of the Qbmove + rigid arm system has been performed in order to retrieve the parameters of the equivalent motor in Fig 2 (for more details see [22]).

All the identified and computed parameters are available in Table III. Moreover, the control framework requires a torque-controlled motor, while a Qbmove is not proposing this control modality. For this reason we have implemented an outer loop controller around the Qbmove device, which translates the desired torque into a desired position using the estimated parameters and second order system model. This bridge between the proposed controller of the paper and the

Component description key figure

VTOL mikrokoprer Quadrotor

⁴

total mass (incl. payload) 1.5kg max. thrust and torque 28N, 1.5Nm Qbmove Variable Stiffness Actuator

²

(VSA)

max. torque 1.2Nm

control frequency 500Hz

Optitrack Motion Capture System (MoCap) 100Hz

6-axis IMU 1kHz

Kalman Filter: IMU+MoCap 1kHz

TABLE II: Characteristics of the main components of the setup.

Preprint version, final version at http://ieeexplore.ieee.org/ 4 2016 IEEE/RSJ IROS

x ₁

z w x w y w g z ₁

A

P _C

₁

B P _M

(a) (b) (c)

Fig. 3: Evolution from theory to application. a) Conceptual sketch of the model in 3D. The motion in Plane-A is controlled using the controller presented in this paper. The motion in Plane-B (except the translational motion along z ₁ ) and the rotation around z ₁ is controlled using a near-hovering controller [26]. b) CAD model of the 3D system and a snapshot from SimMechanics simulation, where the implemented controllers have been tested. Different colors correspond to the different parts of the real system. The results are given in the video attachment of this paper. c) Real system on flight. On board of the VTOL, from top to bottom, there are MoCap markers, an Odroid-XU computer, four brushless motor controllers with their power board, a flight controller (incl. IMU), battery pack, Qbmove with its connectors, a rigid arm attached to it. Red ropes are used only for safety reasons, with no tension on them.

Qbmove is directly implemented as a ROS node. We avoid the details on this part for the sake of brevity.

2) Quadrotor Setup: The Quadrotor setup is extensively described and depicted in both Figs. 3b and 3c.

3) Preliminary Experiment of a Quadrotor with a VSA Arm: In the first experiment we tested the complete system for a trajectory tracking along the z axis while staying at zero on the x axis. Results are given in Fig. 4, where the maximum error for both x ₁ and z ₁ is around 2cm. In this experiment the desired motion of the arm is constant.

In the second experience performed, the absolute link orientation follows a sinusoidal trajectory, while the PVTOL CoM follows another trajectory along the z axis and tries to stay at zero on the x axis. Results are given in Fig. 5, where for x ₁ and z ₁ the maximum errors are around 2cm. Control inputs are omitted here and presented in Fig. 7 of [22].

For both experiments, steady-state errors are observed mainly due to unmodeled effects as, e.g., neglecting the damping of the spring, and the displacement between the P _C

₁

and P _M in the real setup (see Fig. 3a), while in theory we considered them coincident (see Fig. 1).

VII. C ONCLUSIONS

In this paper we presented the dynamic modeling, property analysis, and control of a PVTOL system equipped either

Real Parameters Notation Value Unit

mass of the quadrotor m ˜

1

1.309 kg

mass of the VSA m ˜

m

0.06 kg

mass of the arm m ˜

2

0.098 kg

dis. vec. betw. P

C₁

& P

G

d ˜

G

[0.0 0.0081]

^T

m dis. vec. betw. P

C₂

& P

M

d ˜

2

[0 0.0979]

^T

m inertia of the PVTOL J ˜

1

0.0154 kgm

²

motor inertia J ˜

_m

0.4101 kgm

²

link inertia J ˜

₂

0.0011 kgm

²

spring stiffness ˜ k

_e

3.55 Nm/rad

spring damping k ˜

_f

0.07 Nms/rad

TABLE III: Measured, computed or identified parameters of the setup. The variable ˜ ∗ denotes the quantity ∗ for the experimental setup. Notice that ˜ k f is identified but not used in the controller.

0 5 10 15

−0.5 0 0.5

x

1

, [m ]

PVTOL Position

0 5 10 15

−1.2

−1.1

−1

z

1

, [m ]

PVTOL Position

0 5 10 15

−0.1

−0.05 0 0.05 0.1

θ

1

, [r a d ]

PVTOL Orientation

Time , [s]

0 5 10 15

−0.2

−0.1 0 0.1 0.2 0.3

θ

12

, [r a d ]

Absolute Link Orientation

Time , [s]

measured desired

Fig. 4: First test on controlling the Quadrotor + VSA arm position along x and z directions. A step-like trajectory is followed along the z-axis. Notice that negative z is upwards.

with a rigid- or an elastic-joint arm. We have proven that both systems are differentially flat for the same set of outputs (see Table I as well), and we provided exact linearization tracking controllers for each cases, separately. Extensive numerical tests, provided in [22], show clear differences be- tween the two models; rigid-link setup is more advantageous for precisely tracking tasks such as aerial grasping, while elastic-link setup is more suitable for tasks requiring link velocity amplification such as throwing or hammering. We then performed another numerical validation using the full 3D model of the real setup in SimMechanics, and finally we presented our preliminary experimental results of controlling a quadrotor VTOL equipped with a Qbmove. In fact, a clear trade-off between rigid-link and elastic-link setups directs us to use variable stiffness actuators for a wide range of aerial physical interaction tasks.

This work is a bridge between our previous experi- ences [21] and our future studies, which will include; i) fur- ther experiments using the quadrotor and Qbmove setup, e.g., peg-in-hole or throwing, hammering; ii) extension of the theory to 3D and/or arms with multiple degrees, see [23]

for interesting preliminary results.; iii) use of sensor-based

0 2 4 6 8 10

−0.5 0 0.5

x

1

, [m ]

PVTOL Position

0 2 4 6 8 10

−1.15

−1.1

−1.05

−1

z

1

, [m ]

PVTOL Position

0 2 4 6 8 10

−0.1

−0.05 0 0.05 0.1

θ

1

, [r a d ]

PVTOL Orientation

Time, [s]

0 2 4 6 8 10

−1

−0.5 0 0.5 1

θ

12

, [r a d ]

Absolute Link Orientation

Time, [s]

measured desired

Fig. 5: Preliminary results for trajectory tracking with Quadro- tor+VSA arm setup. The arm attached to the Qbmove is swinging back and forth (see θ ₁₂ ), while Quadrotor VTOL is tracking a stable trajectory along x-direction, and a step-like trajectory along the z- direction. Oscillations on θ 1 are due to the motion of the arm, against which controller is trying to keep x position constant. Notice that negative z is upward.

calibration methods as, e.g., in [29] to retrieve the system parameters on the fly.

VIII. A CKNOWLEDGEMENTS

We thank Anthony Mallet (LAAS-CNRS) for his contri- bution in the software architecture of the experiments.

R EFERENCES

[1] V. Mistler, A. Benallegue, and N. K. M’Sirdi, “Exact linearization and noninteracting control of a 4 rotors helicopter via dynamic feedback,” in 10th IEEE Int. Symp. on Robots and Human Interactive Communications, Bordeaux, Paris, France, Sep. 2001, pp. 586–593.

[2] P. Robuffo Giordano, A. Franchi, C. Secchi, and H. H. B¨ulthoff,

“Experiments of passivity-based bilateral aerial teleoperation of a group of UAVs with decentralized velocity synchronization,” in 2011 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, San Francisco, CA, Sep. 2011, pp. 163–170.

[3] ——, “Bilateral teleoperation of groups of UAVs with decentralized connectivity maintenance,” in 2011 Robotics: Science and Systems, Los Angeles, CA, Jun. 2011.

[4] A. Franchi, C. Masone, H. H. B¨ulthoff, and P. Robuffo Giordano,

“Bilateral teleoperation of multiple UAVs with decentralized bearing- only formation control,” in 2011 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, San Francisco, CA, Sep. 2011, pp. 2215–2222.

[5] C. Secchi, A. Franchi, H. H. B¨ulthoff, and P. Robuffo Giordano,

“Bilateral teleoperation of a group of UAVs with communication delays and switching topology,” in 2012 IEEE Int. Conf. on Robotics and Automation, St. Paul, MN, May 2012, pp. 4307–4314.

[6] A. Franchi, C. Secchi, M. Ryll, H. H. B¨ulthoff, and P. Robuffo Gior- dano, “Shared control: Balancing autonomy and human assistance with a group of quadrotor UAVs,” IEEE Robotics & Automation Magazine, Special Issue on Aerial Robotics and the Quadrotor Platform, vol. 19, no. 3, pp. 57–68, 2012.

[7] H. I. Son, A. Franchi, L. L. Chuang, J. Kim, H. H. B¨ulthoff, and P. Robuffo Giordano, “Human-centered design and evaluation of haptic cueing for teleoperation of multiple mobile robots,” IEEE Trans. on Systems, Man, & Cybernetics. Part B: Cybernetics, vol. 43, no. 2, pp.

597–609, 2013.

[8] B. Y¨uksel, C. Secchi, H. H. B¨ulthoff, and A. Franchi, “Reshaping the physical properties of a quadrotor through IDA-PBC and its application to aerial physical interaction,” in 2014 IEEE Int. Conf.

on Robotics and Automation, HK, China, May. 2014, pp. 6258–6265.

[9] D. J. Lee and C. Ha, “Mechanics and control of quadrotors for tool operation,” in 2012 ASME Dynamic Systems and Control Conference, Fort Lauderdale, FL, Oct. 2012.

[10] G. Gioioso, M. Ryll, D. Prattichizzo, H. H. B¨ulthoff, and A. Franchi,

“Turning a near-hovering controlled quadrotor into a 3D force effec- tor,” in 2014 IEEE Int. Conf. on Robotics and Automation, Hong Kong, China, May. 2014, pp. 6278–6284.

[11] M. Fumagalli, R. Naldi, A. Macchelli, R. Carloni, S. Stramigioli, and L. Marconi, “Modeling and control of a flying robot for contact inspection,” in 2012 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, Vilamoura, Portugal, Oct 2012, pp. 3532–3537.

[12] K. Kondak, K. Krieger, A. Albu-Sch¨affer, M. Schwarzbach, M. La- iacker, I. Maza, A. Rodriguez-Castano, and A. Ollero, “Closed-loop behavior of an autonomous helicopter equipped with a robotic arm for aerial manipulation tasks,” International Journal of Advanced Robotic Systems, vol. 10, pp. 1–9, 2013.

[13] G. Gioioso, A. Franchi, G. Salvietti, S. Scheggi, and D. Prattichizzo,

“The Flying Hand: a formation of uavs for cooperative aerial tele- manipulation,” in 2014 IEEE Int. Conf. on Robotics and Automation, Hong Kong, China, May. 2014, pp. 4335–4341.

[14] S. Kim, S. Choi, and H. J. Kim, “Aerial manipulation using a quadrotor with a two dof robotic arm,” in 2013 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, Tokyo, Japan, Nov. 2013, pp. 4990–4995.

[15] M. Tognon and A. Franchi, “Nonlinear observer-based tracking control of link stress and elevation for a tethered aerial robot using inertial- only measurements,” in 2015 IEEE Int. Conf. on Robotics and Au- tomation, Seattle, WA, May 2015, pp. 3994–3999.

[16] M. Tognon, S. S. Dash, and A. Franchi, “Observer-based control of position and tension for an aerial robot tethered to a moving platform,”

IEEE Robotics and Automation Letters, vol. 1, no. 2, pp. 732–737, 2016.

[17] R. Spica, A. Franchi, G. Oriolo, H. H. B¨ulthoff, and P. Robuffo Giordano, “Aerial grasping of a moving target with a quadrotor UAV,”

in 2012 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, Vilamoura, Portugal, Oct. 2012, pp. 4985–4992.

[18] J. Thomas, J. Polin, K. Sreenath, and V. Kumar, “Avian-Inspired Grasping for Quadrotor Micro UAVs,” in 2013 ASME Int. Design Engineering Technical Conf. and Computers and Information in En- gineering Conf., Portland, OR, Aug. 2013.

[19] SAPHARI, “EU Collab. Project FP7-ICT 287513,” 2011-2015.

[20] D. J. Braun, F. Petit, F. Huber, S. Haddadin, P. van der Smagt, A. Albu- Sch¨affer, and S. Vijayakumar, “Robots driven by compliant actuators:

Optimal control under actuation constraints,” IEEE Trans. on Robotics, vol. 29, no. 5, pp. 1085–1101, 2013.

[21] B. Y¨uksel, S. Mahboubi, C. Secchi, H. H. B¨ulthoff, and A. Franchi,

“Design, identification and experimental testing of a light-weight flexible-joint arm for aerial physical interaction,” in 2015 IEEE Int.

Conf. on Robotics and Autom., Seattle, WA, May 2015, pp. 870–876.

[22] B. Y¨uksel, N. Staub, and A. Franchi, “Explicit computations and further extensive simulations for rigid- or elastic-joint arm,” LAAS- CNRS, Tech. Rep. hal-01345564, July 2016. [Online]. Available:

https://hal.archives-ouvertes.fr/hal-01345564

[23] B. Y¨uksel, G. Buondonno, and A. Franchi, “Differential flatness and control of protocentric aerial manipulators with mixed rigid- and elastic-joints,” in 2016 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, Daejeon, South Korea, Oct. 2016.

[24] S. Lupashin, A. Sch¨ollig, M. Sherback, and R. D’Andrea, “A simple learning strategy for high-speed quadrocopter multi-flips,” in 2010 IEEE Int. Conf. on Robotics and Automation, Anchorage, AK, May 2010, pp. 1642–1648.

[25] A. De Luca, “Decoupling and feedback linearization of robots with mixed rigid/elastic joints,” in 1996 IEEE Int. Conf. on Robotics and Automation, Minneapolis, MN, Apr. 1996, pp. 816–821.

[26] D. J. Lee, A. Franchi, H. I. Son, H. H. B¨ulthoff, and P. Robuffo Giordano, “Semi-autonomous haptic teleoperation control architecture of multiple unmanned aerial vehicles,” IEEE/ASME Trans. on Mecha- tronics, Focused Section on Aerospace Mechatronics, vol. 18, no. 4, pp. 1334–1345, 2013.

[27] V. Grabe, M. Riedel, H. H. B¨ulthoff, P. Robuffo Giordano, and A. Franchi, “The TeleKyb framework for a modular and extendible ROS-based quadrotor control,” in 6th European Conference on Mobile Robots, Barcelona, Spain, Sep. 2013, pp. 19–25.

[28] J. L¨achele, M. Riedel, P. Robuffo Giordano, and A. Franchi, “Swarm- simx and telekyb: Two ros-integrated software frameworks for single- and multi-robot applications,” in Int. Work. on Towards Fully Decen- tralized Multi-Robot Systems: Hardware, Software and Integration, at 2013 IEEE Int. Conf. on Robotics and Automation, Karlsruhe, Germany, May 2013.

[29] A. Censi, A. Franchi, L. Marchionni, and G. Oriolo, “Simultaneous maximum-likelihood calibration of odometry and sensor parameters,”

IEEE Trans. on Robotics, vol. 29, no. 2, pp. 475–492, 2013.

Preprint version, final version at http://ieeexplore.ieee.org/ 6 2016 IEEE/RSJ IROS