Preprint version final version at http://ieeexplore.ieee.org/   Accepted for IEEE Transaction on Robotics 2017  Dynamics, Control, and Estimation for Aerial Robots Tethered by Cables or Bars  Marco Tognon 1   and Antonio Franchi 1  Abstract —We consider the problem of controlling an aerial robot connected to the ground by a passive cable or a passive rigid link. We provide a thorough characterization of this nonlinear dynamical robotic system in terms of fundamental properties such as differential flatness, controllability, and observability. We   prove   that   the   robotic   system   is   differentially   flat   with respect to two output pairs: elevation of the link and attitude of the vehicle; elevation of the link and longitudinal link force (e.g., cable tension, or bar compression). We show the design of   an   almost   globally   convergent   nonlinear   observer   of   the full state that resorts only to an onboard accelerometer and a gyroscope. We also   design   two almost globally convergent nonlinear   controllers   to   track   any   sufficiently   smooth   time- varying trajectory of the two output pairs. Finally we numerically test the robustness of the proposed method in several far-from- nominal conditions: nonlinear cross-coupling effects, parameter deviations, measurements noise and non ideal actuators.  I. I NTRODUCTION  Research on aerial robots has been boosted in the last decade thanks to the availability of low-cost and light-weight technology for onboard sensing and computation. This in- crease of popularity led to development of several methods for different problems such as, e.g., flight control, aerial naviga- tion, planning, sensing, mapping, and transportation. Although the main use of aerial robots has been the one of mobile remote sensors, today their use for physically interactive tasks has become a very popular and promising topic, like the cooperative transportation of suspended loads with cables [1], [2]. Cables are not only used for cooperative transportation but also to tether an aerial vehicle to a ground station. In this case the cable can provide many advantages to the robotic system, such as i)   improved flight stability and reliability   ii)   physical interaction with a ground object,   iii)   enduring power, and  iv)   high bandwidth communication channel with the base station. Applications fields related to this kind of robotics systems are   i)   landing/taking-off from/on moving or sloped platforms [3]–[6],   ii)   inspection and surveillance [7], and  iii)   communication reinforcing using relays. For an aerial vehicle connected by a passive 1   link to the ground, standard flight-control and estimation methods cannot be applied straightforwardly because they do not exploit the full dynamics and capabilities of the new interconnected sys- tem. However, it is hard to design new control and estimation  1 LAAS-CNRS,   Universit ́ e   de   Toulouse,   CNRS,   Toulouse,   France, marco.tognon@laas.fr, antonio.franchi@laas.fr This work has been partially funded by the European Union’s Horizon 2020 research and innovation programme under grant agreement No 644271 AEROARMS.  1 The ‘passive’ adjective differentiates from the ‘active’ case in which the link is attached to a controlled winch that can regulate its length.  methods that consider the system in its whole completeness, due to the nonlinear dynamics and the dynamic coupling between the aerial vehicle and the link. Driven by the relevance of this topic, several control and estimation schemes have been presented in the robotic litera- ture. For the case in which the aerial vehicle is a helicopter the authors of [5] present a method to land it on the deck of a ship in rough sea using a cable. The controller is based on a time-scale separation technique between the rotational and translational dynamics. In the context of [8], the authors of [6] present a control scheme based on a PID scheme together with partial model inversion, to stabilize the flight to a constant horizontal elevation with the goal of both improving hovering in windy conditions and land on a mobile platform. For the case of cable-tethered underactuated multi-rotor system, [9] presents a controller that, under a quasi-static assumption, stabilizes the elevation (angle) of the cable to a constant value using only inertial onboard sensors. The authors of [10] also present a controller to stabilize the elevation of a tethered multi-rotor to a constant value but they ensure the positivity of the cable tension as well. The presented approaches provide a good basis for observa- tion and control of such systems. However, to improve those methods and overcome some of their drawbacks we want to: 1) consider a more   general link   (i.e., a cable or a bar), differently from the state-of-the-art where only a cable link is used. Indeed the use of rigid link can be beneficial for, e.g., sustain part of the platform weight, or allow a full-bilateral physical interaction with a ground object. 2)   track a time varying trajectory . While the methods in the literature are based on a quasi-static assumption, in many practical cases it is important to track a time-varying trajectory to, e.g., let the aerial robot track a moving target. 3)   track a desired link force profile . In many cases is important to precisely regulate the force link to any desired value, possibly including negative link force (pushing) if the link is a bar. However, the methods in the literature are able, in the best case [10], to keep the force link generically positive. 4)   observe the state   from onboard IMU measurements in any dynamic conditions, while in the state-of-the-art this is done at best in a quasi static assumption [9]. In order to achieve these goals we started by analyzing in details the intrinsic properties of the system such as differential flatness, controllability and observability. Furthermore, we ad- dress three main control objectives, described in the following. The first objective is to solve the tracking problem for any time-varying desired link-elevation trajectory while, at the same time, letting the aerial vehicle attitude track any other 1  arXiv:1603.07567v2 [cs.RO] 3 Mar 2017
time-varying trajectory as well. This objective responds to applications requiring to place the robot at a desired altitude and with a desired orientation, e.g., for surveillance-task. The second objective is to solve the elevation tracking problem together with the problem of tracking time-varying force-link trajectories that can assume values both in the positive (tensions) and negative (compressions) domains. For example, if the link is a cable one may like to keep it always taut while at the same time avoid peaks on the tension that can damage the cable or its attaching mechanisms. If the link is a bar then one may want to push against the bar appropriately to perform physical interaction with the environment. The third and last objective is to close the control loop with a minimal sensorial setup. For this purpose we design a nonlinear observer to retrieve the state from any dynamic case using inertial-only onboard measurements, without the need of GPS or vision sensors. This makes the system feasible for both inside and outside environments and also in the case of very low or zero visibility. The ability to work with a minimum number of sensors also increases the robustness to the failure of the other sensors without precluding their additional use. The paper is organized as follows. In Sec. II we derive the dynamic model and we describe the addressed control prob- lems. In the following Sec. III we prove the differential flatness of the system with respect to the two sets of outputs. Then, in Sec. IV we design the controllers for the tracking of these outputs. The observability analysis and the derivation of the observer are presented in Sec. V. A comprehensive numerical validation of the methods is shown in Sec. VI. Finally, in Sec. VII, conclusions and future works are discussed. II. D YNAMIC   M ODEL AND   C ONTROL   P ROBLEM  Similarly to [9], [10] we consider a robotic system com- posed by an underactuated   aerial vehicle   moving on a vertical plane   and   tethered   by   a   link   to   the   ground,   as   depicted in   Fig.   1.   We   define   an   inertial   world   frame ,   F W   ,   with axes   { x W   ,   y W   ,   z W   }   and origin   O W   , and a   body frame ,   F B , attached to the aerial vehicle, with axes   { x B ,   y B ,   z B }   and origin  O B   coinciding with the center of mass (CoM) of the aerial vehicle. The position of   O B , defined in   F W   , is denoted with  p B   = [ x B   y B   z B ] T   ∈   R 3 . Since the vehicle moves on the vertical plane, we have that   y B   ≡   y W   and   y B   ≡   0. 2   The orientation of  F B   is fully known once it is known the angle of rotation   θ  needed to let the orientation of   F W   coincide with the one of  F B   when performing a rotation about   y W   ≡   y B . The aerial vehicle is modeled as a rigid body of mass  m R   ∈   R > 0   and rotational inertia   J R   ∈   R > 0 . Its (untethered) configuration is fully described by   x B ,   z B , and the   attitude   θ   . The vehicle is actuated by two scalar inputs:   i)   the intensity,  f R   ∈   R , of the thrust force vector   f R   =   −   f R z B , and   ii)   the intensity   τ R   ∈   R , of the torque vector   τ R y B . The aerial vehicle is equipped with an inertial sensor composed by   i)   a two- axis accelerometer with axes aligned with   x B   and   z B   and   ii)   a one-axis gyroscope aligned with   y B , whose measurements are  2 Although the system evolves in a 2D plane, defining the frame in 3D is convenient in order, e.g., to define properly the torque and angular velocity vectors for the robot. f R  z W  x W  x B  z B  θ   =   x 3  − m Q g z W  − f L d  φ   =   x 1  l  O W  O B  τ R  Fig. 1: Schematic representation of the system for which two con- trollers and one observer are designed for the control of the two output pairs:   ( φ ,   θ   ) , and   ( φ ,   f L ) . The presence of two opposing propellers is just illustrative, since the same inputs can be realized with different solutions as, e.g., a ducted fan configuration, or another multi-propeller configuration.  denoted by   a   = [ a x   a z ] T   ∈   R 2   and   ω   ∈   R , respectively, where  ω y B   is the angular velocity of the aerial vehicle. The link can be either a bar, a strut or a cable. It is connected at one end to   O B   and at the other end to the fixed point   O W   . Connections are modeled as two passive rotational joints, therefore no rotational constraint holds at the joints. As done in related works, see, e.g. [9]–[12], we assume that the mass and inertia of the link are negligible with respect to the inertial characteristic of the aerial vehicle. However, this assumption is practically relaxed in Sec. VI where we test the proposed method with non-zero mass and inertia of the link. Deformations and elasticity are also negligible in the operative condition. Therefore the link length, denoted with  l   ∈   R > 0 , is fixed. The configuration of the link is described by the   elevation ,   φ   ∈   R , i.e., the angle that the link forms with the  x W   axis. We denote with   f L   ∈   R   the intensity of the internal force acting along the link, called   link force . When the link is pulled the force is called   tension   and   f L   >   0, whereas when it is compressed the link force is called   compression   and   f L   <   0. The mechanical model of the robotic system (i.e., aerial vehicle   plus   link)   can   be   then   easily   derived.   Given   the constraints of the system, the motion is restricted on a circle of radius   l . Thus the configuration of the system is fully described by the generalized coordinates   q   = [ φ θ   ] T   . Applying the Euler-Lagrange approach it is possible to obtain the equations of the dynamic model  M ( q )    ̈ q   +   g ( q ) =   Q ( q ) u   (1) where  M   =  [ J φ   0 0   J θ  ]  =  [ m R l 2   0 0   J R  ]  ,   g   =  [ m R lg d ⊥   ·   e 3  0  ]  ,  Q   =  [ − l R W B   e 3   ·   d ⊥   0 0   1  ]  ,   u   =  [   f R  τ R  ]  =  [ u 1  u 2  ]  ,  and: 3   d   = [ c φ   0   s φ   ] T   ,   d ⊥   = [ − s φ   0   c φ   ] T   R W B   ∈   SO ( 3 )   is the rotation matrix from   F B   to   F W   ,   g   ≈   9 . 81, and 4   e 3   = [ 0 0 1 ] T   .  3 As normal in the literature we define   s ?   =   sin   ( ? )   and   c ?   =   cos   ( ? ) .  4 e i   ∈   R 3   is the canonical vector with 1 in position   i   − th   and zero otherwise.  Preprint version final version at http://ieeexplore.ieee.org/   2   Accepted for IEEE Transaction on Robotics 2016
Defining the state of the system by   x   =   [ φ    ̇ φ θ    ̇ θ   ] T   = [ x 1   x 2   x 3   x 4 ] T   , the dynamic model in state space form results:  ̇ x   =       x 2  a 1 c x 1  x 4  0        +       0   0  a 2 c x 1 + x 3   0 0   0 0   a 3        u ,   (2) where   a 1   =   − g / l ,   a 2   =   1 / ( m R l ) ,   a 3   =   1 / J R   are the constant parameters of the dynamical model. The problems addressed in this paper are the following.  Problem   (Addressed Control Problems) .   Design two control laws for the input   u , computing the control action using only the onboard measurements   a   and   ω   and each achieving the following two different control objectives, respectively.  Objective 1 : let   φ   and   θ   independently track the desired time-varying trajectories   φ d   ( t )   and   θ   d   ( t ) , respectively;  Objective 2 : let   φ   and f L   independently track the desired time-varying trajectories   φ d   ( t )   and f   d L   ( t ) , respectively.  In view of the given control objectives we define the two pairs of outputs:   y a   = [ φ θ   ] T   = [ y a  1   y a  2 ] T   and   y b   = [ φ   f L ] T   =  [ y b  1   y b  2  ] T   . In order to write the outputs as a function of   x   and   u  we first retrieve the link force from the force balance equation  m R    ̈ p B   =   m R ( − l d    ̇ φ 2   +   l d ⊥    ̈ φ ) =   −   f L d   −   f R z B   −   m R g z W   ,   (3) from which we obtain:  y a   =   [ x 1   x 3  ] T   (4)  y b   =  [   x 1 1  a 2   x 2 2   +   a 1  a 2   s x 1  ]  +  [   0   0  s x 1 + x 3   0  ]  u .   (5) III. D IFFERENTIAL   F LATNESS  Differential   flatness   is   a   property   of   certain   dynamical systems for which it exists (at least) an output (with the same dimension of the input), called   flat output , such that both the state and the input can be expressed as an algebraic function of the flat output and its derivatives, up to a finite order [13]. This property is commonly used to generate feasible trajectories for the state and the input by planning only in the output space, which is useful for nonholonomic and underactuated systems. In the following we demonstrate that the system at hand is differentially flat with respect to both outputs, i.e.,   x   and   u   can be written as functions of either   y a   or   y b   and their derivatives.  A. Flatness with Respect to   y a  From   the   definition   of   y a   we   have   directly   that   x   = [ y a  1    ̇ y a  1   y a  2    ̇ y a  2  ] T   .   Then,   inverting   (2)   we   can   write   the inputs as a function of the output and some of its derivatives  u 1   =    ̇ x 2   −   a 1 c x 1  a 2 c x 1 + x 3  =    ̈ y a  1   −   a 1 c y a  1  a 2 c y a  1 + y a  2  =   u 1 ( y a  1 ,    ̇ y a  1 ,    ̈ y a  1 ,   y a  2 )  u 2   =    ̇ x 4 / a 3   =    ̈ y a  2 / a 3   =   u 2 (    ̈ y a  2 ) .  Notice that in order to compute   u 1   as above, it has to hold that   y a  1   +   y a  2   =   φ   +   θ   6   =   π / 2   +   k π   where   k   ∈   Z . In the output plane, the singularity represents a family of parallel lines. This means that if initial and final outputs configurations are on the opposite side of one line, then there are no trajectories connecting the two configurations that avoid the singularity in which   u 1   results undetermined. The same singularity will appear in the controller and in Sec. IV-C we address the issue and we propose a possible solution to it.  B. Flatness with Respect to   y b  First of all we have directly   x 1   =   φ   =   y b  1   and   x 2   =    ̇ φ   =    ̇ y b  1 . Then consider   r   =   − m R    ̈ p B ( y b  1 ,    ̇ y b  1 ,    ̈ y b  1 ,   y b  2 )   −   y b  2 d ( y b  1 )   −   m R g z W  that depends only on   y b  1 ,    ̇ y b  1 ,    ̈ y b  1 ,   y b  2   and some constant parame- ters of the system. Using (3) we get  u 1 z B   =   u 1 [ − s x 3   0   −   c x 3   ] T   =   r ( y b  1 ,    ̇ y b  1 ,    ̈ y b  1 ,   y b  2 ) ,   (6) where   r ( · ,   · ,   · ,   · ) = [ r 1   r 2   r 3 ] T   represents the function from   R 2   ×  R 2   ×   R 2   ×   R 2   to   R 3 . If   ‖ r ‖ 6   =   0, from (6) we can retrieve   u 1  and   x 3   as  u 1   =   ‖ r ‖   =   u 1 ( y b  1 ,    ̇ y b  1 ,    ̈ y b  1 ,   y b  2 )  x 3   =   atan2   ( r 1 / ‖ r ‖ ,   r 3 / ‖ r ‖ ) =   x 3 ( y b  1 ,    ̇ y b  1 ,    ̈ y b  1 ,   y b  2 ) .   (7) Differentiating   x 3   w.r.t. time we obtain   x 4  x 4   =   d dt x 3 ( y b  1 ,    ̇ y b  1 ,    ̈ y b  1 ,   y b  2 ) =   x 4 ( y b  1 ,    ̇ y b  1 ,    ̈ y b  1 ,   y b  1  ( 3 ) ,   y b  2 ,    ̇ y b  2 ) .  Further differentiating   x 4   and using (2) we finally obtain   u 2  u 2   =   J R  d dt x 4 ( y b  1 , . . . ,   y b  1  ( 3 ) ,   y b  2 ,    ̇ y b  2 ) =   u 2 ( y b  1 , . . . ,   y b  1  ( 4 ) ,   y b  2 ,    ̇ y b  2 ,    ̈ y b  2 ) ,  which concludes the proof that   y b   is a flat output for the system in exam in the case   ‖ r ‖ 6   =   0. An extended version of the previous equations can be found in [14].  1) Case of Vanishing Thrust:   If   ‖ r ‖   =   0 one can still obtain the state and the inputs in terms of the outputs. Differentiating both sides of (6) and substituting   u 1   =   ‖ r ‖   =   0 we obtain  ̇ u 1 z B   =    ̇ r ( y b  1 ,    ̇ y b  1 ,    ̈ y b  1 ,   y b  1  ( 3 ) ,   y b  2 ,    ̇ y b  2 )   from which, if   ‖  ̇ r ‖ 6   =   0, we can compute  ̇ u 1   and   x 3 ,   x 4 ,   u 2   as before. Similarly, if   ‖ r ‖   =   ‖  ̇ r ‖   =  . . .   =  ∥ ∥ ∥ r ( r − 1 )  ∥ ∥ ∥   =   0 and   ‖ r r ‖ 6   =   0, we can compute the inputs and the state differentiating (6)   r   times and then applying the same method as before. If all the derivatives of   u 1   =   ‖ r ‖   are zero along the desired output trajectory then   x 3 ,   x 4   and   u 2   are indefinite. In this degenerate case, the thrust is identically zero,   u 1   ≡   0, thus the rotational dynamics of the aerial vehicle and the one of the link are completely unrelated. The trajectory of   φ   evolves only driven by the gravity force and the initial conditions of  φ ,    ̇ φ , while   θ   evolves depending only on the initial conditions of   θ   ,    ̇ θ   and the input torque   u 2 . Notice that in this case, considering opportune higher order of differentiation we were able to prove the flatness with respect to   y b   even for the singular case, i.e.,   u 1   =   0. On the other hand, the singularity of the flat output   y a   (i.e.,  y a  1   + y a  2   =   π / 2 + k π ) it is structural thus impossible to eliminate with a similar technique.  Preprint version final version at http://ieeexplore.ieee.org/   3   Accepted for IEEE Transaction on Robotics 2016
2) Discussion on the Sign of the Thrust:   From (6) the following solution is also feasible  u 1   =   − ‖ r ‖   ,   x 3   =   atan2   ( − r 1 / ‖ r ‖ ,   − r 3 / ‖ r ‖ )   ,  which is the ‘opposite’ of (7). Therefore, from the same output  y b , one can get two possible different attitude and thrust pairs,  z B   =   ±   r  ‖ r ‖   and   u 1   =   ± ‖ r ‖ . These solutions represent the same force vector   f R   =   − u 1 z B , obtained with the vehicle ‘facing up’ and ‘facing down’, respectively. In order to discriminate this ambiguity, exploiting the system continuity, the correct attitude and thrust sign are chosen selecting the closest configuration to the previous state. Moreover, as long as the trajectory does not imply   ‖ r ‖   =   0 for any   t , the sign of   f R   is constant. Even though propellers that can provide negative thrust are not of common use, some solutions exist such as, e.g., variable-pitch and symmetric propellers, with a constant and a time-varying speed of rotation, respectively. In the common case of aerial vehicles with only positive thrust, one should provide desired trajectories that are either sufficiently distant from the condition   ‖ r ‖   =   0 (to have also some robustness against disturbances), or should plan a trajectory that avoids the change of the thrust sign when passing trough the condition  ‖ r ‖   =   0 , e.g., with a cusp or a non-zero time derivative for   r . IV. S TATE -F EEDBACK   C ONTROL  A similar concept to differential flatness is   exact feedback linearizability   [13]. Differential flatness and exact dynamic feedback linearizability are correlated, indeed they are equiv- alent on an open and dense set of the state space [15]. In this section we show that both   y a   and   y b   are also linearizing output for the system (1), and using the   static   and   dynamic feedback linearization   control techniques we can obtain input-output decoupling and full state linearization via static and dynamic feedback respectively. In the following two subsections we propose the design of the controllers for   y a   and   y b   respectively, whose global characteristics are summarized in Tab. I.  A. Controller for   y a  Considering the equation (4) and applying the feedback linearization technique, we see that we have to derivate twice the output in order to see the input appear, obtaining the second and fourth rows of (2):  ̈ y a   =  [ a 1 c x 1  0  ]  +  [ a 2 c x 1 + x 3   0 0   a 3  ]  u   =   b a ( x ) +   E a ( x ) u .  Notice that the decoupling matrix   E a ( x )   is invertible if and only if   x 1   +   x 3   6   =   π / 2   +   k π   with   k   ∈   Z . If this condition holds, assuming   x   known, the control law  u   =   Γ a ( x ,   v a ) =   E a − 1 ( x ) [ − b a ( x ) +   v a ]   ,   (8) where   v a   = [ v a  1   v a  2 ] T   are virtual inputs, brings the system in the form of a decoupled linear system   [ y a  1 ( 2 )   y a  2 ( 2 ) ] T   =  [ v a  1   v a  2  ] T   =   v a . Then, given the desired trajectories   y a  1 d   ( t )   ∈  C 1   and   y a  2 d   ( t )   ∈   C 1   for the link elevation   y a  1   and the vehicle attitude   y a  2   respectively, one can achieve the tracking designing a standard state-feedback linear controller. Moreover the total relative degree 5   r a   =   r a  1   +   r a  2   =   2   +   2   =   4 is equal to the dimension of the state, thus there is no internal dynamics [16]. Notice that the control actions   u   required by this controller may be discontinuous due to, e.g. desired trajectories possess- ing a discontinuity in the second derivative (which, in fact, are not forbidden by the   C 1   requirement) or simply, in the real case, due to some noise in the measurements. This has to be taken into account in case one would like, e.g., to minimize mechanical vibrations of the link. Furthermore, it has to be kept in mind that discontinuous inputs cannot be performed by the physical system in exam because, e.g., the thrust produced by a propeller is an algebraic function of its angular speed and the angular acceleration (and the corresponding air flow change) cannot be infinite. However, if one needs to enforce smoother inputs it possible to apply a dynamic compensator in order to get a sufficiently smooth control signal. For example, if one needs at least a continuous thrust input   u 1 , we can assume as new control input the vector  u ′   = [    ̇ u 1   u 2 ] T   , considering the velocity of the thrust intensity as new controllable input,  ̇ u 1   =    ̇ f R . The output   y a  1   has to be differentiated three times to see the input appear  [ y a  1 ( 3 )  y a  1 ( 2 )  ]  =  [ − a 1 s x 1   x 2   −   a 2 s x 1 + x 3   ( x 2   +   x 4 ) u 1  0  ] ︸   ︷︷   ︸   ̄ b a ( x ,   u 1 ) + E a ( x ) u ′ .  The decoupling matrix does not change, thus, if   x 1   +   x 3   6   =  π / 2   +   k π   with   k   ∈   N , we can apply the control law  u ′   =   Γ a ′ ( x ,   u 1 ) =   E b − 1 ( x )   [ −    ̄ b a ( x ,   u 1 ) +    ̄ v a ]   ,   (9) that brings the system in the form of a decoupled linear system   [ y a  1 ( 3 )   y a  2 ( 2 ) ] T   =   [ v a  1   v a  2  ] T   =    ̄ v a . Moreover the rel- ative degree is still equal to the dimension of the extended state that includes also   u 1 , i.e.,  ̄ r a   =   r a   +   1   =   5   =   n   +   1, thus the linearization is exact and without internal dynamics. This smoother form of the controller will be useful in order to close the control loop with the observer of Sec. V.  B. Controller for   y b  Differently from the previous case, the static feedback with respect to the output   y b   is not enough in order to exactly linearize the system. Indeed, the decoupling matrix in this case results always singular because the input torque does not appear on both elevation’s acceleration and link force, as instead the thrust intensity does. A dynamic compensator is then needed in order to delay the appearance of the thrust in the output derivatives. As done in [12] we consider as new input  ̄ u   = [    ̈ u 1   u 2 ] T   = [    ̄ u 1    ̄ u 2 ] T   , considering the acceleration of the thrust intensity as   new   controllable   input,    ̈ u 1   =    ̈ f R .   Due   to   the   dynamic compensator the actual state of the dynamical system is given by the extended vector  ̄ x   =   [ φ    ̇ φ θ    ̇ θ   u 1    ̇ u 1  ] T   , that contains also the thrust intensity and its derivative. 6   In this condition  5 The sum, for each entry of the output, of the number of times that each output entry has to be differentiated in order to see at least one input appear.  6 This does   not   mean or imply that one has to measure or estimate   u 1   or  ̇ u 1  since they are easily and safely computed as integrals of the new input  ̈ u 1 .  Preprint version final version at http://ieeexplore.ieee.org/   4   Accepted for IEEE Transaction on Robotics 2016
Controller   Controlled   Feedback Linearization   Singularity   Tot. Relative   Internal   Desired   Uncontrolled   Used Output   Degree   Dynamics   Trajectories   Variable   Variables  Γ a   φ ,   θ   Static:   u   = [   f R   τ R ] T   φ   +   θ   =   π  2   +   k π   2   +   2   =   4   No   φ d   ( t )   ∈   C 1 ,   θ   d   ( t )   ∈   C 1   f L   x  Γ a ′   φ ,   θ   Dynamic:   u ′   = [    ̇ f R   τ R ] T   φ   +   θ   =   π  2   +   k π   3   +   2   =   5   No   φ d   ( t )   ∈   C 2 ,   θ   d   ( t )   ∈   C 1   f L   x  Γ b   φ ,   f L   Dynamic:  ̄ u   = [    ̈ f R   τ R ] T   f R   =   0   4   +   2   =   6   No   φ d   ( t )   ∈   C 3 ,   f   d L   ( t )   ∈   C 1   θ   x  TABLE I: Controllers characteristics (without the observer).  the outputs   y 1   and   y 2   have to be differentiated respectively four and two times to see the new input appear  [  y ( 4 )  1  y ( 2 )  2  ]  =   b b (  ̄ x ) +  [ a 2 c x 1 + x 3   − a 2 a 3 s x 1 + x 3   u 1  s x 1 + x 3   a 3 c x 1 + x 3   u 1  ] ︸   ︷︷   ︸  E b (  ̄ x )   ̄ u ,   (10) where   b b (  ̄ x )   depends only to the extended state, and the decoupling matrix   E b (  ̄ x )   is always invertible except for the case   u 1   6   =   0. Assuming   u 1   6   =   0 and  ̄ x   known, the control law  ̄ u   =   Γ b (  ̄ x ) =   E b − 1 (  ̄ x )  [  − b b (  ̄ x ) +   v b ]  ,   (11) where   v b   = [ v b  1   v b  2 ] T   are virtual inputs, brings the system in the form of a decoupled linear system  [  y b  1  ( 4 )   y b  2  ( 2 ) ] T  =  [ v b  1   v b  2  ] T   =   v b . Then, given a desired trajectory   y b  1  d   ( t )   ∈   C 3  for the link elevation   y b  1   and a desired trajectory   y b  2  d   ( t )   ∈   C 1  for the link force   y b  2 , one can achieve the tracking by resorting to a standard state-feedback linear controller for   v b . Notice that the total relative degree   r b   =   r b  1   +   r b  2   =   4   +   2   =   6 is equal to the dimension of the extended state, thus there is no internal dynamics also in this case. This ensures the stability of the attitude of the vehicle while following any desired trajectory   y b d   ( t )   .  C. Discussion on the Controllers  The two control laws (8), and (11) present a singularity in  x 1   +   x 3   =   π / 2   +   k π   and   u 1   =   0, respectively. For the controller  Γ a , the condition   x 1   +   x 3   =   π / 2   +   k π , verified when the link and the thrust direction are parallel, corresponds to a series of parallel lines in the space of   x 1   and   x 3 . In practice this means that for a pair of given initial and final desired conditions, a trajectory that avoids the singularity might not exist (see also discussion in Sec. III-A). On the other hand, the singularity of the controller   Γ b   ( u 1   =   0), corresponds to isolated points in the state space, thus easily avoidable with a planner. If a desired trajectory passes through the singularity then the inversion of the decoupling matrix is not feasible and the controller cannot be applied. Moreover this represents an in- convenience also in the neighborhood of the singularity where small virtual inputs may cause large inputs. Nevertheless, if the singularity occurs for only an instant then the controller is still applicable in practice, e.g., approximating the control law using the   damped least-square (DLS) inversion   [17]. i.e., in the neighborhood of the singularity,   E − 1   is approximated with   E ?   =   E T   ( EE T   +   c 2 I ) − 1 . It is proven that   E ?   is the optimal solution in order to minimize the inversion error while bounding the norm of the input, where   c   ∈   R   is chosen to balance the two objectives. However, one has to plan trajectories that do not require to stay constantly in singularity. Observe that, in order to compute the control actions in (8), and (11), only the knowledge of the state is needed. Also the controller   Γ b   needs only the knowledge of the original state   x . Indeed   u 1 ,    ̇ u 1   are known because are internal variables of the controller, and the outputs and their derivatives can be computed from the state as done, e.g., in (5) and in the intermediate derivation steps that bring to (10). Finally, these controllers satisfy only one of our require- ments (decoupled tracking of the output) but they still need the full state information to be implemented, instead of just relying on the accelerometer and gyroscope measurements, as we want to be the case. This limitation is overcome in Sec. V. V. O BSERVER AND   M EASUREMENT -F EEDBACK   C ONTROL  Measuring the whole state   x   using many sensors in order to compute the control action is often practically unfeasible due to, e.g., the costs and payload limitations of aerial robots. Furthermore, possible sensor failures call for the ability to still control the platform with a forcedly limited number of sensors. Driven by these two practical reasons and also by the intrinsic theoretical appeal of solving control problems with minimal sensing, in this section we show that the standard on- board inertial sensors (i.e., an accelerometer plus a gyroscope) are   sufficient   to   estimate   the   full   state   x   of   the   system, and we show the design of an exact nonlinear observer for that purpose. Furthermore, we show that the combination of the observer and the controller results in an almost-globally asymptotically stable closed loop system. The on-board inertial sensor equipment is composed by a gyroscope, that measures the angular velocity, and an ac- celerometer that measures the specific acceleration of the aerial vehicle, i.e., the linear acceleration of the CoM, minus the gravity, expressed in the body frame, i.e.,  ω   =   x 4   (12)  a   =   R B W   (    ̈ p B   +   g z W   ) = [ a x   a y   a z ] T   ,   (13) where   R B W   =   R W B T   . In the following we omit the second line which is zero by construction, i.e., we assume   a   = [ a x   a z ] T   . Observing the full state of system (2) using the partial mea- surements (12), and (13) is a nontrivial nonlinear observation problem, where the nonlinearities appear both in the system dynamics and in the measurements. We show in the following how this problem can be successfully tackled. Fist of all exploiting (12) we can define the gyroscope measurement as a new input   u 3   =   ω   that let us reduce the system dimension from four to three. Then, substituting (3)  Preprint version final version at http://ieeexplore.ieee.org/   5   Accepted for IEEE Transaction on Robotics 2016
and   (2)   into   (13)   and   performing   some   simple   algebraic manipulation on the accelerometer measurement we obtain       ̇ x 1   ̇ x 2   ̇ x 3      =    0   1   0 0   0   0 0   0   0      x 1  x 2  x 3      +      0  a 1 c x 1   +   a 2 c x 1 + x 3   u 1  u 3      (14)  a   =  [   lc x 1 + x 3  ( x 2 2   +   a 1 s x 1   +   a 2 s x 1 + x 3   u 1  )  ls x 1 + x 3  ( x 2 2   +   a 1 s x 1   +   a 2 s x 1 + x 3   u 1  )   −   a 2 u 1  ]  .   (15) The problem is then ‘reduced’ to the observation of the state  [ x 1   x 2   x 3 ] T   from the knowledge of the measurements   a   and the control inputs   [ u 1   u 3 ] T   . However this problem is still nonlinear both in the system dynamics and in the measurement map. In   order   to   solve   nonlinear   observation   problems   there are mainly two classes of methods: approximate nonlinear observers and exact nonlinear observers. The first class relies on approximating the nonlinearities with linear or almost- linear maps around the current estimate, the main disadvantage being the local approximative nature of the methods. The second class of methods consists in nonlinear systems whose state is analytically proven to converge to the real state of the original system. Designing such observers is in general much more difficult since it is often hard to prove the asymptotic stability of a nonlinear system. However the observers of this class may guarantee (almost) global convergence, this is why we decided to search for an observer in the second class. In the literature of exact nonlinear observers an important role is played by a particular class of systems known as   in triangular from , that, in the case of a three-dimensional state system like (14–15) are written as:  ̇ x   =    0   1   0 0   0   1 0   0   0      x   +    0 0 1      φ   ( x ,   u ) +   λ λ λ   ( u )  w   =   [ 1   0   0 ]   x ,  (16) where   x   ∈   R 3   is the state vector,   u   ∈   R m u   is control input (with   m u   ≥   0),   w   ∈   R   is the measurement and   φ   :   R 3   ×   R m u   →  R ,   λ λ λ   :   R m u   →   R 3   are any nonlinear maps. For this class of nonlinear systems, in order to estimate the state one can use the nonlinear   high gain observer   (HGO) [18] that ensures almost global convergence of the estimated state to the real one. Although at first view system (14–15) does not resemble to a system in triangular form like (16), we shall demonstrate in the following that it can be put in that form using a few appropriate nonlinear transformations.  A. State/Output Transformations and HGO Design  In the following we prove that there exist a change of coor- dinates from the original state   x   to a new state   z   = [ z 1   z 2   z 3 ] T  and from the original measurements   a   to a new measurement  w   such that the system (14–15) appears in triangular form. While doing so we highlight also the intuitions that led us to discover this particular change of coordinates. First   of   all   since   the   term   x 1   +   x 3   occurs   frequently in (14–15) a simplifying choice is to assume   z 1   =   x 1   + x 3 . With this choice we have  ̇ z 1   =    ̇ x 1   +    ̇ x 3   =   x 2   +   u 3   and therefore it is natural to choose   z 2   =   x 2   to obtain  ̇ z 1   =   z 2   +   u 3 , thus matching with the first row of the sought triangular form (16). Now, if we compare the second and third rows of (14), with the corresponding rows of the sought triangular form (16) we see that:   i)   in the triangular form (16) the state-dependent nonlinearity   φ   appears only in the last row of the dynamics, but, on the other hand   ii)   in (14) the nonlinearity appears already in the second row. Therefore, in order to push this ‘undesired’ nonlinearity down from the second to the third row we can define   z 3   =    ̇ z 2   =    ̇ x 2   =   a 1 c x 1   +   a 2 c x 1 + x 3   u 1 . Summarizing, we propose the following change of variables  z 1   =   x 1   +   x 3 ,   z 2   =   x 2 ,   z 3   =    ̇ x 2   =   a 1 c x 1   +   a 2 c x 1 + x 3   u 1 ,   (17) that transforms the system (14–15) in the following form  ̇ z   =    0   1   0 0   0   1 0   0   0    ︸   ︷︷   ︸  A z   +    0 0 1    ︸︷︷︸  B  σ   ′ ( z ,   s x 1   ,   u 1 ,    ̇ u 1 ,   u 3 ) +    u 3  0 0    ︸ ︷︷ ︸  λ λ λ   ( u 3 )  (18)  a   =  [   lc z 1  ( z 2 2   +   a 1 s x 1   +   a 2 s z 1   u 1  )  ls z 1  ( z 2 2   +   a 1 s x 1   +   a 2 s z 1   u 1  )   −   a 2 u 1  ]  ,   (19) where the sole state-dependent nonlinearity   σ   ′   =   a 1 z 2 s x 1   +  a 2 c z 1    ̇ u 1   −   a 2 s z 1   ( z 2   +   u 3 ) u 1   is now appearing in the third row, as desired. Notice that we have, on purpose, left the term   s x 1  untransformed. In the following we show why this choice is convenient instead of directly computing   s x 1   from (17) . To reach the form of (16) it remains to extract a measure- ment of   z 1   from the accelerometer reading. From (19), defining  η   =  √  a 2  x   + ( a z   +   a 2 u 1 ) 2   =   ± l   ( z 2 2   +   a 1 s x 1   +   a 2 s z 1   u 1  )   ,   (20) we can obtain a direct measure of   z 1   writing  w   =   atan2   ( ± a x / η ,   ± ( a z   +   a 2 u 1 ) / η ) =   z 1   +   k π ,   (21) where   k   ∈ { 0 ,   1 } . Notice that the transformation is possible only if   η   6   =   0. In particular, from equations (5) and (20) it results that   η   =   ±   1  m R  (   1  a 2   z 2 2   +   a 1  a 2   s x 1   +   s z 1   u 1  )  =   ±   f L  m R   , thus the transformation (21) requires non zero force along the link,  f L   6   =   0. This correspondence highlights, intuitively, that the condition   η   6   =   0 it is not just related to our particular trans- formation choice but is a structural observability requirement. Indeed, if the force along the link is zero, the aerial vehicle and the link become two independent systems 7   and the onboard inertial sensor is not enough to estimate the entire state. In the design of the observer we need to consider this singularity especially for the cases where the desired link force passes from a tension (   f L   >   0) to a compression (   f L   <   0). The accelerometer is also used to replace   s x 1   into (18) ob- taining the sought triangular form (16). In particular from (20) we can write  s x 1   =   ( ± η / l   −   z 2 2   −   a 2 s z 1   u 1  )   / a 1 .   (22) With reference to (18) we can then write   σ   ′   =   σ   ( z ,   μ μ μ ) , i.e., in terms of only   z   and known quantities   μ μ μ   = [ u 1    ̇ u 1   u 3   ±   η ] T   . Observe that, the sign to be put in front of   η   is ambiguous. It is convenient to recast this ambiguity putting always a  7 In fact, due to the assumption that the mass and rotational inertia of the link are negligible, the link force represents the only coupling force between the two sub-systems.  Preprint version final version at http://ieeexplore.ieee.org/   6   Accepted for IEEE Transaction on Robotics 2016
positive sign and considering two possible values:   η +   = + η  and   η −   =   − η . Corresponding to these quantities we then get two different dynamic models, the one with   ( σ   ( z ,   μ μ μ + ) ,   w + )  and the one with   ( σ   ( z ,   μ μ μ − ) ,   w − )   corresponding to the use of   η +   and   η − , respectively. At each time instant only one choice for   η   is correct, i.e.,   η   =   f L / m R , to which corresponds the correct measure   w   =   z 1   and the correct dynamics. It is not possible, however, to discriminate the correct choice instantaneously from the measurements only. In Sec. V-D we propose a discriminating solution based on the prediction error. Although less intuitive, the computation of   s x 1   into (18) exploiting the accelerometer readings, allows to concentrate the ambiguity only on the sign of   η   obtaining two possible dynamic models. Whereas with the more canonical technique, i.e., inverting the state transformation, we would have four different dynamic models. Indeed from (17) we would get  s x 1   =   ±  √  1   −   ( z 3   −   a 2 c z 1   u 1 ) 2 / a 2 1   that presents another ambi- guity on the sign thus generating four possible combinations of the dynamics and measurement equations. For the design of the observer let us assume that the correct choice between   η +   and   η −   is known. In Sec. V-D we shall propose a method to gain this knowledge. Under this assumption, the described transformation of the state and the measurements have finally transformed the original system (14–15) in an equivalent one in a triangular form, i.e.,  ̇ z   =   Az   +   B σ   ( z ,   μ μ μ ) +   λ λ λ   ( μ μ μ )   and   w   =   [ 1   0   0 ]   z   =   Cz , for which we can use the following high gain observer [18]  ̇ ˆ z   =   A ˆ z   +   B σ   ( ˆ z ,   μ μ μ ) +   λ λ λ   ( μ μ μ ) +   H ( w   −   C ˆ z ) ,   (23) where   H   =   [   α 1  ε α 2  ε 2  α 3  ε 3  ] T   ,   ε   ∈   R > 0 , and   α i   ∈   R > 0   are set such that the roots of   p 3   + α 1   p 2   + α 2   p + α 3   have negative real part 8 .  B. Observation of Original State   ˆ x  From the estimation of   z , in order to obtain the estimation of the original state   x , we note that the state transformation (17) is not directly invertible. One can notice that the only knowledge of   z   is not enough to retrieve   x 1 , indeed from (17) one can extract only   c x 1   . Nevertheless, exploiting also the accelerometer measurements (22), we can write  [ c x 1  s x 1  ]  =  [   1  a 1   ( z 3   −   a 2 c z 1   u 1 )  1  a 1  (   η  l   −   z 2 2   −   a 2 s z 1   u 1  ) ]  =   h ( z ,   u 1 ,   η ) .  The state   x 1   can then be computed as the phase of the unit vector   h ( z ,   u 1 ,   η )   denoted by   ∠   h ( z ,   u 1 ,   η ) . Thus the estimation of the original state is given by ˆ x   =   ˆ x ( ˆ z ,   u 1 ,   η ) =       ∠   h ( ˆ z ,   u 1 ,   η )  ˆ z 2  ˆ z 1   −   ∠   h ( ˆ z ,   u 1 ,   η )  u 3        .   (24) The full observer chain is then depicted in Fig. 2.  8 The difference   ( w   −   C   ˆ ζ ζ ζ   )   stands here for the unique angle   β   ∈   ( − π ,   π ]  such that   β   + C   ˆ ζ ζ ζ   =   w   +   k 2 π   for a certain   k   ∈   Z . w  η  f R ,    ̇ f R  HGO  ˆ x 1   =   ∠   h ( ˆ z 1 ,   ˆ z 2 ,   ˆ z 3 ,   f R ,   η )  ˆ x 2   =   ˆ z 2  ˆ x 3   =   ˆ z 1   −   ∠   h ( ˆ z 1 ,   ˆ z 2 ,   ˆ z 3 ,   f R ,   η )  ˆ x 4   =   ω  ˆ z  η  f R  ˆ x  Fig. 2: Observer.  C. Closed-loop System Stability with State Observation  We show now how the estimated state can be then used to compute the control action, closing the control loop with only on-board sensors, preserving the stability of the global system. First of all notice that if controller   Γ a   in (8) is used to control   y a   then it is not possible to close the loop with the observer (23). In fact, the observer requires the knowledge of  ̇ u 1 , while the static controller   Γ a   directly computes the thrust  u 1   without passing first through its derivative, and, even worse,  u 1   can be even discontinuous and so non-differentiable. Nev- ertheless, one can use the controller   Γ a ′   in (9), thus obtaining a differentiable   u 1   and the knowledge of  ̇ u 1   that becomes an available internal state of the controller. Summarizing,   Γ a ′   can be used with the observer while   Γ a   can be used only with a full knowledge of the state coming from the measurements. Let us now analyze the closed-loop system composed by the plant (2) and the output feedback controller (9) computed on the basis of the estimated state ˆ x . For (24) we can write  u ′   =   Γ a ′ ( ˆ x ) =   Γ a ′ ( ˆ z ,   u 1 ,   η ) ,   (25) i.e., the control law depends directly on the estimation of the HGO, the measures and the state of the dynamic compen- sator. Since the controller is almost globally exponentially convergent, except the case   φ   +   θ   =   π / 2   +   k π , then there exists   ε ?   >   0 such that, for every 0   <   ε   <   ε ? , the closed loop system with controller (25) and observer (23) is exponentially convergent for every state except the cases in which either  φ   +   θ   =   π / 2   +   k π   or   f L   =   0 [18]. Similarly, for the output feedback controller (11) computed using the estimated state ˆ x   there exists   ε ?   >   0 such that, for every 0   <   ε   <   ε ? , the closed loop system with controller  ̄ u   =   Γ b ( ˆ x ,   u 1 ,    ̇ u 1 ) =    ̄ Γ b ( ˆ z ,   u 1 ,    ̇ u 1 ,   η )  and observer (23) is exponentially convergent for every state except the cases in which either   f R   =   0 or   f L   =   0. Although the convergence of the observer is almost global, an initialization phase of the estimation can be useful in order to minimize the transient duration, e.g., using the method proposed in [9] for a quasi static initial condition.  D. Disambiguation of   η   and Observability Discussion  As explained before, the transformation of the measure- ments presents an ambiguity on the sign of   η , that can be considered positive,   η + , or negative,   η − . We show in this section how the correct choice can be easily made. Refer to Fig. 3 for a graphical representation. For each of the two possible choices let us implement an HGO equal to (23)  ̇ ˆ z +   =   A ˆ z +   +   B σ   ( ˆ z + ,   μ μ μ + ) +   λ λ λ   ( μ μ μ + ) +   H ( w +   −   C ˆ z + )   ̇ ˆ z −   =   A ˆ z −   +   B σ   ( ˆ z − ,   μ μ μ − ) +   λ λ λ   ( μ μ μ − ) +   H ( w −   −   C ˆ z − )  Preprint version final version at http://ieeexplore.ieee.org/   7   Accepted for IEEE Transaction on Robotics 2016
Measures  transform.  a  ω  f R ,    ̇ f R  Observer  +  Observer  −  η − ,   w −   Selector  ˆ x +  ˆ x −   ̃ e +   ̃ e −  ˆ x  η + ,   w + Fig. 3: Global observer with disambiguation of   η .  obtaining   two   different   estimations   of   the   state   ( ˆ z + ,   ˆ z − ) , and therefore two different estimations of the original state  ( ˆ x + ,   ˆ x − )   of which only one is correct. In order to select the correct state we propose a discrimination method based on the comparison of the measurement prediction errors. At the first observer we assign a prediction error  ̃ e +   smoothed with an exponential discount factor:  ̇  ̃ e +   =   λ   ( ‖ a   −   ˆ a + ‖ −    ̃ e + ) , where  λ   ∈   R > 0   sets the discount rate, and ˆ a +   is defined as ˆ a +   =  [   lc ˆ x 1 + +   ˆ x 3 +  (   ˆ x 2 2 +   +   a 1 s ˆ x 1 +   +   a 2 s ˆ x 1 + +   ˆ x 3 +   u 1  )  ls ˆ x 1 + +   ˆ x 3 +  (   ˆ x 2 2 +   +   a 1 s ˆ x 1 +   +   a 2 s ˆ x 1 + +   ˆ x 3 +   u 1  )   −   a 2 u 1  ]  .   (26) The other prediction error  ̃ e −   is defined similarly. Then we select the estimation provided by the observer implementation which produces the prediction error closer to zero, i.e., ˆ x   =   ˆ x +  if  ̃ e +   ≤    ̃ e −   and ˆ x   =   ˆ x −   otherwise. In Fig. 3 the full observer with discrimination chain is rep- resented. Notice that the disambiguation of the two observers is not done directly using ˆ z   because is not possible to write the predicted measure ˆ a   as function of ˆ z   without introducing another ambiguity. Indeed, as we saw in Sec. V-A, trying to re- place ˆ x   with ˆ z   into (26) inverting the state transformation (17), we   introduce   an   ambiguity   on   the   sign   of   s x 1   .   Whereas the problem does not hold if we apply this discrimination technique on the original state estimation   ( ˆ x + ,   ˆ x − ) . Notice that for the motions that implies a constant elevation ( x 2   =   0), it is not possible to discriminate the correct observer. Indeed, with   x 2   =   0 and since   w ±   =   z 1   +   k π , after a transient the two observers converge to (ˆ z 1 ±   =   z 1   +   k π , ˆ z 2 ±   =   0, ˆ z 3 ±   =  0), and, using (24), we obtain ( ˆ x 1 ±   =   x 1   +   k π , ˆ x 2 ±   =   0, ˆ x 3 ±   =  x 3 , ˆ x 4 ±   =   x 4 ). Under this condition, from equation (26) it results that the prediction errors of the two observers converge both to zero thus making ineffective the proposed discrimination strategy. Nevertheless, in practice this is not a problem. Indeed, if the controller loop is closed with the wrong observer then the wrong estimation will let the control implement a law that is different from the sole that keeps   x 2   =   0 causing   x 2   6   =   0 and thus the predictions errors will become discriminant. Finally, notice that the ambiguity issue discussed in this section is present only in the initial phases. Whenever the good observer is selected with sufficient certainty, one can switch off the other. For this purpose one can set a confidence threshold on the tracking error of the desired output. If an observer reaches the confidence threshold then this is identified as the correct one and the other one is switched off.  E. Discussion on the Proposed Observer 1) Applicability:   If the link force   f L   is zero then   η   is also zero and   w   cannot be determined. In the very special case that the link force has to be constantly equal to zero, the proposed observer is not applicable 9   (while the controller would still work). Nevertheless, if the desired link force is passing through zero for a sufficiently short time interval, then the proposed observer can be still used in practice by updating the filter without the correction term in that time instants, i.e., imposing  ̇ ˆ z   =   A ˆ z   +   B σ   ( ˆ z ,   μ μ μ ) +   λ λ λ   ( μ μ μ )   if   w   =   0 .  In this way the error dynamics becomes non strictly stable for a short moment but the dynamics returns asymptotically stable as soon as the link force returns to a non-zero value.  2) Robustness:   In order to deal with known drawbacks of the HGO, such as peaking phenomenon and noise sensitivity, many common practical solutions have been presented in the literature, see e.g., [18]. For example, to overcome the peaking phenomenon, it is sufficient to saturate the estimated state on a bounded region that defines the operative regions of the state for the system in exam. In the presence of measurement noise, the use a switched-gain approach can guarantee a quick convergence to the real state during the first phase while reducing the noise effects at steady state. VI. N UMERICAL   V ALIDATION  In this section we present a comprehensive validation of the method with the main purpose of testing it under difficult non- ideal conditions, thus showing both its strengths and possible limits when applied on a real system. We simulate a standard aerial   vehicle   whose   nominal   parameters   are   m R   =   1 [ kg ] ,  J R   =   0 . 25 [ kg m 2 ]   and a link of length   l   =   2 [ m ] . Concerning the controller   Γ b   we set the gains of the linear outer control loop,   k b  1   and   k b  2 , such that the error dynamics of   y b  1   and   y b  2  has poles in   ( − 1 ,   − 1 . 5 ,   − 2 ,   − 2 . 5 )   and   ( − 1 ,   − 1 . 5 )   respectively. While for the controller   Γ a ′  we set the gains   k a ′  1   and   k a ′  2   , such   that   the   error   dynamics   of   y a  1   and   y a  2   has   poles   in  ( − 0 . 5 ,   − 1 ,   − 1 . 5 )   and   ( − 0 . 5 ,   − 1 )   respectively. For the gains of the observer we set   ε   =   0 . 1 and   ( α 1 ,   α 2 ,   α 3 )   such that the root of   s 3   +   α 1 s 2   +   α 2 s   +   α 3   are   ( − 6 ,   − 4 . 5 ,   − 3 ) . Those values guarantee the stability of the closed loop system and a sufficiently rapid convergence of the observer and controller. For the controller   Γ a ′  , the desired trajectory is a smooth step, continuous up to the third order for   φ   and up to the second order for   θ   , from the initial values   φ d  0   =   10 ◦ ,   θ   d  0   =   30 ◦   to the final values   φ d f   =   50 ◦ ,   θ   d f   =   5 ◦ , respectively. For the controller  Γ b , the desired trajectory is a smooth step, continuous up to the fourth order for   φ   and up to the second order for   f L , from the initial values   φ d  0   =   45 ◦ ,   f Ld  0   =   3 [ N ] , to the final values   φ d f   =   135 ◦ ,   f Ld f   =   5 [ N ] , respectively. Smooth step-like trajectories (see Fig. 8), as it will be clear later, have the benefit of clearly showing the performances of the controllers under three important conditions: the initial transient, the tracking of a fast time-varying signal, and the steady state. To obtain a complete validation, in the following we show a concise summary of the results about the stability and robustness of the proposed method under different non-ideal conditions, such as: a) nonzero initial tracking and estimation errors, b) parametric variations, c) generic CoM position and  9 The static observer in [9] requires a nonzero link force as well to work.  Preprint version final version at http://ieeexplore.ieee.org/   8   Accepted for IEEE Transaction on Robotics 2016
" m R  -0.1   0   0.1 0 0.5 1  Phase 1  7 e track 1   7 e track 2   7 e track 3  " m R  -0.1   0   0.1  Phase 2  " m R  -0.1   0   0.1  Phase 3 " l  -0.4   -0.2   0   0.2 0 0.05 0.1 0.15 0.2 0.25   Phase 1  7 e track 1   7 e track 2   7 e track 3  " l  -0.4   -0.2   0   0.2  Phase 2  " l  -0.4   -0.2   0   0.2  Phase 3 " J R  -0.5   0   0.5 0 0.1 0.2 0.3 0.4 0.5   Phase 1  7 e track 1   7 e track 2   7 e track 3  " J R  -0.5   0   0.5  Phase 2  " J R  -0.5   0   0.5  Phase 3 Fig. 4:   Parametric variation - Controller   Γ a ′  (elevation and attitude) . The subscript 1, 2, and 3 correspond to the three different trajectory times. Outside of the displayed range of parametric variation the performances are unacceptable or the closed loop system results to be even unstable.  non-negligible link mass, d) noisy sensor measurements, and e) non-ideal motors. Although not very relevant for the investigation of the robustness of the method, we provide in [14] extra plots relative to the performances of controller and observer for each non-ideal case with some typical values.  A. Validation for Nonzero Initial Tracking/Estimation Errors  In order to show the asymptotic convergence performances of both the controller and the observer we initialize the control system with nonzero initial tracking and estimation errors. After the convergence of the observer, which takes less than one second, the controller   Γ b   is able to steer the outputs along the desired trajectories with zero error. A similar behavior is obtained for the controller   Γ a ′  . Since the results only show that the controller is asymptotically convergent, as expected, we have reported the plots in [14] for limits of space.  B. Parametric Variations  In   a   real   scenario   the   exact   parameters   of   the   system are unknown. The purpose of this section is to investigate the robustness of the proposed method in this scenario. In particular we introduce discrepancies between the real model and the nominal one used in the controller/observer. We notice that in principle one could try to design an adaptive control law that is able to compensate for parametric uncertainties. However, this is clearly a tough objective, be- cause the system is nonlinear and the available measurements " m R  -0.1   0   0.1 0 1 2 3 4   Phase 1  7 e track 1   7 e track 2   7 e track 3  " m R  -0.1   0   0.1  Phase 2  " m R  -0.1   0   0.1  Phase 3 -0.4   -0.2   0   0.2  " l  0 0.5 1  Phase 1  -0.4   -0.2   0   0.2  " l  Phase 2  -0.4   -0.2   0   0.2  " l  Phase 3 " J R  -0.5   0   0.5 0 0.5 1  Phase 1  7 e track 1   7 e track 2   7 e track 3  " J R  -0.5   0   0.5  Phase 2  " J R  -0.5   0   0.5  Phase 3  Fig. 5:   Parametric variation - Controller   Γ b   (elevation and link force) .  are only the (nonlinear) accelerometer and the gyroscope readings. Therefore this goal is left as future work. Instead, we concentrate in this section on assessing the ranges of parameter variations that causes a degradation of the performance that re- mains within an acceptable bound. By doing so, we shall see in fact that the proposed control scheme possesses a remarkable robustness even without the presence of an adaptive design. Considering   l 0 , m R 0   and   J R 0   the real parameter value and  l , m R   and   J R   the nominal one, we set   l   = ( 1   +   ∆ l ) l 0 ,   ( 1   +  ∆ m R ) m R 0   and   ( 1   +   ∆ J R ) J R 0 , where   ∆ m R ,   ∆ l   and   ∆ J R   denote the relative parametric variations. For obtaining a comprehensive analysis we tested the behav- ior for several different parametric variation combinations. The results are plotted in Figs. 4 and 5, where we show the mean tracking error,  ̄ e track , and the corresponding standard deviation  σ  ̄ e track   , defined as  e track ( t ) =  ∥ ∥ y d  1   ( t )   −   y 1 ( t ) ∥ ∥  y d  1   ( t )   +  ∥ ∥ y d  2   ( t )   −   y 2 ( t ) ∥ ∥  y d  2   ( t )   ̄ e track   =   1  t   f   −   t 0  ∫   t   f  t 0  e track ( t ) dt  σ  ̄ e track   =  √  1  t   f   −   t 0  ∫   t   f  t 0  ( e track ( t )   −    ̄ e track ) 2 dt ,  where   t 0   and   t   f   are the initial and final time, respectively. In the plots the solid line corresponds at the mean tracking error while the dashed lines correspond at the mean tracking error plus and minus its standard deviation. The effect of an unknown parameter could also change with respect to the trajectory and in particular with respect to the velocity and acceleration at which the path is followed. Consequently we plotted the mean tracking error,  ̄ e track 1 ,  ̄ e track 2  Preprint version final version at http://ieeexplore.ieee.org/   9   Accepted for IEEE Transaction on Robotics 2016
f R  z W  x W  x B  z B  θ  − m R g z W  − f L d  φ  l  O W  O B  τ R  r BL  − m L g z W  x L  z L   O L Fig. 6: Representation of the more general system and its variables.  and  ̄ e track 3 , for the same type of desired path (smooth step) but executed with three different durations (increasing velocity): 1) 7 [ s ] , 2) 5 [ s ]   and 3) 3 [ s ]   respectively. We also analyze the error behavior dividing the trajectory into three phases: in the   Phase 1   (transient) the desired trajectory is constant and the analysis is more focused on the convergence of the observer;   Phase 2   constitutes the dynamic part where the desired trajectory quickly goes from the initial value to the final one; the   Phase 3 , the last, corresponds to the steady state condition where the desired trajectory is again constant. We show the tracking error for each of the three phases to better understand if a parameter variation affects more the transient, the dynamic phase, or the static one. From Fig. 4 and Fig. 5 one can notice that, as expected, the performances get worse increasing the parametric variation. Furthermore, the same variation has more effect if the trajec- tory is more “aggressive” and it is followed with higher speed (  ̄ e track 3 ). This is due to the fact that with higher speed and acceleration the inertial and Coriolis/centripetal terms become larger, and thus also the error in the feedback linearization increases, which in turn implies a worst tracking. Comparing the performances between the two controllers, we noticed that the controller   Γ a ′  results to be more robust then the controller   Γ b   in term of mean tracking error. This is due to the fact that for the controller   Γ a ′  , the dynamics of one of the controlled outputs, namely   θ   , is not influenced by the parameters such as mass and length of the link. This means that any variation on these parameters does not generates a worse tracking of   θ   d   , which results in a lower tracking error. One can also notice that the mean tracking error is not in general symmetric with respect to the sign of the relative parametric variation. For example for the controller   Γ a ′  it is better to overestimate the mass, and the length of the link rather than underestimating them, while for controller   Γ b   it results to be the opposite, even if these consideration are more relevant for the dynamic phase. Indeed, during the steady state phase the behavior is almost symmetrical. Another fact that appears clear from the plots is that the variation that most influences the performances is the one on the mass of the aerial vehicle. Fortunately, in practice this parameter can be easily measured with high precision.  C. General   Model   with   Generic   CoM   Position   and   Non- negligible Link Mass  The controllers developed in this paper assume that the sys- tem can be represented with the model given in Fig. 1, where the CoM of the aerial vehicle coincides with the attachment point of the link to the vehicle and the link has a negligible mass. Fig. 6 represents instead a more general model for which the assumptions done in Sec. II are not fulfilled. Taking into account the definitions made in Sec. II we then define a   body frame ,   F L , attached to the link, with axes   { x L ,   y L ,   z L }   and origin   O L   coinciding with the center of mass (CoM) of the link. The position of   O L , defined in   F W   , is denoted with  p L   = [ x L   y L   z L ] T   . As for   F B   we have that   y L   ≡   y B   ≡   y W   and  y L   ≡   0. For the validation we model the link as a rigid body of mass   m L   ∈   R > 0   and inertia   J L   ∈   R > 0 . Considering the inertia of the link as the inertia of an infinitesimally thin rigid tie with uniform distributed mass, we have also that   J L   =   m L l 2 / 12. Assuming links with high stiffness, the deformations and the elongations results negligible with respect to the length of the cable itself, in the range of forces of our concern. Therefore the link length is fixed. The link is connected at one end to a fixed point coinciding with   O W   and at the other end to a point rigidly attached to the aerial vehicle whose constant position in   F B   is denoted with   r BL   = [ r x   0   r z ] T   . If   ‖ r BL ‖   =   0 then the link is directly attached to the CoM of the aerial vehicle. The mechanical model of the more general robotic system can be then derived writing the dynamics as the one in (1) plus a disturbance due to the non idealities:  M ( q )    ̈ q   +   g ( q ) +   δ δ δ   ( q ,    ̇ q ,    ̈ q ,   u ) =   Q ( q ) u ,  where  δ δ δ   ( q ,    ̇ q ,    ̈ q ,   u ) =    ̄ M ( q )    ̈ q   +    ̄ c ( q ,    ̇ q ) +    ̄ g ( q )   −    ̄ Q ( q ) u ,   ̄ M   =  [    ̄ J φ   J φθ  J θ φ    ̄ J θ  ]  ,    ̄ c   =  [    ̄ c    ̇ θ   2   ̄ c    ̇ φ 2  ]  ,   ̄ g   =  [   m L  2   lg d ⊥   ·   e 3  − m R lg    ̄ R W B   r BL   ·   e 3  ]  ,    ̄ Q   =  [   0   0  − r BL   ·   e 3   0  ]  ,  where  ̄ R W B   =   ∂   R W B   / ∂ θ   ,    ̄ J φ   =   m L l 2 / 3,    ̄ J θ   =   m R   ‖ r BL ‖ 2 ,   J φθ   =  J θ φ   =   − m R l    ̄ R W B   r BL   ·   d ⊥ ,  ̄ c   =   m R l    ̄ R W B   r BL   ·   d . For a plausible case in which the link consists of a cable of mass   m L   =   0 . 01 m R   and inertia (during taut condition)   J L   =  m L l 2 / 12, and it is attached to the robot in the position   r BL   = [ 0 . 03 0 0 . 03 ] T   [ m ]   with respect to   F B , we noticed that the controlled system is stable but the error does not converge exactly to zero. Indeed, due to the nonzero   ‖ r BL ‖ , the force along the link generates an extra torque on the aerial vehicle that is not compensated and so a constant steady state error appears (the corresponding plots can be found in [14]). In order to understand how each parameter of the more general model affects the tracking performances, as done in Sec. VI-B, we show in Fig. 7 the mean tracking error and its standard deviation for different parameter values and in the three phases described before. In particular the mass of the link is taken as   m L   =   ∆ m L m R . We noticed that the negative effects due to a nonzero offset  r BL   reduce or increase if the rotational inertia is increased or reduced, respectively. Indeed, looking at the rotational dynamics in the case of non zero offset:  ̈ θ R   =   τ R / J R   −   f L d   ·   r BL / J R ,   (27)  Preprint version final version at http://ieeexplore.ieee.org/   10   Accepted for IEEE Transaction on Robotics 2016
r BLx =J R  -0.2   0   0.2 0 0.02 0.04 0.06 0.08  Phase 1  7 e track 1   7 e track 2   7 e track 3  r BLx =J R  -0.2   0   0.2  Phase 2  r BLx =J R  -0.2   0   0.2  Phase 3 r BLz   =J R  -0.2   0   0.2 0 0.2 0.4 0.6 0.8   Phase 1  7 e track 1   7 e track 2   7 e track 3  r BLz   =J R  -0.2   0   0.2  Phase 2  r BLz   =J R  -0.2   0   0.2  Phase 3 " m L  0   0.1   0.2 0 0.1 0.2 0.3 0.4   Phase 1  7 e track 1   7 e track 2   7 e track 3  " m L  0   0.1   0.2  Phase 2  " m L  0   0.1   0.2  Phase 3 Fig.   7:   Controller   Γ b :   mean   tracking   error   when   changing   the parameters of the general model.  one can notice that the effect of the link force on the angular acceleration decreases if the inertia increases. Intuitively, a bigger inertia would mean a bigger mass or a bigger dimension of the vehicle. In the second case, the bigger the vehicle the more the effect of the offset becomes negligible. For this reason in Fig. 7 we plot the mean tracking error with respect to   r BL x   / J R   and   r BL z   / J R , thus normalizing this effect. We did the same test for the controller   Γ a ′  , which resulted to be much more sensitive to link mass and to the offset than the controller   Γ b . This is due to the fact that one of the output, namely the attitude of the aerial vehicle   θ   , is directly influenced by the offset as it is shown in (27). Furthermore, even with a small offset the tracking error is such that the actual trajectory passes through the singularity of the controller  Γ a ′  (see Sec. IV-A) causing an unstable behavior. On the other hand, the controller   Γ b   turned to be much more robust to these sort of structural model variations. From Fig. 7 we can see that the parameters that mostly affect an increase of the error are the entries of   r BL , i.e.,   r BLx   and  r BLz . One can notice that it is more advisable to attach the link such that one is sure that   r BLz   ≤   0, especially if agile motions are required. The effect of the displacement along   x R  is instead almost symmetric. The small asymmetry is due to the particular trajectory passing from the first to the second quadrant. The mean tracking error increases instead almost linearly with respect to the mass of the link. Nevertheless, even with   m L   equal to the 20% of   m R   the closed loop system remains still perfectly stable. [deg]  40 60 80 100 120 140  ' d   '  [N]  1 1.5 2  f   d L   f L  [N]  10.5 11 11.5  f   n R   f R  [Nm]  -0.2 -0.1 0 0.1 0.2   =   n R   = R  [s]  0   5   10   15   20  [deg]  -5 0 5  # n   #  [s]  0   5   10   15   20 0 0.1 0.2 0.3   e tracking  (a) Controller Results. [deg]  50 100 150  '   ^ '  [deg = s]  -20 0 20 40   _ '   ^ _ '  [deg]  -10 -5 0 5   #   ^ #  [deg = s]  -10 0 10  _ #   ^ _ #  [s]  0   5   10   15   20  [m = s 2 ]  -10 -5 0  a x   a z  [s]  0   5   10   15   20  [m = s 2 ]  0 0.1 0.2   e  (b) Observer Results.  Fig. 8:   Controller   Γ b : Noisy measurements. Similar kind of results have been observed also for the controller   Γ a ′  .  D. Noise on the Measurements  In this section we investigate the robustness of the proposed method in presence of noisy measurements, which always exist in reality. We consider both the accelerometer and the gyroscope measures being affected by a white Gaussian noise of variance 0 . 1 [ m / s 2 ]   and 0 . 01 [ rad / s ]   respectively. From   Fig.   8   we   can   observe   that   the   estimated   state shows some noise but the corresponding error remains always bounded. Due to the noisy component on the estimated state the control action presents some oscillations that imply a non exact tracking of the desired trajectory. Nevertheless the tracking error remains small and always bounded. Similar results have been recorded also for the controller   Γ a ′  . Notice that to achieve these results we had to reduce the gains of both the controller and observer. Indeed, high gain values increase the convergence speed but also amplify the sensitivity to noisy measurements. In general the two controllers does not show particularly different behaviors in face of the presence of noise.  E. Non-ideal Motors  In a real scenario, one motor can not instantaneously change the spinning velocity of the propeller, and in turn the thrust produced. Instead the dynamics of the motor is characterized  Preprint version final version at http://ieeexplore.ieee.org/   11   Accepted for IEEE Transaction on Robotics 2016
by a certain time constant,   τ M   ∈   R , that quantifies the time needed to change the motor speed. We analyzed this additional non ideality testing the proposed method with different non- ideal motors characterized by an increasing time constant. The results, extensively presented and discussed in [14], show that the tested system remains stable up to a time constant of 0 . 08 [ s ] , which is completely acceptable in real systems. VII. C ONCLUSIONS  In this work we presented the main intrinsic characteristics of a tethered aerial robot system such as differential flatness, controllability and observability. We then used these results to design two different controllers, the first to achieve the tracking of elevation and pitch angles, and the second for the tracking of elevation angle and link force. To close the control loop we designed a nonlinear observer able to retrieve the state from any dynamic condition using a inertial-only on-board sensors. This showed that inertial only measurements are enough to estimate the full state of the system. An exhaustive numerical validation of the proposed method was presented considering typical non-ideal conditions such as parametric variation, more general model, noisy measurements and non-ideal motors. We have recently started to build a real platform in order to implement and test the method under real experiments. Some preliminary experiments based on the theoretical discoveries presented here are already available in [4]. In the future we plan to further extend such experimental system to take full advantage of the theory shown in this paper. Another possible improvement would be to exploit the flatness property for mo- tion planning respecting the limits and avoiding singularities. Automatic identification of the system parameters and control adaptation are other possible directions of development. R EFERENCES [1]   I. Maza, K. Kondak, M. Bernard, and A. Ollero, “Multi-UAV cooper- ation and control for load transportation and deployment,”   Journal of Intelligent & Robotics Systems , vol. 57, no. 1-4, pp. 417–449, 2010. [2]   M. Manubens, D. Devaurs, L. Ros, and J. Cort ́ es, “Motion planning for 6-D manipulation with aerial towed-cable systems,” in   2013 Robotics: Science and Systems , Berlin, Germany, May 2013. [3]   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. [4]   M. Tognon, A. Testa, E. Rossi, and A. Franchi, “Takeoff and landing on slopes via inclined hovering with a tethered aerial robot,” in   2016 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems , Daejeon, South Korea, Oct. 2016, pp. 1702–1707. [5]   S.-R. Oh, K. Pathak, S. K. Agrawal, H. R. Pota, and M. Garrett, “Approaches for a tether-guided landing of an autonomous helicopter,”  IEEE Trans. on Robotics , vol. 22, no. 3, pp. 536–544, 2006. [6]   L. A. Sandino, M. Bejar, K. Kondak, and A. Ollero, “Advances in modeling and control of tethered unmanned helicopters to enhance hovering   performance,”   Journal   of   Intelligent   &   Robotics   Systems , vol. 73, no. 1-4, pp. 3–18, 2014. [7]   F. Muttin, “Umbilical deployment modeling for tethered UAV detecting oil pollution from ship,”   Applied Ocean Research , vol. 33, no. 4, pp. 332–343, 2011. [8]   EC-SAFEMOBIL, “EU Collab. Project FP7-ICT 288082,” http://www. ec-safemobil-project.eu/. [9]   S. Lupashin and R. D’Andrea, “Stabilization of a flying vehicle on a taut tether using inertial sensing,” in   2013 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems , Tokyo, Japan, Nov 2013, pp. 2432–2438. [10]   M. M. Nicotra, R. Naldi, and E. Garone, “Taut cable control of a tethered UAV,” in   19th IFAC World Congress , Cape Town, South Africa, Aug. 2014, pp. 3190–3195. [11]   R. Naldi, A. Gasparri, and E. Garone, “2012 IEEE conf. on control applications,” in   Cooperative pose stabilization of an aerial vehicle through physical interaction with a team of ground robots , Dubrovnik, Croatia, Oct. 2012, pp. 415–420. [12]   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 Automation , Seattle, WA, May 2015, pp. 3994–3999. [13]   M. Fliess, J. L ́ evine, P. Martin, and P. Rouchon, “Flatness and defect of nonlinear systems: Introductory theory and examples,”   International Journal of Control , vol. 61, no. 6, pp. 1327–1361, 1995. [14]   M. Tognon and A. Franchi, “Extended derivations and additional simu- lations for aerial robots tethered by cables or bars,” LAAS-CNRS, Tech. Rep. hal-01473165, Feb. 2017. [15]   A. De Luca and G. Oriolo, “Trajectory planning and control for planar robots with passive last joint,”   The International Journal of Robotics Research , vol. 21, no. 5-6, pp. 575–590, 2002. [16]   A. Isidori,   Nonlinear Control Systems, 3rd edition .   Springer, 1995. [17]   B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo,   Robotics: Mod- elling, Planning and Control .   Springer, 2009. [18]   H. K. Khalil,   Nonlinear Systems , 3rd ed.   Prentice Hall, 2001.  Marco Tognon   (S’15) received the M.Sc. degree in automation engineering in 2014, from the University of Padua, Italy, with a master thesis carried out at the Max Planck Institute for Biological Cybernetics, T ̈ bingen, Germany. He is currently pursuing the Ph.D. degree at LAAS-CNRS, Toulouse, France. His current research interests include robotics, un- manned aerial vehicles for physical interaction.  Antonio Franchi   (S’07-M’11-SM’16) received the Ph.D.   degree   in   System   Engineering   from   the Sapienza University of Rome, Italy, in 2010, and the Habilitation (HDR) in 2016 from the National Polytechnic Institute of Toulouse, France. In 2009 he was a Visiting Scholar at the University of Cali- fornia at Santa Barbara. From 2010 to 2014 he was first Research Scientist and then Senior Research Scientist and the Project Leader of the Autonomous Robotics and Human-Machine Systems group at the Max Planck Institute for Biological Cybernetics in T ̈ ubingen, Germany. Since 2014 he has been a Permanent CNRS Researcher in the RIS team at LAAS-CNRS, Toulouse, France. His main research interests are on autonomous systems, with a special regard to robot control and estimation, in particular for multiple-robot systems and aerial robotics. He published more than 90 papers in peer-reviewed international journals and conferences. In 2010 he was awarded the IEEE RAS ICYA Best Paper Award. He is an Associate Editor of the IEEE Transaction on Robotics. He is the co- founder of the IEEE RAS Technical Committee on Multiple Robot Systems and of the International Symposium on Multi-robot and Multi-Agent Systems.  Preprint version final version at http://ieeexplore.ieee.org/   12   Accepted for IEEE Transaction on Robotics 2016