A Robust Interaction Control Approach for Underwater Vehicle Manipulator Systems

Shahab Heshmati-Alamdaria,∗, Charalampos P. Bechlioulisa, George C. Karrasa, Alexandros Nikoub, Dimos V. Dimarogonasb, Kostas J. Kyriakopoulosa

aControl Systems Lab, Department of Mechanical Engineering, National Technical University of Athens, 9 Heroon Polytechniou Street, Zografou 15780. E-mail: {shahab, chmpechl, karrasg, kkyria}@mail.ntua.gr bKTH Center for Autonomous Systems, KTH Royal Institute of Technology, SE-100 44, Stockholm, Sweden. E-mail: {anikou, dimos}@kth.se

Abstract In underwater robotic interaction tasks (e.g., sampling of sea organisms, underwater welding, panel handling, etc) various issues regarding the uncertainties and complexity of the robot dynamic model, the external disturbances (e.g., sea currents), the steady state performance as well as the overshooting/undershooting of the interaction force error, should be addressed during the control design. Motivated by the aforementioned considerations, this paper presents a force/position tracking control protocol for an Un- derwater Vehicle Manipulator System (UVMS) in compliant contact with a planar surface, without incorporating any knowledge of the UVMS dynamic model, the exogenous disturbances or the contact stiffness model. Moreover, the proposed control framework guarantees: (i) certain predefined minimum speed of response, maximum steady state error as well as overshoot/undershoot con- cerning the force/position tracking errors, (ii) contact maintenance and (iii) bounded closed loop signals. Additionally, the achieved transient and steady state performance is solely determined by certain designer-specified performance functions/parameters and is fully decoupled from the control gain selection and the initial conditions. Finally, both simulation and experimental studies clarify the proposed method and verify its efficiency. Keywords: Underwater Vehicle Manipulator Systems, Nonlinear Control, Autonomous Underwater Vehicles, Marine Robotics, Force/position Control, Robust Control.

1. Introduction led inevitably to the newal of ROVs with Autonomous Under- water Vehicles (AUVs) as well as to the development of au- During the last decades, unmanned underwater vehicles are tonomous intervention control platforms like the UVMSs that being used widely in various areas such as marine science (e.g., have attracted significant scientific interest during the last years biology, oceanography, archeology) and offshore industry (e.g., 25 [9, 4, 10]. Specifically, during the late 90s, efforts on the de-

5 ship maintenance, inspection of oil/gas facilities, cable burial, sign of UVMSs were made within the pioneering AMADEUS mating of underwater connectors, underwater welding) [1]. In project [11], which were later exploited in UNION [12] and particular, a vast number of the aforementioned applications de- SAUVIM projects [9], where autonomous underwater interven- mand the underwater vehicle to be enhanced with intervention tion was carried out for the ﬁrst time. A more recent European capabilities as well [2, 3], thus raising increasing interest on 30 project which has boosted the underwater robotic interaction

10 Underwater Vehicle Manipulator Systems (UVMSs) [4, 5]. A with relevant results was TRIDENT [13, 14, 15, 16, 17, 18], UVMS consists of an underwater robotic vehicle equipped with where a vehicle-arm system was controlled in a coordinated robotic manipulators. Currently, underwater intervention is car- manner. Another important milestone was achieved in the PAN- ried out by Remotely Operated Vehicles (ROVs) equipped with DORA project [19, 20, 21], where a strong emphasis was given one or multiple manipulators, which allow it to grasp, trans- 35 on the issue of persistent autonomy. The latest related project

15 port and manipulate objects. However, most ROVs are con- in the domain of underwater intervention is the on-going Eu- trolled by a human pilot on a surface ship, via a master-slave ropean project DexROV [22], which focuses on inspection and tele-operation scheme [6, 7, 8]. Therefore, the well-known dis- maintenance tasks in the presence of communication latencies. advantages of human-robot tele-operation (e.g., time delays and increase of human fatigue over time) necessitate for automated It is well known that underwater tasks are very chal- 40 20 and efficient solutions to reduce costs and safety risks, which lenging owing mainly to external disturbances (i.e., sea currents), the lack of appropriate and adequately accurate sens- ing/localization [23] and the unknown (or partially known) con- ∗Corresponding author strained environment (e.g., offshore industry, oil/gas facilities) Email addresses: [email protected] (Shahab Heshmati-Alamdari), [24, 25]. The aforementioned difficulties make the control of [email protected] (Charalampos P. Bechlioulis), 45 underwater manipulator systems a challenging problem that has [email protected] (George C. Karras), [email protected] (Alexandros Nikou), [email protected] (Dimos V. Dimarogonas), [email protected] already gained significant scientific attention within the ma- (Kostas J. Kyriakopoulos) rine robotic community during the last years [25]. Such con-

Preprint submitted to Annual Reviews in Control June 11, 2018 trol problems involve constrained high-dimensional nonlinear105 be achieved by addressing various issues such as: i) the sluggish systems with signiﬁcant complexity regarding the uncertainty response of vehicle‘s thrusters [40, 41, 42], ii) the manipulators

50 of the robot dynamics, the redundancy of the system, the vari- joint limits, iii) the manipulability and iv) the conﬁguration sin- ous operational constraints (e.g., visibility constraints and joint gularities [39]. In general, two strategies at the kinematic [43] limits), the nonlinear coupled dynamics between the underwa- and the dynamic [44] levels respectively have been developed

ter vehicle and manipulator systems [26] as well as the grav-110 for handling the robot‘s redundancy. The former was enhanced ity/buoyancy forces that aﬀect the response of the manipulator. later in [45] for an arbitrary number of priority levels with in-

55 All aforementioned challenges should be taken into consider- ferior computational complexities. That framework, which is ation when designing the control system of a UVMS. Hence, based on the least-squares minimization technique, solves a ﬁ- from a control perspective, achieving such goals highlights the nite number of prioritized secondary tasks (e.g., maintaining

need of employing appropriate methodologies from nonlinear115 manipulators joint limits, increasing the manipulability) in a hi- robot control theory [27]. However, most control schemes that erarchical structure. However, a compromise raises among the

60 have been developed so far for static manipulators and space tasks corresponding to the inequality objectives (e.g., joint lim- robots cannot be used directly on UVMS owing to the afore- its) [46]. In order to tackle this issue, a method was proposed mentioned speciﬁcs. Therefore, the control of UVMS remains based on quadratic programming in [47], which solves equal-

still a challenging task. 120 ity and inequality constraints at any priority level. However, Until the early 1990s, only a few research studies had ap- that approach leads in high dimensional quadratic program-

65 peared that dealt with the UVMS control [28]. Even deriving ming problems and cannot handle the activation or deactiva- the dynamic model and developing an eﬃcient dynamic simu- tion of tasks [48]. The aforementioned strategy was extended lation for underwater vehicles equipped with robotic manipula- later in [49], where instead of a cascade of quadratic program-

tors were studied recently in [29, 30, 31]. However, during the125 ming problems, a single problem is solved that identiﬁes the last years, owing to the enormous demand for more dexterous active set of all the constraints at the same time. Nevertheless,

70 and eﬃcient underwater interaction robotic platforms, many re- that strategy lacks the ability of enabling and disabling inequal- search studies have been conducted towards enhancing the ef- ity tasks without causing discontinuities. Recently, a generic ﬁcacy of UVMSs [6]. In [32], a hybrid position/force control method that allows activating and deactivating tasks without in-

scheme was designed and implemented on a hydraulically ac-130 curring discontinuities was proposed in [48]. For the case of a tuated manipulator. That work was extended later into a hybrid UVMS, based on results initially proposed in [50], the redun-

75 position/force controller and was compared to a fixed-gain ver- dancy resolution has been employed firstly in [39] to avoid the sion for various contact stiffness models [33]. However, the manipulator‘s singular configurations. Later, a classical gra- dynamic coupling between the manipulator and the vehicle was dient projection method [43] was adopted that addressed vari-

not considered. In [34], a control scheme was proposed in order135 ous practical criteria such as minimization of load [51, 52] and to achieve high maneuverability of an underwater manipulator restoring moments [53]. Finally, a method based on a fuzzy

80 mounted on a ROV. A force/torque sensor was assumed to be switching technique was proposed in [54] to overcome the con- installed between the ROV and the manipulator to compensate ﬂict among various secondary tasks. the dynamic eﬀects of the underwater manipulator motion on When a UVMS interacts with the environment via its end-

the vehicle. The control strategy proposed in [35] compensates140 eﬀector, the redundancy resolution formalism becomes a tricky for the force/moment exerted on the vehicle from the manip- problem compared to the unconstrained motion tasks. In this

85 ulator by utilizing the buoyant and gravitational forces, while case, the primary task is deﬁned by the velocity tracking along simultaneously maintaining the contact between the robot end- the tangent space and force tracking along the normal space eﬀector and the environment. In the same direction, the force [46]. It should be noticed that only a small number of pub-

control strategy proposed in [36] aims at stabilizing the vehi-145 lications and research studies have addressed the problems cle when the manipulator operates in a constrained workspace. of force-motion control and redundancy resolution within the

90 Moreover, an impedance control technique was studied in [37], same control framework. In particular, the force control scheme where the UVMS dynamic model was considered as an inte- proposed in [55], handles the force tracking task as the sec- grated system. That work was extended later in [38], where ondary objective while the motion of the end eﬀector is deﬁned

impedance control was combined with hybrid position/force150 as the primary objective task. Moreover, the aforementioned control by means of fuzzy switching. Nevertheless, all afore- method needs reseting when the contact is lost. In a recent

95 mentioned control strategies request accurate knowledge of the work [46], a model free control scheme was proposed for a contact stiﬀness model. UVMS in rigid contact with the environment while simultane- On the other hand, treating a UVMS as an integrated system ously dealing with the aforementioned issues. More speciﬁ-

raises signiﬁcant issues regarding the redundancy of the system,155 cally, by means of a priority based inverse kinematic formula- the uncertainties and the dynamic couplings, which all should tion proposed in [47], the UVMS operational limitations along

100 be considered during the control design. It is well known that with the contact maintenance task are formulated as inequality a UVMS is kinematically redundant owing to the vehicle’s de- constraints within a cascade of quadratic programming prob- grees of freedom [39]. The redundancy of a UVMS might be lems. However, beyond the known implementation drawbacks

exploited in order to coordinate the system in such a way that160 of sliding mode control technique (chattering and high gain ex- the end-eﬀector tracking accuracy is guaranteed, and this may citation) that is adopted at the low level, it requires the accurate 2 knowledge of the bounds of the ﬂuid disturbances. Moreover, the transient and steady state performance of the system is not considered at all. 1 165 In this work , we propose a force-motion control strategy for a UVMS in compliant contact with a planar surface. The pur- pose of the controller is to simultaneously track a desired trajectory along the planar surface and a force along the normal direction. The proposed method does not require any knowledge

170 of either the UVMS dynamic parameters, or the stiffness model, or the disturbance profile. Furthermore, various performance issues such as: i) maintaining the contact, ii) tracking the desired trajectory, iii) inferior overshooting of the interaction force error and iv) robust steady state response are achieved. More- 175 over, the novel formulation of the problem, allows the tracking Figure 1: A graphical illustration of the UVMS end-effector in compliant con- of the desired force and position trajectories to be considered tact with a planar surface. equally prioritized, while it enables us to treat various other operational limitations (e.g., joint limits) as secondary tasks, ˙ > and thus fully decouple them. In particular, the proposed con- that satisfies S(ωe) = Re Re , where

180 trol strategy tackles all aforementioned challenges and further  0 −d d  guarantees predefined behavior in terms of overshoot, conver-  z y  S(d) =  d 0 −d  gence rate and maximum steady state error. Furthermore, the  z x −  robustness of the proposed control strategy with respect to ex- dy dx 0 ternal disturbances is enhanced. Finally, the complexity of the > is the skew-symmetric matrix of the vector d = [dx dy dz] . Let 185 proposed control law is significantly low. It is actually a static also x˙ = [x˙>, ω>]> ∈ 6 denote the velocity of the end-effector scheme involving only a few calculations to derive the control e e R frame. Without loss of generality, we have [4]: signal, which enables its onboard implementation straightfor- wardly. x˙ = J(q)ζ (1) The rest of the manuscript is organized as follows: In > > > n 190 Section-2, the problem is rigorously formulated. The analytic where ζ = [v , q˙m,i] ∈ R is the overall velocity vector that description of the proposed method along with the correspond- involves the body velocities of the vehicle v and the joint veloc- 6×n ing stability proof are presented in Section-3. The efficiency ities of the manipulator q˙m,i, i ∈ {1,..., n − 6} and J(q) ∈ R of the proposed approach is illustrated and validated via simu-200 is the geometric Jacobian Matrix [4]. lated and experimental results in Section-4 . Finally, Section-5 We assume that the UVMS has initially established contact

195 concludes our work. with a planar surface, whose normal and tangential vectors expressed w.r.t the inertial frame {I} are known. Thus, for the sake of simplicity, we consider the inertial frame {I} attached at some point on the surface with its x-axis normal to the surface 2. Problem Formulation pointing inwards (See Fig.1). Now, let us denote the unit vector normal to the contact surface and the generalized normal vector as n = [1 0 0]> ∈ 3 and n = [n> 0>]> ∈ 6 respectively. We Consider an n degrees of freedom UVMS in compliant con- s R s 3 R > > > n also assume that the end-effector is rigid, thus the contact com- tact with a planar surface. Let q = [qa , qm] ∈ R be the 2 > > > 6 pliance arises from the planar surface . Hence, the deformation state variables of the UVMS, where qa = [η1 , η2 ] ∈ R > χ is given as a function of xe as follows: involves the position vector η1 = [xv, yv, zv] and the orien- > tation η2 = [φ, θ, ψ] of the vehicle expressed in the Euler- > χ = n xe = xe (2) angles representation with respect to (w.r.t) an inertial frame {I} s n−6 and qm ∈ R is the angle vector of the manipulator’s joints. and its derivative is calculated by: Consider also the frame {E} attached at the end-effector of the > 3 > UVMS described by a position vector xe = [xe, ye, ze] ∈ R χ˙ = ns x˙e = x˙e (3) and a rotation matrix Re = [ne, oe, αe] w.r.t the inertial frame {I}. Let also ωe be the rotational velocity of the end-effector During an intervention task, the UVMS exerts an interaction wrench λ ∈ R6 at the contact, which can be measured by a force/torque sensor attached to its end-effector. This interaction 1Preliminary results of this work were presented in the 20th World Congress of the International Federation of Automatic Control (IFAC) 2017 [56]. In the current version, analytic results on force/position/orientation tracking control 2In case of UVMS with soft tip, the compliance may arise either from the tip with guaranteed contact maintenance and experimental results are provided in side or the surface or both. Thus, the deformation can also be derived without addition. affecting the subsequent analysis. 3 wrench can be decomposed into: (i) nn>λ that is normal to the surface direction, a desired position trajectory on the surface > surface and (ii) (I6×6 − nn )λ involving tangential forces and and possibly attain a desired orientation related to the contact torques, owing to tangential deformations and friction terms. In surface. Hence, we define the force, position, orientation and this work, we assume that the normal force magnitude f = n>λ the overall errors as: is a positive and continuously differentiable nonlinear function d of the material deformation χ: e f = f − f ∈ R, (8a) " # " d# ey ye − ye 2 f = Φ(χ), ∀χ ≥ 0. (4) ep , = d ∈ R , (8b) ez ze − ze e  The aforementioned general formulation includes several force  o1  1 e e  n × nd o × od α × αd ∈ 3, deformation models such as the Hertz model [57] (Φ(χ) = o ,  o2  = e + e + e R (8c) 3 2   2 kχ 2 , k > 0) or the quadratic model Φ(χ) = kχ , k > 0 [58]. eo3 The time derivative of the normal force magnitude in view of > ∈ 6 e , [e f , ey, ez, eo1 , eo2 , eo3 ] R (8d) (3) is then given by: Notice that for the orientation error e , we have employed the ˙ o f = ∂Φ(χ)x ˙e (5) outer product formulation [59, 60] of the end-effector rotation matrix R and the desired rotation matrix Rd to relax the repre- dΦ ∗ e where ∂Φ(χ) = dχ is strictly positive for all χ ≥ χ > 0, where ∗ sentation singularity issue that is inherent in case Euler angles χ is any strictly real positive number. Thus, there is an un- 3 ∗ are adopted . Differentiating (8a)-(8c) with respect to time and known strictly positive constant ∂Φ such that: in view of (5), we obtain: ∗ ∗ ∂Φ(χ) ≥ ∂Φ > 0, ∀χ ≥ χ . (6) d e˙ f = ∂ f (χ)x ˙e − f˙ , (9a) " # " d# Without loss of generality, the dynamics of the UVMS in e˙y y˙e − y˙e e˙p = = d , (9b) complaint contact with the environment can be formulated as e˙z z˙e − z˙e [4]: e˙   o1    d e˙o = e˙o2  = Lωe − Lω , (9c) M(q)ζ˙+C(q, ζ)ζ+ D(q, ζ)ζ+ g(q)+ J>(q)λ+δ(q, ζ, t)=τ (7)   e˙o3 where δ(q, ζ, t) encapsulates bounded unmodeled terms and ex- where L is defined as: ternal disturbances (sea waves and currents). Moreover, τ ∈ Rn denotes the control input at the joint/thruster level, M(q) is the 1h d d d i L = S(ne)S(n ) + S(oe)S(o ) + S(αe)S(α ) (10) positive definite inertial matrix, C(q, ζ) represents coriolis and 2

205 centrifugal terms, D(q, ζ) models dissipative effects, g(q) en- 225 which is full rank when the relative orientation between the capsulates the gravity and buoyancy effects and J>(q)λ rep- d o frames Re and R is confined less than 90 for an angle-axis resents the effect of the external forces/torques applied at the local parametrization and hence is not restrictive for practical end-effector owing to the contact. cases [60]. Finally, the problem to be solved in this work is formulated 210 as follows: 3.1. Control Design Problem 1. Given a UVMS, a desired force profile along the An appropriate methodology to meet the control objectives normal to the surface direction f d(t), a desired position trajec- of this work is the prescribed performance control technique d d > 2 [61, 62, 63], which is adapted here in order to achieve prede- tory [ye (t), ze (t)] ∈ R on the planar surface, as well as a smooth rotation matrix target Rd(t) = [nd(t), od(t), αd(t)], defined transient and steady state response bounds for the errors.

215 sign a feedback control law, without incorporating any infor- By prescribed performance control we mean that the force, po- mation regarding either the UVMS dynamics or the force defor- sition and orientation errors evolve strictly within a predefined mation model, such that the following are satisfied: region that is bounded by decaying functions of time, which is mathematically expressed as: 1. Predefined behavior in terms of overshoot, convergence rate and maximum steady state error on the −Miρi(t) < ei(t) < Miρi(t), i ∈ { f, y, z, o1, o2, o3}, ∀t ≥ 0 (11) 220 force/position/orientation tracking errors; 2. Contact maintenance; with ∞ ∞ 3. Bounded closed loop signals; ρi ρi ρi(t) = 1 − exp(−lit)+ , 4. Robustness against external disturbances. max {Mi,Mi} max {Mi,Mi} i ∈ { f, y, z, o1, o2, o3}. (12) 3. Control Methodology 3 Other representations, such as unit quaternions, can also be considered in Typically, in a force/position control problem the robot end- order to describe the orientation errors. For more details the reader is referred effector should track a force trajectory along the normal to the to [59]. 4 > 230 The constants Mi, Mi, are selected such that (11) is satisfied at ε(ξx),[ε f (ξ f ), εy(ξy), εz(ξz), εo1 (ξo1 ), εo2 (ξo2 ), εo3 (ξo3 )] , (17) ∞ t = 0 (i.e., −Mi < ei(0) < Mi). The constant ρ = limt→∞ ρi(t) i with ξ = [ξ , ξ , ξ , ξ , ξ , ξ ]>, P(t) diag [ρ (t)] represents the maximum allowable size of ei(t) at the steady x f y z o1 o2 o3 , i i∈{ f,y,z,o ,o ,o } state, which can be set arbitrarily small to a value reflecting 1 2 3 and K = diag [ki] is a positive diagonal gain matrix. the resolution of the measurement device, thus achieving prac- i∈{ f,y,z,o1,o2,o3} r 235 tical convergence of ei(t) to zero. Furthermore, the decreas- Subsequently, the task-space desired motion profile x˙ can be ing rate of ρi(t), which is affected by the constant li introduces expressed equivalently in the configuration space via: a lower bound of the required speed of convergence of ei(t), r # r # 0 n ζ (t) = J(q) x˙ + In×n − J(q) J q x˙ ∈ R (18) while the maximum overshoot is prescribed less than Miρi(0) or Miρi(0). Thus, the appropriate selection of the performance where J(q)# denotes the generalized pseudo-inverse [64] of the 0 240 functions ρi(t), i ∈ { f, y, z, o1, o2, o3} as well as of the design Jacobian J(q) and x˙ denotes secondary tasks (e.g., maintain-

constants Mi, Mi, i ∈ { f, y, z, o1, o2, o3} encapsulates perfor-270 ing manipulator’s joint limits, increasing manipulability) to be mance characteristics for the corresponding tracking errors ei, regulated independently since they do not contribute to the end- i ∈ { f, y, z, o1, o2, o3}. effector’s velocity [48] (i.e., they belong to the null space of the In particular, meeting the performance bounds for the force Jacobian J(q))4. 245 error, as described in (11), allows us further to guarantee a pri- Level II-a: Define the velocity error vector as: ori that contact with the surface is never lost (i.e., f (t) ≥ f ∗ > ∗ > − r ∈ n 0, ∀t ≥ 0 for a positive constant f ) and excessive interaction eζ (t) , [eζ1 (t),..., eζn (t)] = ζ(t) ζ (t) R (19) ∗ forces are avoided (i.e, f (t) ≤ f > 0, ∀t ≥ 0 for a posi- ∗ and select the corresponding performance functions: tive constant f > f ∗ > 0). In this spirit, M , M and ρ (t) f f f 0 ∞ ∞ d ∗ ρζi (t) = (ρζ − ρζ ) exp(−lζi t) + ρζ , i = 1,..., n (20) 250 are selected to further satisfy: inft≥0{−M f ρ f (t) + f (t)} > f , i i i ∗ d 0 ∞ sup {M f ρ f (t) + f (t)} < f . Hence, owing to (12) it can with ρ > |e (0)|, ρ > 0 and l > 0, i = 1,... n. Notice t≥0 ζi ζi ζi ζi be verified that the satisfaction of the performance bounds for that similarly to the force/position/orientation errors we intend the force error guarantees further that 0 < inft≥0{−M f ρ f (t) + to enforce transient and steady state response on the velocity d d f (t)} ≤ f (t) ≤ supt≥0{M f ρ f (t) + f (t)}, ∀t ≥ 0. Finally, in errors eζi (t), i = 1,..., n as well by satisfying: 255 view of (6), there exist constants ∂ f , ∂ f such that 0 < ∂ f ≤ −ρζi (t) < eζi (t) < ρζi (t), ∀t ≥ 0 i = 1,..., n (21) ∂ f (χ) ≤ ∂ f . In the sequel, we propose a state feedback control protocol Level II-b: Define the transformed velocity error vector:

that does not incorporate any information regarding the UVMS > > h 1 + ξζ1 1 + ξζn i dynamic model (7) or the deformation model and achieves εζ (ξζ ),[εζ1 (ξζ1 ), . . . , εζn (ξζn )] = ln ,..., ln 1 − ξζ1 1 − ξζn 260 force/position/orientation tracking of the corresponding smooth (22) and bounded desired trajectories with prescribed transient and steady state response. The overall control architecture is illus- where trated in Fig.2. > −1 Level I-a: Select the performance functions ρi(t) and the cor- ξζ (t) , [ξζ1 , . . . , ξζn ] = Pζ (t)eζ (t) (23) 265 responding positive parameters Mi, Mi such that −Mi < ei(0) < denotes the normalized velocity error vector, with Pζ (t) = Mi, ∀i ∈ { f, y, z, o1, o2, o3} and the desired transient and steady diag [ρζi (t)] and design the state feedback control law: state performance specifications are properly encapsulated. i=1,...,n Level I-b: Define the transformed errors εi as: −1 τ(eζ (t), t) = −Kζ Pζ (t)Rζ (ξζ )εζ (ξζ ) (24) 1 + ξi ! Mi εi(ξi) = ln , i ∈ { f, y, z, o1, o2, o3}, (13) where 1 − ξi Mi h 2 i R ξ ζ ( ζ ) = diag 2 (25) where i=1,...,n 1 − ξ ζi ei(t) ξi(t) = , i ∈ { f, y, z, o1, o2, o3}, (14) and Kζ > 0 is a diagonal gain matrix. ρi(t) denote the normalized force/position/orientation errors and se-275 Theorem 1. The proposed state feedback control law (13)-(25) lect the end-effector reference velocity: enforces tracking of: i) the desired normal force trajectory d d d > 2 f (t), ii) the desired position trajectory [ye (t), ze (t)] ∈ R r −1 −1 x˙ (ξx, t) = −KP (t)diag[I3×3, L ]R(ξx)ε(ξx) (15) on the planar surface as well as iii) the smooth rotation matrix target Rd(t) = [nd(t), od(t), αd(t)] with the desired transient and where 280 steady state performance. " 1 + 1 # Mi Mi R(ξx) = diag , (16) 4 ξi ξi For more details on task priority based control and redundancy resolution i∈{ f,y,z,o1,o2,o3} 1 − 1 + Mi Mi for UVMSs the reader is referred to [48] and [54]. 5 external disturbance Velocity

f d(t), yd(t), zd(t), Rd(t) Level I Level II r UVMS Environment ζ (e, t) τ(eζ , t)

Proposed Control Algorithm

Position/Orientation Force

Figure 2: The closed loop block diagram of the proposed control scheme.

Proof. First, let us deﬁne the overall normalized error vector a time interval [0, tmax) such that ξ(t) ∈ Ωξ, ∀t ∈ [0, tmax) is > > > ξ = [ξx , ξζ ] . Diﬀerentiating (14) and (23) with respect to ensured. time and substituting the system dynamics (7), (9), (15), (19) Phase B: In Phase A, we proved that ξ(t) ∈ Ωξ, ∀t ∈ [0, tmax), and (24), we obtain the closed loop system dynamics: thus it can be concluded that:

ei(t) ξi(t) = ∈ (−Mi, Mi), i ∈ { f, y, z, o1, o2, o3} (29a) ˙ ρ (t) ξx , hx(ξx, t) i eζ −1 h i −1 ξ (t) = i ∈ (−1, 1), i = 1,..., n (29b) = −P (t)diag ∂Φ(χ), I5×5 KP (t)R(ξx)ε(ξx) ζi ρζ " i −1 h i + P (t) diag ∂Φ(χ), I2×2, L JPζ (t)ξζ for all t ∈ [0, tmax), from which we obtain that ei(t) and e (t) are lower and upper bounded by −M ρ (t), M ρ (t) and # ζi i i i i h i> d d d d > −ρζi (t), ρζi (t), respectively. Therefore, the transformed error − f˙ , y˙ , z˙ , [Lω ] − P˙ (t)ξx (26) vectors εi(ξi) and εζi (ξζi ) designated in (13) and (22), respectively, are well-defined for all t ∈ [0, tmax). Hence, consider the positive definite and radially unbounded function: ˙ ξζ (ξζ , t) , hζ (ξζ , t) 1 1 1 1 = −P−1(t)M−1 K P−1(t)R (ξ )ε (ξ )− V(ε) = ε>(ξ )ε(ξ ) = ε2 + ||ε ||2 + ||ε ||2 (30) ζ ζ ζ ζ ζ ζ ζ 2 x x 2 f 2 p 2 o −1 h −1 r r − Pζ (t) M C · (Pζ (t)ξζ + ζ ) + D · (Pζ (t)ξζ + ζ ) > > with εp = [εp1 , εp2 ] and εo = [εo1 , εo2 , εo3 ] . Differentiating > ri + g + J λ + δ(t) + P˙ ζ (t)ξζ + ζ˙ (27) V(ε) with respect to time and substituting (26) results in:

> −1 h i −1 > which can be written in compact form as: V˙ = − ε (ξx)R(ξx)P (t)diag ∂Φ(χ), I5×5 KP (t)R(ξx)ε (ξx) > −1 ˙ > > > + ε (ξx)R(ξx)P (t)Bx (31) ξ , h(ξ, t) = [hx (ξx, t), hζ (ξζ , t)] (28) where 285 Let us define the open set Ωξ = Ωξx × Ωξζ with h i h i> Ωξ (−M f , M f ) × (−My, My) × (−Mz, Mz) × (−Mo , Mo ) × d d d d > x , 1 1 Bx =diag ∂Φ(χ), I2×2, L JPζ (t)ξζ − f˙ , y˙ , z˙ , [Lω ] −P˙ (t)ξx (−M , M )×(−M , M ) and Ω (−1, 1)n. In what follows, o2 o2 o3 o3 ξζ , (32) we proceed in two phases. First we ensure the existence of a unique maximal solution ξ(t) of (28) over the set Ωξ for a time It is well known that the Jacobian J is bounded by def- d d d d 290 interval [0, tmax) (i.e., ξ(t) ∈ Ωξ, ∀t ∈ [0, tmax)). Then, we prove inition. Moreover, since, f˙ (t), y˙ (t), z˙ (t), w (t), ρi(t) i ∈ that the proposed controller guarantees, for all t ∈ [0, tmax) the { f, y, z, o1, o2, o3} and ρζi (t), i = 1,..., n are bounded by con- boundedness of all closed loop signals as well as that ξ(t) re- struction and ξi, ξζi are also bounded within the compact sets mains strictly within the set Ωξ, which leads by contradiction to Ωξx and Ωξζ owing to (29), we conclude the existence of a posi- tmax = ∞ and consequently to the satisfaction of (11) and (21), tion constantε ¯ such that: 295 thus completing the proof. Phase A: The set Ωξ is nonempty and open. Moreover (11) |ε(ξx(t))| ≤ ε,¯ ∀t ∈ [0, tmax) (33) and (21) leads to −Mi < ξi(0) < Mi, i ∈ { f, y, z, o1, o2, o3} Furthermore, from (13), and invoking the inverse logarithmic and −1 < ξζ (0) < 1, i = 1,..., n. Thus, we guarantee that i function, we obtain: ξx(0) ∈ Ωξx and ξζ (0) ∈ Ωξζ . Additionally, h(ξ, t), as defined 300 in (28), is continuous on t and locally Lipschitz on ξ over Ωξ. −Mi <ξ ≤ξi(t)≤ξ¯i < Mi, ∀t ∈ [0, tmax), i ∈ { f, o1, o2, p1, p2, p3} Therefore, the hypotheses of the Theorem 54 in [65] (pp. 476) i hold and the existence of a maximal solution ξ(t) of (28) on (34) 6 where: 315 Remark 1. From the aforementioned proof, it is worth notic- exp(¯ε) − 1 exp(¯ε) − 1 ing that the proposed control scheme does not incorporate any − ¯ ξ = Mi M , ξi = Mi (35) information regarding the matrices M, C, D, g, the defor- i exp(¯ε) + i exp(¯ε) + Mi Mi Mi mation model Φ(χ) or the external disturbances δ(t), which all affect only the size of ε¯ and ε¯ζ but leave unaltered the 305 Owing to (34) and (18) it can concluded that the reference 320 achieved convergence properties as (39) dictates. In fact, the velocity vector ζr remains bounded for all t ∈ [0, t ) as well. max actual transient and steady state performance is determined Moreover, invoking ζ = ζr(t) + P (t)ξ from (23), we also con- ζ ζ by the selection of the performance functions ρ (t) and perfor- clude the boundedness of ζ for all t ∈ [0, t ). Finally, differ- i max mance parameters M , M , i ∈ { f, y, z, o , o , o }. More specif- entiating ζr(t) w.r.t time and employing (26), (29) and (34), we i i 1 2 3 r ically, for any initial force/position/orientation tracking error, 310 conclude the boundedness of ζ˙ (t), ∀t ∈ [0, tmax) too. 325 the performance functions ρ (t) and parameters M , M , i ∈ Now let us consider the positive definite and radially un- i i i 1 2 { f, y, z, o1, o2, o3} are selected such that: i) −Mi < ei(0) < Mi bounded function Vζ (εζ ) = 2 ||εζ || . Differentiating Vζ with respect to time, substituting (27) and employing the continuity of and ii) the desired transient and steady state performance spec- r ifications are incorporated. M, C, D, g, λ, δ, ξx, ξζ ,P˙ ζ , ζ˙ , ∀t ∈ [0, tmax), we obtain: ˙ −1 −1 Remark 2. It should be noted that the selection of the con- Vζ ≤ ||Pζ Rζ (ξζ )εζ || Bζ − λM Kζ ||Pζ Rζ (ξζ )εζ || 330 trol gains affects both the quality of evolution of the errors ∀t ∈ [0, tmax), where λM is the minimum eigenvalue of the pos- ei, i ∈ { f, y, z, o1, o2, o3} inside the corresponding performance −1 itive definite matrix M and Bζ is a positive constant indepen- envelopes as well as the control input characteristics (e.g., de- dent of tmax, that satisfies: creasing the gain values leads to increased oscillatory behavior −1 r r within the prescribed performance envelope described by (39), Bζ ≥||M C · (Pζ ξζ + ζ (t)) + D · (Pζ ξζ + ζ (t)) 335 which is improved when adopting higher values, enlarging, > r however, the control effort both in magnitude and rate). Addi- + g + J λ + δ(t) + P˙ ζ ξζ + ζ˙ || tional fine tuning might be needed in real-time scenarios to re- Thus, we conclude that: tain the required control input signals within the feasible range that can be implemented by the actuators. Similarly, the con- ||εζ (ξζ )|| ≤ ε¯ζ , ∀t ∈ [0, tmax) (36) 340 trol input constraints impose an upper bound on the required speed of convergence of ρ t , i ∈ { f, y, z, o , o , o }. Hence, the Furthermore, from (25) and invoking |εζi | ≤ ε¯ζ , we obtain: i( ) 1 2 3 selection of the control gains ki , i ∈ { f, y, z, o1, o2, o3} and Kζ − < ξ ≤ ξ t ≤ ξ¯ < , ∀t ∈ , t , i ,..., n 1 ζi ζi ( ) ζi 1 [0 max) = 1 (37) can have positive influence on the overall closed loop system where response. More specifically, notice that (33) and (36) provide 345 bounds on ε(t) and εζ (t). Therefore, invoking (15) and (24) we − exp(¯εζ ) − 1 exp(¯εζ ) − 1 ¯ can select the control gains ki, i ∈ { f, y, z, o1, o2, o3} and Kζ such ξζi = , ξζi = (38) − exp(¯εζ ) + 1 exp(¯εζ ) + 1 that ζr and τ are retained within certain bounds. Nevertheless, which also leads to the boundedness of the control law (24) for (33) and (36) involve via the terms Bx and Bζ the parameters of the model, the external disturbances and the desired perfor- all t ∈ [0, tmax). 350 mance specifications. Thus, an upper bound of the dynamic pa- Subsequently, we will show that tmax can be extended to in- 0 rameters of the system as well as of the exogenous disturbances finity. Obviously, notice by (34) and (37) that ξ(t) ∈ Ωξ , 0 0 should be given in order to extract any relationships between Ω × Ω , ∀t ∈ [0, tmax), where: ξx ξζ the achieved performance and the input constraints. Finally, in 0 Ω =[ξ , ξ¯ ] × [ξ , ξ¯ ], ×[ξ , ξ¯ ], ×[ξ , ξ¯ ], ×[ξ , ξ¯ ], ×[ξ , ξ¯ ] the same vein, the selection of the velocity performance func- ξx f f y y z z o1 o1 o2 o2 o3 o3 355 0 tions ρζi (t), i = 1,..., n affects similarly both the evolution of Ω =[ξ , ξ¯ ] × ... × [ξ , ξ¯ ], ξζ ζ1 ζ1 ζn ζn the force/position/orientation errors within the corresponding performance envelopes as well as the control input character- are nonempty and compact subsets of Ωξx and Ωξζ , respectively. 0 istics. Hence, assuming that tmax < ∞ and since Ωξ ⊂ Ωξ, Proposition C.3.6 in [65] (pp. 481) dictates the existence of a time instant 0 0 0 4. Results t ∈ [0, tmax) such that ξ(t ) < Ωξ, which is a clear contradiction. Therefore, tmax = ∞. Thus, all closed loop signals remain 0 360 In this section, the theoretical findings are verified via both bounded and moreover ξ(t) ∈ Ω , ∀t ≥ 0. Finally, from (14) ξ simulation and experimental studies. The simulation results and (34) we conclude that: were conducted using a dynamic simulation environment built exp(¯ε)−1 exp(¯ε)−1 r − − ≤ ≤ in MATLAB . The experimental results were conducted in a Miρi(t)< Mi M ρi(t) ei(t) Mi ρi(t)< Miρi(t) exp(¯ε)+ i exp(¯ε)+ Mi test tank employing a small UVMS. Mi Mi (39) 365 4.1. Simulation study

for i ∈ { f, y, z, o1, o2, o3} and for all t ≥ 0, which completes the The UVMS model considered in the simulation is an AUV proof. equipped with a small 4 DoFs manipulator attached at the bow 7 of the vehicle (see Fig.3). The design parameters of the AUV and the robotic manipulator are given in Tables 1-3. The dy-

370 namic equations of UVMS were derived based on the Newton- Euler approach [29] and their simulation was carried out using a dynamic simulation environment built in MATLABr with sampling time 0.1 sec, which is common in a real time operation with an underwater robotic system. We considered a

375 scenario involving 3D motion, where the end-effector of the UVMS is in compliant contact with a planar surface with stiff- 2 N ness model f = kχ , k = 300 m2 which is unknown to the controller. The initial configuration is depicted in Fig.3. We as-

Table 1: Vehicle parameters Parameter Value Unit Degree of freedoms 6 Length 0.64 m Figure 3: Workspace including the UVMS in compliant contact with a planar surface. Height 0.24 m Width 0.25 m Table 4: Performance parameters- First level Mass in air 12 kg Parameter Value

M p , M p 0.2 Table 2: Denavit-Hartenberg parameters of the robotic arm 1,2 1,2 Mo1,2,3 , Mo1,2,3 0.3 Link di θi ai αi ∞ π ρ f 0.4 1 L1 q1 0 − 2 ∞ π ρ 0.03 − p1,2 2 0 q2 2 L2 0 ∞ π π ρ o1,2,3 0.07 3 0 q3 + 2 −L3 2 li, i{ f, p1,2, o1,2,3} 2.0 4 L4 q4 0 0

Table 3: Parameters of the robotic arm 395 were considered unknown for the controller. Furthermore, the Parameter Value Unit secondary task velocities (18) were designed appropriately to Link 1 Length(L1) 0.077 m avoid the violation of the manipulator joint limits. However, Link 2 Length(L2) 0.147 m more complicated secondary tasks (e.g., manipulability) could Link 3 Length(L3) 0.028 m also be incorporated following the task priority based control Link 4 Length(L4) 0.075 m 400 techniques [48, 66]. In addition, in order to test the robustness Link 1 Mass 0.1 kg of the proposed scheme, in the subsequent simulation study Link 2 Mass 0.2 kg the dynamics of the UVMS were affected by external distur- Link 3 Mass 0.1 kg bances in the form of slowly time varying sea currents acting Link 4 Mass 0.12 kg along x, y and z axes modeled by the corresponding velocities {I} π m Link Diameter 6 cm 405 vi = 0.1 sin( 25 t) s , i ∈ {x, y, z}. Finally, bounded measurement noise of normal distribution with 5% standard deviation was considered during the simulation study. sumed that the UVMS is in contact with the compliant envi- The results are depicted in Fig.4-Fig.7. The evolution of the 380 f . N ronment exerting a force normal to the surface (0) = 0 5 . position trajectory on the surface is presented in Fig.4. It can The desired force profile along the normal to the surface di- 410 be easily observed that the actual position of the end-effector f d t N rection is set as ( ) = 4 . Moreover, the UVMS should (indicated by red color) converges to the desired one (indicated track a desired position trajectory on the surface and attain a d by green color) and follows the desired trajectory profile. Fig.5 perpendicular orientation (i.e., R = I3×3) with respect to the ∗ ∗ ∗ presents the evolution of the actual force exerted by the UVMS 385 surface. The constants f and f are chosen as f = 0.1N with respect to the desired force profile. Moreover, it can be ∗ and f = 5N in order to ensure a priori that the contact with415 seen that the force exerted by the UVMS remained inside the the surface is never lost and that excessive interaction forces desired region, securing that the contact is never lost and the are avoided. In this spirit, as it was described earlier, we set exerted force never overshoots the predefined value. The evo- M f = 3.9, M f = 1. All other performance parameters at the lution of the errors at the first and second level of the proposed 390 kinematic level are given in Table-4. Moreover, the control controller are illustrated in Fig.6 and Fig.7, respectively. It can gains were selected as ki = 0.2, j ∈ { f, p1, p2, o1, o2, o3}. The420 be concluded that even with the influence of external distur- design parameters regarding to the second level were chosen bances as well as measurements noise, the errors in all direc- 0 as: ρ = 1.0, K = 2I × . Notice that the dynamic parame- tions converge close to zero and remain bounded below and ζi ζ n n ters of the UVMS as well as the stiffness of the planar surface above by the corresponding performance functions. 8 1 0.4

0 0.2 )

) -1 t

0.2 t

( (

1 0

o e -2 e -0.2 -3 0.1 -4 -0.4

) 0 10 20 30 40 0 10 20 30 40

m (

s 0

i 0.3 0.4 x a 0.2 Z 0.2

0.1 )

-0.1 )

( (

0 2 0

e e -0.1 -0.2 -0.2 -0.2

-0.3 -0.4 0 10 20 30 40 0 10 20 30 40

-0.3 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4 Y axis (m) 0.2 0.2

Figure 4: The evolution of the position trajectory on the surface. The desired 0.1

)

t ( trajectory and the actual position of the end-eﬀector on the surface are indicated (

0 3 0

e e by green and red color respectively. The end-eﬀector position converges and -0.1 -0.2 follows the desired trajectory proﬁle. -0.2

6 -0.3 -0.4 0 10 20 30 40 0 10 20 30 40 time(s) time(s) 5 Figure 6: The evolution of the errors at the ﬁrst level of the proposed control scheme. The errors and performance bounds are indicated by blue and red color

4 respectively.

)

N (

e 3

r o F f(t) 2 f d d M f ;f + f d !Mf ;f + f 1 $ f f$

0 0 5 10 15 20 25 30 35 40 45 time(s) Figure 5: The evolution of the force trajectory. The desired constant force and the actual force exerted by the UVMS are indicated by green and red color respectively.

4.2. Experimental Results

425 This section demonstrates the eﬃcacy of the proposed position/force control scheme via an experimental procedure with a small UVMS.

4.2.1. Experimental Setup The experiments were carried out inside the NTUA, Con-

430 trol Systems Lab test tank, with dimensions 5m × 3m × 1.5m (Fig. 8). In the bottom of the tank, sparse visual features have Figure 7: The evolution of the errors at the second level of the proposed control been added in order to improve optical-ﬂow velocity estima- scheme. The errors and performance bounds are indicated by blue and red color tions. The vehicle used in this work is a 4 DoFs Seabotix respectively. LBV [67], actuated in Surge, Sway, Heave and Yaw via a 4

435 thruster set conﬁguration. The vehicle is equipped with a small 3D acceleration, angular velocity and orientation measurements

custom-made 4 DoFs underwater manipulator, with revolute445 at 100Hz. Finally, a marker localization system based on the joints and a compliant (spring-based) end-eﬀector. The system ArUco library [68] is employed in order to determine the pose is equipped with two cameras: i) a down-looking Sony PlaySta- of the vehicle with respect to the panel (see Fig. 9). A state es- tion Eye camera, with 640 × 480 pixels at 30 frames per second timation algorithm based on the Complementary Filters notion,

440 (fps) enclosed in a waterproof housing and ii) a forward looking as in [69], delivers the pose, velocity and acceleration estimates

camera (Seabotix LBV default camera) with 640×480 pixels at450 of the underwater vehicle by fusing data from the available on- 25 frames per second (fps). The vehicle is also equipped with board sensors while the position and velocity of the arm joints an S BG IG−500A AHRS, delivering temperature-compensated are available via the on-board encoders. The force measure- 9 0.5

-0.5

f e

-1

-1.5 Figure 8: The NTUA, Control Systems Lab test tank and Position–Force Control Scenario

-2

0 10 20 30 40 50 60 70 time

Figure 10: The evolution of the force error during the experiment. Perpendicu- lar force f along x panel local axis.

0.2

0.15

0.1

0.05

y 0 e

-0.05

-0.1

-0.15

-0.2 0 10 20 30 40 50 60 70 time

Figure 9: a) Load cells mounted on the panel, b)Visual features for state esti- 0.2

mation 0.15

0.1 ments along the desired direction (perpendicular to the panel) 0.05

z 0 are acquired by 4 load–cells, properly mounted on the base cor- e -0.05 455 ners of the panel, as shown in Fig. 9. At each time instant, the -0.1 sum of the 4 load–cells is incorporated as force feedback to the -0.15 -0.2 system. The load–cells are connected directly to the control PC, 0 10 20 30 40 50 60 70 via a PhidgetBridge data I/O device. Finally, the software im- time plementation of the state estimation algorithm as well as of the Figure 11: The evolution of the end-eﬀector position errors along y and z direc- 460 proposed position/force control scheme was conducted in C++ tion during the experiment. and Python under the Robot Operating System (ROS) [70]. 0.4

4.2.2. Experimental results 0.2 1

o 0 This subsection demonstrates experimental results obtained e by the proposed position/force control scheme. In this scenario, -0.2 -0.4 465 the UVMS end-eﬀector should follow a line trajectory along the 0 10 20 30 40 50 60 70 panel, while maintaining a normal constant force to the panel. time We consider that the end-eﬀector has already achieved contact 0.4

with the panel. The desired force is set to f d = 5N, while the 0.2 2

o 0 end-eﬀector must perform a periodical motion between −0.05m e -0.2 470 and 0.05m along the panel y local axis, while the position along -0.4 z local axis should remain constant. The state responses are 0 10 20 30 40 50 60 70 shown in Fig. 10-12, while the trajectory of the end-eﬀector on time the plane is depicted in Fig. 13. As it can be easily observed, the 0.4

proposed position/force control scheme fulﬁlled the interaction 0.2 3

o 0 475 task successfully. e -0.2

Video -0.4 0 10 20 30 40 50 60 70 time A video demonstrating the aforementioned experimental re- sult of the proposed methodology can be found in a HD video Figure 12: The evolution of the end-eﬀector orientation errors direction during at the following url: https://youtu.be/PCHLsUb-vM0 the experiment. 10 Trace on the Panel 0.1 [5] H. Farivarnejad, S. Moosavian, Multiple impedance control for object ma- 515 0.08 nipulation by a dual arm underwater vehicle-manipulator system, Ocean Engineering 89 (2014) 82–98. 0.06 [6] H. Shim, B.-H. Jun, P.-M. Lee, H. Baek, J. Lee, Workspace control system of underwater tele-operated manipulators on an rov, Ocean Engineering 0.04 37 (11-12) (2010) 1036–1047.

0.02 520 [7] P. Londhe, S. Mohan, B. Patre, L. Waghmare, Robust task-space con-

) trol of an autonomous underwater vehicle-manipulator system by pid-like

m 0 ( fuzzy control scheme with disturbance estimator, Ocean Engineering 139 Z (2017) 1–13. -0.02 [8] S. Mohan, J. Kim, Coordinated motion control in task space of an au-

-0.04 525 tonomous underwater vehicle-manipulator system, Ocean Engineering 104 (2015) 155–167. -0.06 [9] G. Marani, S. Choi, J. Yuh, Underwater autonomous manipulation for intervention missions auvs, Ocean Engineering 36 (1) (2009) 15–23. -0.08 [10] E. Simetti, G. Casalino, Whole body control of a dual arm underwater

-0.1 530 vehicle manipulator system, Annual Reviews in Control 40 (2015) 191– -0.1 -0.08 -0.06 -0.04 -0.02 0 0.02 0.04 0.06 0.08 0.1 Y (m) 200. [11] D. Lane, D. O’Brien, M. Pickett, J. Davies, G. Robinson, D. Jones, Figure 13: The trajectory of the end-effector w.r.t the horizontal plane. Perpen- E. Scott, G. Casalino, G. Bartolini, G. Cannata, A. Ferrara, D. Angel- dicular force is kept constant at all times. leti, M. Coccoli, G. Veruggio, R. Bono, P. Virgili, M. Canals, R. Pallas, 535 E. Gracia, C. Smith, Amadeus: Advanced manipulation for deep underwater sampling, IEEE Robotics and Automation Magazine 4 (4) (1997) 34–45. 480 5. Conclusions [12] V. Rigaud, . Coste-Manire, M. Aldon, P. Probert, M. Perrier, P. Rives, D. Simon, D. Lane, J. Kiener, A. Casals, J. Amat, P. Dauchez, This work presents a robust force/position control scheme for540 M. Chantler, Union: Underwater intelligent operation and navigation, a UVMS in compliant contact with the environment, with great IEEE Robotics and Automation Magazine 5 (1) (1998) 25–34. [13] P. Sanz, P. Ridao, G. Oliver, G. Casalino, C. Insaurralde, C. Silvestre, applications in underwater robotics (e.g. sampling of the sea or- C. Melchiorri, A. Turetta, Trident: Recent improvements about au- ganisms, underwater welding, object handling). The proposed tonomous underwater intervention missions, IFAC Proceedings Volumes 545 (IFAC-PapersOnline) 3 (PART 1) (2012) 355–360. 485 control scheme does not required any a priori knowledge of the [14] M. Prats, J. Garcia, S. Wirth, D. Ribas, P. Sanz, P. Ridao, N. Gracias, UVMS dynamical parameters or the stiffness model. It guaran- G. Oliver, Multipurpose autonomous underwater intervention: A systems tees a predefined behavior in terms of desired overshoot, tran- integration perspective, 2012 20th Mediterranean Conference on Con- sient and steady state response and it is robust with respect to trol and Automation, MED 2012 - Conference Proceedings (2012) 1379– external disturbances and measurement noises. Moreover, the550 1384. [15] J. Fernndez, M. Prats, P. Sanz, J. Garca, R. Marn, M. Robinson, D. Ribas, 490 proposed controller exhibits the following important character- P. Ridao, Grasping for the seabed: Developing a new underwater robot istics: i) it is of low complexity and thus can be easily used in arm for shallow-water intervention, IEEE Robotics and Automation Mag- most UVMSs ii) the performance of the proposed scheme (e.g., azine 20 (4) (2013) 121–130. desired overshoot, steady state response) is a priori and explic-555 [16] E. Simetti, G. Casalino, S. Torelli, A. Sperind, A. Turetta, Floating underwater manipulation: Developed control methodology and experimen- itly imposed by certain designer-specified performance func- tal validation within the trident project, Journal of Field Robotics 31 (3) 495 tions, and is fully decoupled by the control gains selection, thus (2014) 364–385. simplifying the control design. [17] M. Prats, D. Ribas, N. Palomeras, J. Garca, V. Nannen, S. Wirth, J. Fern- 560 ndez, J. Beltrn, R. Campos, P. Ridao, P. Sanz, G. Oliver, M. Carreras, N. Gracias, R. Marn, A. Ortiz, Reconfigurable auv for intervention missions: A case study on underwater object recovery, Intelligent Service Acknowledgments Robotics 5 (2012) 19–31. [18] D. Ribas, P. Ridao, A. Turetta, C. Melchiorri, G. Palli, J. Fernan- This work was supported by the ROBOCADEMY, Marie565 dez, P. Sanz, I-auv mechatronics integration for the trident fp7 project, Curie ITN Grant Agreement no FP7-608096 funded by the EU IEEE/ASME Transactions on Mechatronics 20 (5) (2015) 2583–2592. th [19] D. Lane, F. Maurelli, P. Kormushev, M. Carreras, M. Fox, K. Kyriakopou- 500 action 7 Framework Programme - The 2013 People Work Pro- los, Persistent autonomy: The challenges of the pandora project, IFAC gramme - EC Call Identifier FP7-PEOPLE- 2013-ITN, Imple- Proceedings Volumes (IFAC-PapersOnline) 9 (PART 1) (2012) 268–273. mentation Mode: Multi-ITN. 570 [20] A. Carrera, N. Palomeras, D. Ribas, P. Kormushev, M. Carreras, An intervention-auv learns how to perform an underwater valve turning, OCEANS 2014. [21] A. Carrera, N. Palomeras, N. Hurtos, P. Kormushev, M. Carreras, Learn- References ing multiple strategies to perform a valve turning with underwater cur- 575 rents using an i-auv, MTS/IEEE OCEANS 2015 - Genova: Discovering [1] T. Fossen, Guidance and control of ocean vehicles, Wiley, New York. Sustainable Ocean Energy for a New World. 505 [2] P. Ridao, M. Carreras, D. Ribas, P. Sanz, G. Oliver, Intervention auvs: [22] J. Gancet, D. Urbina, P. Letier, M. Ilzokvitz, P. Weiss, F. Gauch, G. An- The next challenge, Annual Reviews in Control 40 (2015) 227–241. tonelli, G. Indiveri, G. Casalino, A. Birk, M. Pfingsthorn, S. Calinon, [3] F. Bruno, M. Muzzupappa, L. Barbieri, A. Gallo, G. Ritacco, A. Lagudi, A. Tanwani, A. Turetta, C. Walen, L. Guilpain, Dexrov: Dexterous under- M. F. La Russa, S. A. Ruffolo, G. M. Crisci, M. Ricca, et al., The comas580 sea inspection and maintenance in presence of communication latencies, project: New materials and tools for improving the in situ documenta- IFAC-PapersOnLine 28 (2) (2015) 218–223. 510 tion, restoration, and conservation of underwater archaeological remains, [23] D. Rizzo, F. Bruno, L. Barbieri, M. Muzzupappa, Kinematic perfor- Marine Technology Society Journal 50 (4) (2016) 108–118. mances evaluation of a hydraulic underwater manipulator, in: OCEANS [4] G. Antonelli, “Underwater Robots”, Springer Tracts in Advanced 2017-Aberdeen, IEEE, 2017, pp. 1–6. Robotics, Springer International Publishing, 2013. 11 585 [24] F. Zhang, G. Marani, R. Smith, H. Choi, Future trends in marine robotics, dant manipulators: Generalizing the task-priority framework to inequality IEEE Robotics and Automation Magazine 22 (1) (2015) 14–21 and 122. task, IEEE Transactions on Robotics 27 (4) (2011) 785–792. [25] G. Marani, S. Choi, J. Yuh, Real-time center of buoyancy identification [48] E. Simetti, G. Casalino, A novel practical technique to integrate inequality for optimal hovering in autonomous underwater intervention, Intelligent control objectives and task transitions in priority based control, Journal Service Robotics 3 (3) (2010) 175–182. 660 of Intelligent and Robotic Systems: Theory and Applications 84 (1-4) 590 [26] M. Dunnigan, G. Russell, Evaluation and reduction of the dynamic cou- (2016) 877–902. pling between a manipulator and an underwater vehicle, IEEE Journal of [49] A. Escande, N. Mansard, P.-B. Wieber, Hierarchical quadratic program- Oceanic Engineering 23 (3) (1998) 260–273. ming: Fast online humanoid-robot motion generation, The International [27] B. Siciliano, L. Sciavicco, L. Villani, Robotics: modelling, planning and Journal of Robotics Research 33 (7) (2014) 1006–1028. control., Springer Verlag. 665 [50] S. Chiaverini, Singularity-robust task-priority redundancy resolution for 595 [28] J. Yuh, Design and control of autonomous underwater robots: A survey, real-time kinematic control of robot manipulators, IEEE Transactions on Autonomous Robots 8 (1) (2000) 7–24. Robotics and Automation 13 (3) (1997) 398–410. [29] I. Schjølberg, T. I. Fossen, Modelling and control of underwater vehicle- [51] N. Sarkar, T. Podder, Coordinated motion planning and control of au- manipulator systems, in: in Proc. rd Conf. on Marine Craft maneuvering tonomous underwater vehicle-manipulator systems subject to drag opti- and control, Citeseer, 1994. 670 mization, IEEE Journal of Oceanic Engineering 26 (2) (2001) 228–239. 600 [30] T. J. Tarn, G. Shoults, S. Yang, A dynamic model of an underwater vehi- [52] Z. Ismail, M. Dunnigan, Tracking control scheme for an underwater cle with a robotic manipulator using kane’s method, Autonomous Robots vehicle-manipulator system with single and multiple sub-regions and sub- 3 (2) (1996) 269–283. task objectives, IET Control Theory and Applications 5 (5) (2011) 721– [31] S. McMillan, D. E. Orin, R. B. McGhee, Efficient dynamic simulation of 735. an underwater vehicle with a robotic manipulator, IEEE Transactions on675 [53] J. Han, J. Park, W. Chung, Robust coordinated motion control of an under- 605 Systems, Man, and Cybernetics 25 (8) (1995) 1194–1206. water vehicle-manipulator system with minimizing restoring moments, [32] M. Dunnigan, D. Lane, A. Clegg, I. Edwards, Hybrid position/force con- Ocean Engineering 38 (10) (2011) 1197–1206. trol of a hydraulic underwater manipulator, Control Theory and Applica- [54] S. Soylu, B. Buckham, R. Podhorodeski, Redundancy resolution for un- tions, IEE Proceedings - 143 (2) (1996) 145–151. derwater mobile manipulators, Ocean Engineering 37 (2-3) (2010) 325– [33] A. Clegg, M. Dunnigan, D. Lane, Self-tuning position and force control680 343. 610 of an underwater hydraulic manipulator, Robotics and Automation, 2001. [55] G. Antonelli, S. Chiaverini, N. Sarkar, External force control for underwa- Proceedings 2001 ICRA. IEEE International Conference on 4 (2001) ter vehicle-manipulator systems, Robotics and Automation, IEEE Trans- 3226–3231 vol.4. actions on 17 (6) (2001) 931–938. [34] J.-H. Ryu, D.-S. Kwon, P.-M. Lee, Control of underwater manipulators [56] S. Heshmati-alamdari, A. Nikou, K. Kyriakopoulos, D. Dimarogonas, A mounted on an rov using base force information, Proceedings - IEEE In-685 robust force control approach for underwater vehicle manipulator sys- 615 ternational Conference on Robotics and Automation 4 (2001) 3238–3243. tems, IFAC-PapersOnLine 50 (1) (2017) 11197–11202. [35] H. Kajita, K. Kosuge, Force control of robot floating on the water utilizing [57] K. L. Johnson, K. L. Johnson, Contact mechanics, Cambridge university vehicle restoring force, Intelligent Robots and Systems, 1997. IROS ’97., press, 1987. Proceedings of the 1997 IEEE/RSJ International Conference on 1 (1997) [58] S. Arimoto, P. T. A. Nguyen, H.-Y. Han, Z. Doulgeri, Dynamics and con- 162–167 vol.1. 690 trol of a set of dual fingers with soft tips, Robotica 18 (1) (2000) 71–80. 620 [36] L. Lapierre, P. Fraisse, P. Dauchez, Position/force control of an under- [59] F. Caccavale, C. Natale, B. Siciliano, L. Villani, Resolved-acceleration water mobile manipulator, Journal of Robotic Systems 20 (12) (2003) control of robot manipulators: A critical review with experiments, Robot- 707–722. ica 16 (5) (1998) 565–573. [37] Y. Cui, T. Podder, N. Sarkar, Impedance Control of Underwater Vehicle- [60] L. Sciavicco, B. Siciliano, Modelling and control of robot manipulators, Manipulator Systems (uvms), IEEE/RSJ International Conference on In-695 Springer Science & Business Media, 2012. 625 telligent Robots and Systems (1999) 148–153 vol.1. [61] C. Bechlioulis, G. Rovithakis, Prescribed performance adaptive control [38] Y. Cui, N. Sarkar, A unified force control approach to autonomous under- for multi-input multi-output affine in the control nonlinear systems, IEEE water manipulation, Robotics and Automation, 2000. Proceedings. ICRA Transactions on Automatic Control 55 (5) (2010) 1220–1226. ’00. IEEE International Conference on 2 (2000) 1263–1268 vol.2. [62] C. P. Bechlioulis, G. A. Rovithakis, Robust partial-state feedback pre- [39] G. Antonelli, S. Chiaverini, Task-priority redundancy resolution for un-700 scribed performance control of cascade systems with unknown nonlinear- 630 derwater vehicle-manipulator systems, Proceedings - IEEE International ities, IEEE Transactions on Automatic Control 56. Conference on Robotics and Automation 1 (1998) 768–773. [63] C. Bechlioulis, G. Rovithakis, A low-complexity global approximation- [40] D. R. Yoerger, J. G. Cooke, J.-J. E. Slotine, Influence of thruster dynamics free control scheme with prescribed performance for unknown pure feed- on underwater vehicle behavior and their incorporation into control sys- back systems, Automatica 50 (4) (2014) 1217–1226. tem design, IEEE Journal of Oceanic Engineering 15 (3) (1990) 167–178.705 [64] B. Siciliano, J. J. E. Slotine, A general framework for managing multi- 635 [41] L. L. Whitcomb, D. R. Yoerger, Preliminary experiments in model- ple tasks in highly redundant robotic systems, Advanced Robotics, 1991. based thruster control for underwater vehicle positioning, IEEE Journal ’Robots in Unstructured Environments’, 91 ICAR., Fifth International of Oceanic Engineering 24 (4) (1999) 495–506. Conference on (1991) 1211–1216 vol.2. [42] R. Bachmayer, L. L. Whitcomb, M. A. Grosenbaugh, An accurate four- [65] E. D. Sontag, Mathematical Control Theory, Springer, London, U.K., quadrant nonlinear dynamical model for marine thrusters: Theory and ex-710 1998. 640 perimental validation, IEEE Journal of Oceanic Engineering 25 (1) (2000) [66] E. Simetti, G. Casalino, Manipulation and transportation with coopera- 146–159. tive underwater vehicle manipulator systems, IEEE Journal of Oceanic [43] A. Liegeois, Automatic supervisory control of the configuration and be- Engineering. havior of multibody mechanisms, IEEE transactions on systems, man, [67] Seabotix lbv150. and cybernetics 7 (12) (1977) 868–871. 715 URL http://www.teledynemarine.com/lbv150-4/?BrandID=19 645 [44] O. Khatib, A unified approach for motion and force control of robot ma- [68] S. Garrido-Jurado, R. M. noz Salinas, F. Madrid-Cuevas, M. Mar´ın- nipulators: The operational space formulation, IEEE Journal on Robotics Jimenez,´ Automatic generation and detection of highly reliable fiducial and Automation 3 (1) (1987) 43–53. markers under occlusion, Pattern Recognition 47 (6) (2014) 2280 – 2292. [45] P. Baerlocher, R. Boulic, An inverse kinematics architecture enforcing [69] P. Marantos, Y. Koveos, K. Kyriakopoulos, Uav state estimation using an arbitrary number of strict priority levels, The visual computer 20 (6)720 adaptive complementary filters, IEEE Transactions on Control Systems 650 (2004) 402–417. Technology 24 (4) (2016) 1214–1226. [46] E. Olguin-Diaz, G. Arechavaleta, G. Jarquin, V. Parra-Vega, A [70] M. Quigley, B. Gerkey, K. Conley, J. Faust, T. Foote, J. Leibs, E. Berger, passivity-based model-free force–motion control of underwater vehicle- R. Wheeler, A. Ng, Ros: an open-source robot operating system, in: Proc. manipulator systems, IEEE Transactions on Robotics 29 (6) (2013) 1469– of the IEEE Intl. Conf. on Robotics and Automation (ICRA) Workshop 1484. 725 on Open Source Robotics, Kobe, Japan, 2009. 655 [47] O. Kanoun, F. Lamiraux, P.-B. Wieber, Kinematic control of redun-