A forward-looking SONAR and dynamic model-based AUV navigation strategy: Preliminary validation with FeelHippo AUV

Ocean Engineering xxx (xxxx) xxx

Contents lists available at ScienceDirect

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

A forward-looking SONAR and dynamic model-based AUV navigation strategy: Preliminary validation with FeelHippo AUV Matteo Franchi a, b, *, Alessandro Ridolfi a, b, Marco Pagliai a, b a b

Department of Industrial Engineering, University of Florence, via di Santa Marta 3, 50139, Florence, Italy Interuniversity Center of Integrated Systems for the Marine Environment (ISME), Italy1

A R T I C L E I N F O

A B S T R A C T

Keywords: AUVs Underwater robotics Autonomous navigation Acoustic odometry SONAR

Reliable navigation systems are fundamental for Autonomous Underwater Vehicles (AUVs) to perform complex tasks and missions. It is well known that the Global Positioning System (GPS) cannot be employed in underwater scenarios; thus, during missions below the sea’s surface the real-time position is usually obtained with expensive sensors, such as the Doppler Velocity Log (DVL), integrated within a navigation filter such as an Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), or Dead Reckoning (DR) strategies. The goal of this work is to develop an underwater navigation system that does not rely on a DVL and where linear speed estimations are obtained exploiting data from a Forward-Looking SONAR (FLS) or, in its absence, taking advantage of a dynamic model that presents a reduced set of parameters. The proposed solution is validated through the use of navigation data obtained during sea trials undertaken in July 2018 with FeelHippo AUV at La Spezia (Italy), at the NATO Science and Technology Organization Center for Maritime Research and Experimentation (CMRE).

1. Introduction Today, Autonomous Underwater Vehicles (AUVs) are widely employed for many applications in diverse fields, and the availability of a precise and reliable navigation system is of utmost importance. Regardless of the kind of mission the underwater vehicle is required to perform, the final result is affected by its overall navigation perfor­ mance. Moreover, it is well known that the unavailability of the Global Positioning System (GPS) signal (like other electromagnetic signals) in the underwater domain (Barclay, 2003), makes the localization and navigation task more difficult. The majority of underwater navigation systems are based on either the Kalman Filter (KF) (Kalman, 1960), the Extended Kalman Filter (EKF) (Bar-Shalom et al., 2004), or the Unscented Kalman Filter (UKF) (Allotta et al., 2016c), (Costanzi et al., 2018) typically employed when non-linearities in the dynamic description of the system arise. As the simplest solution (despite their straightforward philosophy), Dead Reckoning (DR) strategies have proven satisfyingly reliable if the available sensors are sufficiently accurate. Generally speaking, all require knowledge of the linear speed of the AUV, which is (for the majority of times) obtained by means of specialized and ad’hoc

underwater sensors such as the Doppler Velocity Log (DVL). In addition, absolute underwater position information can be ob­ tained using beacons, with two types of system most commonly employed being the Long BaseLine (LBL) and the Ultra-Short BaseLine (USBL) (Leonard and Bahr, 2016). Conversely, as stated in (Melo and Matos, 2017), the main disadvantages are the total cost, the deployment and recovery time (especially for the LBL), and a detailed calibration process, which is necessary to obtain optimal positioning accuracy (especially for the USBL). To ease the localization problem, static and/or dynamic local sensors networks composed of localizing acoustic devices have gained attention (Yoerger et al., 2007) and (Bahr et al., 2009). Single beacon localization has been proposed, whereby a vehicle (usu­ ally known as beacon vehicle) with good quality positioning information is able to transmit range information acoustically to one or more un­ derwater vehicles, as described in (Tan et al., 2014) and (Webster et al., 2013). In addition, Moving Long BaseLine (MLBL) systems—that are a generalization of the LBL—have been suggested. Here, as stated in (Yan et al., 2015), the arrays of transponders are fully mobile and self-calibrating; thus, they do not constrain the operating site to a fixed area. Contributions can be found for example in (Curcio et al., 2005) and (Bishop et al., 2010).

* Corresponding author. Department of Industrial Engineering, University of Florence, via di Santa Marta 3, 50139, Florence, Italy. E-mail address: [email protected] (M. Franchi). 1 www.isme.unige.it https://doi.org/10.1016/j.oceaneng.2019.106770 Received 3 May 2019; Received in revised form 18 October 2019; Accepted 23 November 2019 0029-8018/© 2019 Elsevier Ltd. All rights reserved.

Please cite this article as: Matteo Franchi, Ocean Engineering, https://doi.org/10.1016/j.oceaneng.2019.106770

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx

Over the last two decades, several strategies exploiting optical or acoustic payload to solve localization problems have been proposed. In particular, Simultaneous Localization And Mapping (SLAM) techniques have been successfully applied. Further information concerning under­ water SLAM can be found in (Chaves et al., 2017). Optical sensors (e.g. optical cameras) are relatively inexpensive with high resolution information provided at high refresh rates, but adverse conditions in the underwater domain are likely to be present. For example, it is well known that turbid waters, turbulence, sediment, or poor lighting conditions can lead to a limited visibility range (usually less than 10 m), thus jeopardizing operations with optical devices (Mallios et al., 2014). As a consequence, researchers have often relied on acoustic payload sensors, such as SONAR and more recently Forward-Looking SONAR (FLS), considering them a more robust option �s et al., 2015). Acoustic payloads present higher costs and lower (Hurto resolution than optical ones. On the other hand, the former are able to penetrate the water even in poor visibility conditions for longer ranges (e.g., FLS can reach distances greater than 150 m) (Mallios et al., 2014). More information concerning the state-of-the-art in underwater navigation and localization (which is beyond the scope of this work) can be found in (Leonard and Bahr, 2016), and (Paull et al., 2014). The main contribution of the work presented here focuses on pro­ moting the use of FLS to aid underwater navigation, proposing a solution able to estimate linear speed without exploiting DVL measurements. Working on FLS images, a Fourier-based registration method is employed and, to cope with feature-poor environments, a dynamic model that exploits a reduced set of parameters is used when no linear speed measurement from the acoustic payload is available. The results are shown to be comparable with those obtained with a DVL-based navigation system. The authors are aware that the achieved perfor­ mance is unlikely to be better with respect to a DVL-based navigation system, even in the near future; indeed, the authors wish to point out that even if the proposed solution is shown to work without exploiting any DVL measurement, cooperation with the DVL is possible. This way, more linear speed measurements could be obtained. In addition to this, using FLS to aid navigation could potentially outline other advantages. In fact, using an augmented set of devices able to provide navigation information represents an intrinsic boost in redundancy, preventing failures due, for example, to underwater sensor-denied scenarios (such as DVL-denied scenarios when proximity to the seafloor or other sur­ faces takes place (Miller et al., 2010) or when a substantial number of gaseous bubbles is present). Moreover, although bigger AUVs enable the use of more complex instrumentation and are able to carry a heavy payload, smaller AUVs are constrained to limited payload carrying ca­ pabilities. Hence, in addition to constituting a valuable research interest, multitasking on-board sensors represent a solution that offers compactness and avoids the use of some instruments. The results of this study were obtained using navigation data gath­ ered during the European Robotics League/Student Autonomous Un­ derwater Vehicles Challenge 2018 (ERL/SAUC-E 2018) competition (Ferri et al., 2015) and (Ferri et al., 2017) held in La Spezia (Italy) in July 2018. This involved using FeelHippo AUV, a lightweight vehicle developed by the Department of Industrial Engineering of the University of Florence (UNIFI DIEF). During the above-mentioned tests, FeelHippo AUV navigated by DR with its standard equipment (see (Allotta et al., 2017a,b), and (Costanzi et al., 2016)) and the proposed solution was tested and validated offline. Some points are worth highlighting. First, this work focused on post-processed data; however, a testing framework as close as possible to a real online test was adopted. In particular, all the sensor data have been used as input for the proposed solution with a time synchronization based on the time at which messages were received. Moreover, the validation was performed using the same nav­ igation software present on FeelHippo AUV. Second, the performed underwater mission was planned to try to simulate a composite scenario, where different elementary motions were present: turns at right angles, sharp turns, short transects (around 3.0 m), medium transects (around

16.0 m), and longer ones (around 30.0 m). Last, the underwater envi­ ronment was not known a priori; both high informative seabed areas and poor ones were present. The obtained results are promising, and on-line underwater tests will be implemented in the near future. The remainder of the paper is organized as follows: related works in the underwater domain are described in Section 2, whereas Section 3 is dedicated to preliminaries useful in the development of this work and to the description of the current navigation system present on FeelHippo AUV. Section 4 addresses the proposed navigation strategy, whereas Section 5 focuses on the mechanical design and on the hardware ar­ chitecture of FeelHippo AUV. Section 6 illustrates the conducted sea trials and performance of the proposed navigation solution. Finally, Section 7 draws conclusions. 2. Related works The vast majority of contributions concerning acoustic payload in navigation-aiding are focused on constraining the navigation drift of the AUV; thus, particular emphasis has been paid to the use of these devices complementary to a standard navigation sensors set (comprising, for example, a DVL, Inertial Measurement Unit (IMU), and Fiber-Optic Gyro (FOG) see (Paull et al., 2014)). Accordingly, as stated by (Durrant-Whyte and Bailey, 2006), and (Li et al., 2018), navigation can be adjusted, for example, by means of self-correction using environmental landmarks, whereby observations of features or landmarks (assumed time invariant) €unl, and re-observation of the landmarks are crucial (Hidalgo and Bra 2015). Therefore, acoustic payload is usually included in a SLAM framework. SONAR-based SLAM is approached in two ways, depending on whether a set of range and bearing measurements is produced (this can be achieved with MultiBeam Echosounders (MBEs)), or an acoustic image of the scene is acquired (this can be achieved with FLSs, Side Scan SONARs (SSSs), or Mechanically Scanned Imaging SONARs (MSISs)) (Ribas et al., 2010). A concise overview of the main contributions for navigation purposes is reported here, and for further information, the interested reader is encouraged to read (Ribas et al., 2010), and (Valencia and Andrade-Cetto, 2018). In (Walter et al., 2008) an FLS-based SLAM for performing a ship hull inspection is shown. Here, FLS images are related by means of features extracted from different frames. Conversely, the velocity relative to the ship hull is retrieved exploiting a DVL. This therefore constitutes a part of the navigation system. A similar solution based on SSS and re-observation of landmarks previously detected to reduce navigation drift is presented in (Ruiz et al., 2004). In (Johannsson et al., 2010), a drift-free navigation solution during harbor surveillance and ship hull inspection by means of an FLS is proposed, where a DVL to measure velocity relative to a surface and a ring laser gyro are present on board. Another feature-based solution is proposed in (Li et al., 2018), where a ship hull inspection is undertaken. A SLAM algorithm that makes use of an FLS as the sole perceptual sensor to perform navigation drift correction of a DVL/IMU-based odometry navigation framework is �s et al., 2015), and (Hurto �s et al., outlined. Many authors (such as (Hurto 2013)) have indicated that feature detection by means of acoustic im­ ages might be difficult in a generic natural underwater environment; feature-based techniques present repeatability and discrimination issues due to, for example, low resolution and illumination changes. Conse­ quently, several researchers have tried to use either features at a region level or the entire image. A study by (Aykin and Negahdaripour, 2012) describes a technique implemented on FLS images able to detect stable and reliable features located at region rather than at pixel level. However, although the proposed solution is considered able to improve the precision of AUV navigation, no specific result was outlined. In �s et al., 2015), a mosaicing framework is presented, able to create (Hurto acoustic maps along various vehicle track-lines, where both trans­ lational and rotational 2D motions can be handled. However, the 2

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx

frame is centered in the Center of Gravity (CG) of the vehicle with the forward motion direction represented by the x-axis (surge) and the zaxis (heave) pointing down fOb xb yb zb g. Lastly, the y-axis (sway) com­ pletes a right-handed reference frame. To describe the kinematic model of the vehicle, SNAME notation is employed (Fossen et al., 1994): see Fig. 1. The AUV is represented with η ¼ ½η1 η2 �T , where η1 indicates the position of the AUV, and η2 its orientation. In particular, a triplet of Euler angles expressed with respect to a fixed frame, namely Roll (ϕ), Pitch (θ), and Yaw (ψ) (RPY) is used. Moreover, the linear and angular velocities of the vehicle with respect to a body-fixed reference frame is

Fig. 1. The SNAME notation.

denoted with ν ¼ ½ν1 ν2 �T , the vector of forces and moments acting on the AUV is τ 2 R6 , whereas T 2 Rm , and u 2 Rm with m the number of motors (in this case four) are a vector that collects the thrusts and the vector of rotational speed of the motors. For the sake of clarity, the ki­ nematic model of the AUV is reported in Eq. (1), Eq. (2). h iT �T � η ¼ ηT1 ηT2 ; η1 ¼ x y z ; �T � (1) η2 ¼ ½ϕ θ ψ �T ; ν ¼ νT1 νT2 ;

authors state that the proposed framework is tailored for an offline approach, where the trajectory followed by the vehicle is computed a posteriori, and integration in an online navigation system is only hy­ pothesized. In (Ferreira et al., 2015), a real-time mosaicing framework for FLS applications is presented. Here, the purpose is to propose an Automatic Target Recognition (ATR) technique in the context of mine detection. The FLS is used as a sole perceptual sensor and no integration with the navigation filter is proposed. In (Ribas et al., 2008), and (Mallios et al., 2014) two EKF are used in parallel. DR navigation is performed with the first, where a DVL and an Attitude and Heading Reference System (AHRS) are employed. The second makes use of range and bearing data retrieved from an MSIS and the filter is updated via feature extracted from the environment in the former, and via a prob­ abilistic scan matching in the latter. In (White et al., 2010), six SLAM techniques have been applied to the exploration and mapping of ancient cisterns, but the proposed solution is said to be capable of mapping and navigating underwater tunnel systems (where it is much easier to retrieve information from the surrounding environment); however, the open sea navigation problem is not tackled. In (Norgren and Skjetne, 2018) the authors propose an iceberg mapping method using an AUV equipped with an MBE by estimating the position and orientation of the iceberg. In particular, a top-level EKF provides the relative position and orientation between the AUV and the iceberg, whereas a bottom-level estimator is based on the Bathymetric distributed Particle SLAM (BPSLAM) algorithm. Among other navigation sensors, an upwards-looking DVL is considered in the solution. In (Rahman et al., 2018), the authors employ data from a stereo camera, angular velocity and linear acceleration data from an IMU, and range data from an MSIS. In this work, experimental tests have been conducted in rich environ­ ments such as artificial shipwrecks and a submerged bus, but no quan­ titative evaluation of the navigation is given. To the authors’ best knowledge, few proposals focused on effectively substituting a DVL in favor of an FLS for underwater navigation are present. For example, in (Song et al., 2018) a method is presented that relies on optical and acoustic images for localization purposes. A mixed approach that uses an FLS, a standard camera, or a DR strategy is pro­ posed. Although the solution is tested using data gathered from exper­ imental tests, the navigation performance of the technique is not thoroughly investigated.

ν1 ¼ ½u v w�T ;

ν2 ¼ ½p q r�T (2a)

η_ ¼ JðηÞν �

η_ 1 η_ 2

"

� ¼

Rnb ðη2 Þ

03�3

03�3

E 1 ðη2 Þ

#�

ν1 ν2

� (2b)

where Rnb represents the rotation matrix between the body and the fixed reference system and E is the Euler matrix. In addition, the forces and moments on the AUV and the thrusts carried out by the motors are linked using the linear model:

τ ðν; uÞ ¼ BTðν; uÞ

(3)

where B is a constant matrix (assuming the vehicle and the thrusters are not reconfigurable) that depends upon the thruster poses with respect to the CG. Its expression is reported in Eq. (4) and Eq. (5) (assuming, as already stated, that the body frame is centered in the CG). � � B B¼ 1 ; (4) B2 with � � B1 ¼ ⋯nbmi ⋯ ; � � � B2 ¼ ⋯ Pbmi � nbmi ⋯ ;

(5)

where nbmi is the axis of the i-th motor expressed in the body frame

fOb xb yb zb g and Pbmi is the thruster center of the i-th motor with respect to the CG expressed in the body frame. According to (Fossen et al., 1994), the dynamic of the AUV can be described as M ν_ þ CðνÞν þ DðνÞν þ gðηÞ ¼ τ ðν; uÞ

(6)

M is the mass matrix, CðνÞ is the centrifugal and Coriolis matrix, DðνÞ is the damping matrix, gðηÞ takes into account the effects of gravity and buoyancy, and τ ðν; uÞ describes the map between the vehicle speed (ν) and the rotational speed of the motors (u) to the resultant thrust action on the vehicle.

3. Preliminaries and notation First, the notation employed in the rest of the work, the kinematic and dynamic modelling of an AUV, the imaging geometry model of an FLS, and the key concepts of Fourier-based registration methods are reported. After, a brief description is given of the current navigation algorithm present on FeelHippo AUV.

3.2. FLS imaging model and phase correlation technique The area covered by an FLS that insonifies the sea bottom can be described using spherical coordinates ðR; α;βÞ, where R is the range, α is the azimuth angle, and β is the elevation angle. Let us assume a reference frame fOFS xFS yFS zFS g centered on the FLS center with the x-axis pointing forward, the z-axis pointing down and the y-axis completing a righthanded reference frame.

3.1. Kinematic and dynamic modelling of the AUV The pose of the AUV (in terms of position and attitude) is retrieved with respect to a local Earth-fixed reference frame with axes pointing North, East, and Down (NED frame) fON xN yN zN g, whereas the body 3

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx

Fig. 2. The imaging model of an FLS.

The position and orientation of the FLS with respect to the fOb xb yb zb g frame can be represented as follows (see Eq. (7)). ! pFS RFS b AFS ¼ ; (7) O> 1

Fig. 3. FeelHippo AUV navigation filter block scheme, where DS stands for depth sensor. The attitude estimation filter can be found in (Costanzi et al., 2016), whereas the position estimation can resort to either a DR strategy or to an UKF-based estimator (Allotta et al., 2016c).

RFS b

where is the matrix between the FLS and the body reference system and pFS is the position of the FLS with respect to the CG expressed in the fOb xb yb zb g frame. In the following, the uppercase will indicate a 3D point, whereas with the lowercase the same 3D point is projected on the image plane. The coordinates of a generic 3D point P in the fOFS xFS yFS zFS g reference system, expressed in spherical coordinates, are (see Fig. 2): 2 3 2 3 X R cos α cos β 4 5 4 P ¼ Y ¼ R sin α cos β 5 (8) Z R sin β

When pure translations are considered (θ0 ¼ 0, see Eq. (12)), Eq. (11) becomes Eq. (13). � (12) it1 ðx; yÞ ¼ it2 x sx ; y sy ; It1 ðm; nÞ ¼ e

It1 ðm; nÞI �t2 ðm; nÞ �¼e Cðm; nÞ ¼ �� � �It1 ðm; nÞI �t2 ðm; nÞ�

cðx; yÞ ¼ F � s¼

j2π ðmsx þnsy Þ

It2 ðm cosθ0 þ n sinθ0 ; m sinθ0 þ n cosθ0 Þ

;

(14)

1

fCðm; nÞg

� sx ¼ argmaxfcðx; yÞg sy ðx;yÞ

(15) (16)

In conclusion, by solving Eq. (16), the location of the peak of the cross-power spectrum in Eq. (14) can be obtained, and therefore the translation s between it1 ðx; yÞ and it2 ðx; yÞ can be found (see Eq. (12)). In the rest of the paper, because of the digital nature of the images involved, the discrete case of the FT is employed. More information regarding phase correlation applied to discrete cases with subpixel translation estimation can be found in (Foroosh et al., 2002). 3.3. Current navigation system A detailed treatment of the current navigation system of FeelHippo AUV is beyond the goal of this work; however, a brief description will be provided in the following. In order to retrieve an accurate estimate of the pose of the vehicle with respect to a local NED frame, FeelHippo AUV employs a parallel structure, where attitude is independently estimated from position and it constitutes an input that is fed to the position estimation filter (see Fig. 3). In particular, concerning attitude, the filter structure has been originally proposed in (Mahony et al., 2008), and then suitably modified by the authors in (Allotta et al., 2016a), (Costanzi et al., 2016), whose principle aim is to integrate angular rate changes measured by gyroscopes and correct the obtained values exploiting

(10)

where sx 2 R and sy 2 R are the translation, which can be put together in � � s s ¼ x 2 R2 and θ0 2 R is the rotation angle. Their FT is sy It1 ðm; nÞ ¼ e

j2πðmsx þnsy Þ

where * denotes the complex conjugate. It is easy to note that the inverse FT of Eq. (14) (reported in Eq. (15)) is a 2D Dirac function centered on s and its location can be found with Eq. (16).

More information can be found in (Walter, 2008), (Negaharipour, 2012). An image in the spatial domain is indicated with the lowercase iðx; yÞ, whereas an image in the Fourier domain is expressed with the uppercase Iðm;mÞ. An image collected at a certain absolute time ti 2 R is defined as iti ðx; yÞ or Iti ðm; nÞ in the spatial and the Fourier domains, respectively, where Iti ðm; nÞ ¼ F fiti ðx; yÞg and with F is denoted as the Fourier Transform (FT). The phase correlation algorithm (or its variants), which is a Fourierbased method, ((De Castro and Morandi, 1987) and (Reddy and Chat­ terji, 1996)), has been widely used for registration of roto-translated optical images over the years, such as in (Ojansivu and Heikkila, 2007), (Tzimiropoulos et al., 2010), (Li et al., 2007) or (Eustice et al., 2002) in the underwater domain; differently, its use in FLS is less prevalent. The key concept is based on the so-called Fourier shift property. In the Fourier domain, a shift between two functions (e.g., images) appears as a linear phase shift. Given two images it1 ðx; yÞ and it2 ðx; yÞ with t1 < t2 , let us suppose that it2 ðx; yÞ is a translated and rotated replica of it1 ðx; yÞ sx ;

(13)

It2 ðm; nÞ:

In this case, from Eq. (13), the normalized cross-power spectrum is

The nonlinear model that projects a 3D point P on the point p belonging to the image plane is: � � � � � � 1 x R cos α X p¼ ¼ : (9) ¼ y R sin α cos β Y

it1 ðx; yÞ ¼ it2 ðx cosθ0 þ y sinθ�0 x sinθ0 þ y cosθ0 sy

j2π ðmsx þnsy Þ

(11)

4

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx

F1 ðνÞ ¼

Af Cu ρν21x sgnðν1x Þ ; 2

(18)

where m is the mass of the vehicle, ρ is the density of the water, Af is the frontal area of the vehicle, and Cu is the drag along the surge axis. Generally, the drag effect depends upon, among other things, the speed of the AUV relative to the viscous fluid. In light of Assumption 4, Eq. (18) (where just the velocity of the vehicle is taken into account) can be considered a good approximation. With regard to added masses, acceleration terms are usually numerically negligible: underwater missions for the most part are composed of phases within which the vehicle moves forward at an approximately constant speed. As a consequence, the dry mass m can be considered a good approximation. Assumption 5. Roll and pitch motion cannot be actively controlled (see Section 5.1), but their dynamics are controlled by hydrostatic sta­ bility: their variations are thus limited and can be neglected. Assumption 6. mated with

Fig. 4. FeelHippo AUV during sea trials.

Ti ðν; ui Þ ¼ � � kjui jgðsgnðui ÞVa;i Þ ; ¼ sgnðui Þ ku2i p

accelerometers and magnetometers. With regard to position estimation, FeelHippo AUV is able to navigate exploiting a DR strategy and, addi­ tionally, it can resort to an UKF-based estimator, where a mixed kine­ matic/dynamic vehicle model is proposed. Taking into account longitudinal dynamics, only a reduced computational burden is present on the processing unit. A complete treatment is detailed in (Allotta et al., 2016c), and (Allotta et al., 2017a,b).

where i refers to the i th motor and 8 for x � 0 < 0 gðxÞ ¼ x for 0 < x � jui jp : jui jp for x > jui jp

4. Proposed solution

(19)

(20)

and where p is the propeller pitch, k is a construction parameter that relates motor thrust and propeller speed at bollard conditions (i.e. when the advance speed Va;i ¼ 0), see Eq. (22) and Va;i , which is the speed of the i th motor, can be expressed as a function of the speed vector ν � �� b (21) Va;i ¼ nbT 0b mi ν1 þ ν2 � Pmi

First, the main assumptions on which this work is based are outlined. Second, how to estimate the linear speed of the AUV is broadly treated. Last, how the proposed solution has been integrated in the current navigation framework is described. Assumption 1. Most of the dynamics of the AUV take place in the longitudinal direction. In fact, even if FeelHippo AUV does not properly possess a torpedo shape, it can be easily noted (see Fig. 4) that longi­ tudinal direction is still that of minimal resistance. In order to reduce battery consumption, FeelHippo AUV mainly performs missions along the longitudinal direction.

k¼

� Tðν; uÞ�� 2 u �Va;i ¼0

(22)

Assumption 6 has already been exploited by the authors in (Allotta et al., 2016b) and more information can be found in (Pivano et al., 2009), and (Carlton, 2012).

Assumption 2. Because the body reference frame of FeelHippo AUV is aligned with the vehicle principal axes of inertia, the mass matrix can be considered as diagonal.

Assumption 7. The FLS is positioned at a fixed pose with respect to the vehicle’s frame. In addition to this, the FLS, which is mounted on the bow of the vehicle, is positioned approximately parallel with respect to the sea bottom that is supposed not to change abruptly, in order to insonify wider areas.

Assumption 3. The maximum vehicle speed is not too high­ —approximately 1 m/s (see Table 2)—thus the coupling between the dissipative effects can be neglected, leading to a diagonal damping matrix. In addition, only a quadratic damping term has been taken into account.

Assumption 8. The elevation angle (around 7� –10� for most typical FLS devices) can be considered small. Assumptions 7 and 8 make it possible to take advantage of a 2D approximated model instead of a complete non-linear projection model (see Eq. (9)). In other words, the projection p is substituted with the orthogonal projection b p as reported in Fig. 2; thus, Eq. (9) becomes Eq. (23). 2 3 � � X 1 0 0 4 5 b p� Y (23) 0 1 0 Z

Assumption 4. Gravitational, centripetal, and Coriolis effects have been neglected, and added masses are not considered. Sea currents are hypothesized to be small. Assumptions 1–4 are similar to those made by the authors in (Allotta et al., 2016c). Therefore, the classic dynamic description of (Fossen et al., 1994) and reported in Eq. (6) can be simplified for the longitudinal motion, with Eq. (17). mν_ 1x ¼ mu_ ¼ τ1x ðν; uÞ þ F1 ðνÞ:

The four-quadrant motor characteristic is approxi­

A similar proposal can be found in (Ferreira et al., 2014), (Ferreira �s et al., 2015) and has already been exploited by the et al., 2015), (Hurto authors in (Franchi et al., 2018). In light of the proposed imaging model, and neglecting roll and pitch variations (see Assumption 5), it can be seen that a 3-parameter Euclidean transformation (plane roto-translation) puts two different FLS views of the same object in

(17)

F1 ðνÞ, obtained from the dynamic model in (Fossen et al., 1994), can be further simplified with the aim of reducing the number of parameters, as follows: 5

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx

Section 3.2) is the most suitable to face mutable underwater scenarios. Anyway, to make this method work (it is worth noting that this consideration is not limited to phase correlation), it is necessary for an overlap between the images. Nevertheless, the magnitude of the mini­ mum overlapping area cannot be known a priori. Indeed, it can depend upon the informative content of the insonified area. For example, an overlapping close to 100% can produce incorrect results if the sea bot­ tom in uninformative; on the other hand, an overlap of about 50% can be sufficient if a rich environment is present. For FLS application, �s et al., 2015) and remarkable contributions can be found in (Hurto �s et al., 2013). (Hurto Here, the proposed method is the core of the FLS-based linear speed estimation and it can be divided into three steps. In the first step, raw FLS images are filtered and rotated to be pairwise aligned (Section 4.1.1.1). In the second, the linear translation between two subsequent FLS images is retrieved (Section 4.1.1.2). In the third step, the linear speed estimation is computed (Section 4.1.1.3). 4.1.1.1. Pre-filtering and alignment. For each step, two subsequent raw FLS images (e.g. it1 and it2 ) are collected together with their absolute acquisition times (e.g. t1 and t2 ). The FLS is a device that intrinsically operates with polar variables (namely range R and bearing angle α in Fig. 2), but when the raw acoustic image is created, it is usually represented in the Cartesian space. Therefore, a fan shape, clearly visible in Fig. 6, arises. This particular fan-shaped image presents an abrupt transition between the actual image content (inside the fan) and the background (outside the fan). Thus, when the FT is applied to all the image content, frequency com­ ponents not related to the real image content are inevitably introduced. To overcome this issue, the fan-shaped contour of FLS images is typically smoothed performing a windowing operation (see the right FLS image in Fig. 6) before applying the FT. More information about window func­ tions in the Fourier domain is presented, for example, in (Harris, 1978). Smoothing the raw FLS image implies a slight loss in the image content (see the right FLS image in Fig. 6). However this seems not to have negatively affected the speed estimation method. From Eq. (10), it can be noted that rotations and translations (if both present) are coupled. Generally, it is well known that when quantities are coupled it might be difficult to estimate them separately. Therefore, because of the importance of rotation evaluation, the authors have decided to retrieve them not working on images but relying on specialized sensors and algorithms. That proposed by the authors in (Costanzi et al., 2016), briefly introduced in Section 3.3 and visible in Fig. 5 has therefore been used. By knowing the location of the FLS, pFS in Eq. (7), the rotation center position of FeelHippo AUV and the output of the attitude estimation filter present in Fig. 5, it1 can be rotated to be aligned with it2 . It is worth highlighting that because the acoustic insonification of a scene can produce different results according to the relative attitude between the source (the FLS) and the target, the pro­ posed method may lead to registration issues when rotations are not small. Conversely, given the FLS acquisition rate (usually more than 1 Hz) and the relative slow dynamics of the underwater vehicle, this phenomenon can usually be neglected.

Fig. 5. Block scheme of the proposed strategy. The attitude estimation filter can be found in (Costanzi et al., 2016), whereas the position estimation filter proposed in this work is a DR strategy.

relation (a demonstration can be found for example here (Walter, 2008)). Hence, Fourier-based methods that are known to manage up to similarity transformations (De Castro and Morandi, 1987) can be employed. 4.1. Speed estimation of the vehicle The navigation framework depicted in Fig. 3 is modified to obtain the solution shown in Fig. 5. Therefore, it is clear that the core aim of this paper is to show how to estimate the AUV speed in order to mimic DVL measurements. To this end, the proposed solution utilizes two different approaches. In fact, a generic underwater scenario can present both areas where the sea bot­ tom contains a high informative content (such as wrecks or debris) and regions where the sea floor is invariable and unaltered. Briefly, the case of unavailability of linear speed measurements from the FLS must be handled. Therefore, to address different situations, a dynamic model that exploits a reduced set of parameters is employed when no linear speed measurement from the FLS is available; this complementarity of the two solutions is shown to achieve promising results. 4.1.1. FLS-based speed estimation The key process through which a pair of overlapped images obtained from different viewpoints (namely two images that insonify a common region) are related each other is called registration or, more specifically, pairwise registration. In the last decade, researchers have tried to use feature detection methods both at pixel and region levels, but it is obvious that the presence of stable and conspicuous features is neces­ sary. Although computationally heavy, a phase correlation method (see

Fig. 6. Image windowing procedure. Note the smooth contours on the right image, especially on the top arc. 6

Ocean Engineering xxx (xxxx) xxx

M. Franchi et al.

Fig. 7. The spatial domain representation of the normalized cross-power spectrum. 3D (a) and 2D (b) version. (b) is zoomed in on the peaks. Note the presence of several peaks close to the right one that could negatively affect the peak detection procedure.

Fig. 8. The spatial domain representation of the normalized cross-power spectrum after the application of an LP Butterworth filter. 3D (a) and 2D (b) version. (b) is zoomed in on the main peak. Note the presence of a unique and recognizable peak.

4.1.1.2. Phase correlation for linear translation. The phase correlation technique is performed, and the translation s (see Eq. (16)) between the two subsequent FLS images it1 and it2 is computed from the peak on the normalized cross-power spectrum (Eq. (14), Eq. (15), and Eq. (16)). The typical noise present in FLS images leads to a cross-power spectrum cðx; yÞ with several peaks in the spatial domain, and to over­ come this issue, filtering operations are necessary to Cðm; nÞ before applying Eq. (15). In this work, with the aim of maintaining the computational burden as low as possible, a non-adaptive Low-Pass (LP) Butterworth filter (a commonly used filter in image processing), defined in Eq. (24), has been tested, and further references can be found in (Weeks, 1996). The final tuning parameters have been obtained after testing different combinations. A fourth order Butterworth filter with a cut-off frequency of 110.0 pixels (the image resolution is 894 � 477 pixels) has been found to be the best solution in terms of overall behavior. Because the choice of the cut-off frequency (as well as the filter order) is not adaptive, its performance might have issues when different kinds of sea bottom (e.g. sandy, rocky, or when feature-rich environments are present) are encountered. On the other hand, the achieved results seem not to be significantly affected by this fact (see Section 6), at the same time keeping the complexity low. Hðm; nÞ ¼

� 1þ

1

�2l ;

spectrum gives a measure of the goodness of the obtained translation and a binary acceptance law, based on the value of r in Eq. (25) is performed. In particular, if r is more than 0.97, the registration is deemed correct; otherwise, it is discarded. p ¼ maxfcðx; yÞg r¼1

cðx; yÞ ; p

(25)

where cðx; yÞ is the average value of the normalized cross-power spec­ trum and p is its actual peak. In Fig. 7 and Fig. 8, the normalized crosspower spectrum cðx; yÞ is shown both without and with the above­ mentioned LP filtering action to Cðm; nÞ, respectively. The presence of the LP filter leads to a surface where the peak is clearly visible and thus simpler to detect (see Fig. 8). 4.1.1.3. Body speed estimation. Given the exact (absolute) arrival time of the two FLS images t1 and t2 and the retrieved translation s (see Eq. (15)), the latter is mapped from pixel to meters (or another similar physical quantity) by taking advantage of the following (linear) transformation:

(24)

d D ¼ RMAX ; h

Dðm;nÞ D0

(26) �

� � � dx Dx 2 R2 is a distance in pixel, D ¼ 2 R2 is the same dy Dy physical quantity expressed in meters, RMAX is the maximum range delivered by the FLS, and h is the maximum FLS range expressed as

where d ¼

where Hðm; nÞ is the transfer function, D0 is the cut-off frequency, l is the

filter order, and Dðm; nÞ ¼ ½ðm M=2Þ2 þ ðn N=2Þ2 �1=2 , where M is the number of columns and N is the number of rows. Regardless of the specific filtering approach, the amplitude of the normalized cross-power 7

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx

� first, a test campaign to characterize the propulsion system; � second, a test campaign to characterize the longitudinal drag. With regard to the propulsion model presented in Eq. (19), new tests with the aim of obtaining a complete characterization of FeelHippo AUV propulsion system BlueRobotics T200 (blu, 2019) (see Section 5.1) have been conducted and the experimental work presented by the authors in (Allotta et al., 2017a,b) has been improved. In particular, new tests have been performed with different voltage supplies to gain more insight into the propulsion system. To this end, a new test rig has been designed and developed, see Fig. 11. Furthermore, taking into account dead-zones between the command drive and the motor thrust, the propulsion model presented in Eq. (19) and proposed in (Allotta et al., 2017a,b) has been enhanced, and the final result is shown in Eq. (29). A quantitative evaluation of the analytical expression in Eq. (19) requires the knowledge of the propeller pitch p, which is a construction parameter (usually provided by the manufacturer) and of the coefficient k. This relates motor thrust and propeller speed at bollard conditions (i.e. when the advance speed Va;i ¼ 0), see Eq. (22). Bollard thrust tests have been performed in a testing pool, and the thrust–propeller rotational speed-voltage supply triad has been measured in several working conditions. Voltage supplies have been increased at discrete steps: 12.0, 14.0, 16.0, 22.0, and 25.2 V and for each value, the thrust has been measured varying the propeller rotational speed. Because of the asymmetry of the BlueRobotics T200 propeller, both forward (first quadrant motor oper­ ation) and backward (third quadrant motor operation) tests have been conducted. The complete results are presented in Fig. 12 and Fig. 13 where the bollard coefficients are 0.0128 N2 for the forward motion and for the backward one 0.008753 N2, respectively. In the first case, the coefficient of determination R2 defined in Eq. (28) is 0.9922, whereas in the second it is 0.9639.

Fig. 9. Conversion from pixel to meters (or another similar physical quantity).

number of rows (pixel) in the FLS image (see Fig. 9). The conversion factor RMAX h in Eq. (26) is true for displacement along the rows of the image, but it is an approximation for those along the columns. Exploiting Assumption 1 (most of FeelHippo AUV’s motion takes place along the surge axis) and given the placement of the FLS (see Fig. 15), the majority of the displacements takes place along the row of the images and the approximation can be sufficient. At this point, the body speed estimation can be easily computed.

ν1 ¼

s t2

t1

:

(27)

It should be stressed that in light of the proposed method (Section 3.2), only the speed in the plane xb yb can be obtained. Nevertheless, the speed along the zb -axis can be estimated using the DS. Before using the retrieved speed as input for the navigation filter (see Fig. 5), its value is checked with the maximum performance that FeelHippo AUV is able to carry out. In particular, if the longitudinal speed is more than 1.0 m/s and the lateral speed exceeds 0.2 m/s (see Table 2) the retrieved velocity is discarded; the oldest image, namely it1 , is deleted and a new one is acquired. A complete overview of the proposed method is depicted in Fig. 10.

R2 ¼ 1

SSres ; SStot

(28)

where SSres is the sum of squares of residuals and SStot is the total sum of squares. It is worth highlighting how the exerted thrust does not depend on the voltage supply, but is uniquely influenced by the rotational speed (see Figs. 12 and 13). This is not surprising, as the bollard coefficient is only affected by the propeller and nozzle geometry (given a certain rotational speed). The results presented in Fig. 13 (backward motion) are slightly worse because of the compression localized on the load cell, leading to small skids. This is due to the connection clearance between the load cell and the test rig bar, which adversely affects the

4.1.2. Model-based speed estimation The identification of the most important hydrodynamic parameters is a crucial step in developing a reliable model-based navigation system. Two kinds of identification tests have been undertaken:

Fig. 10. The workflow of the speed estimation method exploiting FLS images. 8

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx

Fig. 11. The experimental test rig set-up. At the top of the page the physical system, whereas at the bottom a schematic representation.

Fig. 12. Bollard thrust tests for the forward motion (first quadrant motor operation).

measurements, see Fig. 11. The experimental test rig set-up was composed of a TAS510 load cell by HT Sensor Technology CO., LTD. (htt, 2019) and a CF350-FFT spectrum analyzer by ONO SOKKI CO., LTD. (ono, 2019) for precise rotational speed measurements for the propeller. To amplify the thrust exerted by the motors mechanically, a leverage system composed of standard aluminum profiles was designed. More specifically, the thrust measurement is obtained as the result of the ratio between the thruster arm and the cell arm and their lengths can be easily regulated by a screw. A schematic representation is visible in Fig. 11b). Currently, during underwater operations, FeelHippo AUV is not equipped with a sensor able to measure the actual propeller rotational speed, such as Hall effect-based speed sensors (see Section 5.1); only its reference value is available. Conversely, exploiting Assumption 4 and noting that the propulsion system normally presents fast dynamics with respect to the relatively slow dynamics of the vehicle counterpart, the committed error can be considered a further approximation with respect to the one already introduced by the four-quadrant motor characteristic in Eq. (19) and Eq. (29) such that, even if it represents a satisfying description, it inevitably introduces unknown model errors. Briefly, reference values for the rotational speed below a certain threshold are not actually followed, where the drive command signal is not sufficient to produce a rotational movement for the thruster. In Fig. 14 it can be cleared noted that the minimum rotational speed that can be actually obtained depends on the voltage supply. Indeed, given a certain command drive logic, the lower the supply voltage, the less the output voltage. Furthermore, the resulting behavior is clearly not affected by the direction of motion. To prove this, the slopes of the lines depicted in Fig. 14 are almost the same (in absolute value), at

0.5974 and 0.5866. This is not surprising, as it is known that a linear relation between rotational speed and voltage supply holds for a Direct Current (DC) motor. The point to highlight from the above treatment is that in case of the unavailability of direct speed measurements, the coupling between thruster and command driver needs to be investigated. In conclusion, Eq. (19) becomes Eq. (29). � � �� kjb u i jgðsgnðb u i ÞVa;i Þ Ti ðν; b ; (29) u i Þ ¼ dðb u ; VÞ sgnðb u i Þ kb u 2i p where b u is the reference value for the rotational speed of the i th motor, V is the voltage supply, and dðxÞ is defined in Eq. (30). 8 < 0 ​ for ​ x � u ðVÞ dðx; VÞ ¼ 0 ​ for ​ x � uþ ðVÞ ; (30) : 1 ​ otherwise ​ where u (backward motion) and uþ (forward motion) are the boundary values for the dead-zone, that depend upon the voltage supply level. If the reference value for the rotational speed falls within this interval, the exerted thrust is zero. For the longitudinal drag, the authors imple­ mented a standard least squares (LS) estimation technique applied to data (longitudinal thrust and longitudinal speed) gathered under steadystate conditions (when the AUV moves forward at approximately con­ stant speed). A complete overview of the procedure developed by UNIFI DIEF (even if applied to another AUV) can be found in (Allotta et al., 2018). A list of the identified main hydrodynamic parameters is reported in Table 1, where V is the voltage supply. 9

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx

Fig. 14. Relation between the boundary values for the dead-zone and the supply voltage level.

Fig. 13. Bollard thrust tests for the backward motion (third quadrant motor operation).

FeelHippo AUV: description First, a brief overview of the mechanical design of FeelHippo AUV and a description of its hardware is reported. 5.1. Mechanical design and hardware description FeelHippo AUV, visible in Fig. 15, has been designed and developed both for the participation in student robotics competitions (e.g., SAUC-E 2013 and ERL SAUC-E in 2018) and for undertaking research topics. Several autonomous underwater navigation missions, principally in shallow waters, have been performed since 2013. The main features of the vehicle are reduced dimensions and weight, making it a compact and reliable underwater platform. A Plexiglass® hull with an internal diameter of 200 mm and 5 mm thickness constitutes the central body of the vehicle, where the nonwatertight hardware and electronics are housed. The connection be­ tween the central part of the vehicle and the two outermost domes is provided by two metal flanges and two O-rings that guarantee a watertight connection. In addition, six metal bars act as tie rods, increasing the overall stiffness results. Four thrusters arranged in a vectored configuration (two on the stern and one each on both lateral sides tilted at 45� ) are connected to Feel­ Hippo AUV by means of custom-made plastic parts. Except for the roll and pitch motion (that are limited by hydrostatic stability), the rest of the Degrees Of Freedom (DOFs) of the vehicle (translational motion and yaw) can be controlled. A list of all the electronic devices and sensor sets with which Feel­ Hippo AUV is equipped is listed as follows:

Fig. 15. FeelHippo AUV in 2019. The FLS (the black device) is mounted on the bow of the vehicle.

� U-blox 7P precision GPS; � Orientus Advanced Navigation AHRS, composed of triaxial acceler­ ometers, gyroscopes, and magnetometers, together with a KVH DSP 1760 single-axis high precision FOG for attitude estimation of the vehicle; � Nortek DVL1000 DVL, measuring linear velocity and acting as DS. The device is located on the bottom of the vehicle close to the CG; � an EvoLogics S2CR 18/34 acoustic mode; � one Teledyne BlueView M900 2D FLS mounted on the bow of the vehicle;

� Intel i-7 Mobile CPU (used for onboard processing); 10

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx

Table 1 FeelHippo AUV main hydrodynamic data. FeelHippo AUV main hydrodynamic parameters A f Cu ρ [N/s2m2] 2 Frontal area (Af ) [m2]

65 0.18

Water density (ρ) [kg/m3] Drag (surge) coefficient (Cu )

1025 0.7046

Propeller pitch (p) [m/rev] Forward bollard coefficient kþ [Ns2]

0.094 0.0128

Forward dead-zone boundary limit uþ [Hz]

4:8833 þ 0:5866ðV

Backward bollard coefficient k [Ns2] Backward dead-zone boundary limit u [Hz]

0.008753 4:8167

0:5974ðV

14Þ 14Þ

Table 2 FeelHippo AUV physical data and performance. FeelHippo AUV main characteristics Dimensions [mm]

approx. 600 � 640 � 500

Dry mass [kg] Max longitudinal speed [m/s] (kn) Max lateral speed [m/s] (kn) Max depth [m] Autonomy [h]

35 approx. 1 (2) approx. 0.2 (0.4) 30 2–3

Fig. 17. Navigation results for the FLS-based solution described in Section 4.1. The ground-truth path is in red and the proposed solution is in blue. “START” and “STOP” indicate the first and last (underwater) point of the ground-truth, respectively; whereas “GPS FIX” stands for the position obtained after resur­ facing. The matching between the proposed solution and the ground-truth can be easily noted. (For interpretation of the references to colour in this figure legend, the reader is referred to the Web version of this article.)

� one WiFi access point Ubiquiti Bullet M2 for fast, high-band, short­ range communication on the sea’s surface; � one radio modem 868 þ by RF Design employed to send short messages from high distances on the sea’s surface; � one bottom-looking ELP 720p MINI IP camera; � one Microsoft Lifecam Cinema forward-looking camera, which can be used for the tele operated guide; � two lateral ELP 1080p MINI IP cameras. 6. Navigation results The results reported in this section refer to the navigation data retrieved during a mission performed at the ERL SAUC-E competition, held in La Spezia (Italy) in July 2018. The mission was executed at the desired depth of 2 m with a reference longitudinal cruise speed of 0.5 m/ s, lasting 960.0 s and covering approximately 220.0 m. In addition, the FLS range was set to 10.0 m. Water current measurements were not available during the day of the trial, but, on the other hand, the test site described here is a small basin at the NATO STO CMRE, where water currents are usually of low intensity and thus negligible. The area of interest was a rectangle with the approximate dimensions

Fig. 18. Navigation results exploiting solely model-based speed estimations. The ground-truth path is in red and the proposed solution is in blue. “START” and “STOP” indicate the first and last (underwater) point of the ground-truth, respectively; whereas “GPS FIX” stands for the position obtained after resur­ facing. (For interpretation of the references to colour in this figure legend, the reader is referred to the Web version of this article.)

36 � 20 m. The underwater scenario presented both areas where the sea bottom contained a high informative content (two underwater struc­ tures, each approximately 2.2 � 3.2 � 1.2 m composed of underwater plastic pipes, were positioned on the sea bottom, see Fig. 16, with some metallic chains also present), and regions where the sea floor was basically unaltered and flat. The vehicle relied on the sensors and the payload described in Sec­ tion 5.1 for the extent of the mission and it performed the required navigation tasks exploiting a DVL-based DR strategy with the navigation filter structure shown in Fig. 3. Moreover, GPS fixes were acquired

Fig. 16. One of the two underwater structures placed on the sea bottom by the competition organizers. 11

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx

Fig. 21. Mean error over time for the tested solutions as defined in Eq. (32). It can be easily noticed how the presence of FLS measurements bounds the error drift.

Fig. 19. Navigation results exploiting solely model-based (without dead-zone) speed estimations. The ground-truth path is in red and the proposed solution is in blue. “START” and “STOP” indicate the first and last (underwater) point of the ground-truth, respectively; whereas “GPS FIX” stands for the position ob­ tained after resurfacing. (For interpretation of the references to colour in this figure legend, the reader is referred to the Web version of this article.)

Fig. 20. Current error for the tested solutions as defined in Eq. (31). For the FLS-based strategy, the maximum error remains below 1.5 m most of the time. During the first (approximately) 100 s, the error is zero because FeelHippo AUV is on surface and the GPS signal is used. The jumps before 1000 s are due to the first GPS fix after resurfacing, so they represents the resurfacing error of the navigation strategies.

before diving and after resurfacing. The DVL-based underwater navi­ gation strategy as well as the GPS fixes were used as ground-truth, to validate the proposed FLS-based solution. It is useful to highlight that although the used ground-truth does not provide an absolute positioning system (except from GPS fixes before diving and after surfacing), the navigation appears reliable because of the small error after resurfacing (around 3.5 m, see Figs. 17– 19f), where the relative value is less than 2% after about 16 min of autonomous navigation. To assess the goodness of the proposed solution, the speed estimation technique reported in Section 4 and explained in Fig. 5 was thus tested and the results can be found in Fig. 17. Here, it should be stressed that the results were obtained in postprocessing. On the other hand, as explained in Section 1, a testing framework as close as possible to a real online test was adopted. Furthermore, with the aim of evaluating the impact of FLS-based speed measurements, a simulation is presented in Fig. 18 where Feel­ Hippo AUV navigates solely exploiting the model described in Section 4.1.2.

Fig. 22. Body-speed comparison along surge axis: readings from Nortek DVL1000 are depicted above in red and employed as ground-truth to validate the proposed solutions (reported below). The error between FLS-based esti­ mations and the ground-truth is in blue, whereas the error between modelbased estimations and the ground-truth is in green. (For interpretation of the references to colour in this figure legend, the reader is referred to the Web version of this article.)

12

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx

Fig. 23. Body-speed comparison along surge axis: readings from Nortek DVL1000 are depicted in red, whereas the FLS-based estimations are in blue and the modelbased ones in green. On the left, the steady-state condition is investigated, whereas on the right speed estimation when FeelHippo AUV turns in place is proposed. From left to right, the first column refers to the transect from eighth to ninth WP, the second to the transect from fourth to fifth WP, the third to the turn on the second WP, and the fourth to the turn on the seventeenth WP.

For the sake of completeness, another simulation that exploits the model in Section 4.1.2 without the introduced dead-zone was per­ formed. The goal was to highlight that in case of unavailability of direct rotational speed measurements for the thrusters, the relationship be­ tween the reference command value provided by the driver and the produced thrust needs to be further investigated. The final results are presented in Fig. 19. The ground-truth navigation was compared with the proposed so­ lution and the results are presented in Fig. 20 and Fig. 21, where it can be easily noted how FLS-based speed measurements improve the overall performance. The employed metrics are defined in Eq. (31) and Eq. (32). � � � � ek ¼ �η1GT (31) η1TS � k

k

P i¼k ek ¼

i¼0 ei

k

estimations, the obtained results, due to erroneous peak detection, exceeded the maximum speed of the vehicle (see Table 2) 16 times over, approximately, 900 registrations (around 1,8%). Thus, in light of the “SPEED CHECK” in Fig. 10 these estimations have not been employed by the navigation filter. From Fig. 22 it is not easy to understand the ad­ vantages and disadvantages of the employed speed estimation methods; therefore, a further comparison is depicted in Fig. 23. To this end, two straight transects and two turns—where FLS-based speed estimations are almost able to cover the full duration of each operation—have been considered (turns on the second and the seventeenth Way-Point (WP) and two straight transects, the first from fourth to fifth WP and the second from eighth to ninth WP, see Fig. 24b) for further information). Almost able to cover means that FLS-based estimations are continuously present for almost all the extent of the considered operation. For each operation, two simulations were compared: one with the model-based speed estimations only, and another with FLS-based speed estimations only. It is worth noting that the plotted results comparing speeds in the same way are delivered to the navigation filter. In particular, model-based speed estimations have the same frequency as the navigation filter, whereas those obtained with the FLS in the bestcase scenario (rich sea bottom) have a frequency equal to the refresh rate of the raw FLS images. Moreover, until the arrival of the next FLS image, the speed is maintained at the previous value. By observing Fig. 23, it can be noted that FLS-based speed estimation works well both during turns and when the vehicle moves forward. Moreover, the model-based solution performs worse when the vehicle does not move at approximately constant speed. This is not surprising, recalling Assumption 4 (added masses are neglected); and, moreover, because dead-zone arises at low rotational speed its nonlinear contri­ butions are more affected here. Finally, how the underwater scenario affects FLS-based speed esti­ mations is presented. In particular, it can be easily understood that in the

;

(32)

where ek 2 Rþ denotes the navigation error at the instant k 2 N, η1GT

k

and η1TS indicate the position of the AUV at instant k with respect to a k

fON xN yN zN g NED frame, according to the ground-truth and to the cur­ rent tested solution, respectively, and ek 2 Rþ is the mean of all the computed errors ek . To gain greater insights into the proposed results, further analysis is presented. A comparison among the model-based estimated speed, the FLSbased one and DVL readings is shown in Fig. 22. Because most of the vehicle dynamics take place on the surge axis, the reported speeds are those occurring along this axis. The figure depicts what happens for the extent of the underwater mission. It is worth noting that for the first (approximately) 100 s FeelHippo AUV is on the surface; thus, only DVL readings are reported. In addition, for what concerns FLS-based speed 13

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx

exploiting an FLS-based DR strategy without using DVL readings, but not obstructing cooperation. The navigation approach has been tested and validated through data gathered during sea trials undertaken in La Spezia (Italy) at the NATO STO CMRE in July 2018. The presented solution is shown to achieve a mean error of approximately 0.9 m with a maximum error of approxi­ mately 2.0 m with respect to a ground-truth solution constituted by a DVL-based DR strategy with GPS fixes, before diving and after resurfacing. In Section 6, it is shown how speed estimates from FLS improve a simple model-based navigation system (see Fig. 17, Fig. 18, Figs. 19, Fig.20, and Fig. 21). Indeed, a simple dynamic model, by virtue of the introduced assumptions (Assumptions 1–6), obviously cannot fully describe the complex nonlinearities that arise during the motion, being too simple a solution. Future developments will involve testing of the navigation strategy on FeelHippo AUV during experimental campaigns. Additionally, the authors are planning to integrate the proposed solution into the UKFbased navigation filter already developed by them in (Allotta et al., 2016c). In fact, the presented technique tries to exploit (primarily) FLS measurements, whereas model-based estimations only possess a com­ plementary role. Nevertheless, the same importance is given to each contribution (both from FLS and from model), but this situation does not accurately describe the reality. Hence, a probabilistic description of the system, by means for example of an UKF estimator, may outperform the current proposed solution. Ultimately, reliable rotation estimation via FLS images is a subject worth further investigation. Accordingly, the obtained AUV would be a compact platform capable of navigating with an even more reduced set of on-board sensors. Declaration of competing interest The authors declare that they have no known competing financial interests or personal relationships that could have appeared to influence the work reported in this paper. Acknowledgement The research leading to these results has been partially supported by the European project EUMarineRobots (EUMR), which received funding from the European Union’s Horizon 2020 research and innovation program under grant agreement No 731103. Fig. 24. Navigation results for the FLS-based solution described in Section 4.1.

References

first part of the mission the error remains bounded to values below 1 m. Here, the speed from the FLS is present most of the time, due to the pier basin inside the NATO STO CMRE, as noted in Fig. 24b). Indeed, the pier makes the FLS images a rich source of information. After, because of the presence of less informative sea bottom areas, the error grows due to several model-based speed estimations. In addition, when FeelHippo AUV is close to the two underwater structures, the error is small again and comparable with that identified in the first part of the mission. This can be understood by observing the location of the two underwater structures in Fig. 24b) and the corresponding navigation error (see the rectangle 0 m/10 m East and 25 m/-20 m North in Fig. 24a). In conclusion, if the sea bottom is reach and FLS-based speed estimation can be performed, the obtained results present good performance. Otherwise, when the dynamic model is used several times, the overall estimation performance becomes worse.

Reddy, B.S., Chatterji, B.N., 1996. An fft-based technique for translation, rotation, and scale-invariant image registration. IEEE Trans. Image Process. 5, 1266–1271. Allotta, B., Caiti, A., Costanzi, R., Fanelli, F., Fenucci, D., Meli, E., Ridolfi, A., 2016. A new auv navigation system exploiting unscented kalman filter. Ocean. Eng. 113, 121–132. Allotta, B., Costanzi, R., Fanelli, F., Monni, N., Ridolfi, A., 2016. Underwater vehicles attitude estimation in presence of magnetic disturbances. In: Robotics and Automation (ICRA), 2016 IEEE International Conference on. IEEE, pp. 2612–2617. Allotta, B., Caiti, A., Chisci, L., Costanzi, R., Di Corato, F., Fantacci, C., Fenucci, D., Meli, E., Ridolfi, A., 2016. An unscented kalman filter based navigation algorithm for autonomous underwater vehicles. Mechatronics 39, 185–195. Allotta, B., Conti, R., Costanzi, R., Fanelli, F., Gelli, J., Meli, E., Monni, N., Ridolfi, A., Rindi, A., 2017. A low cost autonomous underwater vehicle for patrolling and monitoring. Proc. Inst. Mech. Eng. M J. Eng. Marit. Environ. 231, 740–749. Allotta, B., Costanzi, R., Fanelli, F., Monni, N., Paolucci, L., Ridolfi, A., 2017. Sea currents estimation during auv navigation using unscented kalman filter. IFAC-PapersOnLine 50, 13668–13673. Allotta, B., Costanzi, R., Pugi, L., Ridolfi, A., 2018. Identification of the main hydrodynamic parameters of typhoon auv from a reduced experimental dataset. Ocean. Eng. 147, 77–88. Aykin, M., Negahdaripour, S., 2012. On feature extraction and region matching for forward scan sonar imaging. In: 2012 Oceans. IEEE, pp. 1–9. Bahr, A., Leonard, J.J., Fallon, M.F., 2009. Cooperative localization for autonomous underwater vehicles. Int. J. Robot. Res. 28, 714–728. Bar-Shalom, Y., Li, X.R., Kirubarajan, T., 2004. Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software. John Wiley & Sons.

7. Conclusion This paper illustrates how FeelHippo AUV is able to navigate 14

M. Franchi et al.

Ocean Engineering xxx (xxxx) xxx Mahony, R., Hamel, T., Pflimlin, J.-M., 2008. Nonlinear complementary filters on the special orthogonal group. IEEE Trans. Autom. Control 53, 1203–1218. Mallios, A., Ridao, P., Ribas, D., Hern� andez, E., 2014. Scan matching slam in underwater environments. Aut. Robots 36, 181–198. Melo, J., Matos, A., 2017. Survey on advances on terrain based navigation for autonomous underwater vehicles. Ocean. Eng. 139, 250–264. Miller, P.A., Farrell, J.A., Zhao, Y., Djapic, V., 2010. Autonomous underwater vehicle navigation. IEEE J. Ocean. Eng. 35, 663–678. Negaharipour, S., 2012. On 3-d scene interpretation from fs sonar imagery. In: Oceans. IEEE, pp. 1–9, 2012. Norgren, P., Skjetne, R., 2018. A Multibeam-Based Slam Algorithm for Iceberg Mapping Using Auvs. IEEE Access. Official website of HT sensor Technology CO.,LTD [Online; accessed April 2019]. http ://www.htc-sensor.com/products/156.html. Official website of ONO SOKKI CO.,LTD [Online; accessed April 2019]. https://www.ono sokki.co.jp/English/english.htm. Ojansivu, V., Heikkila, J., 2007. Image registration using blur-invariant phase correlation. IEEE Signal Process. Lett. 14, 449–452. Paull, L., Saeedi, S., Seto, M., Li, H., 2014. Auv navigation and localization: a review. IEEE J. Ocean. Eng. 39, 131–149. Pivano, L., Johansen, T.A., Smogeli, Ø.N., 2009. A four-quadrant thrust estimation scheme for marine propellers: theory and experiments. IEEE Trans. Control Syst. Technol. 17, 215–226. Rahman, S., Li, A.Q., Rekleitis, I., 2018. Sonar visual inertial slam of underwater structures. In: IEEE International Conference on Robotics and Automation (ICRA). IEEE, pp. 1–7, 2018. Ribas, D., Ridao, P., Tard� os, J.D., Neira, J., 2008. Underwater slam in man-made structured environments. J. Field Robot. 25, 898–921. Ribas, D., Ridao, P., Neira, J., 2010. Underwater SLAM for Structured Environments Using an Imaging Sonar, vol. 65. Springer. Ruiz, I.T., De Raucourt, S., Petillot, Y., Lane, D.M., 2004. Concurrent mapping and localization using sidescan sonar. IEEE J. Ocean. Eng. 29, 442–456. Song, S., Kim, B., Kim, J., Yu, S.-C., 2018. Real-time reliability evaluation of optical and acoustic images for feature-based localization of auv. In: 2018 OCEANS-MTS/IEEE Kobe Techno-Oceans (OTO). IEEE, pp. 1–6. Tan, Y.T., Gao, R., Chitre, M., 2014. Cooperative path planning for range-only localization using a single moving beacon. IEEE J. Ocean. Eng. 39, 371–385. Tzimiropoulos, G., Argyriou, V., Zafeiriou, S., Stathaki, T., 2010. Robust fft-based scaleinvariant image registration with image gradients. IEEE Trans. Pattern Anal. Mach. Intell. 32, 1899–1906. Valencia, R., Andrade-Cetto, J., 2018. Mapping, Planning and Exploration with Pose SLAM. Springer. Walter, M.R., 2008. Sparse Bayesian Information Filters for Localization and Mapping. Technical Report. Woods Hole Oceanographic Institution MA Dept of Ocean Engineering. Walter, M., Hover, F., Leonard, J., 2008. Slam for ship hull inspection using exactly sparse extended information filters. In: Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on, IEEE, pp. 1463–1470. Webster, S.E., Walls, J.M., Whitcomb, L.L., Eustice, R.M., 2013. Decentralized extended information filter for single-beacon cooperative acoustic navigation: theory and experiments. IEEE Trans. Robot. 29, 957–974. Weeks, A.R., 2019. Fundamentals of electronic image processing, SPIE optical engineering press Bellingham, 1996. Official website of blue robotics Inc. [Online; accessed April 2019]. https://www.bluerobotics.com/. White, C., Hiranandani, D., Olstad, C.S., Buhagiar, K., Gambin, T., Clark, C.M., 2010. The Malta cistern mapping project: underwater robot mapping and localization within ancient tunnel systems. J. Field Robot. 27, 399–411. Yan, W., Chen, W., Cui, R., 2015. Moving long baseline positioning algorithm with uncertain sound speed. J. Mech. Sci. Technol. 29, 3995–4002. Yoerger, D.R., Jakuba, M., Bradley, A.M., Bingham, B., 2007. Techniques for deep sea near bottom survey using an autonomous underwater vehicle. Int. J. Robot. Res. 26, 41–54.

Barclay, L., 2003. Propagation of Radiowaves, 2. Iet. Bishop, A.N., Fidan, B., Anderson, B.D., Do� gançay, K., Pathirana, P.N., 2010. Optimality analysis of sensor-target localization geometries. Automatica 46, 479–492. Carlton, J., 2012. Marine Propellers and Propulsion. Butterworth-Heinemann. Chaves, S.M., Galceran, E., Ozog, P., Walls, J.M., Eustice, R.M., 2017. Pose-graph slam for underwater navigation. In: Sensing and Control for Autonomous Vehicles. Springer, pp. 143–160. Costanzi, R., Fanelli, F., Monni, N., Ridolfi, A., Allotta, B., 2016. An attitude estimation algorithm for mobile robots under unknown magnetic disturbances. IEEE ASME Trans. Mechatron. 21, 1900–1911. Costanzi, R., Fanelli, F., Meli, E., Ridolfi, A., Caiti, A., Allotta, B., 2018. Ukf-based navigation system for auvs: online experimental validation. IEEE J. Ocean. Eng. 1–9. Curcio, J., Leonard, J., Vaganay, J., Patrikalakis, A., Bahr, A., Battle, D., Schmidt, H., Grund, M., 2005. Experiments in moving baseline navigation using autonomous surface craft. In: Proceedings of OCEANS 2005 MTS/IEEE. IEEE, pp. 730–735. De Castro, E., Morandi, C., 1987. Registration of Translated and Rotated Images Using Finite Fourier Transforms. IEEE Transactions on pattern analysis and machine intelligence, pp. 700–703. Durrant-Whyte, H., Bailey, T., 2006. Simultaneous localization and mapping: part i. IEEE Robot. Autom. Mag. 13, 99–110. Eustice, R., Pizarro, O., Singh, H., Howland, J., 2002. Uwit: underwater image toolbox for optical image processing and mosaicking in matlab. In: Underwater Technology, 2002. Proceedings of the 2002 International Symposium on. IEEE, pp. 141–145. Ferreira, F., Djapic, V., Micheli, M., Caccia, M., 2014. Improving automatic target recognition with forward looking sonar mosaics. IFAC Proc. Vol. 47, 3382–3387. Ferreira, F., Djapic, V., Micheli, M., Caccia, M., 2015. Forward looking sonar mosaicing for mine countermeasures. Annu. Rev. Contr. 40, 212–226. Ferri, G., Ferreira, F., Djapic, V., 2015. Boosting the talent of new generations of marine engineers through robotics competitions in realistic environments: the sauc-e and eurathlon experience. In: OCEANS 2015-Genova. IEEE, pp. 1–6. Ferri, G., Ferreira, F., Djapic, V., 2017. Multi-domain robotics competitions: the cmre experience from sauc-e to the european robotics league emergency robots. In: OCEANS 2017-Aberdeen. IEEE, pp. 1–7. Foroosh, H., Zerubia, J.B., Berthod, M., 2002. Extension of phase correlation to subpixel registration. IEEE Trans. Image Process. 11, 188–200. Fossen, T.I., et al., 1994. Guidance and Control of Ocean Vehicles, vol. 199. Wiley, New York. Franchi, M., Ridolfi, A., Zacchini, L., 2018. A forward-looking sonar-based system for underwater mosaicing and acoustic odometry. In: Autonomous Underwater Vehicles (AUV), 2018. IEEE/OES, IEEE (in press). Harris, F.J., 1978. On the use of windows for harmonic analysis with the discrete fourier transform. Proc. IEEE 66, 51–83. Hidalgo, F., Br€ aunl, T., 2015. Review of underwater slam techniques. In: Automation, Robotics and Applications (ICARA), 2015 6th International Conference on. IEEE, pp. 306–311. Hurt� os, N., Nagappa, S., Cufí, X., Petillot, Y., Salvi, J., 2013. Evaluation of registration methods on two-dimensional forward-looking sonar imagery. In: OCEANS-Bergen, 2013 MTS/IEEE. IEEE, pp. 1–8. Hurt� os, N., Ribas, D., Cufí, X., Petillot, Y., Salvi, J., 2015. Fourier-based registration for robust forward-looking sonar mosaicing in low-visibility underwater environments. J. Field Robot. 32, 123–151. Johannsson, H., Kaess, M., Englot, B., Hover, F., Leonard, J., 2010. Imaging sonar-aided navigation for autonomous underwater harbor surveillance. In: Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on. IEEE, pp. 4396–4403. Kalman, R.E., 1960. A new approach to linear filtering and prediction problems. J. Basic Eng. 82, 35–45. Leonard, J.J., Bahr, A., 2016. Autonomous underwater vehicle navigation. In: Springer Handbook of Ocean Engineering. Springer, pp. 341–358. Li, L., Qu, Z., Zeng, Q., Meng, F., 2007. A novel approach to image roto-translation estimation. In: Automation and Logistics, 2007 IEEE International Conference on. IEEE, pp. 2612–2616. Li, J., Kaess, M., Eustice, R.M., Johnson-Roberson, M., 2018. Pose-graph slam using forward-looking sonar. IEEE Robot. Autom. Lett. 3, 2330–2337.

15