Optimal docking pose and tactile hook-localisation strategy for AUV intervention: The DIFIS deployment case

Ocean Engineering 46 (2012) 33–45

Contents lists available at SciVerse ScienceDirect

Ocean Engineering journal homepage: www.elsevier.com/locate/oceaneng

Optimal docking pose and tactile hook-localisation strategy for AUV intervention: The DIFIS deployment case Panagiotis Sotiropoulos a,n,1, Niccolo Tosi b, Fivos Andritsos a, Franck Geffard b a b

European Commission, Joint Research Centre, IPSC, Ispra,VA 21027, Italy CEA, LIST, Interactive Robotics Laboratory, Fontenay-aux-Roses, F-92265, France

a r t i c l e i n f o

abstract

Article history: Received 2 December 2010 Accepted 20 February 2012 Editor-in-Chief: A.I. Incecik Available online 22 March 2012

The DIFIS project has proposed a new solution for the immediate intervention directly on tanker wrecks so as to contain any leakages and prevent eventual pollution. The method could be extended also to oil well-blow-out cases such as the recent accident in the Gulf of Mexico. The DIFIS deployment typically requires the use of ROVs and dedicated dynamic-positioning ships that increase the cost significantly and make the operations weather-dependent. Eventual AUV use would result in much more efficient and flexible deployment procedures. The scenario studied here consists of a hook-grasping task that is part of the DIFIS mooring procedure. The overall objective is to automate certain processes enabling the use of AUVs or, at least, enhancing the currently foreseen ROV operations. A two-step method is presented consisting of a genetic algorithm for the determination of the optimum docking pose for the vehicle, and a particle filter algorithm that runs on a later stage for the tactile localisation of the hook. The method proposed is rather generic and can be extended to several steps of the DIFIS Deployment procedure, or even to other AUV intervention missions in a semi-structured environment. Results from the two algorithms are also presented and discussed. & 2012 Elsevier Ltd. All rights reserved.

Keywords: Autonomous underwater vehicles Docking Tactile localisation Manipulation Underwater operations

1. Introduction The increasing oil and gas demand is forcing oil companies to explore new drilling areas leading offshore platforms to sites of constantly bigger depths, several kilometres below the sea surface, with all the related risks. Blowouts such as the one in the Gulf of Mexico in May 2010 cannot be excluded. Regulations and new methods of prompt containment interventions at the seabed, right at the source of the pollution, will be required for sustainable offshore hydrocarbon exploitation. Triggered from another recent catastrophe, that of the PRESTIGE, the DIFIS (Double Inverted Funnel for Intervention System) project proposed a new solution to deal with tanker wrecks and preventing environmental disasters (DIFIS; Andritsos et al., 2007, 2008; Cozijn et al., 2008; Konstantinopoulos and Andritsos, 2008). The basic concept relies on gravity forces to channel the flux of spilt fuel towards the surface. Leaking fuel is collected by a moored fabric dome covering the wreck and channelled through a large riser tube to an open inverted reservoir, the buffer bell, 20–30 m below the sea surface. The buffer bell serves for buffer storage, as a separator and, through its buoyancy, keeps the whole system in tension. Fig. 1

n

Corresponding author. Tel.: þ390332785358. E-mail addresses: [email protected], [email protected] (P. Sotiropoulos). 1 Present Address: University of Patras, Mechanical and Aeronautics Engineers Department, Rio 26504, Greece. Tel.: þ302610996142. 0029-8018/$ - see front matter & 2012 Elsevier Ltd. All rights reserved. doi:10.1016/j.oceaneng.2012.02.005

shows an initial conceptual model of the dome and the buffer bell. DIFIS, with some re-engineering to take account of the methane gas, can be applied to contain deep sea oil well blow-out accidents. DIFIS system has many advantages: it is simple, entirely passive (apart from the periodical off-loading of the collected oil from the buffer bell), once installed does not require operations with Remotely Operated Vehicle’s (ROVs) and it is rough weather tolerant. However, its deployment requires substantial preparation and intense underwater remote manipulation activities supported by specialised equipment and dynamic positioning (DP) vessels. This represents a significant part of its overall cost. Optimising the deployment procedures and, in particular, using Intervention-Autonomous Underwater Vehicles (I-AUVs) instead of ROV-based activities would decrease the overall DIFIS intervention cost and add substantially to its flexibility. I-AUVs possess certain advantages such as the significant reduction of the size of the support vessel, while no DP vessel is needed, and the fact that it is not required to remain on site for the entire mission. Thus, the cost of the mission is reduced accordingly. Moreover, since I-AUVs can operate untethered, their deployment can be immediate regardless of the seastate on site, while such a free-to-move vehicle could prove more successful in a complex environment, avoiding umbilical management. In the framework of DIFIS, several ROV underwater operations have been envisioned for the Dome Deployment Stage, namely:

 Installation of transducers for a Long Base Line (LBL) acoustic positioning system,

34

P. Sotiropoulos et al. / Ocean Engineering 46 (2012) 33–45

Fig. 1. DIFIS Dome (left) and Buffer Bell (right) deployment.

 Intervention on the existing leaks in order to limit the oil flow,  Connection of the mooring lines on the dome. Hereafter, the connection operation of the mooring lines on the dome, listed above, is described more in detail. During the deployment stage the dome initially remains folded as it is lowered into the water. A set of mooring concrete cubes are released by the support vessel. Three of these cubes would serve as anchoring points for the dome and the rest would aid to unfold and retain under tension the dome itself. A hook that would be connected to each cube through a rope has to be used to anchor the dome. According to the current planning, an ROV, as illustrated in Fig. 2, is used to execute this task. During the process, the pilot has to identify the cube, dock on it using suction cups, localise the hook, grasp it and attach it to the dome. A method that would turn the overall process to a partially or fully autonomous process would benefit from the use of an I-AUV for these tasks or even favour the current ROV operations.

2. Navigation, localisation and docking techniques The envisioned scenarios for the I-AUV operations presented in Section 1 are harsh as the shipwreck is typically situated on the seabed, possibly under strong currents and heavy oil leakage. Hence, in order to perform the mooring task in an as-wide-aspossible set of scenes, the robot has to be able to choose among multiple strategies to accomplish each of the identified subtasks. Before the actual manipulation takes place, the procedure done by the robot can be divided into three subtasks, namely, navigation and on-site localisation, robot docking, and hook localisation. 2.1. I-AUV navigation and on-site localisation Regarding the navigation and the localisation of the vehicle on the site, previous related works envisioned the adoption of an LBL Acoustic Positioning System. Several algorithms have been proposed for AUV localisation using such a system. Miller et al. (2010) presented a robust navigation system for AUVs combining the inertial measurement unit of the vehicle and the localisation measurements from the LBL, while Scherbatyuk (1995) proposed an algorithm for position and velocity estimation using range data from only one transponder. Navigation could also be achieved through other acoustic methods, mainly based on sonar guidance and common navigation sensors such as DVL, IMU etc. A homing method on an acoustic target was proposed by Stokey et al. (1997) for the

docking phase of REMUS AUV. The method, although reliable for long range navigation, becomes impractical at close ranges due to the high update rates required. Thus, for close-range navigation and especially for the robot docking phase, the use of mainly acoustic guidance is not sufficient and the methods discussed in the literature usually utilise both acoustic and vision techniques (Krupinski et al., 2008). 2.2. I-AUV docking As discussed by Grosset et al. (2002) and Weiss et al. (2009), in order for the I-AUV to perform a fine intervention, it is necessary to dock near the target, since dynamic-positioning control would not provide the adequate error compensation in the presence of sea currents. Even in the case of ROVs, docking is preferred over other solutions, due to the higher accuracy guaranteed by such method. As described above, AUVs docking methods combine acoustic and vision localisation techniques. Generally, vision-localisation techniques can either use passive or active targets. Adopting passive vision techniques, Evans et al. (2003) studied the trajectory control problem during the homing of an I-AUV on an intervention-panel, and proposed a pose-estimation algorithm based on sonar and camera data to control the robot. Negre et al. (2008) proposed a docking method for AUVs using selfsimilar landmarks on the target so that the vehicle could selflocalise using the camera. Palmer et al. (2009) proposed a system for I-AUV short-range navigation in order to approach and dock on an offshore intervention panel using data from a camera-based technique for feature extraction and common navigation sensors. As for the active vision techniques, Lee et al. (2003) introduced a method for AUV docking using one camera on the vehicle and an array of lights on the docking station. Krupinski et al. (2008) introduced a method to perform I-AUV docking on an intervention panel by coupling sonar data with visual information using active markers. Though active markers require an energy source on the docking platform itself, it is rather robust compared to passive vision techniques especially in limited visibility situations. 2.3. Autonomous localisation of the target and manipulation Regarding the autonomous localisation of a target object for manipulation purposes, a combination of underwater cameras and ultrasound motion trackers has been proposed by Marani et al. (2009). Vision provides absolute pose estimation of the object, but at low sample rate and with the need of a light source, which could significantly reduce AUV autonomy. On the other hand, motion trackers can provide reliable and high sample-rate

P. Sotiropoulos et al. / Ocean Engineering 46 (2012) 33–45

35

AUV docking problem are the ones most frequently discussed in the literature, as summarised in Section 2. In this work, we focus on the target localisation and manipulation subtask, proposing a new solution based on a genetic algorithm to calculate the best docking position and tactile sensing exploration using a force–torque sensor to localise the hook. The proposed solution is intended to widen the set of possible strategies available to the AUV to accomplish the subtasks involved in the mooring operation. The intervention scenario is composed of a mooring cube situated on the seabed. On its top surface, a hook (Fig. 8) is attached to the cube by a rope latched to a handle. The final objective of the robot is to find the optimum place to dock on the cube, localise the hook and grasp it. A depiction of the scenario appears in Fig. 4 when the I-AUV is approaching the cube. The I-AUV is equipped with a 6-DOF elbow manipulator arm and is assumed to be able to dock on the determined optimal location. The docking is to be performed using common suction cups, as the ones illustrated in Fig. 3 (Perry Slingsby Systems, 2010). As mentioned in Section 2.1, the LBL system installed on site should be sufficient for the positioning and the navigation of the vehicle since position accuracy of 0.09 m could be achieved (Bingham et al., 2006), while a mapping of the area prior to the I-AUV deployment could lead to accurate navigation towards the cubes. More in detail, a mapping of the area immediately after the deployment of the cubes would provide substantial data for accurate I-AUV navigation towards each of the cubes. In the absence of such a map, visual patterns or active markers could be placed on the cubes to assist the I-AUV homing and docking process. This would require that the I-AUV be equipped with a camera apart from the common navigation instruments. The method proposed here is a two-step approach consisting of first calculating an appropriate docking pose for the vehicle, and later localising the hook in order to grasp it. The I-AUV optimal docking pose algorithm and hook-pose tactile localisation procedure are described in detail. Since the cube would rest on site some time before the actual deployment takes place, a displacement might occur around its initial position due to sea currents and its descending process. Therefore, in this paper the

Fig. 3. Suction foot with attachment arm. Fig. 2. ROV deployment.

information, but the measurement is relative to the position of the tracking probe, and sensors have to be installed on the target object. Moreover, stereoscopic cameras proved to become unreliable when the target is too close to the point of view (Woods et al., 1994).

3. Proposed methodology and scenario description Among the three identified subtasks involved in I-AUV mooring operations, the navigation and on-site localisation problem and the

Fig. 4. Scenario depiction.

36

P. Sotiropoulos et al. / Ocean Engineering 46 (2012) 33–45

Fig. 5. Uncertainty area on the cube and the global coordinate system.

actual position of the hook is not given but it is supposed to be situated in the ABCDA area illustrated in Fig. 5, and has to be localised using the force–torque sensor attached to the end effector. ABCDA represents the uncertainty due to both dockingposition errors and hook displacement. The determination of the docking pose is accomplished with the use of a genetic algorithm (GA) to maximise the dexterity of the robot over the hook’s area, and a particle filter (PF) algorithm is adopted to process point-wise measurement in order to infer the position and orientation of the object. For the formulation of the GA’s objective function, the manipulability measure w, as defined by Yoshikawa (1990), on the outer edges of a predefined uncertainty space is taken into consideration along with the distance between the proposed docking pose and the vehicle’s current pose. The physical constraints imposed by the environment are also incorporated into the algorithm. To complete the tactile localisation procedure, a 3D model of the target object is used to define a touch strategy and identify the position and orientation of the object. Despite the fact that the method presented here deals with a specific problem of a certain underwater application, the same strategy is rather generic and it could be easily adopted in any common I-AUV scenario in a semi-structured environment.

manipulability ellipsoid of the manipulator in order to provide a quantitative measure of a robot’s ability for manipulation inside its workspace. Aspragathos (1996) proposed a globalised version of the Yoshikawa’s manipulability index in order to determine the optimal location of a continuous path on the manipulator’s workspace. Other indices have also been suggested such as the condition number of the Jacobian by Salisbury and Craig (1982) and the manipulator velocity ratio (MVR) by Dubey and Luh (1988) as a measure of kinematic performance for the control of redundant manipulators. In addition, an index for the velocity efficiency for a robot moving its end-effector along a path based on the minimum MVR along it was proposed by Aspragathos and Foussias (2002). Meanwhile, task related dexterity indices are also presented in literature, such as the one presented by Yashima and Yamawaki (2009) where the optimum grasping is considered for a certain manipulation task. The vehicle having acquired its relative pose to the target using the LBL system and the map of the site should estimate the optimum docking pose in order to navigate towards it. The algorithm through an optimisation process returns a docking pose that assures high dexterity for the manipulator. A method for the determination of the docking pose of an Unmanned Underwater Vehicle (UUV) has been introduced by Sotiropoulos et al. (2010), though the location of the hook was a priori known and only the centre point of the task area was taken under consideration. In order to guarantee dexterity for the end effector during the search of the hook, in the predefined uncertainty area ABCDA the manipulability measure of the arm is calculated in the four outer corners A, B, C and D. The objective is to assure a minimum of dexterity in all the four points that would lead to a sufficient range of speed for the end effector while trying to localise the hook and later while attempting to grasp it. The method described here was designed to return 3-D pose po ¼[x, y, z, y, j, g], estimating the full 6-DOFs of the vehicle. In this specific case though, only [x, y, z, g] are concerned since the rest are restricted by the fact that the vehicle is intended to dock on the top of the cube’s surface. The actual pose of the cube itself does not affect the algorithm since the computations are relative to the global coordinate system (GCS), G situated on its top surface in Fig. 6.

4.1. Optimisation problem Prior to setting the actual optimisation problem and formulating the objective function, certain terms are to be defined. For this

4. Determination of the optimum docking pose While the I-AUV would attempt to dock near the target, the determination of the optimum docking pose is of vital importance to the success of the overall mission. Such a pose would guarantee that there exists at least one high-dexterity configuration for the manipulator in order to localise the hook and perform the grasping task in hand in a faster and more robust way. In addition, a high-dexterity docking pose would avoid singular configurations for the manipulator that would require a re-docking of the I-AUV to a more suitable pose. Faster interventions and the avoidance of possible re-dockings for the vehicle, will reduce mission time. To define a measure of manipulator’s dexterity an appropriate index had to be selected. Various indices have been proposed so far in order to quantify the dexterity. Yoshikawa (1990) proposed dexterity indices based on the kinematic and the dynamic

Fig. 6. AUV and base coordinate system.

P. Sotiropoulos et al. / Ocean Engineering 46 (2012) 33–45

scenario, there are two main coordinate systems set as shown in Fig. 6, the GCS and the manipulator’s base coordinate system (BCS) B situated on the I-AUV. The vehicle’s current position refers to the position of the suction cup at its bottom surface and is given with respect to B as B pdp ¼ ½ B xdp B ydp B zdp . The proposed docking point on the cube with respect to G is defined by G pd ¼ ½ G xd G yd G zd . The AUV’s current position with respect to G G pi is then derived by "

G

pi

# ¼ G T oB

1

B

pdp 1 

ð1Þ

where G T oB is a homogeneous transformation matrix. The manipulator’s base point for the proposed docking position is defined as Gp¼[x y z] and along with the angle g about the z-axis describes the candidate optimal pose. The candidate optimal pose is taken into account for the calculation of the manipulability measure in the four corners of the predefined area. Since the manipulator has 6 degrees of freedom (DOFs) there are up to eight solutions for each point (A, B, C and D) from the current candidate optimal pose. The configuration that provides the maximum manipulability measure for its point is retained and a min operator is applied afterwards to provide the overall minimum manipulability associated with the current pose. This way a lower limit of dexterity is guaranteed. The manipulability measure for each pose is derived through the solution of the inverse kinematics problem and the calculation of the Jacobian matrix and it is defined as wðyÞ ¼

qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi detð J T ðyÞ JðyÞÞ

ð2Þ

where J(y) is the Jacobian matrix, and the vector y(x, y, z, g) is one of the possible joint configurations derived by the inverse kinematics solution for the considered pose. The inverse kinematics solution and the calculation of the Jacobian matrix are based on the exponential products method and the Paden-Kahan sub-problems (Murray et al., 1994). While setting the optimisation problem, the manipulability measure as it is computed for every candidate pose is not the only criterion taken into account. The Euclidean distance between the I-AUV’s current position and the candidate optimal pose has to be considered in order to select the pose closer to the current one and thus minimise the energy needed for the vehicle to reach it. The term of the distance is calculated as following: L¼

qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ðG xi G xd Þ2 þ ðG yi G yd Þ2 þ ðG zi G zd Þ2

f obj ¼ aMðwÞbLðx,y,zÞ

ð4Þ

M(w) is the manipulability measure function described as (

minðmaxðwAi Þ,maxðwBi Þ,maxðwC i Þ,maxðwDi ÞÞ,

4.2. Genetic algorithm In order to solve the optimisation problem set above, a GA was implemented and used. The optimum placement of the base of the robot for dexterous manipulation using GAs is a problem that is widely discussed in the literature. Mitsi et al. (2008) proposed a method for determining the optimum robot base location using a hybrid genetic algorithm. Tian and Collins (2005) used a GA to define the optimum base location for a two-link planar manipulator. The choice of the GA was based on the fact that it offers several advantages over other optimisation methods, such as gradient methods, in the sense that it requires only the objective function and not its derivative. GAs can find a near optimum solution even if the objective function is not continuous and they can perform robustly even in complex search spaces avoiding getting trapped in local minima. Moreover, additional constraints could be easily specified inside the algorithm. In the studied case, the constraints of the optimisation variables are incorporated into the definition of the chromosome. Each chromosome consists of the x, y and z coordinates for its position and g angle for its orientation and it is represented as a binary chromosome of the form: x 10y11

y 10y11

z 10y11

c 01y11

The length of every part of the chromosome depends on the range of field of values of each variable and the selected accuracy. The field of values for the x, y, z coordinates is defined by the upper surface of the cube, adding a margin d along the z-axis, where the final docking pose should be found. As a consequence the base point coordinates are bounded according to the following relations: ðxmin cube rx rxA Þ,

ðxD rx r xmax cube Þ

ð6Þ

ðymin cube r y ryA Þ,

ðyB r yr ymax cube Þ

ð7Þ

zA o z rzA þ B zbp þ d

ð8Þ

B

where zbp is the relevant z-axis distance between the manipulator and the vehicle’s docking point. With the termination of the algorithm, a near optimum docking pose is acquired that would act as an input for the control system of the I-AUV, and from which the tactile localisation method could afterwards be performed with ease.

ð3Þ

Finally, the objective function that is to be maximised for this problem is given below

MðwÞ ¼

37

l

u

if yj o yj o yj ,j ¼ 1,. . .,6

0,otherwise

ð5Þ where wi(y) is the manipulability measure for the ith configuration of the manipulator in every edge of the predefined area, as defined by Eq. (2) and y ¼[y1yyjyy6] the joint angles that are derived by the inverse kinematics solution for the current candidate optimal pose [x, y, z, g]. yl and yu are the limits for every joint. a, b are the weighting factors used to transform the two terms of the objective function to comparable amounts, since they may take different magnitude of values.

5. Tactile localisation of the hook Tactile localisation is generally achieved by processing data obtained from force–torque sensors which provide x–y–z coordinates and normal vector of the touched point. Even if the collected measurements are only a partial representation of object state, the combination of several readings may be successfully adopted to univocally identify the position, the orientation and the grasping points of the object. For the localisation task studied in this work, a probabilistic, model-based inference method is adopted. The prior knowledge of the object geometry and the touch measurements are processed to localise the hook within the solution space. In modelbased algorithms, the geometric characteristics of the object are assumed to be known, usually in the form of a mesh created from a 3D model. A few strategies have been developed to reconstruct position and orientation from point-wise measurements. Deterministic approaches proved to be effective in estimating a single hypothesis of state in 2D (Grimson and Lozano-Perez, 1984).

38

P. Sotiropoulos et al. / Ocean Engineering 46 (2012) 33–45

However, probabilistic approaches based on Bayesian estimation (Gadeyne and Bruyninckx, 2001) seem to be the most powerful localisation strategies, as, theoretically, they can be applied to any geometry. Specifically, Gadeyne and Bruyninckx have applied Monte Carlo Method to a 3-DOF localisation problem. In their algorithm, they discretise the solution space and perform resampling to further refine the n-dimension grid until the required accuracy is reached. Morover, a compliant cube-in-corner operation has been implemented in Gadeyne et al. (2005). In particular, a sequence of compliant contact-state formations to position the cube in the corner was calculated. Contact-state transitions were recognised, e.g. changing from edge-to-face to face-to-face formations, using both a Kalman filter and a particle filter. Petrovskaya et al. (2007) have developed and implemented an algorithm to localise a box in a 6-DOF solution space using a basic active sensing technique to establish contact with five different faces of the solid rectangle, and processed the data with a particle filter. With the I-AUV docked in the defined pose to assure high manipulability over the search space, the robot arm is moved to collect information and localise the hook by exploring the environment and establishing contacts with a spherical end effector (Fig. 7) coupled with a force–torque sensor. The geometric properties of the probe allow to measure the contact-point coordinates and normal vector following the technique firstly introduced by Stokey et al. (1997). The acquired information is used as input for a Bayesian estimation algorithm. With respect to previous related research, this paper introduces a new strategy to sense the environment in order to localise a solid object on a flat surface. The 3-DOF localisation problem is tackled by dividing the procedure into an exploration phase along an Archimedean spiral trajectory followed by a local inspection through a series of vertical depth samples. Spiral exploration, first suggested by Chhatpar and Branicky (2001), allows an efficient investigation of the search space paying off a small displacement of the hook after the initial contact. The suggested depth-sample technique allows information collection to localise the object without causing any displacement. In addition, the chosen target object is geometrically complex and does not present symmetry characteristics to simplify the localisation task. The presented procedure can be optimised by adapting the search parameters to the geometric characteristics of the

Fig. 8. Hook geometry (dimensions in mm).

environment, namely:

 The spiral increment should not be bigger than the smallest  

edge of the object bounding box. This guarantees contact with the object during the exploration phase. The speed of the end effector during the robot has to be set so that the caused displacement is limited and does not affect task accomplishment. The distance of the grid points used to take depth samples should not be bigger than the smallest edge of the object bounding box. This allows the collection of an adequate number of contact points for the inference.

5.1. Bayesian estimation In Bayesian solid-object localisation, the position and orientation of solid objects is inferred by building a probability distribution function (PDF) over the solution space O, which is generically a portion of Rn . More specifically, the PDF is calculated using (1) the prior belief about the problem, (2) the measurement system characteristics, (3) the known three-dimensional model of the object. For a set of measurements D ¼ fY m g, where Y m is the m-th measurement collecting x–y–z coordinates and normal vector components of the touched point, the posterior probability of the generic state X of having caused the measurements is given by PðX9DÞ ¼ PðD9XÞ

PðXÞ PðDÞ

ð9Þ

In other words, the conditional probability of the generic state X to cause the set of measurements D is proportional to PðD9XÞ, i.e. PðX9DÞpPðD9XÞ:

ð10Þ

Information from both the model and the measurements are used to build the PDF PðD9X ðiÞ Þ. This likelihood distribution represents the probability of the set of measurements being caused by the states X ðiÞ . The Kalman Filter technique makes simplifying assumptions on the PDF, whereas the Monte-Carlo Method (Thrun et al., 2005) performs sampling over the solution space. Kalman Filter estimates position and orientation by minimising the mean square error estimate. Generally, Kalman Filter is effective for tracking problem. It can be successfully adopted for object localisation if

 the PDF can be modelled as uni-modal,  the system and the measurements are linear. Fig. 7. Spherical end effector.

Instead, Monte-Carlo Method discretises the solution space by generating a set of n-dimension points (particles) fX ðiÞ g, each of

P. Sotiropoulos et al. / Ocean Engineering 46 (2012) 33–45

them representing a portion of O, and consequently builds the PDF on them. Creating a fine uniform grid over the solution space implies a high computational cost. Therefore, ‘‘smart’’ sampling techniques have been developed to locate particles in highprobability regions. Sequential Monte-Carlo methods, also known as particle filter (PF) methods, perform the calculation of the PDF and resample the solution space at every filtering step. A review of resampling techniques is presented by Maskell and Gordon (2001). 5.2. Particle filter algorithm For this application, a particle filter algorithm, able to process the data obtained during the touching procedure, is defined in order to localise an object whose geometry is known. In this model, the solid object is represented as a polygonal mesh consisting of a set of faces ff i g. Therefore, each particle corresponds to a specific configuration of the object in the six-dimensional solution space. For each particle, the probability is calculated for each face to cause the measurement. The measurement noise is assumed to be Gaussian. Hence, for a generic face, f i , the probability of it causing the measurement Y m is given by the multiplication of the position probability and orientation probability ðmÞ PðY ðmÞ 9f i Þ ¼ PðY ðmÞ p 9f i ÞPðY n 9f i Þ:

In particular, position probability is calculated as ! 1 1 ½dðY ðmÞ p ,f i Þ2 ffiffiffiffiffiffi p 9f Þ ¼ exp  PðY ðmÞ i p 2 err p 2 2perr p

ð11Þ

ð12Þ

where dðY ðmÞ p ,f i Þ is the shortest Euclidean distance between the measured contact point and the face f i and err p 2 is the variance of the contact position measurement. Orientation probability is calculated as ! 2 1 1 :Y ðmÞ n normalðf i Þ: ffiffiffiffiffiffi p ð13Þ PðY ðmÞ 9f Þ ¼ exp  i n 2 err 2n 2perr n where err n 2 is the noise variance from the normal-vector measurement, and it can be expressed using the coefficient r as err n ¼ rerr p :

ð14Þ

As a strategy to establish correspondence between particles faces and measurement points are required, for a generic particle, the face with the highest probability is considered to be the one that caused the measurement (Fox et al., 1999), i.e. P ðmÞ ¼ maxðPðY ðmÞ 9f i ÞÞ: k i

ð15Þ

Having associated a priori probability to all the particles for each measurement Y ðmÞ , the following weight function for the generic k-th particle is defined and has to be maximised Q ðmÞ Pk ð16Þ Wk ¼ m C where C is the normalising constant to make the weight distribution a partition of unity X C¼ W k: ð17Þ k

The weight function described here takes into account the measurement noise, as expressed in Eqs. (12) and (13). To smooth the PDF, an artificially-inflated noise technique (Petrovskaya et al., 2007) is applied to sharpen the PDF as the number of filters is increased. Basically, at the beginning of the filtering, a high noise err p is considered, so that the obtained PDF is smooth over the sampled solution space, due to negative sign of the exponential element. As the filters run, err p is progressively reduced until it

39

reaches the value of the real measurement noise, so that the calculated PDF function is narrow. Artificially-inflated noise proved to be an effective tool in reducing the risk of converging to local minima, even though the number of filters to perform is increased. In this implementation, err p is scaled down as illustrated in the following equation: err p ðn þ 1Þ ¼

err p ðnÞ 2d

ð18Þ

where d lies in the range of ð0,1:0. Basically, since one can visualise err p ðnÞ as the radius of the inflated noise at the n-th time step, the volume of the noise sphere representing the measurement noise is reduced at each filtering step. Resampling is performed using Eq. (16), and the distribution of the new particles set is calculated as clonesk ¼ c  W k  pcs

ð19Þ

where clonesk is the number of particles that will be created at the (n þ1)th step from the k-th particle, pcs is the total number of particles, and c is a resampling coefficient such as 0 r c r1. At each time step, the new particles set is created from the previous sample distribution using the information gained from the weight function. Each particle will be the parent of a number of clone particles proportional to its weight. Therefore, next-step particles particles will be originated from the high-weight particles of the previous time step. The c coefficient is used to keep a sound number of completely randomly-generated particles helping the algorithm not to be prone to local minima. In this work, c ¼ 0:9.

5.3. Hook localisation As the AUV docks on the mooring cube, the hook is to be localised on the top surface. No prior knowledge about the position and orientation of the hook is assumed. As the hook is positioned on the cube top surface, the task can be simplified into a 3-DOF problem, i.e. the hook unknown coordinates are B x, B y and B g . To collect tactile information, e.g. point-wise measurements of contact-point coordinates and normal vector, a set of different approaches can be adopted to solve the problem of ‘‘where to sense next’’. In this example, the hook is lying on the top surface and it is likely to move if touched from the side, whereas a touch from above is not expected to affect its position. Since no prior knowledge about the pose of the hook is available, the configuration of the object cannot be used to define an optimal strategy, at least at the initial step. Therefore, it is chosen to divide the touching sequence into two sub-sequences. Firstly, the initial contact with the object is to be established, then a further investigation of its position is performed. Initially, the end effector is moved following an Archimedean spiral trajectory on a plane parallel to the top surface, 25 mm above it. In the presented examples, the increment in the spiral trajectory is set to 45 mm, i.e. equal to the smallest edge of the hook bounding box. This allows the robot to quickly cover the search area to establish the first contact with the object, and also prevents the end-effector from interfering with the rope attaching the hook. Chhatpar and Branicky (2001) proved spirals to be the most efficient trajectory in first-contact-exploration operations. Even if a lateral contact is expected to provoke a displacement of the object, the speed of the end effector can be set to limit the after-contact pose change of the object. Moreover, if the end effector is moved along the trajectory with constant speed, water friction is constant in magnitude and, therefore, a proper threshold on the force resultant can be set in order to detect the contact. As the threshold is reached, the

40

P. Sotiropoulos et al. / Ocean Engineering 46 (2012) 33–45

Table 1 Joint angle limits. Lower limit (rad)

Angle

Upper limit (rad)

 3.14  2.57  2.53  4.71  2.01  4.71

y1 y2 y3 y4 y5 y6

3.14 2.27 2.53 4.71 2.44 4.71

in the sense that the first (base) link’s length has been considered as L1¼0.1 m instead of 0.478 m. The lengths of the two remaining links (forearm and arm) are L2¼L3¼0.425 m. The manipulator is displayed on its zero-angle configuration. Fig. 9. Spiral trajectory on the mooring block used to localise the hook in pose_A.

6.1. Retrieving the optimal docking pose After running the GA several times, its parameters for the problem in hand have been tuned and set as displayed below. pc 0.15

pm 0.03

Population 100

Generations 500

a 10

b 1.35

where

    

Fig. 10. Spiral trajectory on the mooring block used to localise the hook in pose_B.

knowledge about the position of the first contact is used to reduce the investigation space and focus on a high-interest area. In order to calculate the hook pose with good approximation, proper information has to be provided to the particle filter algorithm. Ideally, the set of contact points and normal vectors measured by the force–torque sensor have to define an overconstrained configuration. However, a universal strategy to achieve such a set of information for any kind of solid objects is not available, as the number and type of measurements required is strongly dependent on the geometry of the object. In this case, it is chosen to perform a series of vertical depth samples following a square grid centred on the first-contact point. The fineness of the grid is dependent on the geometric characteristics of the object to localise. In this case, steps of 25 mm sides are used as this distance corresponds to the length of the smallest edge on the hook top surfaces. In the examples presented further on, two different grids have been adopted, a coarse one with 200 mm side and fine one with 250 mm side. Figs. 9 and 10 illustrate two examples of both the spiral trajectory used to find the first contact and the square grid used to obtain the vertical touches.

6. Results The cube’s and the AUV’s dimensions in metres, along the x, y and z axis are 2  2  2 (m) and 1.5  1.2  1 (m), respectively. The joints’ angle limits of the manipulator are given in Table 1. The manipulator is modelled in Matlab Robotics toolbox (Corke, 1996) with a few simplifications (Fig. 11). Though, the end effector’s position with respect to the base is always respected. For the specific case, the original model of the TX 90 manipulator has been modified

pc: is the probability of crossover, pm: is the probability of mutation, Population: is the population size, Generations: is the maximum number of generations, a, b the weighting factors described in Eq. (4).

The algorithm was tested for four different AUV starting positions S1 ¼[222], S2 ¼[  222], S3 ¼[ 2  22] and S4 ¼[2 22]. Indicative results are demonstrated in Table 2. Figs. 12 and 13 show the resulting docking pose on the cube for the first case with the help of three points. More in detail, the yellow point N indicates the actual docking position while the red point M depicts the manipulator’s base position. In order to have a better understanding of the rotation, the third blue point P is introduced to represent the projection of the manipulator’s base on the z-surface on the height of the docking point. In Table 3, a list of the four configurations for the case of S1 can be found. As observed in the figures, the algorithm can produce a docking pose that guarantees a minimum manipulability for the four outer corners of the predefined uncertainty area while selecting the pose closer to its starting position. Indicatively, the last configuration of the manipulator is illustrated in Figs. 14 and 15. The angles as observed are within the joint limits of the manipulator. 6.2. Hook localisation simulation results The proposed hook-localisation approach has been tested with a series of simulations, focused on measuring the performance of the developed inference method. Two different poses were investigated, conventionally referred to as pose_A and pose_B, as shown in Figs. 9 and 10. When the contact is established along the spiral trajectory, the grid is built with its centre corresponding to the firsttouch point. Both a coarse and a fine grid were used in order to show the differences in the pose reconstruction using the two sets of information. For both poses using both grids, ten runs were launched with 10, 20, 40, 80, 160 and 320 particles investigating a square workspace of 500 mm side corresponding to the uncertainty area ABCDA shown in Fig. 5. An artificial noise with err p converging to 1 mm was adopted. For manipulation purposes, the estimation is

P. Sotiropoulos et al. / Ocean Engineering 46 (2012) 33–45

41

Fig. 11. Zero-angle configuration.

Table 2 GA test results.

S1 S2 S3 S4

fobj

x (m)

y (m)

z (m)

c (rad)

w

 1.4269  1.5857  1.4404  1.7274

0.3033  0.3806  0.3218 0.3218

0.3820 0.2507  0.3669  0.2705

0.2500 0.2452 0.2476 0.2310

 2.4228  0.9209 0.7483 2.2694

0.1888 0.1773 0.1873 0.1670

Fig. 12. Final docking pose starting from S1.

considered correct with a translation error smaller than 5 mm and a rotation error smaller than 5 degrees. Figs. 16–19 illustrate the translation error for both the studied poses using the two different grids, and with respect to the number of particles. For each run, error and standard deviation are plotted. For both pose_A and pose_B, illustrated in Figs. 9 and 10, the PF is able to converge to a solution within the allowed error when the fine grid is adopted. Figs. 20–23 present the translation and rotation error for both the poses and adopted grids. The graphs show how the PF behaves well when the number of particles is higher than 80, but is affected by local-minima problems with less than 80 particles, regardless of the number of contact points used as input. The algorithm tested in these examples proved to be effective in localising the hook when processing data obtained from the fine grid. Local minima represent the main source of errors. The

minimum number of particles required to make the error converge below the manipulability limit is significantly low.

7. Conclusions In this work, an overview on the subtasks involved in the mooring operations during the DIFIS dome deployment has been provided, namely navigation and on-site localisation, robot docking and hook localisation. State-of-the-art techniques to accomplish each subtask have been summarised and discussed. The research results presented in this paper focus on robot docking and hook localisation. The considered underwater scenario consists of an I-AUV attempting to dock on a mooring cube and then localise a latched hook lying on an uncertainty area on the top surface. A new approach to calculate the best docking position in

42

P. Sotiropoulos et al. / Ocean Engineering 46 (2012) 33–45

Table 3 Manipulator’s configurations for S1. S1 Point Point Point Point

A ( 0.25,  0.25,0) B ( 0.25, 0.25, 0) C (0.25,0.25, 0) D (0.25,  0.25, 0)

q1

q2

q3

q4

q5

q6

0.204  0.397 0.827 0.847

 0.548  1.224  2.321 0.049

0.241 1.341 2.229  1.109

1.147e  16 3.142 3.142 7.921e  16

1.878  1.454  1.663 2.031

2.219  0.322  1.546 1.576

Fig. 13. Top view of the docking pose for S1.

Fig. 14. Manipulator’s configuration to reach point D.

Fig. 15. Top view of the manipulator.

P. Sotiropoulos et al. / Ocean Engineering 46 (2012) 33–45

43

Fig. 16. Pose_A—coarse grid.

Fig. 19. Pose_B—fine grid.

Fig. 17. Pose_A—fine grid.

Fig. 20. Pose_A—coarse grid.

Fig. 18. Pose_B—coarse grid.

order to guarantee high dexterity over the target area has been introduced, together with a hook-localisation technique using a force–torque sensor.

Fig. 21. Pose_A—fine grid.

A GA was adopted to calculate a near-optimal docking pose, assuring a minimum of dexterity for a manipulator’s end effector while reaching the four outer edges of the search area. The

44

P. Sotiropoulos et al. / Ocean Engineering 46 (2012) 33–45

Acknowledgements This work was sponsored by the FREESUBNET project, Contract number MTRN-CT-2006–036186.

References

Fig. 22. Pose_B—coarse grid.

Fig. 23. Pose_B—fine grid.

distance of the vehicle’s initial pose was also taken into account. The results from four different starting points were presented and it can be observed that a final docking pose closer to the starting point was always selected. To localise the hook, an inference method based on Bayesian estimation has been proposed. To establish the first contact with the object, the chosen strategy was to follow a spiral trajectory on a plane parallel to the cube top surface. Once the object is detected, the position of the hook is inferred by taking depth samples following a square grid over the first-touch location. The proposed algorithm was tested with a series of simulations and proved to localise the hook when sensing over a fine grid. The overall method could be embedded in the vehicle’s control system and perform the described task in an autonomous way enabling the use of I-AUVs for the mission in hand. Though sea trials have yet to be performed, this seems to be a rather promising method. Further developments towards autonomous procedures for the remaining stages of the DIFIS deployment procedures would make the use of I-AUVs a feasible reality, reducing the deployment cost substantially, shortening the intervention time and making the overall DIFIS method even more flexible and appealing.

Andritsos, F., et al., 2007. Recuperation of oil trapped in ship-wrecks: the DIFIS concept. In: Proceedings of the International Symposium on Maritime Safety, Security and Environmental Protection (SSE 07), Athens. Andritsos, F., et al., 2008. Avoidance of wreck polution. PSDMH J., 407. Aspragathos, N.A., 1996. Optimal location of path following tasks in the workspace of a manipulator using genetic algorithms. In: Proceedings of the ARK’96. Kluwer Academic Publishers, Portroz, Slovenia. Aspragathos, N.A., Foussias, S., 2002. Optimal location of a robot path when considering velocity performance. Robotica 20 (02), 139–147. Bingham, B., et al., 2006. Integrating precision relative positioning into JASON/ MEDEA ROV operations. Mar. Technol. Soc. J. 40 (1), 87–96. Chhatpar, S.R., Branicky, M.S., 2001. Search strategies for peg-in-hole assemblies with position uncertainty. In: Proceedings of the 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems.. Corke, P.I., 1996. A robotics toolbox for MATLAB. IEEE Robotics Autom. Mag. 3 (1), 24–32. Cozijn, H., et al., 2008. DIFIS—double inverted funnel for the intervention on ship wrecks. In: Proceedings of the International Oil Spill Conference (IOSC 2008), Savannah, Georgia, USA. DIFIS, 2005. DIFIS Project Web Site. Available from: //http://www.ifremer.fr/difis/S. Dubey, R., Luh, J.Y.S., 1988. Redundant robot control using task based performance measures. J. Robotic Syst. 5 (5), 409–432. Evans, J., et al., 2003. Autonomous docking for Intervention-AUVs using sonar and video-based real-time 3D pose estimation. In: Proceedings of the OCEANS 2003. Fox, D., et al., 1999. Monte Carlo localization: efficient position estimation for mobile robots. In: Proceedings of the National Conference on Artificial Intelligence, pp. 343–349. Gadeyne, K., et al., 2005. Bayesian hybrid model-state estimation applied to simultaneous contact formation detection and geometrical parameter estimation. Int. J. Robotics Res. 24, 615–630. Gadeyne, K., Bruyninckx, H., 2001. Markov techniques for object localization with force-controlled robots. In: Proceedings of the Tenth International Conference on Advanced Robotics. Grimson, W.E.L., Lozano-Perez, T., 1984. Model-based recognition and localization from sparse range or tactile data. Int. J. Robotics Res. 3 (3), 3–35. Grosset, D., et al., 2002. Quasi-rigid docking of AUV for underwater manipulations. In: Proceedings of International Workshop on Computer Science and Information Technologies, CSIT, Patras, Greece. Konstantinopoulos, P., Andritsos, F., 2008. DIFIS —Greek innovation for the avoidance of wreck polution. Naval J. 168, 566. (Hellenic Navy). Krupinski, S., et al., 2008. Investigation of autonomous docking strategies for robotic operation on intervention panels. In: Proceedings of the OCEANS 2008. Lee, P. M. et al., 2003. Visual servoing for underwater docking of an autonomous underwater vehicle with one camera. In: Proceedings of the Oceans Conference Record (IEEE), vol. 2, pp. 677-682. Marani, G., et al., 2009. Underwater autonomous manipulation for intervention missions AUVs. Ocean Eng. 36 (1), 15–23. Maskell, S., Gordon, N., 2001. A tutorial on particle filters for on-line nonlinear/ non-Gaussian Bayesian tracking. Target Tracking: Algorithms and Applications (Ref. no. 2001/174), IEE. Miller, P.A., et al., 2010. Autonomous underwater vehicle navigation. IEEE J. Oceanic Eng. 35 (3), 663–678. Mitsi, S., et al., 2008. Determination of optimum robot base location considering discrete end-effector positions by means of hybrid genetic algorithm. Robotics Comput.Integrated Manuf. 24 (1), 50–59. Murray, R., et al., 1994. A Mathematical Introduction to Robotic Manipulation. CRC. Negre, A. et al., 2008. Robust vision-based underwater target identification and homing using self-similar landmarks. Springer Tracts in Advanced Robotics, vol. 42, pp. 51–60. Palmer, T. et al., 2009. Vision based localization system for AUV docking on subsea intervention panels. OCEANS ’09 IEEE. Bremen, Germany. Perry Slings by Systems, 2010. Perry Slings by Systems. Available from: //http:// www. perryslingsbysystems.com.S. Petrovskaya, Anna, et al., 2007. Touch based perception for object manipulation. In: Proceedings of the Robot Manipulation Workshop. Salisbury, J.K., Craig, J.J., 1982. Articulated hands. Int. J. Robotics Res. 1 (1), 4–17. Scherbatyuk, A.P., 1995. The AUV positioning using ranges from one transponder LBL. In: OCEANS’95 MTS/IEEE: Challenges of our Changing Global Environment, Conference Proceedings. Sotiropoulos, P., et al., 2010. Determination of the optimum docking position of an underwater unmanned vehicle using a genetic algorithm. In: Proceedings of the ICIAR’10, San Francisco. Stokey, R. et al., 1997. Docking system for REMUS, an autonomous underwater vehicle. In: Proceedings of the Oceans Conference Record (IEEE), vol. 2, pp. 1132–1136.

P. Sotiropoulos et al. / Ocean Engineering 46 (2012) 33–45

Thrun, S., et al., 2005. Probabilistic Robotics (Intelligent Robotics and Autonomous Agents). The MIT Press. Tian, L., Collins, C., 2005. Optimal placement of a two-link planar manipulator using a genetic algorithm. Robotica 23 (2), 169–176. Weiss, P. et al., 2009. FreeSUB: Dynamic stabilization and docking for autonomous Underwater vehicles. In: Proceedings of the 13th International Symposium on Unmanned Untethered Submersible Technology(UUST), Durham, USA.

45

Woods, A., et al., 1994. Experiences of using stereoscopic video with an underwater remotely operated vehicle. Underwater Intervention. San Diego, California, pp. 34–38. Yashima, M., Yamawaki, T., 2009. Task-oriented accuracy measure for dexterous manipulation. In: Proceedings of the IEEE International Conference on Robotics and Biomimetics, ROBIO 2008. Yoshikawa, T., 1990. Foundations of Robotics. The MIT Press, Cambridge Massachusetts.