Development and Testing of Navigation Algorithms for Autonomous Underwater Vehicles [1st ed.] 978-3-030-15595-7;978-3-030-15596-4

Table of contents :
Front Matter ....Pages i-xix
Introduction (Francesco Fanelli)....Pages 1-11
Involved Vehicles (Francesco Fanelli)....Pages 13-24
Mathematical Background (Francesco Fanelli)....Pages 25-39
Navigation Filter (Francesco Fanelli)....Pages 41-71
Results (Francesco Fanelli)....Pages 73-93
Conclusion (Francesco Fanelli)....Pages 95-96
Back Matter ....Pages 97-97

Citation preview

Springer Theses Recognizing Outstanding Ph.D. Research

Francesco Fanelli

Development and Testing of Navigation Algorithms for Autonomous Underwater Vehicles

Springer Theses Recognizing Outstanding Ph.D. Research

Aims and Scope The series “Springer Theses” brings together a selection of the very best Ph.D. theses from around the world and across the physical sciences. Nominated and endorsed by two recognized specialists, each published volume has been selected for its scientific excellence and the high impact of its contents for the pertinent field of research. For greater accessibility to non-specialists, the published versions include an extended introduction, as well as a foreword by the student’s supervisor explaining the special relevance of the work for the field. As a whole, the series will provide a valuable resource both for newcomers to the research fields described, and for other scientists seeking detailed background information on special questions. Finally, it provides an accredited documentation of the valuable contributions made by today’s younger generation of scientists.

Theses are accepted into the series by invited nomination only and must fulfill all of the following criteria • They must be written in good English. • The topic should fall within the confines of Chemistry, Physics, Earth Sciences, Engineering and related interdisciplinary fields such as Materials, Nanoscience, Chemical Engineering, Complex Systems and Biophysics. • The work reported in the thesis must represent a significant scientific advance. • If the thesis includes previously published material, permission to reproduce this must be gained from the respective copyright holder. • They must have been examined and passed during the 12 months prior to nomination. • Each thesis should include a foreword by the supervisor outlining the significance of its content. • The theses should have a clearly defined structure including an introduction accessible to scientists not expert in that particular field.

More information about this series at http://www.springer.com/series/8790

Francesco Fanelli

Development and Testing of Navigation Algorithms for Autonomous Underwater Vehicles Doctoral Thesis accepted by University of Florence, Italy

123

Author Dr. Francesco Fanelli Department of Industrial Engineering University of Florence Florence, Italy

Supervisor Prof. Benedetto Allotta Department of Industrial Engineering University of Florence Florence, Italy

ISSN 2190-5053 ISSN 2190-5061 (electronic) Springer Theses ISBN 978-3-030-15595-7 ISBN 978-3-030-15596-4 (eBook) https://doi.org/10.1007/978-3-030-15596-4 Library of Congress Control Number: 2019935544 © Springer Nature Switzerland AG 2020 This work is subject to copyright. All rights are reserved by the Publisher, whether the whole or part of the material is concerned, specifically the rights of translation, reprinting, reuse of illustrations, recitation, broadcasting, reproduction on microfilms or in any other physical way, and transmission or information storage and retrieval, electronic adaptation, computer software, or by similar or dissimilar methodology now known or hereafter developed. The use of general descriptive names, registered names, trademarks, service marks, etc. in this publication does not imply, even in the absence of a specific statement, that such names are exempt from the relevant protective laws and regulations and therefore free for general use. The publisher, the authors and the editors are safe to assume that the advice and information in this book are believed to be true and accurate at the date of publication. Neither the publisher nor the authors or the editors give a warranty, expressed or implied, with respect to the material contained herein or for any errors or omissions that may have been made. The publisher remains neutral with regard to jurisdictional claims in published maps and institutional affiliations. This Springer imprint is published by the registered company Springer Nature Switzerland AG The registered company address is: Gewerbestrasse 11, 6330 Cham, Switzerland

To my family

Supervisor’s Foreword

With an ever increasing interest for what lies below the ocean’s surface, the use of autonomous underwater robots is rapidly becoming a common practice, both within industry and academia. Nonetheless, the demanding accuracy requirements needed to successfully complete autonomous tasks in such a hostile environment call for precise and reliable navigation systems. Addressing the abovementioned issues, this thesis focuses on the study of self-localization techniques for underwater robots. In particular, exploiting only sensors which are commonly mounted on board underwater vehicles (thus not requiring external instrumentation, which comes with relevant cost and deployment time), attitude and position estimation algorithms are derived. The theoretical argumentation, illustrated with clarity and scientific rigor, is paired with a considerable share of validation results composed of simulation results exploiting real navigation data, or field validation tests aimed at assessing the effectiveness of the developed solutions in a real-world scenario. Indeed, field testing constitutes a relevant share of the research activity described in this thesis, giving value and significance to the whole work: the developed navigation algorithms, successfully validated, pave the way for additional research activity, and practical field application in a wide variety of sectors. Florence, Italy October 2018

Prof. Benedetto Allotta

vii

Parts of this thesis have been published in the following documents: Journals Costanzi R., Fanelli F., Meli E., Ridolfi A., Caiti A., Allotta B., UKF-Based Navigation System for AUVs: Online Experimental Validation, IEEE Journal of Oceanic Engineering, Vol. PP, Issue 99, pp. 1–9 (2018). Costanzi R., Fanelli F., Monni N., Ridolfi A., Allotta B., An Attitude Estimation Algorithm for Mobile Robots Under Unknown Magnetic Disturbances, IEEE/ASME Transactions on Mechatronics, Vol. 21, pp 1900–1911, Apr. (2016). Allotta B., Caiti A., Costanzi R., Fanelli F., Fenucci D., Meli E., Ridolfi A., A new AUV navigation system exploiting unscented Kalman filter, Journal of Ocean Engineering, Vol. 113, pp. 121–132 (2016). Allotta B., Costanzi R., Fanelli F., Monni N., Ridolfi A., Single axis FOG aided attitude estimation algorithm for mobile robots, Journal of Mechatronics, Vol. 30, pp. 158–173 (2015). International Conferences Allotta B., Costanzi R., Fanelli F., Monni N., Paolucci L., Ridolfi A., Sea currents estimation during AUV navigation using Unscented Kalman Filter, Proceedings of the 20th IFAC World Congress, Toulouse (FR), pp. 13668–13673, July 9–14 (2017). Costanzi R., Fanelli F., Ridolfi A., Allotta B., Simultaneous navigation state and sea current estimation through augmented state Unscented Kalman Filter, Proceedings of the MTS/IEEE OCEANS’16 Monterey, Monterey (CA, US), Sept. 19–22 (2016). Allotta B., Costanzi R., Fanelli F., Monni N., Ridolfi A., Underwater Vehicles Attitude Estimation in presence of Magnetic Disturbances, Proceedings of the IEEE International Conference on Robotics and Automation, Stockholm (SE), pp. 2612– 2617, May 16–21 (2016). Allotta B., Caiti A., Costanzi R., Fanelli F., Fenucci D., Meli E., Ridolfi A., Unscented Kalman Filtering for Autonomous Underwater Vehicles, Proceedings of ECCOMAS MARINE 2015, Rome, June 15–17 (2015). Allotta B., Caiti A., Chisci L., Costanzi R., Di Corato F., Fanelli F., Fantacci C., Fenucci D., Meli E., Ridolfi A., A comparison between EKF-based and UKF-based navigation algorithms for AUVs localization, Proceedings of the MTS/IEEE OCEANS’15 Genova, Genova (IT), May 18–21 (2015). Book Chapters Caiti A., Costanzi R., Fenucci D., Allotta B., Fanelli F., Monni N., Ridolfi A., Marine Robots in Environmental Surveys: Current Developments at ISME— Localisation and Navigation, Marine Robotics and Applications, pp. 69–86 (2018).

ix

Acknowledgements

First and foremost, I would like to thank my supervisor, Prof. Benedetto Allotta, whose knowledge and commitment allowed me to work in the field of robotics for 3 years, making a longlasting wish come true. These very few lines will never be enough to express my gratitude. Special thanks go to my colleagues and friends, past and present, of the Mechatronics and Dynamic Modeling Laboratory of the Department of Industrial Engineering of the University of Florence. I have spent only little time with some of them, while others have mentored me throughout this whole journey; in both cases, the distinction between colleague and friend soon ceased to exist, as we shared experiences ranging from the frustration of failed attempts to the excitement of witnessing that something you spent much time and effort into works as planned. For their loyalty, I am grateful to my lifetime friends; times may change, but the trust I have in them will always be the same. Finally, I would like to thank my family for supporting and loving me during the highs and lows of these years. To all the people mentioned here goes my sincere gratitude, for they have contributed to shape the person I am and the one that I want to be. Francesco Fanelli

xi

Contents

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.1 Overall Framework . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 State of the Art of Underwater Navigation Techniques 1.3 Contribution and Thesis Structure . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

1 2 4 7 8

2 Involved Vehicles . . . . . . . 2.1 Typhoon Class AUVs . 2.2 MARTA AUV . . . . . . 2.3 FeelHippo AUV . . . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

13 14 16 19

3 Mathematical Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 AUV Kinematic and Dynamic Model . . . . . . . . . . . . . . . . . . 3.1.1 AUV Dynamic Model in the Presence of Sea Currents 3.2 Unscented Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2.1 Unscented Transform . . . . . . . . . . . . . . . . . . . . . . . . . 3.2.2 Unscented Kalman Filter Algorithm . . . . . . . . . . . . . . 3.3 Sensors Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

25 25 29 30 32 34 34 39

4 Navigation Filter . . . . . . . . . . . . . . . . . . . . . . . 4.1 Attitude Estimation Filter . . . . . . . . . . . . . 4.1.1 Magnetometer Calibration . . . . . . . 4.1.2 Attitude Estimator Design Changes . 4.2 Position Estimation Filter . . . . . . . . . . . . . 4.2.1 AUV State-Space Model . . . . . . . . 4.2.2 Sea Current Estimation . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

41 42 44 53 60 61 64 70

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . . . . . .

. . . .

. . . . . . . .

. . . .

. . . . . . . .

. . . .

. . . . . . . .

. . . .

. . . . . . . .

. . . .

. . . . . . . .

. . . .

. . . . . . . .

. . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

xiii

xiv

Contents

. . . . .

73 73 83 87 93

6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

95

Appendix: Author’s Biography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

97

5 Results . . . . . . . . . . . . . . . 5.1 Attitude Estimator . . . 5.2 Position Estimator . . . 5.3 Sea Current Estimator . References . . . . . . . . . . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

Nomenclature

Measurement units for non-uniform quantities are not reported. A 1 in the units field denotes a unitless quantity. Af ;i Cð”Þ CA ð”Þ CD; i CRB ð”Þ Dð”Þ K M MA MRB Mm N P Q R RNB ð·2 Þ RNU TBN ð·2 Þ V W X Y Z dNP dNU

Projection of the area of the hull on a plane perpendicular to the i-axis (m2 ) Centripetal and Coriolis effects matrix Added centripetal and Coriolis effects matrix i-axis drag coefficient (1) Rigid body centripetal and Coriolis effects matrix Hydrodynamic damping effects matrix Body-fixed x-axis torque (Nm) Body-fixed y-axis torque (Nm) Added mass matrix Rigid body mass and inertia matrix Mass and inertia matrix Body-fixed z-axis torque (Nm) State covariance Process noise covariance Measurement noise covariance Earth-fixed frame to body-fixed frame rotation matrix (1) Earth-fixed frame to USBL-fixed frame rotation matrix (1) Angular velocity to ·2 time derivative transformation matrix (1) Volume (m3 ) Magnetometer Soft Iron, Scale Factor, and Misalignment effect (1) Body-fixed x-axis force (N) Body-fixed y-axis force (N) Body-fixed z-axis force (N) Earth-fixed frame GPS measurement noise (m) Earth-fixed frame USBL measurement noise (m) xv

xvi

da dg dm dv · ·1 ·2 ” ”1 ”2 ” Nc; h ”c ” Nc ”r !BIMU !c ¿ ¿1 ¿2 dd df fO B x B y B z B g fO N x N y N z N g !cFOG !m FOG !r ; !m ; Xc / ˆ q BB HN;B ? HN Hd PGPS PNGPS PNUSBL PNU WB aBIMU af bg

Nomenclature

IMU acceleration measurement noise (m/s2 ) IMU angular velocity measurement noise (rad/s) Compass magnetic field measurement noise (AU) DVL measurement noise (m/s) Earth-fixed pose Earth-fixed position (m) Earth-fixed orientation (rad) Body-fixed velocity Body-fixed linear velocity (m/s) Body-fixed angular velocity (rad/s) North and East current components (m/s) Body-fixed current velocity Earth-fixed current velocity Relative velocity IMU gyroscope bias (rad/s) Correction term of the attitude estimation filter (rad/s) Body-fixed vector of forces and torques Body-fixed force (N) Body-fixed torque (Nm) Depth sensor measurement noise (m) FOG measurement noise (rad/s) Body-fixed reference frame Earth-fixed reference frame FOG measured angular rate after compensation of Earth’s angular rate effect (rad/s) FOG measured angular rate (rad/s) Weights of the Unscented Transform (1) Roll angle (rad) Yaw angle (rad) Water density (kg/m3 ) Body frame buoyancy (N) Body frame estimate of Earth’s magnetic field projected on the plane orthogonal to acceleration (T) Earth’s magnetic field (T) Magnetometer Hard Iron effect (AU) GPS fix Earth-fixed frame GPS measured position (m) Earth-fixed frame USBL measured position (m) Earth-fixed frame USBL position (m) Body frame gravitational force (N) IMU measured acceleration (m/s2 ) Filtered accelerometer measurements (m/s2 ) IMU measured angular velocity (rad/s)

Nomenclature

g g· ð·Þ mB mc mc? rBb u v vBDVL w x y h N dDS  diag Ix ; Iy ; Iz m p q r rH;h t u v w x y z

xvii

Gravitational acceleration (m/s2 ) Gravitational and buoyancy effects vector Compass measured magnetic field (AU) Calibrated magnetic field measurements (T) Projection of calibrated magnetic field measurements on the plane orthogonal to acceleration (T) Body frame position of the center of buoyancy (m) Control input vector Measurement noise DVL measured velocity (m/s) Process noise State vector Measurement vector Pitch angle (rad) Depth sensor measured depth (m) Principal inertia matrix (kgm2 ) Mass (kg) Body-fixed x-axis angular velocity (rad/s) Body-fixed y-axis angular velocity (rad/s) Body-fixed z-axis angular velocity (rad/s) Magnitude of the horizontal projection of the magnetic field (T) Time (s) Body-fixed x-axis linear velocity (surge motion) (m/s) Body-fixed y-axis linear velocity (sway motion) (m/s) Body-fixed z-axis linear velocity (heave motion) (m/s) Earth-fixed x-axis position (m) Earth-fixed y-axis position (m) Earth-fixed z-axis position (m)

Acronyms

ADCP AHRS AUV DS DVL EKF FOG GNC GPS IMU INS KF LBL MEMS MMSE NECF NED ROV RV SNAME UKF USBL UT UUV

Acoustic Doppler Current Profiler Attitude and Heading Reference System Autonomous Underwater Vehicle Depth Sensor Doppler Velocity Log Extended Kalman Filter Fiber Optic Gyroscope Guidance, Navigation, and Control Global Positioning System Inertial Measurement Unit Inertial Navigation System Kalman Filter Long BaseLine Micro-Electro-Mechanical Systems Minimum Mean Square Error Nonlinear Explicit Complementary Filter North, East, and Down Remotely Operated Vehicle Random Variable Society of Naval Architects and Marine Engineers Unscented Kalman Filter Ultra Short BaseLine Unscented Transform Unmanned Underwater Vehicle

xix

Chapter 1

Introduction

Over the past few decades, the field of marine engineering witnessed a significant growth. The exploration of what lies below the surface and the exploitation of the resources available in the ocean depths attracted (and continue to attract) scientists and businessmen in equal measure. Regardless of the specific background, people involved in underwater operations often resort to the aid of robots, since the environment they work in is essentially hostile to humans. Wave disturbances, sea currents, high pressure, scarce visibility (due to suspended particles or to insufficient light penetration) are only some of the disruptions that operators must face during at-sea activities. To increase the success chance of undersea tasks, many different underwater robots have been developed throughout the years, both manned and unmanned. Among Unmanned Underwater Vehicle (UUVs), a first classification can be made according to operational mode: teleoperated vehicles, referred to as Remotely Operated Vehicle (ROVs), are constantly connected by a cable to both their power supply and to a control station, where a specifically trained operator exploits feedback acquired by onboard sensors (and transmitted through the connection cable) to guide the vehicle; alternatively, as with many modern ROVs, the cable is used for data exchange only while the necessary power is stored on board using batteries. On the other side, the majority of Autonomous Underwater Vehicle (AUVs) are completely autonomous (actually, a few examples of acoustically controlled AUVs exist): such vehicles do not require human intervention (except for deployment and recovery), are usually equipped with electric batteries and possess dedicated systems used to control their motion. Historically, the first AUVs were employed in the military field; nowadays, such vehicles can be considered a cost-affordable solution for many applications in the most diverse sectors. Just to name a few examples: they are employed in the Oil & Gas sector to carry out high-depth tasks in environments which are unsuitable for divers; they find application in the field of underwater geology, biology, and archeology, © Springer Nature Switzerland AG 2020 F. Fanelli, Development and Testing of Navigation Algorithms for Autonomous Underwater Vehicles, Springer Theses, https://doi.org/10.1007/978-3-030-15596-4_1

1

2

1 Introduction

where the payload they are often equipped with is used for data collection and detection and monitoring activities of targets of interests on the seabed; they are even employed for search and rescue tasks. Indeed, there are numerous companies commercializing autonomous vehicles, while scientists and researchers develop their own prototypes or employ commercial products to test novel algorithms. To satisfy the demanding accuracy requisites needed to successfully complete the above-mentioned tasks (or, generally speaking, all kinds of missions AUVs are commonly employed for), autonomous vehicles require the availability of precise and reliable navigation and self-localization systems. Indeed, underwater localization and autonomous navigation have been considered challenging research topics by the scientific community for many years, and they continue to draw scientists’ attention. Many navigation strategies and different algorithms have been developed over the years, trying to establish efficient strategies to cope with the difficulties posed by the marine environment; the research activity carried out during the Ph.D. period headed in that direction, aiming at improving existing pose estimation algorithms or at validating new solutions suitable for the underwater field. The remainder of this chapter is organized as follows: Sect. 1.1 describes the framework in which the research activity took place, while a review of significant contributions on the topic of underwater navigation and localization, which also serves as a motivation for the algorithms developed during the Ph.D. period, is reported in Sect. 1.2. Finally, Sect. 1.3 states the main contribution of the research and illustrates the structure of the thesis.

1.1 Overall Framework The research activity was conducted at the Mechatronics and Dynamic Modeling Laboratory (MDM Lab) of the Department of Industrial Engineering of the University of Florence (DIEF). The MDM Lab has been active in the field of underwater robotics since 2011, thanks to the participation in the Tuscany region project TecnicHe per l’Esplorazione Sottomarina Archeologica mediante l’Utilizzo di Robot aUtonomi in Sciami (THESAURUS) [42], coordinated by the Research Center “E. Piaggio” of the University of Pisa. One of the goals of the THESAURUS project was the creation of a swarm of AUVs to perform cooperative autonomous surveys of areas of archaeological importance in the Tuscan Archipelago. In addition to being equipped with acoustical and optical payload used to identify and geolocalize potential objects of interest lying on the sea floor, the AUVs of the swarm would be able to communicate acoustically, both among themselves and with additional fixed stations, in order to create an underwater network to be used to plan, execute, and monitor cooperative underwater operations. For the project, the University of Florence developed and built the Typhoon class AUVs (presented in details, together with the other AUVs of the MDM Lab, in a dedicated chapter of this thesis), middle-sized AUVs able to fulfill the requirements of the project thanks to their dedicated Guidance, Navigation, and Control (GNC) system and their ability to equip diverse payload.

1.1 Overall Framework

3

Before the end of the THESAURUS project, successfully concluded in 2013, the MDM Lab assumed the role of coordinating partner of the European FP7 project ARcheological RObot systems for the World’s Seas (ARROWS) [37]. Gathering partners from all over Europe, the goal of the ARROWS project was the creation of underwater vehicles with the aim of reducing the costs of undersea archaeological operations. The needs of underwater archaeologists, identified during the project, led to the development of robotic tools able to adapt in response to the mutating necessities of an archaeological campaign, so that they could be efficiently used throughout all its phases. During the project, the MDM Lab developed the modular AUV MArine Robotic Tool for Archaeology (MARTA): indeed, thanks to the ability to modify its physical structure (by rearranging or even removing some of its modules), MARTA can be successfully employed in a wide variety of operations. In addition, since the connections at the ends of each module are standardized, new modules with different capabilities can be suitably developed and easily added to the body of the AUV. Since 2014, the University of Florence has been part of the Interuniversity Center of Integrated Systems for the Marine Environment (ISME) [35], which gathers research institutions from all over Italy and serves as a common basis for joint operations in the marine field. In particular, several test campaigns were carried out in the Ligurian Sea under the banner of ISME, thanks to the logistic support of the Naval Experimentation and Support Center (Centro di Supporto e Sperimentazione Navale (CSSN)) of the Italian Navy; the performed tests allowed to assess the performance of the vehicles of the MDM Lab, and helped identify potential areas of improvement. In 2016, the University of Florence led the Bridging Robots for Underwater Communication Enrichment (BRUCE) project, subproject of the European FP7 SUNRISE project [41]. During BRUCE, the possibility of using underwater vehicles as mobile nodes within an acoustic network composed of heterogeneous nodes was investigated. In particular, MARTA AUV, equipped with two acoustic communication devices produced by different manufacturers, successfully acted as an “interpreter” allowing for data exchange between two other nodes of the network which would not be able to communicate otherwise (since they were lacking a common communication protocol). At the time of writing, the MDM Lab is working on the Autonomous underwater Robotic and sensing systems for Cultural HEritage discovery cOnServation and in sitU valorization (ARCHEOSUb) project [36]. Based on the knowledge of the marine environment and on the AUV development skills acquired during previous projects, the University of Florence is actively participating in the creation of a new low-cost autonomous vehicle which will be used to survey and monitor underwater sites of archaeological interest. Throughout the years, in addition to the participation in national and European research projects, the University of Florence took part in several student and nonstudent robotics competitions. These events, in addition to constituting a valuable opportunity to test underwater vehicles outside of the pressing deadlines of research projects, foster the exchange of knowledge between students and researchers and enhance the inventiveness and the adaptability of the participants, which usually face mutating scenarios requiring the development of new solutions while deal-

4

1 Introduction

ing with strict time and resources constraints. A team from the University of Florence took part in the Student Autonomous Underwater Vehicles Challenge-Europe (SAUC-E) [40] competition in 2012, 2013, and 2016, while in 2015 the team participated in euRathlon [38]; finally, it took part in the European Robotics League (ERL) Emergency Robots competition in September 2017 [39]. The SAUC-E competition consists in a series of underwater trials rewarding autonomy and innovative approaches; euRathlon and ERL further increase the challenge, recreating the aftermath of a catastrophe in order to evaluate how air, land, and sea robots perform in different tasks (either single- or multi-domain cooperative missions) in a scenario which forbids human intervention. In 2013 and 2016, the team from the University of Florence placed third in the SAUC-E competition, while in 2017 it placed second in the ERL Emergency Robots air-and-sea sub-challenge, and fourth in the grand challenge (comprising all the domains).

1.2 State of the Art of Underwater Navigation Techniques Despite the growing interest that ocean engineering has received in recent years, autonomous underwater navigation can still be considered a challenging task. The high level of performance required, which is further enhanced if multiple vehicles are simultaneously employed, and the limitations due to the physics of the marine environment, e.g. the fact that the Global Positioning System (GPS), which is widely used for localization by land and air robots, cannot be exploited underwater, lead to a redefinition of what can be considered as the minimum accuracy standard. As a consequence, the choice of the best navigation system for a specific application is sometimes influenced more by the raw quality of the employed sensors than by the preference for one estimation algorithm over another. As of today, one of the main challenges that needs to be tackled is the precise localization of underwater vehicles. If attitude information is available, the simplest strategy to estimate AUV position is to integrate velocity measurements over time starting from a known position (a technique known as dead reckoning). However, due to the presence of noise and bias on the employed measurements, dead reckoning strategies are likely to be subjected to an unavoidable drift of the estimation error over time, which becomes particularly significant for long navigation missions. To avoid the necessity of dedicated error reset procedures (such as periodic resurfacings), which would be time- and power-consuming, it is not uncommon for AUVs to make use of additional, more elaborate estimators. The choice of a suitable algorithm or a fine tuning of the parameters it depends on are likely to increase the overall navigation performance of a vehicle, helping reduce the gap compared to top-of-the-range sensors; such feature is particularly desirable in the case of small AUVs or, generally speaking, low-cost applications. Hence, many different navigation algorithms have been developed throughout the years, and many sources can be found within scientific literature. A first major distinction can be made based on how attitude is estimated: indeed, the problems of estimating the position and the orientation of a

1.2 State of the Art of Underwater Navigation Techniques

5

mobile robot can be tackled together, leading to a complete pose estimation algorithm, or separately; in the second case, attitude is often used as input to the position estimation filter. Both options are valid, and have been extensively investigated by the scientific community. A widely used approach for both position and attitude estimation consists in the use of a Kalman Filter (KF) [29], a recursive estimator which exploits a mathematical model of the physical system under study and measurements of the quantities output by such system to compute an accurate estimate of its internal state. Since the standard KF cannot be used in the case of nonlinear models, several KF-based algorithms have been developed to deal with such situations, being the Extended Kalman Filter (EKF) and the Unscented Kalman Filter (UKF) the most common [11, 27, 28, 54]. In particular, since the mathematical models used to describe the physical behavior of AUVs are usually nonlinear (more on that in subsequent chapters of this thesis), KF extensions are employed most of the time. Indeed, thanks to its straightforward implementation, the EKF is widely employed, and numerous literature sources regarding its use in the marine field (and not limited to AUVs) are available (see for instance [1, 2, 17, 18, 32, 44, 51] for examples of the employment of the EKF for different marine-related applications); in addition, especially when paired with a simple vehicle model (often limited to a purely kinematic description of motion), it offers a fast and light (in terms of required computational resources) navigation solution. An alternative to the EKF may be constituted by UKF-based navigation algorithms. Since the EKF revolves around the linearization of nonlinear model equations (i.e. the computation of derivatives with respect to the variables of interest), it is likely to give rise to several difficulties if applied to highly nonlinear or non-differentiable models; the UKF, on the other side, being completely derivative-free, is not affected by such limitations and may be even used with more elaborate vehicle models, able to better capture the underlying physical phenomena with respect to, e.g., a simplified kinematic model. Several contributions regarding the application of the UKF in the marine field can be found in literature. For instance, in [10, 23], the authors simulate the behavior of different AUVs during the execution of autonomous underwater tasks, with the control loops of such vehicles closing on UKF-based navigation filters; in [46], a simulated comparison between the EKF and the UKF is proposed, where the filters are exploited to estimate the unknown hydrodynamic coefficients used to model the interaction of an AUV with water. Simulation results show that the UKF may offer satisfying performance, even outperforming the EKF; in [7], instead, the suitability of the UKF for underwater terrain-based navigation (i.e. navigation with the aid of a known map; in the cited contribution, the availability of bathymetric data is assumed) is evaluated using experimental data. Nonetheless, despite the encouraging results documented in literature, UKF-based approaches have not yet been extensively exploited in the underwater field, falling behind the EKF in terms of usage. To ease the localization problem, local sensor networks (both static and dynamic) composed of one or more localizing acoustic devices can be suitably exploited [57, 59]. For instance, recent studies investigated the possibility to employ measurements

6

1 Introduction

of the distance between a vehicle and a single acoustic beacon to increase the accuracy of the estimated position of the former; such information is acquired by the vehicle during navigation and fused with onboard sensors data. This approach, commonly referred to as single beacon localization, has indeed received increasing recognition over the years (see e.g. [8, 9, 12, 13, 16, 19, 26, 30, 52, 55, 56] and references contained therein). Concerning attitude estimation, many high-precision navigation systems currently rely on high-grade optical gyrocompasses; nonetheless, many different estimation algorithms (suitable for use in low-cost applications) have been developed over the years. In addition to KF-based solutions, other commonly adopted strategies involve nonlinear observers and complementary filters (the latter indicating algorithms which fuse measurements possessing complementary spectral characteristics). For instance, starting from the work presented in [47], in [34, 53] the authors propose a quaternionbased nonlinear attitude observer where angular rate measurements are integrated to obtain an attitude estimate, and the results is then corrected by comparison with an orientation approximation computed directly from linear acceleration and magnetic field measurements. In [24, 31], the authors propose an observer which does not require a separate attitude computation to be used as correction term, relying on direct measurements of fixed reference vectors instead; the need for fixed vectors has then been removed in [22]. Indeed, the estimator proposed in [31] represents a major contribution on the subject of attitude estimation, and has become a standard reference in the field. As with position estimators, the availability of acoustic localizing devices constitutes an useful mean to increase the accuracy of attitude estimates. For instance, in [50] the authors employ distance measurements from a vehicle to two or more acoustic devices to replace magnetic field measurements in the case of navigation close to magnetic disturbances, while in [21] it is shown how both the inputs and the outputs of a nonlinear attitude observer can be used in conjunction with an external position reference system to provide a complete position, velocity, and attitude estimator. Closely related to underwater navigation (and tackled during the Ph.D. period) is the problem of sea current estimation. Several examples regarding the use of underwater vehicles for current estimation can be found in literature: for instance, in [45] the authors report results of experimental tests where vehicle-mounted current measurement sensors (such as Acoustic Doppler Current Profiler (ADCPs)) are used to evaluate current velocity fields in coastal environments; in [58], instead, it is shown how ADCPs and a dynamic model of an autonomous vehicle can be suitably employed to estimate the velocity of deep-sea hydrothermal plumes. Additionally, the knowledge of the intensity and the direction of currents acting on an underwater vehicle can be actively exploited to increase the performance of the navigation system of the latter, even becoming of critical importance in some scenarios: for example, in [33, 48, 49, 60] current measurements, dead reckoning, and KF-based estimators are used for navigation when velocity measurements with respect to the water column below the vehicle are the only available velocity measurements (i.e. when velocity with respect to the bottom cannot be estimated).

1.2 State of the Art of Underwater Navigation Techniques

7

A different approach (similarly to what will be presented in this thesis) consists in estimating sea current without resorting to its direct measurement, avoiding the necessity of dedicated instrumentation: for instance, in [25] vehicle velocity measurements (with respect to the sea bottom) are paired with a kinematic vehicle model and used as input for a KF, while in [43] range measurements from acoustic beacons are used in conjunction with an EKF to estimate horizontal current intensity.

1.3 Contribution and Thesis Structure The research activity carried out during the Ph.D. period focused on the development of pose estimation algorithms for mobile robots, with special regard to AUVs. Starting from existing scientific literature, state-of-the-art navigation solutions (and their limitations) were analyzed, in order to identify those issues that could open up for improvements or novel strategies. In particular, the proposed contributions rely only on sensors which can be housed on board a vehicle; while external equipment (e.g. acoustic devices) constitutes an undeniable navigation help, it automatically implies an increase of the overall cost of at-sea operations, since such instrumentation is often expensive and may require time and dedicated logistics to be efficiently deployed. During the initial phase of the research, a complete pose estimation algorithm based on the UKF was developed, and its performance was compared with that of an EKF-based filter. This first comparison, executed offline exploiting navigation data acquired during previous experimental campaigns, was performed in order to evaluate the suitability of the developed filter for estimation of AUVs motion. Following the encouraging results obtained during this phase [3, 4], the research activity continued in parallel on the topics of attitude and position estimation. On the one hand, the goal was to derive an attitude estimator with reduced sensitivity with respect to magnetic disturbances, which constitute a major source of orientation estimation error, maintaining at the same time the overall (monetary) cost limited; on the other hand, in light of the contributions identified within the state of the art and supported by the results of the above-mentioned preliminary tests, the primary objective was to assess the real performance of an UKF-based position estimator in a real scenario, to investigate its applicability and reliability when used with underwater vehicles. Both issues were satisfyingly addressed during the Ph.D. period: starting from [31], a nonlinear attitude observer, robust with respect to magnetic disturbances and suitable for use in the underwater field, was developed and successfully tested online [6, 14], and it is now used on the vehicles of the MDM Lab. The derived algorithm is able to cope with the performance limitations that accompany the use of compasses in the case of magnetically perturbed environments, without requiring the use of top-of-the-range sensors. Concerning the UKF-based estimator, the presence of the standalone attitude observer allowed to simplify the structure of the filter; for instance, attitude was removed from the set of variables that need to be estimated, constituting a time-varying input instead. The

8

1 Introduction

focus was then given to the online validation of the derived solution, carried out during suitable experimental campaigns (whose results are presented in this work). In addition, the possibility of estimating sea current within the developed navigation filter was investigated. The designed strategy falls in the category of those current estimators which do not make use of external instrumentation: indeed, the vehicle model employed within the UKF-based position estimator was revised in order to include the effect of sea current on vehicle dynamics, while the same sensor data used for position estimation were taken into account. The main objective was to evaluate if current estimation could be included within the already developed navigation filter, without the need of (expensive) additional equipment or without incurring in an unacceptable computational load increase with respect to position estimation only. The feasibility of the devised approach was at first verified thanks to an observability analysis (presented in subsequent chapters of this thesis); then, exploiting real navigation data, simulations were carried out to establish the goodness of the developed solution [5, 15]; in view of the promising results, future sea tests will be performed in order to fully validate the developed strategy. The remainder of the thesis is organized as follows: Chap. 2 describes the AUVs of the MDM Lab in details, while Chap. 3 reviews the mathematical background required to introduce the navigation algorithms developed during the Ph.D. period, which constitute the subject of Chap. 4. Chapter 5 presents the results of tests aiming at validating the above-mentioned algorithms, and Chap. 6 concludes the work.

References 1. Alcocer A, Oliveira P, Pascoal A (2007) Study and Implementation of an EKF GIB-based Underwater Positioning System. Control Eng Pract 15(6):689–701 2. Alessandri A, Caccia M, Indiveri G, Veruggio G (1998) Application of LS and EKF techniques to the identification of underwater vehicles. In: Proceedings of the 1998 IEEE International Conference on Control Applications. Trieste (IT), pp 1084–1088 3. Allotta B, Caiti A, Chisci L, Costanzi R, Di Corato F, Fanelli F, Fantacci C, Fenucci D, Meli E, Ridolfi A (2015) A comparison between EKF-based and UKF-based navigation algorithms for AUVs localization. In: Proceedings of the MTS/IEEE OCEANS’15 Genova, Genova (IT) 4. Allotta B, Caiti A, Costanzi R, Fanelli F, Fenucci D, Meli E, Ridolfi A (2016) A new AUV navigation system exploiting unscented Kalman filter. J Ocean Eng 113:121–132 5. Allotta B, Costanzi R, Fanelli F, Monni N, Paolucci L, Ridolfi A (2017) Sea currents estimation during AUV navigation using Unscented Kalman Filter. In: Proceedings of the IFAC 2017 World Congress, Toulouse (FR) 6. Allotta B, Costanzi R, Fanelli F, Monni N, Ridolfi A (2015) Single axis FOG aided attitude estimation algorithm for mobile robots. J Mechatronics 30:158–173 7. Ånonsen KB, Hallingstad O (2007) Sigma point Kalman filter for underwater terrain-based navigation. In: Proceedings of the IFAC Conference on Control Applications in Marine Systems, Bol (Hr) 8. Arrichiello F, De Palma D, Indiveri G, Perlangeli G (2015) Observability analysis for single range localization. In: Proceedings of the MTS/IEEE OCEANS’15 Genova, Genova (IT) 9. Bahr A, Leonard JJ, Fallon MF (2009) Cooperative localization for autonomous underwater vehicles. Int J Robot Res 28:714–728

References

9

10. Barisic M, Vasilijevic A, Nad D (2012) Sigma-point unscented Kalman filter used for AUV navigation. In: 20th Mediterranean Conference on Control and Automation, Barcelona (ES) 11. Bar-Shalom Y, Li XR, Kirubarajan T (2001) Estimation with applications to tracking and navigation: theory algorithms and software. Wiley 12. Bayat M, Aguiar AP (2013) AUV range-only localization and mapping: observer design and experimental results. In: Proceedings of the 2013 European Control Conference, pp. 4394– 4399. Zürich (CH) 13. Bayat M, Crasta N, Aguiar AP, Pascoal AM (2016) Range-based underwater vehicle localization in the presence of unknown ocean currents: theory and experiments. IEEE Trans Control Syst Technol 24(1) 14. 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 Mechatronics 21:1900–1911 15. Costanzi R, Fanelli F, Ridolfi A, Allotta B (2016) Simultaneous navigation state and sea current estimation through augmented state unscented Kalman Filter. In: Proceedings of the MTS/IEEE OCEANS’16 Monterey. Monterey (CA, US) 16. Crasta N, Bayat M, Aguiar AP, Pascoal AM (2014) Observability analysis of 3D AUV trimming trajectories in the presence of ocean currents using single beacon navigation. In: Proceedings of the 19th World Congress of the International Federation of Automatic Control. Cape Town (ZA), pp 4222–4227 17. El-Hawary F, Jing Y (1995) Robust regression-based EKF for tracking underwater targets. IEEE J Ocean Eng 20(1):31–41 18. Fossen TI, Sagatun SI, Sørensen AJ (1996) Identification of dynamically positioned ships. J Control Eng Pract 4:369–376 19. Gadre AS, Stilwell DJ (2005) Underwater navigation in the presence of unknown currents based on range measurements from a single location. In: Proceedings of the 2005 American Control Conference. Portland (OR, US), pp 656–661 20. Gebre-Egziabher D, Elkaim G, David Powell J, Parkinson B (2006) Calibration of strapdown magnetometers in magnetic field domain. J Aerosp Eng 19(2) 21. Grip HF, Fossen TI, Johansen TA, Saberi A (2012) Integrated satellite and inertial navigation with gyro bias estimation and explicit stability guarantees. In: Proceedings of the 2005 American Control Conference. Montreal (CA) 22. Grip HF, Fossen TI, Johansen TA, Saberi A (2012) Attitude estimation using biased gyro and vector measurements with time-varying reference vectors. IEEE Trans Autom Control 57(5) 23. Hajiyev C, Ata M, Dinc M, Soken H (2012) Fault tolerant estimation of autonomous underwater vehicle dynamics via robust UKF. In: Proceedings of the 13th International Carpathian Control Conference. High Tatras (SK) 24. Hamel T, Mahony R (2006) Attitude estimation on SO(3) based on direct inertial measurements. In: Proceedings of the 2006 IEEE International Conference on Robotics and Automation. Orlando (FL, US), pp 2170–2175 25. Hegrenaes Ø, Hallingstad O (2011) Model-aided INS with sea current estimation for robust underwater navigation. IEEE J Ocean Eng 36(2) 26. Jouffroy J, Reger J (2006) An algebraic perspective to single-transponder underwater navigation. In: Proceedings of the 2006 IEEE International Conference on Control Applications. Munich (DE), pp 1789–1794 27. Julier SJ, Uhlmann JK (2004) Unscented filtering and nonlinear estimation. In: Proceedings of the IEEE, vol 92(3), pp 401–422 28. Julier SJ, Uhlmann JK (1997) A new extension of the Kalman filter to nonlinear systems. In: Proceedings of the SPIE Signal Processing, Sensor Fusion and Target Recognition VI Conference, vol 3068 29. Kalman RE (1960) A new approach to linear filtering and prediction problems. Trans ASME J Basic Eng 82(D):35–45 30. Larsen MB (2000) Synthetic long baseline navigation of underwater vehicles. In: Proceedings of the MTS/IEEE OCEANS 2000 Conference and Exhibition. Providence (RI, US), pp 2043– 2050

10

1 Introduction

31. Mahony RE, Hamel T, Pflimlin JM (2008) Nonlinear complementary filters on the special orthogonal group. IEEE Trans Autom Control 53(5):1203–1218 32. Mallios A, Ridao P, Ribas D, Maruelli F, Petillot Y (2010) EKF-SLAM for AUV navigation under probabilistic sonar scan-matching. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Taipei (CN) 33. Medagoda L, Williams SB, Pizarro O, Jakuba MV (2011) Water column current profile aided localisation combined with view-based SLAM for autonomous underwater vehicle navigation. In: Proceedings of the 2011 IEEE International Conference on Robotics and Automation. Shanghai (CN), pp 3048–3055 34. Nijmeijer H, Fossen TI (1999) New directions in nonlinear observer design. Springer 35. Official ISME website: www.isme.unige.it 36. Official website of the ARCHEOSUb project: www.archeosub.eu 37. Official website of the ARROWS project: www.arrowsproject.eu 38. Official website of the euRathlon competition: www.eurathlon.eu 39. Official website of the euRobotics European Robotics League: www.eu-robotics.net/robotics_ league 40. Official website of the SAUC-E competition: www.sauc-europe.org 41. Official website of the SUNRISE project: www.fp7-sunrise.eu 42. Official website of the THESAURUS project:www.thesaurus.isti.cnr.it 43. Osborn J, Qualls S, Canning J, Anderson M, Edwards D, Wolbrecht E (2015) AUV state estimation and navigation to compensate for ocean currents. In: Proceedings of the MTS/IEEE OCEANS’15 Washington. Washington (DC, US) 44. Ridao P, Ribas D, Hernàndez E, Rusu A, (2011) USBL/DVL navigation through delayed position fixes. In: Proceedings of the 2011 IEEE International Conference on Robotics and Automation. Shanghai (CN), pp 2344–2349 45. Rogowski P, Terrill E (2015) Mapping velocity field in coastal waters using an autonomous underwater vehicle. In: Proceedings of the 2015 IEEE/OES 11th Conference on Current, Waves and Turbulence Measurement. St. Petersburg (FL, US) 46. Sabet M, Sarhadi P, Zarini M (2014) Extended and unscented Kalman filters for parameter estimation of an autonomous underwater vehicle. J Ocean Eng 91:329–339 47. Salcuedan S (1991) A globally convergent angular velocity observer for rigid body motion. IEEE Trans Autom Control 36(12) 48. Stanway MJ (2012) Contributions to automated realtime underwater navigation. Ph.D. dissertation, MIT/WHOI Joint Program 49. Stanway MJ (2011) Dead reckoning through the water column with an acoustic doppler current profiler: field experiences. In: Proceedings of the MTS/IEEE OCEANS’11 Kona. Waikoloa (HI, US) 50. Stovner BB, Johansen TA (2017) Hydroacoustically aided inertial navigation for joint position and attitude estimation in absence of magnetic field measurements. In: Proceedings of the 2017 American Control Conference. Seattle (WA, US), pp 1211–1218 51. Vaganay J, Baccou P, Jouvencel B (2000) Homing by acoustic ranging to a single beacon. In: Proceedings of the MTS/IEEE OCEANS 2000 Conference and Exhibition. Providence (RI, US), pp 1457–1462 52. Vallicrosa G, Ridao P (2016) Sum of gaussian beacon range-only localization for AUV homing. Annu Rev Control 42:177–187 53. Vik B, Fossen TI (2001) A nonlinear observer for GPS and INS integration. In: Proceedings of the 40th IEEE Conference on Decision and Control. Orlando (FL, US), pp 2956–2961 54. Wan EA, van der Merwe R (2001) The unscented Kalman filter. Kalman filtering and neural networks. Wiley, pp 221–280 55. Webster SE, Eustice RM, Singh H, Whitcomb LL (2009) Preliminary deep-water results in single-beacon one-way-travel-time acoustic navigation for underwater vehicles. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. St. Louis (MO, US), pp 2053–2060

References

11

56. Webster SE, Walls JM, Whitcomb LL, Eustice RM (2013) Decentralized extended information filter for single-beacon cooperative acoustic navigation: theory and experiments. IEEE Trans Robot 29(4) 57. Whitcomb LL, Yoerger LL, Singh H (1999) Combined Doppler/LBL based navigation of underwater vehicles. In: Proceedings of the 11th International Symposium on Unmanned Untethered Submersible Technology. Durham (NH, US) 58. Yoerger DR, Murray PG, Stahl F (2001) Estimating the vertical velocity of buoyant deep-sea hydrothermal plumes through dynamic analysis of an autonomous vehicle. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Maui (HI, US) 59. Yoerger DR, Jakuba M, Bradley AM, Bingham B (2007) Techniques for deep sea near bottom survey using an autonomous underwater vehicle. Int J Robot Res 26:41–54 60. Zhang Y (1998) Current velocity profiling from an autonomous underwater vehicle with the application of kalman filtering. M.Sc thesis, MIT/WHOI Joint Program

Chapter 2

Involved Vehicles

This chapter is dedicated to the AUVs of the MDM Lab. At the time of writing, the MDM Lab owns four vehicles, while a fifth is currently under development and will be ready for sea trials by mid 2018. Despite them being developed for specific purposes, e.g. research projects whose goal was to ease operations of underwater archaeologists (Typhoon AUVs and MARTA AUV, Sects. 2.1 and 2.2) or even student robotics competitions (FeelHippo AUV, introduced in Sect. 2.3), they now constitute a fleet capable of completing different underwater tasks. Indeed, for a particular mission, be it a long range navigation task or an inspection of confined underwater spaces, the most suitable vehicle or even the best configuration in the case of modular vehicles such as MARTA (this feature will be detailed in Sect. 2.2) can be chosen. In addition, such choice can be based only on the characteristics of the vehicles, since the mission planning, control, and monitoring interface is the same for all of them. During the Ph.D. period, all the available vehicles (as the results of Chap. 5 will show) have been employed to validate and evaluate the performance of the developed navigation algorithms; this chapter describes each of them in details, to get a grasp of their different characteristics and of their role within the MDM Lab fleet. The description covers the materials used and the construction, as well as payload and navigation sensors. For what concerns the last category, a detailed discussion about the sensors which are usually mounted on underwater vehicles and the quantities they are able to output is given in Chap. 3; hence, in this chapter they are only briefly introduced. In particular, all the AUVs of the MDM Lab are equipped with the following navigation sensors (or a subset of them): • GPS, used to measure position and to initialize the navigation filter of the vehicles while they are on surface; • Depth Sensor (DS); • Doppler Velocity Log (DVL), used to measure linear velocity;

© Springer Nature Switzerland AG 2020 F. Fanelli, Development and Testing of Navigation Algorithms for Autonomous Underwater Vehicles, Springer Theses, https://doi.org/10.1007/978-3-030-15596-4_2

13

14

2 Involved Vehicles

• Inertial Measurement Unit (IMU) and triaxial compass, used to measure the orientation of the vehicles; • single-axis Fiber Optic Gyroscope (FOG), used to improve the accuracy of the orientation estimate; • acoustic localization systems, used to measure the position of the vehicles while the latter navigate underwater.

2.1 Typhoon Class AUVs Typhoon class AUVs (Figs. 2.1 and 2.2) are middle-sized AUVs developed by the MDM Lab in the framework of the THESAURUS project. The main physical characteristics of the vehicle, together with the achievable performance, are reported in Table 2.1. The custom-made external hull is made of fiberglass; a central battery pack is used to power the motors and all the electronic devices present on board and housed within the hull, which are in turn controlled by two different computers (an industrial PC104 and an Intel i-7 board). The high level GNC system is based on the Robot Operating System (ROS), which also serves as an interface between the processing units and onboard instrumentation (the same applies to all the AUVs of the MDM Lab). The positioning of the thrusters on the vehicle is shown in the design of Fig. 2.3; six oil-filled thrusters with fixed pitch, custom-made (obtained through additive manufacturing) propellers (two rear, two lateral and two vertical) are used to actively control translational motion and yaw and pitch angles, while roll stability is guaranteed by hydrostatics (i.e. the correct positioning of gravity and buoyancy centers ensures limited roll motion). At the time of writing, the MDM Lab owns two

Fig. 2.1 Typhoon class AUV at sea

2.1 Typhoon Class AUVs

15

Fig. 2.2 Typhoon class AUV hovering underwater Table 2.1 Typhoon class AUV physical data and performance Typhoon class AUV characteristics Length [mm] External diameter [mm] Mass [kg] Max speed [kn] Max depth [m] Autonomy [h]

3600 350 130–180 (dep. on payload) 5–6 300 >8

fully working Typhoon class prototypes; these vehicles, named TifOne and TifTu respectively, possess distinct navigation sensors and payload. In particular, Table 2.2 reports the navigation sensors mounted on each AUV. In addition to the acoustic systems reported in Table 2.2, both vehicles are able to communicate using WiFi or radio waves (while on surface). Furthermore, Typhoon AUVs can be equipped with different types of payloads, either optical or acoustical (for example, Fig. 2.2 shows TifOne AUV equipped with a pair of bottom-looking cameras and a side scan sonar, visible near the stern of the vehicle). Thanks to their physical structure, autonomy, and equipped navigation sensors, the Typhoon AUVs can be used to perform a variety of autonomous missions at sea (even in cooperation, given the presence of dedicated acoustic communication and localization systems mounted directly on board), ranging from medium-range navigation tasks (even in the presence of moderate sea currents) to survey and inspection of smaller areas, where their hovering capability (granted by the positioning of the thrusters) and the availability of diverse payload may be exploited to ease the work of divers or even substitute them during the execution of different tasks, with special focus on underwater archeology-related operations.

16

2 Involved Vehicles

Fig. 2.3 Positioning of the thrusters on Typhoon class AUVs Table 2.2 TifOne and TifTu sensor sets Typhoon class AUVs sensor sets Sensor PC104 GPS module STS Sensors DTM depth sensor EvoLogics S2CR 18/34 Xsens MTi-100 IMU + compass KVH DSP 1750 FOG Teledyne Explorer DVL

TifOne ✓ ✓

TifTu ✓ ✓

Acoustic modem ✓

Localizing system ✓

✓ ✓

✕ ✕

2.2 MARTA AUV MARTA is a torpedo-shaped AUV developed and built by the MDM Lab in the framework of the European project ARROWS. During ARROWS the MDM Lab cooperated again with archaeologists in order to provide them with a flexible and specialized solution to ease their activities at sea. Being archeology-related tasks of the most diverse nature, the main characteristic of MARTA AUV (visible in Figs. 2.4 and 2.5) is its modularity. Indeed, the vehicle is composed of different modules, each one dedicated to a particular function (e.g. propulsion, payload, power supply), and its final structure can be customized according to the required mission profile. For its standard configuration (Fig. 2.6), the physical characteristics of the vehicle and the obtainable performance are reported in Table 2.3; thanks to its reduced weight, MARTA is deployable from a small boat.

2.2 MARTA AUV

Fig. 2.4 MARTA AUV

Fig. 2.5 MARTA AUV hovering underwater

Fig. 2.6 MARTA AUV standard configuration modules layout

17

18

2 Involved Vehicles

Table 2.3 MARTA AUV physical data and performance MARTA AUV characteristics Length [mm] External diameter [mm] Mass [kg] Max speed [kn] Max depth [m] Autonomy [h]

3500 180 85 4 150 4–5 (per battery module)

Each module of the vehicle is made of anticorodal aluminum type 6082-T6; such material constitutes indeed a good trade-off between malleability, lightness and mechanical strength. In order to achieve high resistance against salt water corrosion, the hull of the vehicle underwent a hard anodizing process. Modules are connected by means of plastic wires, resulting in fast and simple assembly and disassembly procedures. Two O-rings are placed at each interface to ensure a watertight connection; additionally, a vacuum pump is used to create a depressurization of 0.3 bar in order to guarantee a better alignment between the modules and to provide adequate stiffness. The modularity is also ensured from an electrical point of view: indeed, the electrical interfaces of each module of the vehicle are identical on both sides (including power supply cables with connectors preventing potential polarity inversion, and an Ethernet cable used to exchange data), in order to allow each module to be added, repositioned, or removed according to the specific necessities of the mission to perform. Analogously to the Typhoon AUVs, six thrusters are used to control five degrees of freedom (roll stability is ensured by hydrostatics and by the presence of the fins in the lower part of the hull); underwater-specific thrusters, manufactured by BlueRobotics and consisting of a brushless motor sealed with an epoxy waterproof coating, have been paired with custom-made fixed pitch propellers. Concerning navigation sensors, MARTA is equipped with the following devices: • Xsens MTi-G-710 IMU-enhanced GNSS/INS and Attitude and Heading Reference System (AHRS); • First Sensor SSI series depth sensor; • LinkQuest NavQuest 600 Micro DVL; • KVH DSP 1760 single-axis FOG. These devices are connected to two onboard computers (one ODROID-XU board and one Intel i-7 Mobile CPU), which also control communication devices (a WiFi access point, a radio modem and an EvoLogics S2CR 18/34 acoustic modem) and optical and acoustical payload. At the time of writing, one Basler ace camera is mounted on the stern module, while a Teledyne BlueView M900 2D forward-looking sonar is present in the bow of the vehicle. A visual comparison between Typhoon AUVs and MARTA is shown in Fig. 2.7: all the vehicles have been developed to pro-

2.2 MARTA AUV

19

Fig. 2.7 Typhoon and MARTA AUVs

vide underwater archaeologists with reliable and cost-affordable solutions to facilitate on-field operations. While they all succeed at this task (possessing, e.g., desirable features such as hovering capabilities or the possibility of housing diverse payload), different situations may favor the use of one over the others. For instance, thanks to their bulkier structure and increased autonomy, Typhoon AUVs may be preferred for longer navigation missions or when environmental conditions are harsher; on the other side, its reconfigurability, easy assembly, transportation, and deployment make MARTA a better choice where flexibility is the main requirement.

2.3 FeelHippo AUV While Typhoon AUVs and MARTA constitute the outcome of specific research projects, the background of FeelHippo AUV is quite different. Indeed, the latter has been designed and developed specifically for the participation in student robotics competitions. Such competitions gather students and researchers from all around the world and constitute, in addition to a considerable learning experience for all the involved people, an useful testbed for state-of-the-art underwater technologies. FeelHippo was used by a team of the University of Florence during SAUC-E 2013, euRathlon 2015, and ERL Emergency Robots in 2017. The development of FeelHippo dates back to 2013; the first prototype (visible in Fig. 2.8), being built on a low budget and outside the scope of a dedicated research project, offered relatively poor performance (in terms of autonomous navigation and available payload) if compared with the other AUVs of the MDM Lab.

20

2 Involved Vehicles

Fig. 2.8 FeelHippo AUV, first prototype (2013)

Fig. 2.9 FeelHippo AUV, 2015 version

In 2015, the vehicle was revamped for the participation in euRathlon competition (Fig. 2.9): additional instrumentation, not present on the original prototype, was added in order to improve its navigation and payload acquisition capabilities.

2.3 FeelHippo AUV

21

In addition to student competitions, FeelHippo has been used for short navigation missions, mainly in shallow waters, from 2015 onward. Thanks to the sensors added for the competition (whose inclusion was thought to be, in an initial phase, temporary) the level of performance achieved was satisfying; hence, it was decided to incorporate such devices within the standard equipment of the vehicle. From early to mid 2017, FeelHippo underwent a major overhaul, in terms of both mechanical components (identifying those parts and subsystems that could be redesigned in order to increase overall functionality) and navigation sensors (permanent integration of new instrumentation required indeed a general revision of the the electronics of the vehicle, in order to optimize the occupied volume); in its current version (Fig. 2.10), FeelHippo can be efficiently used as a small survey and inspection AUV, suitable for use in present and future research projects (for instance, it will be employed during the ARCHEOSUB project) or, generally speaking, autonomous sea operations, constituting an useful addition to the MDM Lab fleet. The description given in this section refers to the vehicle in its current form, occasionally highlighting relevant improvements made over the 2015 version. The main characteristics of the vehicle are shown in Table 2.4; the reduced dimensions and weight, together with the convenient handles visible in Fig. 2.10, allow for easy transportation and deployment (no more than two people are required, and even deployment from shore is possible). The central body of FeelHippo is composed of a plexiglass pipe with an internal diameter of 200 mm and 5 mm thickness, which houses all the non-watertight hardware and electronics. Two metal flanges constitute the connection between the central body and the plexiglass domes at each end of the main pipe, and two O-rings ensure a watertight connection between the former and the domes; six metal bars serve as tie rods, and are used to increase overall stiffness and to ease the connection of external components (such as motors or payload) to the main body. All the metal parts are made of hard anodized anticorodal aluminum type 6061-T6. An exploded view drawing of the main body of the vehicle is shown in Fig. 2.11. Batteries are located within the two cylindrical pipes in the lower part of the vehicle (Fig. 2.12 shows an exploded view drawing of battery housings), which are connected to two tie rods of the main body. Four thrusters (in vectored configuration), used to control translational motion and yaw (limited roll and pitch are guaranteed by an intelligent distribution of masses), are connected with the central frame by custommade plastic parts. The oil-filled thrusters present in the 2015 version of the vehicle

Table 2.4 FeelHippo AUV physical data and performance FeelHippo AUV characteristics Dimensions [mm] Mass [kg] Max speed [kn] Max depth [m] Autonomy [h]

approx. 600×640×500 35 2 35 2–3

22

Fig. 2.10 FeelHippo AUV, 2017 version

Fig. 2.11 Exploded view drawing of the central body of FeelHippo

2 Involved Vehicles

2.3 FeelHippo AUV

23

Fig. 2.12 Exploded view drawing of FeelHippo battery housings

were replaced with underwater-specific thrusters manufactured by BlueRobotics, employed with their original nozzle. Concerning internal electronics, all the components are mounted on two parallel plexiglass planes, placed on linear guides which facilitate assembly and maintenance operations (allowing to easily extract internal components from within the central body of the vehicle). An Intel i-7 Mobile CPU is used for on board processing, while the sensor set FeelHippo is equipped with includes: • U-blox 7P precision GPS; • Xsens MTi-300 AHRS; • Nortek DVL1000 DVL, which is also able to measure depth. With respect to the 2015 version (where a LinkQuest DVL was used, as visible in the stern of the vehicle in Fig. 2.9), the DVL has been placed under the central body of the AUV. Indeed, being such component quite heavy (approximately 2.7 kg in air), this choice increases stability and reduces the effect of restoring moments; • KVH DSP 1760 single-axis FOG. For what concerns communication, in addition to a WiFi access point and a radio modem, an EvoLogics S2CR 18/34 acoustic modem is used underwater. Regarding payload, the following devices are currently mounted on the vehicle: • one Microsoft Lifecam Cinema forward-looking camera, which also allows teleoperated guide; • one bottom-looking ELP 720p MINI IP camera;

24

2 Involved Vehicles

• two lateral ELP 1080p MINI IP cameras, used for stereo vision; • one Teledyne BlueView M900 2D forward-looking sonar. Despite its reduced size, FeelHippo is able to equip diverse payload, both optical and acoustical. Furthermore, thanks to its particular structure, additional small devices (such as, e.g., supplementary cameras or LED illuminators) can be added to the main body of the vehicle with ease.

Chapter 3

Mathematical Background

This chapter introduces the adopted notation and reviews the fundamental theoretical and mathematical concepts used throughout the thesis. At first, the classical kinematic and dynamic modeling of an AUV is proposed, taking into account the presence of sea currents [4]; then, the principles of Kalman filtering are reviewed [8], with special focus given to the UKF [1, 7], which constitutes the basis for the position estimation filter developed during the Ph.D. period. Finally, mathematical models for sensors commonly mounted on underwater vehicles and used in the underwater field, introduced in Chap. 2, are given.

3.1 AUV Kinematic and Dynamic Model This section introduces the equations describing the motion of a (generally speaking) six-degree-of-freedom vehicle freely moving within a fluid. The notation used throughout this work is that commonly adopted for marine vehicles (Society of Naval Architects and Marine Engineers (SNAME) [4]). Firstly, two suitable reference frames are introduced describe the   to conveniently vehicle motion (see Fig. 3.1): an Earth-fixed frame O N x N y N z N , whose origin is on water surface and whose axes point North, East, and Down (NED), respectively  (NED frame, or N frame); and a body-fixed frame O B x B y B z B (body frame, or B frame), usually centered in the center of mass of the vehicle, with its x-axis pointing in the direction of forward motion, its z-axis pointing down, and its y-axis completing a right-handed reference frame. Throughout the thesis, superscripts N and B will be used to indicate the frame a quantity is expressed in. The pose of the vehicle with respect to the Earth-fixed frame is denoted with vector η, defined as follows:   η = η 1 η 2 , η 1 = [x y z] , η 2 = [φ θ ψ] ,

© Springer Nature Switzerland AG 2020 F. Fanelli, Development and Testing of Navigation Algorithms for Autonomous Underwater Vehicles, Springer Theses, https://doi.org/10.1007/978-3-030-15596-4_3

(3.1)

25

26

3 Mathematical Background

Fig. 3.1 Earth-fixed and body-fixed reference frames

where η 1 represents the fixed-frame position of the center of gravity of the vehicle, and η 2 is its orientation, expressed using a suitable triplet of Euler angles. In this context, roll φ, pitch θ, and yaw ψ angles are used. The velocity of the vehicle, on the other side, is expressed with respect to the body frame, and is denoted with:   ν = ν 1 ν 2 , ν 1 = [u v w] , ν 2 = [ p q r ] ,

(3.2)

being u, v, and w the linear velocities along the axes of the body frame (surge, sway, and heave motion), and p, q, and r the angular velocities about such axes. Finally, vector   τ = τ 1 τ 2 , τ 1 = [X Y Z ] , η 2 = [K M N ]

(3.3)

indicates the body-frame vector of forces and torques acting on the vehicle. The following relation links the time derivative of η to ν:  R BN (η 2 ) 03×3 , η˙ = J (η 2 )ν , J (η 2 ) = 03×3 TBN (η 2 ) 

(3.4)

being R BN (η 2 ) the rotation between the fixed and the body frame and TBN (η 2 ) the transformation matrix between angular velocity and the time derivatives of the chosen Euler angles. Throughout the thesis, explicit dependence on η 2 will be often omitted for the ease of reading. The dynamics of the vehicle is governed by the vector equation: Mm ν˙ + C(ν)ν + D(ν)ν + gη (η) = τ (ν, u) , where the terms on the left-hand side have the following meaning: • • • •

Mm : mass and inertia matrix; C(ν): centripetal and Coriolis effects matrix; D(ν): hydrodynamic damping effects matrix; gη (η): gravitational and buoyancy effects vector.

(3.5)

3.1 AUV Kinematic and Dynamic Model

27

Finally, the dependence of τ from ν and from the vector of control inputs to the system u has been made explicit. Since the quantities on the left-hand side of (3.5) are the sum of multiple contributions due to the motion of the vehicle and to its interaction with the surrounding fluid, they are analyzed in details here below. Mass and Inertia Matrix Mm Matrix Mm is composed of the sum of two terms: Mm = M R B + M A ,

(3.6)

where M R B takes into account the motion of the vehicle. Assuming that the body frame is centered in the center of gravity of the vehicle and that the body-frame axes coincide with the principal axes of inertia, M R B assumes the following simplified form:   diag {m, m, m} 03×3   MR B = , (3.7) 03×3 diag Ix , I y , Iz where diag{·} indicates a diagonal matrix whose diagonal elements are the terms enclosed in braces, m is the mass of the vehicle, and Ix , I y , and Iz are the principal moments of inertia. M A , instead, is usually denoted with the term added mass matrix, and takes into account the fact that due to the higher density (with respect to air) of the fluid the vehicle moves in, larger accelerations are required to move not only the vehicle, but also the surrounding fluid itself. Assuming complete submersion, three planes of symmetry, and moderate advance speed, it is possible to express M A as follows: M A = −diag{X u˙ , Yv˙ , Z w˙ , K p˙ , Mq˙ , Nr˙ } ,

(3.8)

where, e.g., the added mass force X A along the x-axis is given by X A = X u˙ u˙ , X u˙ =

∂X . ∂ u˙

(3.9)

Centripetal and Coriolis Effects Matrix C(ν) Matrix C(ν) can be defined as follows: C(ν) = C R B (ν) + C A (ν) .

(3.10)

C R B (ν) includes the rigid body centripetal and Coriolis effects, while C A (ν), analogously to M A , models the additive effects due to the motion within the fluid. Under the same hypotheses made for Mm , they assume the form

28

3 Mathematical Background

C R B (ν) =

 S [mp mq mr ] 03×3

S



03×3 Ix p I y q Iz r

 ,



03×3 S  [X u˙ u X v˙ v Z w˙ w]

  C A (ν) = ,  S [X u˙ u X v˙ v Z w˙ w] S K p˙ p Mq˙ q Nr˙ r

(3.11)



(3.12)

where operator a 3-by-3 skew-symmetric matrix from a 3-by-1 vector:  S (·) builds  e.g., if v¯ = v¯ x v¯ y v¯ z ∈ R3 , then ⎡

⎤ 0 −v¯ z v¯ y S(¯v) = ⎣ v¯ z 0 −v¯ x ⎦ . −v¯ y v¯ x 0 Hydrodynamic Damping Effects Matrix D(ν) Matrix D(ν) models the dissipative effects due to the motion within the fluid; the main damping contributions are given by nonlinear skin friction due to turbulent boundary layers, and by viscous damping force due to vortex shedding. Damping effects are highly nonlinear and coupled; for underwater vehicles, to a first approximation, coupling effects and terms higher than the second order (with respect to body-fixed velocity) can be neglected, which in turn equals to assume a diagonal structure for D(ν). Finally, since quadratic terms dominate over linear terms, D(ν) can be approximated with   D(ν) = −diag X u|u| |u| Yv|v| |v| Z w|w| |w| K p| p| | p| Mq|q| |q| Nr |r | |r | .

(3.13)

An expression for the first three coefficients on the right-hand side of (3.13) (i.e. those relative to damping forces) is as follows [4]: 1 Di ν1,i = ρA f,i C D,i |ν1,i | , i = x, y, z , 2

(3.14)

where ρ represents water density, A f,i is the projection of the area of the hull of the vehicle on a plane perpendicular to the i-axis, and C D,i is the drag coefficient, which quantifies the fluid resistance. Generally speaking, the elements of D(ν) (i.e. the damping coefficients) depend on characteristic parameters of the fluid, such as its density and its Reynolds number; their determination, which falls beyond the scope of this research activity, is not trivial, and is usually carried out resorting to semi-empirical methods based on practical experimentation [3, 4, 11].

3.1 AUV Kinematic and Dynamic Model

29

Gravitational and Buoyancy Effects Vector gη (η) Gravitational and buoyancy effects acting on the vehicle can be computed separately and then combined into vector gη (η). Let g = [0 0 g]

(3.15)

denote gravitational acceleration expressed in the Earth-fixed frame, with g = 9.806 m/s2 . Additionally, let V denote the total volume of the vehicle. Gravitational forces, applied to the center of gravity and expressed in the body frame, are:  W B = R BN [0 0 mg] .

(3.16)

Buoyancy, on the other side, is applied to the center of buoyancy of the vehicle (which, generally speaking, does not coincide with the center of gravity), and equals to:  (3.17) B B = − R BN [0 0 ρgV ] . Let rbB denote the position of the center of buoyancy in the body frame; then, vector gη (η) can be computed as follows: 

WB + BB gη (η) = − B rb × B B

 ,

(3.18)

where × indicates cross product.

3.1.1 AUV Dynamic Model in the Presence of Sea Currents The dynamic model reported in (3.5) assumes no environmental disturbances. However, in practical sea scenarios, a vehicle navigating underwater may be subjected to temporary or persistent sea currents of different intensities. Such disturbances, if neglected, can possibly affect the performance of any navigation filter. Sea current can be modeled as follows:   ν cN = u cN vcN wcN 0 0 0 .

(3.19)

In (3.19), current velocity ν cN , expressed in the NED frame, has been assumed irrotational, an hypothesis that may be considered true when taking into account the extension of a typical operational area of an AUV. Current can then be expressed in the body frame using the following relation: 

 R BN 03×3 N ν . 03×3 03×3 c

 ν c = J c ν cN =

(3.20)

30

3 Mathematical Background

Additionally, let us assume that the Earth-fixed current is constant, so that ν˙ cN = 0

(3.21)

holds true. In particular, this implies that the time derivative of the sea current expressed in the body frame is given by: ν˙ c = J˙c ν cN + J c ν˙ cN = J˙c ν cN .

(3.22)

The first 3 × 3 block of J˙c can be computed recalling that the angular velocity of the vehicle expressed in the fixed frame is given by R BN ν 2 , and that by virtue of the properties of rotation matrices and of the skew-symmetric operator the following relation holds:

 R˙ BN = S R BN ν 2 R BN = R BN S (ν 2 ) R BN R BN = R BN S (ν 2 ) . Hence:

(3.23)

    c = R BN S (ν 2 ) = S (ν 2 ) R BN = −S (ν 2 ) R BN , J˙11

(3.24)

   −S (ν 2 ) R BN 03×3 N ν˙ c = ν . 03×3 03×3 c

(3.25)

which leads to

In order to take into account the effects of sea currents within the AUV dynamic model, it is possible to rewrite (3.5) in terms of the relative velocity ν r = ν − ν c : Mm ν˙ r + C(ν r )ν r + D(ν r )ν r + gη (η) = τ (ν r , u) ,

(3.26)

where the terms have the same meaning as in (3.5).

3.2 Unscented Kalman Filter Given a generic dynamical system, it is possible to define a set of variables (usually representing physical quantities) which completely describe its state. Knowledge about the components of such set (commonly referred to as the state of the system) is used to determine the behavior of the system itself. In the general case, the evolution of a dynamical system (or, equivalently, of its state) is governed by the following equation: x˙ = f (t, x, u, w) ,

(3.27)

being x the state vector, and f (·) a generic nonlinear, time-varying function (for the ease of reading, explicit dependence on time t of the function arguments has

3.2 Unscented Kalman Filter

31

been omitted). Usually, the physical phenomena describing the behavior of the state variables are not known in their completeness, and f (·) only represents a model for the evolution of the system; hence, process noise vector w is used to describe modeling uncertainties. The direct knowledge of the state variables is frequently unavailable; instead, it is possible to dispose of a set of measurements (or observations, often obtained from sensors) which are related to the state variables themselves. Let y denote the measurement vector; then, it is possible to write: y = h (t, x, u, v) ,

(3.28)

where the measurements vector is a function of the state, of the input (even if theoretically possible, this condition is rarely verified, and strictly causal systems are often considered), and of the measurement noise v used to model sensor accuracy (time dependence has been omitted again). Together, (3.27)–(3.28) are usually referred to as the state-space representation of the system. Furthermore, it may be useful to derive a discrete-time representation of (3.27)– (3.28); throughout this thesis, uniform sampling with respect to time is assumed: given a fixed time interval (the sampling time), any two subsequent samples of the time-continuous involved variables or functions are separated by one sampling time. Hence, (3.27)–(3.28) can be rewritten as follows:  xk = fk−1 (xk−1 , uk−1 , wk−1 ) yk = hk (xk , vk )

,

(3.29)

where subscript k indicates the k-th time instant. Note that, for what has been stated before, input dependence within the measurement equation has been removed. When the state of the system is not available, suitable observers (or estimators) need to be employed in order to compute an accurate estimate of its components. The vast majority of state estimators for dynamical system are based on the Kalman Filter and on its extensions [1, 7, 8, 15]. The KF is a recursive observer based on the alternation of two steps: • Prediction: during the prediction step, the current estimate and the model of the system are used to predict the behavior of the state at the subsequent iteration; • Update (or Correction): the predicted state is corrected using available measurements in order to produce a more accurate estimate. In the particular case of linearity of the system (3.29) with respect to state and input, and with the additional assumption of gaussianity and uncorrelation of initial state and process and measurement noises (the last hypothesis holds true in many applications), it can be proved that the KF is the optimal estimator in the MMSE sense.

32

3 Mathematical Background

For nonlinear systems, the KF cannot be applied as it is; however, it is possible to resort to suitable Kalman Filter extensions which can be applied to such systems. These estimators are usually based on linearization or on sampling techniques: an example of a filter of the first category is the EKF, which linearizes the nonlinear model and measurement functions in a neighborhood of the current estimate in order to apply the recursions of the standard KF. Sampling-based observers, on the other side, perform a sampling operation on the probability density functions used to compute the optimal estimate (it is worth remembering that the MMSE estimator is indeed the conditional expectation of the state given the available information, i.e. past inputs and past and present measurements); such operation can be deterministic (as with, e.g., the UKF) or probabilistic (e.g. Particle Filters). During the Ph.D. period, a navigation filter for the estimation of the position of an AUV based on the UKF has been developed and validated; hence, its structure is illustrated in details here below.

3.2.1 Unscented Transform The UKF is based on the Unscented Transform (UT), a deterministic sampling technique which allows the computation of the mean and the covariance matrix of a Random Variable (RV) which undergoes a generic nonlinear transformation by propagating a minimum set of its samples and exploiting the knowledge of the mean and of the covariance of the starting variable. Let a ∈ Rna be a RV with mean a¯ and covariance Pa , and let b = g(a) ∈ Rn b denote the RV obtained by propagating a through the nonlinear function g(·) : Rna → Rn b . Then, the UT of a propagated through g(·), denoted with U T (¯a, Pa , g(·)), is obtained through the following steps: • generate 2n a +1 samples (n a +1 is the minimum necessary number of samples) called the σ-points a , each one with the dimension of a, exploiting the knowledge of a¯ and of Pa . The σ-points are usually chosen as symmetric with respect to the average value a¯ , i.e.:   a = [· · · a¯ · · · ] + ωσ 0na ×1 a − a , where a is such that Pa = a aT (e.g. the matrix square root or the Cholesky factor) and ωσ is a weight factor; • propagate the σ-points through g(·): G = g(a ) ∈ Rn b ×2na +1 ; ¯ the covariance matrix Pb and the cross-covariance Pab from • compute the mean b, the σ-points of the propagated RV:

3.2 Unscented Kalman Filter

33

b¯ = Gω m Pb = Gc G T Pab = a c G T for suitable weights ω m ∈ R2na +1 and c ∈ R2na +1×2na +1 . The weights ωσ , ω m and c influence the result of the Unscented Transform; their choice is not unique, and several literature sources can be found regarding their determination [6, 15]. For the navigation filter developed during the Ph.D. period, they are computed as in Algorithm 1. Algorithm 1 UKF weights computation Function UKF weights computation Data: α, β, γ Result: ωσ , ω m , c ωσ = α2 (n a + γ); λ = α2 (n a + γ) − n a ; 0 = λ(n + λ)−1 ; ωm a ωc0 = λ(n a + λ)−1 + (1 − α2 + β); i = ω i = (2 (n + λ))−1 , i = 1, ..., 2n ; ωm a a c  0 ω 1 · · · ω 2n a ; ω m = ωm m m   ω c = ωc0 ωc1 · · · ωc2n a ;

 = I2n a +1 − 11×2n a +1 ⊗ ω m ;  = diag{ω c }; c = T ; end

In Algorithm 1 I is the identity matrix, 1 is a vector whose elements are all equal to 1, and ⊗ is the Kronecker product. As regards the input parameters of the algorithm, α determines the spread of the σ-points around the mean value of the starting RV, β is used to include prior knowledge about the latter, and γ is an additional free parameter. More details about the effects of the variation of such parameters and of different choices of the UT weights on the result of the transformation can be found in [6, 15] and references contained therein. It can be proven [6, 15] that the moments computed using the propagated σ-points are accurate at least to the second order (of a Taylor series expansion) for a generic nonlinearity.

34

3 Mathematical Background

3.2.2 Unscented Kalman Filter Algorithm The UT constitutes the core of the Unscented Kalman Filter; in the case of nonlinear state evolution and measurement functions, it is indeed used for both the prediction and the correction steps. An implementation of the UKF algorithm is given in Algorithm 2, assuming additive (zero mean) process and measurement noise (assumptions that may be reasonable in a wide variety of situations): Algorithm 2 UKF algorithm Function UKF algorithm Step UKF prediction Data: xˆ k−1|k−1 , Pk−1|k−1 , fk−1 (·) Result: xˆ k|k−1 , Pk|k−1 (ˆxk|k−1 , P¯k|k−1 ) = U T (ˆxk−1|k−1 , Pk−1|k−1 , fk−1 (·)); Pk|k−1 = P¯k|k−1 + Q k−1 ; end Step UKF correction Data: xˆ k|k−1 , Pk|k−1 , hk (·) Result: xˆ k|k , Pk|k xy (ˆyk|k−1 , S¯k , Pk ) = U T (ˆxk|k−1 , Pk|k−1 , hk (·)); Sk = Rk + S¯k ; xy L k = Pk Sk−1 ; ek = yk − yˆ k|k−1 ; xˆ k|k = xˆ k|k−1 + L k ek ; Pk|k = Pk|k−1 − L k Sk L kT ; end end

In Algorithm 2, ˆ· denotes an estimate, P is the state covariance, and Q and R are the covariance matrices of process and measurement

noise. The state is recursively estimated starting from an initial guess xˆ 0|0 , P0|0 , where subscript j|k means “at time instant j, given information up to the k-th time instant”.

3.3 Sensors Modeling This section is devoted to the mathematical modeling of sensors and instruments commonly employed in the underwater field. Such devices, introduced in Chap. 2, are either mounted on vehicles and used for autonomous navigation, or are placed in fixed positions (e.g. on land, on water surface, or on the sea bottom) in order to create satellite and/or acoustic localization systems and networks. The working principle of each sensor, together with the mathematical equations used to model their behavior, is reported here below.

3.3 Sensors Modeling

35

Global Positioning System A vehicle-mounted GPS receiver exploits signals from satellites orbiting Earth to localize itself (at least four satellites are needed at any time); the position of the receiver is usually computed in geodetic coordinates (latitude, longitude, and altitude). Since the attenuation of electromagnetic waves impacting on a medium depends on wave frequency and on medium conductivity, the GPS signal (frequency of the order of G H z) cannot be used underwater (with conductivity much higher than air’s, and increasing with salinity), and this even without taking into account surface reflections. However, a GPS receiver is commonly mounted on almost all AUVs and used, e.g., for the initialization of navigation filters or to reset the navigation error if the vehicle resurfaces. In order to exploit GPS fixes, a preliminary measurements conversion from geodetic coordinates to the local NED frame is necessary; hence, a GPS model can be expressed as follows:

PGN P S = fG P S PG P S + δ P , O N = η 1 + δ NP ,

(3.30)

where PG P S is the GPS fix, O N are the geodetic coordinates of the point assumed as origin of the NED frame, and PGN P S is the NED frame measured position obtained with the conversion function fG P S (·). δ P is a noise term added to model sensor accuracy; it is worth noting that, after the conversion into NED coordinates, the resulting measurement noise δ NP will possess different spectral properties than those of the noise affecting raw data. Depth Sensor Depth sensors use pressure measurements in order to estimate depth. Indeed, such sensors measure local pressure according to the equation p DS = ploc + b p + δ p ,

(3.31)

where the sensed quantity p DS is the sum of local pressure ploc , of sensor bias b p , and of measurement noise δ p . The pressure measurement is then converted into a depth measurement using the following hydrostatic relation: N , p DS − patm = ρgd DS

(3.32)

being patm the local atmospheric pressure (measured by the sensor during the iniN can be expressed as: tialization phase); hence, the measured depth d DS N = z + δd , d DS

(3.33)

with measurement noise δd . Note that the pressure bias b p cancels out in the subtraction in (3.32).

36

3 Mathematical Background

Doppler Velocity Log A DVL is usually mounted on the hull of an underwater vehicle, and is used to measure its three-dimensional velocity with respect to the body frame. Its functioning is based on the Doppler effect: the device periodically emits an acoustic signal at fixed frequency towards the sea bottom, and computes its velocity (expressed in the body frame) by analyzing the frequency shift between the emitted signal and its echo reflected by the seabed. Alternatively, if the instrument is not able to obtain or maintain the bottom lock (be it because the seabed is too far below, because of a particular topology of the latter, or even because of different harsh environmental conditions), it may be possible to lock on the water column and to compute velocity with respect to water itself. Both strategies can be efficiently adopted (for instance, [2, 10] and references contained therein for some interesting results using DVLs in water profiling mode). Throughout this thesis, it is assumed that the device is able to maintain the bottom lock; the following mathematical model has been adopted: v BDV L = ν 1 + bv + δ v ,

(3.34)

where the measured velocity v BDV L is the sum of the true body-fixed velocity, of the sensor bias bv and of the measurement noise δ v . Inertial Measurement Unit IMUs are sensors based on inertia. They are composed of triaxial accelerometers and gyroscopes that measure three-dimensional acceleration and angular rate. Different technologies can be used to manufacture IMUs, offering high versatility for what concerns the trade-off between cost and performance. The large diffusion of MicroElectro-Mechanical Systems (MEMS) witnessed in recent years resulted in the spread of low cost and lightweight IMUs, which are nowadays massively employed in many different scientific and industrial sectors. The classical definition of IMU indicates an instrument possessing accelerometers and gyroscopes only; however, in the recent past such term has been used to describe a variety of inertial systems, and even to refer to complete AHRSs or Inertial Navigation Systems (INSs). Indeed, it is not uncommon for MEMS IMUs to also integrate a triaxial magnetometer, used to measure the magnetic field surrounding the sensor. Throughout this work, the term IMU will be used according to its classical meaning; even if all the AUVs of the MDM Lab are equipped with triaxial accelerometers, gyroscopes, and magnetometers packed within the same device, the presence of a compass will always be acknowledged. For a MEMS IMU, the following mathematical models have been adopted (the measured quantities are expressed in the body frame): • Accelerometer:



a IBMU = R BN η˙ 1 − g + ba + δ a ,

(3.35)

3.3 Sensors Modeling

37

where δ a represents measurement noise and ba is the accelerometer bias, responsible for a shift of the acceleration vector from its true direction (in this context, accelerometer bias has been neglected as properly calibrated accelerometers are assumed). It is worth noting that, for the considered field of application, the proper acceleration of an AUV η˙ 1 is usually very small; hence, the direction of a IBMU constitutes a good approximation of the fixed vertical axis expressed in the body frame. • Gyroscope: (3.36) ω IBMU = ν 2 + bg + δ g , where the measured quantity ω IBMU is the sum of the true angular velocity, of a time-varying bias bg , and of additive measurement noise δ g . Magnetometer The used model is the following:  m B = W R BN H N + Hd + δ m ,

(3.37)

being H N Earth’s magnetic field expressed in the fixed frame, W and Hd the disturbances due to local magnetic interferences (whose effects will be analyzed in details in Chap. 4) and δ m measurement noise (a similar magnetometer model can be found, for instance, in [5, 9, 14]). Fiber Optic Gyroscope A FOG is a high-precision gyroscope, ensuring a considerable performance improvement over MEMS gyroscopes. Its working principle is based on the Sagnac effect: two laser beams are forced in opposite directions into an optic fiber ring constituting the core of the device; their relative phase shift at the exit of the ring is indeed proportional to the angular rate of the instrument. For a single-axis sensor, the following model has been adopted: ω mFOG = ω F OG + be + b f + δ f .

(3.38)

The measured quantity ω mFOG is the sum of the true angular rate of the instrument ω F OG , of the component of Earth’s rotation sensed by the gyro be , of an additional bias term b f , and of measurement noise δ f . Due to the high instrument resolution and accuracy, the sensor is capable of measuring Earth’s rotation, whose effect is included in the term be (such bias can be nonetheless compensated, see Chap. 4, while the remaining bias b f is very low and can be often neglected during practical operation). FOG find common use in high-precision attitude sensors (e.g. optical gyrocompasses) employing triaxial optical gyroscopes to estimate the attitude of the body they are mounted on [12, 13].

38

3 Mathematical Background

Acoustic Localization Systems As stated before, GPS cannot be exploited below the surface, since GPS waves penetrate underwater no more than a very short distance; nonetheless, low frequency acoustic (i.e. pressure) signals may be used to communicate even at high depths. To this aim, the majority of AUVs are equipped with one or more acoustic modems. Additionally, acoustic modems may constitute the basis for underwater localization systems, the most common being Long BaseLine (LBL) and Ultra Short BaseLine (USBL) systems. The former are composed of a series of acoustic transducers positioned on the seabed at known position and relative distance (which is often in the range of hundreds of meters, and delimits the perimeter of the operational area), which emit an acoustic ping in response of a signal received by a vehicle-mounted acoustic modem. Knowing the speed of sound in water, once the pings of the network nodes reach the vehicle, the latter is able to localize itself by, e.g., triangulating the received data. An USBL system, on the other side, integrates within a single instrument both the acoustic transceiver and a series of transducers positioned at a relative known distance. When an acoustic signal reaches the USBL modem, a processing unit within the device is able to compute the position of the signal source by comparing the times of arrival and the phase differences of the data received by its transducers, exploiting the knowledge of the speed of sound in water. While a LBL system usually reaches a much higher accuracy level with respect to an USBL system (due to the higher extension of the baseline and to the independence on the transducer-vehicle distance), the main advantage associated with the latter is that no fixed infrastructure is needed, and only a single device is required. Throughout the Ph.D. period, an USBL modem has been conveniently used to localize the AUVs employed during the performed experimental tests; additionally, it has been used as ground truth to evaluate the performance of the developed navigation filter. Hence, a mathematical model for such sensor is given here below. Ultra Short BaseLine An USBL system computes the position of a compatible acoustic modem with respect to itself, in spherical coordinates. Let PUN and RUN denote, respectively, the position and the orientation of the USBL modem with respect to the NED frame; additionally, let (rU , θU , φU ) indicate range, polar angle and azimuth angle of a suitable USBLcentered spherical coordinate system. Then, the position measured by the sensor can be written as:





⎤ ⎡ rU + δrU sin θU + δθU cos φU + δφU

PUN S B L = PUN + RUN ⎣ rU + δr U sin θU + δθ U sin φU + δφU ⎦ = η 1 + δUN , rU + δrU cos θU + δθU (3.39) where δrU , δθU , and δφU are measurement noises affecting sensor readings. As for the GPS, once the measurements is expressed in the NED frame, resulting noise δUN will not maintain the same spectral characteristics of δrU , δθU , and δφU .

References

39

References 1. Bar-Shalom Y, Li XR, Kirubarajan T (2001) Estimation with applications to tracking and navigation: theory algorithms and software. Wiley 2. De Palma D, Arrichiello F, Parlangeli G, Indiveri G (2017) Underwater localization using single beacon measurements: observability analysis for a double integrator system. J Ocean Eng 142:650–665 3. Faltinsen OM (1990) Sea loads on ships and offshore structures. Cambridge University Press 4. Fossen TI (1994) Guidance and control of ocean vehicles, 1st edn. Wiley, UK 5. Gebre-Egziabher D, Elkaim G, David Powell J, Parkinson B (2006) Calibration of strapdown magnetometers in magnetic field domain. J Aerosp Eng 19(2) 6. Julier SJ, Uhlmann JK (2004) Unscented filtering and nonlinear estimation. Proc IEEE 92(3):401–422 (Nov 8) 7. Julier SJ, Uhlmann JK (1997) A new extension of the Kalman filter to nonlinear systems. In: Proceedings of the SPIE signal processing, sensor fusion and target recognition VI conference, vol 3068, July 28 8. Kalman RE (1960) A new approach to linear filtering and prediction problems. Trans ASME J Basic Eng Ser D 82:35–45 9. Kok M, Schön TB (2014) Maximum likelihood calibration of a magnetometer using inertial sensors. Proceedings of the 19th world congress of the international federation of automatic control, pp. 92–97, Cape Town (ZA), Aug 24–29 10. Medagoda L, Kinsey JC, Eilders M (2015) Autonomous underwater vehicle localization in a spatiotemporally varying water current field. In: Proceedings of the 2015 IEEE international conference on robotics and automation, pp. 565–572, Seattle WA, US. May 26–30 11. Sen D (2000) A study on sensitivity of maneuverability performance of the hydrodynamic coefficients for submerged bodies. J Ship Res 44(3):186–196 12. Spielvogel AR, Whitcomb LL (2017) A stable adaptive attitude estimator on SO(3) for truenorth seeking gyrocompass systems: theory and preliminary simulation evaluation. In: Proceedings of the 2017 IEEE international conference on robotics and automation, pp. 3231–3236. Singapore (SG), May 29–June 3 13. Spielvogel AR, Whitcomb LL (2015) Preliminary results with a low-cost fiber-optic gyrocompass system. In: Proceedings of the MTS/IEEE OCEANS’15 Washington, pp. 1–5, Washington DC, US. Oct 19–22 14. Vasconcelos JF, Elkaim G, Silvestre C, Oliveira P, Cardeira B (2011) A geometric approach to strapdown magnetometer calibration in sensor frame. IEEE Trans Aerosp Electron Syst 47(2):1293–1306 15. Wan EA, van der Merwe R (2001) The unscented Kalman filter. Kalman filtering and neural networks, pp 221–280. Wiley

Chapter 4

Navigation Filter

This chapter is dedicated to the AUV navigation filter developed during the Ph.D. period. Such filter is able to estimate the complete pose of an UUV fusing the measurements of onboard sensors in real-time. In particular, a parallel structure has been chosen (Fig. 4.1): attitude is estimated independently using IMU, compass, and FOG data, and constitutes an input for the position estimation filter. The starting point is the estimator proposed in [2, 6]: sensor data and a mixed kinematic/dynamic vehicle model are exploited within an UKF in order to estimate a twelve-dimensional state composed of η and ν. However, the experimental activity carried out at the MDMLab highlighted the high sensitivity of magnetometers with respect to operation in the underwater environment: indeed, many typical tasks executed under the surface involve proximity to sources of magnetic disturbances (e.g., navigation close to industrial platforms or metal debris due to modern wrecks encountered during seabed inspection); since magnetometers are commonly used to estimate the heading of a vehicle, the presence of perturbed magnetic fields may possibly lead to large navigation errors. In order to maintain a satisfying level of navigation accuracy, a dedicated strategy aiming at evaluating the validity of magnetometer output, using disturbance-free measurements only and recognizing and rejecting corrupted compass readings, providing at the same time a solution to estimate the three-dimensional attitude of an UUV without resorting to erroneous compass data, has been developed during the Ph.D. period. The resulting estimator, described in details in Sect. 4.1, allows the reduction of the dimension of the state vector with respect to [2, 6], since attitude can be accurately estimated by a standalone filter and then input into the position estimation filter. The latter consists in an UKF-based algorithm, exploiting GPS (on surface), depth, and velocity measurements in order to compute an estimate of the absolute position of the vehicle. Additionally, the derived navigation solution has been improved by including the online estimation of sea currents acting on the vehicle. As stated in Sect. 3.1.1, neglecting the presence of currents is likely to negatively affect the performance of the navigation filter. In this work, sea current is estimated by adding a suitable current model within the state-space model of the whole system; an estimate of the © Springer Nature Switzerland AG 2020 F. Fanelli, Development and Testing of Navigation Algorithms for Autonomous Underwater Vehicles, Springer Theses, https://doi.org/10.1007/978-3-030-15596-4_4

41

42

4 Navigation Filter

Fig. 4.1 Navigation filter block scheme

N Earth-fixed horizontal current components (see Fig. 4.1, where ν c,h represents the N first two components of ν c ) is computed on board, without the need of additional sensors (such as ADCPs), which would increase the overall cost of the system. Following the logic of the block scheme of Fig. 4.1, this chapter is organized as follows: in Sect. 4.1, the above-mentioned attitude estimation filter is presented in details; in addition, a magnetometer calibration procedure applicable when the rotation of the vehicle is limited (e.g., for many typical AUVs, whose attitude is usually constrained within a neighborhood of the horizontal plane) is proposed. Calibration must indeed be performed prior to the estimation process, in order to increase the reliability of compass readings. Then, in Sect. 4.2, the position estimation filter is described; in particular, Sect. 4.2.1 presents the state-space vehicle model the UKF-based filter is applied to, while Sect. 4.2.2 shows how it is possible to modify such model in order to take into account the effects of local marine currents on vehicle dynamics, allowing for their online estimation without requiring additional instrumentation; finally, an observability analysis is provided in order to justify the proposed approach.

4.1 Attitude Estimation Filter The attitude estimation filter developed during the Ph.D. period is based on the Nonlinear Explicit Complementary Filter (NECF) proposed in [15], whose name derives from the structural similarity of the algorithm to linear complementary filters. In recent years, the NECF has become a standard reference in the field of attitude estimation; furthermore, it has been proven to converge to real orientation (a formal proof, based on Lyapunov theory, is provided by the authors in [15]).

4.1 Attitude Estimation Filter

43

Let us assume that the vehicle whose orientation has to be estimated (with respect to the Earth-fixed frame) is equipped with a device composed of a triaxial gyroscope, a triaxial accelerometer, and a triaxial magnetometer (which will be commonly referred to as an IMU and compass system); additionally, let us assume that such device is rigidly fixed to the vehicle, so that the term sensor frame may be equivalently used to indicate the body-fixed frame, since the two differ at maximum by a constant rotation describing the sensor mounting. At each iteration of the NECF, an estimate of the orientation of the sensor is computed; the basic principle is to integrate the angular rate changes measured by the gyroscopes about the body-frame axes, and to correct the obtained values exploiting accelerometers and magnetometers readings. If at least two fixed and nonparallel Earth frame directions can be measured in the sensor frame, the algorithm converges to the exact attitude. For the considered system, this condition is satisfied: particularly, assuming that the proper acceleration of the vehicle is negligible (as it is usually the case with underwater robots) accelerometer readings give an estimate of the direction of the gravitational acceleration, and they are employed to correct roll and pitch gyro integration; magnetometers, on the other hand, measure the direction of Earth’s magnetic field (i.e. the direction of magnetic North), and thus they can be used to compute an estimate of the yaw angle. Furthermore, the NECF presented in [15] is able to estimate the time-varying bias of the gyroscope. The filter is a dynamical system governed by the following equations:     N , R˙ˆ BN = Rˆ BN S ω IBMU − bˆ g + k P S (ω c ) , Rˆ BN (t = 0) = Rˆ B,0

(4.1)

˙ bˆ g = −k I ω c ,

(4.2)

ωc =

n 

ki viB × vˆ iB , ki ≥ 0 , i = 1, . . . , n .

(4.3)

i=1 N exploiting the sum In (4.1), the attitude estimate Rˆ BN is computed starting from Rˆ B,0 of a term proportional to gyro measured angular rate ω IBMU and gyro estimated bias bˆ g and a correction term based on accelerometers and magnetometers readings (being the two terms weighted by the positive gains k P , k I ). To give more details about the correction term, let viN , with i = 1, . . . , n, be a set of known fixed-frame directions, and let

and

  viB = R BN viN + δ i

(4.4)

  vˆ iB = Rˆ BN viN

(4.5)

denote, respectively, its measurement in the sensor frame (affected by noise δ i ) and its estimate (computed using the estimated rotation matrix Rˆ BN ); then, the term ω c is the weighted sum of the misalignments between the measured directions and

44

4 Navigation Filter

their estimates output by the filter (computed as the cross product between each pair of vectors). In particular, in the considered case the direction of the vertical axis (measured by accelerometers) and the direction of the magnetic field (read by magnetometers) are taken into account; the weights ki are chosen according to the relative confidence in each measurement viB .   Regarding convergence, let R˜ N = Rˆ N R N and b˜ g = bg − bˆ g denote, respecB

B

B

tively, the orientation and the gyro bias errors; then, n > 1, through the use of  for  ˜ b˜ g is locally exponentially Lyapunov theory it has been proven in [15] that R, stable to (I, 0). However, field experiments carried out using the NECF in its original formulation (4.1)–(4.3), using a commercial IMU and compass system to estimate the attitude of an underwater vehicle, highlighted some issues related to the particular field of application and whose effects are especially relevant in the case of corrupted magnetic field measurements. Hence, a set of design changes, justified by the characteristics of the underwater environment an by the limitations imposed by the adopted sensors, have been applied to the original structure of the filter in order to better adapt it to the underwater field. The beneficial effect of such modifications has then been verified with additional experimental testing. The resulting filter [4, 7] has proven to be able to compute an accurate attitude estimate, being at the same time resilient to magnetic field disturbances. At the moment of writing, it is employed as the standard attitude estimator on board the AUVs of the University of Florence, and its output constitutes the input to the position navigation algorithm. In view of the above-mentioned considerations, the remaining of this section is organized as follows: at first, the procedure adopted to calibrate the onboard magnetometer is presented; calibration is indeed necessary in order to compensate those magnetic disturbances which rotate with the vehicle. Then, the applied design changes to the original formulation of the NECF are illustrated and justified.

4.1.1 Magnetometer Calibration A triaxial magnetometer measures the direction and the intensity of the total magnetic field around the device. It cannot, however, distinguish between Earth’s magnetic field and additive magnetic disturbances. The direction of Earth’s magnetic field depends on the geographical location, and several calculators exploit one of the available geomagnetic field models [19] to determine its components from latitude and longitude. In this context, it is assumed that, given the restricted dimensions of a typical operational area of an AUV, Earth’s magnetic field is constant with respect to the NED frame. In the absence of disturbances, the measurements (expressed in the body frame) obtained by arbitrarily rotating a magnetometer in every possible orientation would lie on the surface of a sphere with its center in the origin and whose radius is the magnitude of the field at the

4.1 Attitude Estimation Filter

45

geographical location where such rotation is performed. However, in the presence of magnets or ferromagnetic objects, the measurements locus is shifted and deformed. Magnetic disturbances conceptually fall into two different categories: external (environmental) disturbances, and disturbances rotating with the sensor. As regards the latter, they can be further characterized as Hard Iron or Soft Iron disturbances, whose effect is the following: • Hard Iron disturbances: permanent magnets and magnetized objects, such as electronic subsystems in the proximity of the sensor, give rise to the so-called Hard Iron effect: these objects are the source of a permanent magnetic field, constant in all directions, whose effect is the addition of a constant bias Hd on the magnetometer output (see 3.37); • Soft Iron, scale factor and misalignment disturbances: using the simplified affine model of (3.37), matrix W can be factored as follows: W = Wmis Ws f W S I .

(4.6)

Wmis takes into account the misalignment between sensor axes and vehicle axes, including a non perfect orthogonality between the three axes of the magnetometer; Ws f models the different sensitivity of the device on its three axes, introducing a different scaling factor along the three directions; finally, W S I represents the Soft Iron effect: ferromagnetic materials close to the sensor, such as iron and nickel, produce a local magnetic field, whose magnitude is related to the angle of incidence of Earth’s magnetic field on the material itself. Thus, this effect changes as the orientation of the sensor varies. All these contributions, combined into W , have the effect of deforming the measurements sphere into an ellipsoid, arbitrarily tilted in space. If both kinds of disturbance are present, measurements taken while rotating the sensor in space would lie on the surface of an ellipsoid (due to W ) centered at a certain offset from the origin (due to Hd ). For instance, Figs. 4.2 and 4.3 show the magnetometer measurements (expressed in the sensor frame) acquired by the triaxial magnetometer of a Xsens MTi-300 AHRS taken while rotating such device in space. In the first case, the rotation was performed in a disturbance-free environment; sensor readings lie with good approximation on the surface of a sphere centered at the origin of the axes. The radius of the sphere does not reflect the true magnitude of the magnetic field, since the measurements are given by the employed sensor in arbitrary units. On the other side, measurements shown in Fig. 4.3 were taken in the same location as in the previous test, but metal objects were preliminarily attached to the sensor case: it is easy to note the deformation of the previously obtained sphere into an ellipsoid and the shift its center is subjected to. While external disturbances cannot be predicted (requiring a suitable identification and rejection strategy, such as the one proposed in Sect. 4.1.2), magnetic disturbances rotating with the sensor can be completely compensated exploiting suitable calibration procedures. The most common techniques are based on ellipsoid fitting methods [16]: after collecting measurements while rotating the sensor in a sufficient number

46

4 Navigation Filter Magnetometer measurements Origin Best fitting sphere

-0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8

-0.5

0

0.5 -0.5

0.5

0

Fig. 4.2 Magnetometer measurements in the absence of magnetic disturbances Magnetometer measurements Origin Best fitting ellipsoid

-2

-1.5

-1

-0.5

0

0.5 -2 -1 0 1

0

1

2

Fig. 4.3 Magnetometer measurements in the presence of magnetic disturbances

of different orientations in space, the best fitting ellipsoid (in a least square sense) is determined [10]; once its center, radii and axes are known, the offset Hd and the scaling W can be corrected. Other commonly used techniques involve optimization: for example, in [14, 22], the calibration process is cast as a maximum likelihood problem, and is solved using optimization tools.

4.1 Attitude Estimation Filter

47

While these techniques are computationally efficient and provide satisfying performance in a large number of situations, they are not applicable when sensor rotation about one or more axes is constrained. This is usually the case of the considered field of application: roll and pitch angles of an AUV are typically limited to a strict range. This restriction implies that the set of measurements to be used for the calibration procedure does not usually include enough information content along one space dimension (the vertical direction) to completely identify the shape of the ellipsoid. Tests in a controlled environment have been conducted to verify if, by executing a complete z-axis turn of the vehicle, measurements would be sufficient to correctly fit the data to an ellipsoid. However, even in the presence of currents, which cause a rotation of the vehicle out of the horizontal plane, ellipsoid fitting has not been proven to be a reliable technique to estimate the shape of the locus of magnetometer measurements. This reflects the impossibility of identifying an ellipsoid from an (approximately) elliptical data set. Hence, a simple and fast calibration technique which could approximate at best the theoretical locus on which the uncorrupted measurements lie, using only data (which roughly lie on a plane) acquired during a complete turn of the vehicle about its z-axis, has been derived. During the calibration turn, it is especially important that the roll and the pitch angles of the vehicle remain close to zero; to ensure this condition, the following characteristics are exploited: • Geometrical shape: long and slender AUVs (torpedo-shaped AUVs) naturally dampen pitch rotations; • Pitch control: MARTA and Typhoon AUVs actively control the pitch degree of freedom, thus guaranteeing limited values for such angle if proper reference signals are applied; • Hydrostatic stability: generally speaking, the correct positioning and alignment of the centers of buoyancy and gravity ensures limited roll and pitch motions and increases the overall stability of the vehicle. The idea behind the developed calibration algorithm is to perform a planar ellipse fit (the only one available when roll and pitch angles are constrained) on the data set constituted by the complete magnetometer readings. The derived technique is exact if the measurements are taken while the vehicle is horizontal, and remains sufficiently reliable for small roll and pitch angles values (within the usual ranges for an AUV). Let us assume that the x-axis of the NED reference frame is pointing towards geodetic North; then, on every point of Earth’s surface (exception made for the magnetic poles) Earth’s magnetic field has three nonzero components: one in the North-South direction (HxN ), one in the East-West direction (HyN ), and a component directed towards Earth’s center (vertical component HzN ). Since these components are known at each location, and can be easily computed using suitable calculators (e.g., [19]), the locus given by measurements taken on the horizontal plane is a well identifiable circle on the theoretical magnetic field sphere: it is indeed a circle whose

48

4 Navigation Filter

axis is parallel to the z-axis of the sensor, whose center is on such axis at the height of HzN and whose radius is given by r H,h =



(HxN )2 + (HyN )2 .

(4.7)

Figure 4.4 is given for a better understanding: if both Hard and Soft Iron disturbances are present, the circle in Fig. 4.4 is mapped onto an ellipse which is arbitrarily rotated in space (see Fig. 4.5). The idea behind the proposed calibration procedure is to compare the theoretical circular locus of measurements on the horizontal plane with the ellipse it is deformed into by magnetic disturbances, in order to determine the transformation that maps the former onto the latter and to compensate the disturbances by applying the inverse transformation to magnetometer readings. The developed algorithm is composed of the following steps: firstly, the measurements ellipse is rotated so that it lies on a horizontal plane; then, a planar ellipse fitting procedure is applied to determine the center, the radii and the tilt of the resulting ellipse; finally, these quantities are compensated and measurements are mapped back onto the theoretical circle. In practice, not all the measurements are exactly acquired at zero pitch and roll; hence, magnetometer readings do not perfectly lie on a plane. The first step of the calibration algorithm consists in computing the minimal distance plane (in a least square sense)  that best fits all the points (Fig. 4.6a). Such calculation can be executed resorting to linear algebra. Consider the standard plane equation: (4.8) P C = 0 , where P = [x y z  1] is a point on the plane, represented as an augmented vector, and C = [a b c d ] are the coefficients defining the plane (i.e. the unknowns of the problem). Given N points (which, in the considered case, will be the compass readings), the following linear system can be written: P˜ C = e ,

Fig. 4.4 Magnetic field measurements on the horizontal plane

(4.9)

4.1 Attitude Estimation Filter

49

Fig. 4.5 Magnetic field measurements locus deformation in the presence of both Hard and Soft Iron magnetic disturbances

being

⎡

x˜1 y˜1 z˜ 1 ⎢ .. .. .. ⎢ . . . ⎢ P˜ = ⎢ ⎢ x˜i y˜i z˜ i ⎢ . .. .. ⎣ .. . . x˜ N y˜ N z˜ N

⎤ 1 .. ⎥ .⎥ ⎥ 1⎥ ⎥ , .. ⎥ .⎦ 1

(4.10)

  and each x˜i y˜i z˜ i a different measurement. e ∈ R N is a vector representing the error to be minimized (if all points lie on the plane, then e is the null vector). The solution of such system is the closest vector to the kernel of P˜ , which is given by the right-singular vector corresponding to the minimum singular value obtained from the singular value decomposition of P˜ . Once the best fitting plane has been determined, its normal vector is given by: n = [a b c ] ,

(4.11)

and any point P can be found by assigning two coordinates and determining the third one by inversion of (4.8). The projection of a generic magnetometer reading miB on  is then given by:    mi = miB − n miB − P n .

(4.12)

The following step consists in computing the necessary rotation to align  with the horizontal plane. The “outwards” pointing normal nout  (pointing towards the half-

50

4 Navigation Filter

(a) Minimal distance plane

(b) Alignment with the horizontal plane

(c) Ellipse fitting

(d) Vertical offset correction

Fig. 4.6 Magnetometer calibration procedure

4.1 Attitude Estimation Filter

51

Fig. 4.7 Computation of nout 

space which does not contain the origin of the axes) is determined: since Eq. (4.8) is invariant with respect to a constant scale factor, nout  can be either n or −n ; hence, (4.8) cannot be used to discern the correct direction of nout  . This indeterminacy can as follows: be solved by computing nout  nout 

       m3 − m 1 × m2 − m1    . = ±    m − m × m − m  3 1 2 1

(4.13)

  The sign is chosen positive if m 1 , m2 , and m3 lie counter-clockwise on the ellipse (see Fig. 4.7); this condition can be verified considering the sign of the gyroscope angular rate during the calibration turn. Let z N = [0 0 1] denote the direction of the fixed z-axis; in order to align  with the horizontal plane, it is necessary to rotate such plane of the angle given by:

θ = cos −1

   z N nout 

(4.14)

about the axis whose direction can be computed with the cross product: N a = nout  ×z .

(4.15)

θ and a can be used to compute a suitable rotation matrix R(θ , a ) resorting to axis-angle parametrization of rotations [21], which is then applied to the projected points (Fig. 4.6b): (4.16) mih = R(θ , a )mi . In (4.16), the superscript h denotes measurements parallel to the horizontal plane (referred to as horizontal measurements in the following). Then, a standard ellipse fitting procedure can be applied to determine the best fitting ellipse (in a least square sense) to the points (Fig. 4.6c). Without going into

52

4 Navigation Filter

details (see [9, 11] for theoretical proof), given a point P E = [x E y E ] , it lies on the ellipse defined by coefficients C E = [a E b E c E d E e E f E ] if the following equation holds true: (4.17) a E x E2 + b E x E y E + c E y E2 + d E x E + e E y E + f E = 0 . h h h T m i,y m i,z ] , i = 1, . . . , M E , the best For a set of M E different points mih = [m i,x fitting ellipse is obtained by solving the following generalized eigenvalue problem:

SE C E = λ E K E C E CTE K E C E

(4.18)

=1,

(4.19)

where λ E is a scalar value, S E is the scatter matrix built from measurements: S E = P˜E P˜E ∈ R6×6 ⎡ h 2 h h (m 1,x ) m 1,x m 1,y .. .. ⎢ ⎢ . . ⎢ h 2 h h (m ) m P˜E = ⎢ i,x i,x m i,y ⎢ ⎢ .. .. ⎣ . .

(m hM E ,x )2 m hM E ,x m hM E ,y

h 2 (m 1,y )

.. .

h m 1,x

h m 1,y

.. .. . . h 2 h h (m i,y ) m i,x m i,y .. .. .. . . . (m hM E ,y )2 m hM E ,x m hM E ,y

⎤ 1 .. ⎥ .⎥ ⎥ 1⎥ ⎥ , .. ⎥ .⎦ 1

and K E is the constant matrix defined as follows: ⎤ ⎡ 0 0 2000 ⎢0 −1 0 0 0 0⎥ ⎥ ⎢ ⎢2 0 0 0 0 0 ⎥ ⎥ . ⎢ K =⎢ ⎥ ⎢0 0 0 0 0 0 ⎥ ⎣0 0 0 0 0 0 ⎦ 0 0 0000

(4.20)

(4.21)

(4.22)

  Once the components of C E have been determined, the center C x C y , the radii   Rx R y , and the tilt  of the ellipse (the latter indicating the directions the ellipse is scaled along) can be determined; once these quantities are known, its points can be mapped onto the desired circle using the following transformation: ⎛

⎡ r H,h /Rx 0 r H,h /R y mic = ⎝ Rz N () ⎣ 0 0 0 ⎤ ⎡ 0 ⎦ , 0 −⎣ h − HzN m i,z

⎡ ⎤⎞⎞ ⎛ ⎤ Cx 0 0⎦ (Rz N ()) ⎝mih − ⎣C y ⎦⎠⎠ + 0 1 (4.23)

where Rz N () denotes a rotation of an angle  about the fixed z-axis. At the end of the calibration process, the corrected (calibrated) compass measurements mic lie

4.1 Attitude Estimation Filter

53

on a circle with radius r H,h and whose center is on the z-axis at the height of HzN (Fig. 4.6d). In order to validate the developed calibration procedure several tests have been conducted, where the algorithm has been employed to calibrate a compass in both disturbance-free and magnetically perturbed environments; the results of these tests are reported in Chap. 5.

4.1.2 Attitude Estimator Design Changes As stated at the beginning of Sect. 4.1, the use of the standard formulation of the NECF with a commercial IMU and compass system to estimate the attitude of an AUV may give unsatisfying performance; hence, a set of design changes applied to better adapt the algorithm for use in the underwater field are illustrated and justified in this section. Filter on Accelerometer Measurements Through suitable preliminary tests performed out of the water, it has been experimental observed that oscillating movements of the IMU on the horizontal plane cause undesired variations of the roll and pitch angles extracted from the rotation matrix output by the filter; this happens because the accelerometer interprets sudden horizontal movements of the sensor as deviations of the vertical direction, which in turn affect the computation of the rotation matrix. To produce accurate attitude estimates, this phenomenon must be identified and corrected in real-time. To smooth the obtained roll and pitch angles profiles and to reduce the amplitude of these variations, accelerometer measurements have been filtered as follows: a f = Fa (z) a IBMU ,

(4.24)

being a f the filtered measurement, and Fa (z) the transfer function of a suitable digital filter. Particular care has to be taken when choosing the filter order and its cutoff frequency, since a wrong choice of such parameters could result in an undesirable estimate delay. Several filters with different cutoff frequencies have been tested; a second-order filter has then been adopted for the final version of the algorithm, constituting an effective trade-off between estimate accuracy and readiness of the NECF. In particular, Fa (z) is obtained by discretization of the filter with continuous transfer function: fa (4.25) Fa (s) = (s + f a )2 using the bilinear transform F(z) = F(s)|s= T2z−1 , z−1

(4.26)

54

4 Navigation Filter

being f a the cutoff frequency, and T the time between two subsequent filter iterations. Additionally, since only the direction of the acceleration vector is needed, a IBMU is normalized before being filtered by Fa (z); the same operation is executed on a f before its use in ω c (notation has not been changed for the ease of reading). Known Directions Choice Convergence of the NECF has been demonstrated if the direction of at least two known nonparallel fixed vectors can be measured in sensor frame. Considering devices equipped with accelerometers and magnetometers, a common choice is to use the measurements of gravitational acceleration (i.e. the vertical direction) and of Earth’s magnetic field. These quantities are employed in the correction term ω c , where they are compared with their estimates to generate the correction term. The proposed modification consists in choosing only the component of the measured magnetic field which is orthogonal to the direction of acceleration instead of the complete magnetometer measurement (refer to Fig. 4.8; a similar approach can be found in [13, 17]). This choice is justified by the following considerations: first of all, it is worth noting that the plane orthogonal to acceleration approximately coincides with the horizontal plane all the time; this is because the proper acceleration of the vehicle is often negligible if compared with gravity, and because rotations about the x-and y-axis are usually constrained. This means that accelerometer readings measure the body frame direction of the fixed vertical axis z N . In addition, accelerometer measurements are more reliable than magnetometer readings, since they are less susceptible to external error sources. Hence, even if no external magnetic disturbance is present, including the vertical component of the measured magnetic field does not produce further benefits with respect to only using the information given by accelerometers. Thus, once the corrected compass readings mc have been obtained through calibration, their projection onto the plane orthogonal to acceleration is computed:     (4.27) mc⊥ = mc − a f mc a f .

Fig. 4.8 Projection of the measured magnetic field on the plane orthogonal to acceleration

4.1 Attitude Estimation Filter

55

Thanks to the previous observations, mc⊥ (once expressed in the fixed frame) is a vector pointing towards magnetic North; thus, it is compared in ω c with the estimate: ˆ ⊥N ,B = H



  R BN H N

⊥

,

(4.28)

where subscript ⊥ indicates the same projection operation applied to the measurements; additionally, the same considerations made for measured acceleration (concerning normalization) hold. Time Varying Gains In Eq. (4.3), the gains ki of the correction term are constant and fixed before execution. However, during the normal functioning of the filter, unpredictable transitory errors may affect the measurements provided by accelerometers or magnetometers. These dangerous situations should be detected in real-time and the corresponding gains should be scaled according to the actual reliability of each measurement, in order to preserve the accuracy of the computed estimate. Hence, after setting a suitable initial value for each gain, the latter are eventually scaled over time (never exceeding the initial value) so as to reflect the real-time reliability of each measurement. Since in the underwater field proper vehicle accelerations are usually small, while it is quite common to encounter sources of magnetic disturbance, acceleration measurements will be, generally speaking, more reliable than compass readings; in view of this consideration, the accelerometer-related initial gain will be higher than its magnetometer-related counterpart. The scaling criterion is different for acceleration and magnetic field readings, and is detailed here below. • Acceleration gain: in order to avoid that sudden accelerations along some directions generate a wrong contribution to the correction term of the filter, the acceleration gain k1 is linearly decreased with acceleration magnitude if high accelerations occur. During initialization, the average acceleration magnitude a¯ is computed to be used as a reference term. Then, k1 is set according to the relative distance Da between the norm of acceleration measurements and a: ¯    ||a f || − a¯   . (4.29) Da =   a¯

Fig. 4.9 Computation of gain k1

56

4 Navigation Filter

Referring to Fig. 4.9: Dth represents a threshold value which, if exceeded, causes the decrease of the gain until the maximum value Dmax is reached, at which k1 is set to zero. Denoting with k1in the initial (constant) gain, k1 is determined according to Algorithm 3; Algorithm 3 k1 computation Data: k1in , Dth , Dmax Result: gain k1 Compute Da as in (4.29); if Da < Dth then k1 = k1in ; else if Dth α1  α2 > α2 then if αm m th  th  d ; k2 = k2in 1 − k d /kmax k2 ≥ 0; k d + +; k u = 0; else    u k2 = k2 + k2in − k2 k u /kmax ; k2 ≤ k2in ; k u + +; k d = 0; end

angles to become greater than the threshold values; this can occur if large accelerations arise (which is not likely to happen in the field of underwater robotics) or 1 2 , αm fall during motion transients. However, this effect is only temporary and αm again below the threshold in a short amount of time. This solution may appear quite conservative; nevertheless, if a precise gyroscope is available, it is indeed better to discard good magnetometer readings than running the risk of exploiting corrupted magnetic measurements, thus compromising the accuracy of the yaw estimate. The choice of the threshold values depends on the sensitivity of the selected hardware: suitable tests must be executed to evaluate the different effects of both magnetic disturbances and motion transients on the control angles, in order to set threshold values which guarantee that corrupted readings are promptly discarded and, at the same time, reliable measurements are taken into account. After the introduced modifications, it is now possible to specify the particular form of ω c for the considered application:   ˆ ⊥N ,B . ω c = k1 a f × Rˆ BN z N + k2 mc⊥ × H

(4.33)

FOG Integration The majority of commercial IMU and compass systems possess internal algorithms that fuse the raw data coming from the sensors they are equipped with in order to estimate their own orientation. The use of a standalone attitude estimation filter, on the other side, yields the possibility of increasing the accuracy of the computed estimate by using data that come from sensors that are not originally present within the device. This is extremely useful, e.g., in low cost applications, where cheaper MEMS sensors can be used together with more precise devices. In the considered case, a single-axis FOG has been mounted with its sensitive axis parallel to the IMU gyroscope z-axis. Its measurement completely substitutes the angular rate change read on that axis by the IMU gyroscope within the filter.

4.1 Attitude Estimation Filter

59

This choice is justified by the following consideration: the magnetometer-related contribution in ω c has the purpose of obtaining an accurate estimate of the yaw angle. In the presence of magnetic disturbances the gain k2 is set to zero, and the yaw estimate is obtained by raw integration of angular velocity. Due to the relatively low precision of a MEMS gyroscope, a significant yaw drift is registered even if the sensor is not moving. However, since the FOG possesses a much lower drift with respect to MEMS gyroscopes, the employment of its reading for the z-axis angular rate change measurement allows to reach a high level of accuracy; through its use, the risk of unacceptable growth of the integration error when the magnetometer is not employed is avoided. Nonetheless, due to the high sensitivity of a FOG, the device is able to sense Earth’s rotation, producing a nonzero output of up to 15◦ per hour (which is, in this context, detrimental) even if the component is not rotating. However, this effect can be compensated exploiting the knowledge of the latitude at which the sensor is operating and the information regarding the attitude of the vehicle it is mounted on. Referring to Fig. 4.11: it is assumed that the sensor is operating in Earth’s northern hemisphere; however, the compensation procedure is conceptually the same on the whole planet surface. Let ω E denote Earth’s angular velocity; its magnitude, expressed in radians per second, is then (4.34) ω E = ||ω E || ∼ = 7.2921 · 10−5 . Latitude  is supposed to be known. The idea is to determine the component of Earth’s angular rate acting on the body z-axis and subtract it from the sensor reading. At first, it is convenient to express Earth’s angular velocity in the NED frame as follows: ⎡ ⎤ ω E cos () ⎦ 0 ω NE = ⎣ (4.35) −ω E sin () .

Fig. 4.11 FOG correction term computation

60

4 Navigation Filter

Exploiting the current attitude estimate, the bias to be subtracted from the device’s measurement is the third component of the vector:   ω EB = Rˆ BN ω NE ,

(4.36)

which gives the corrected FOG measurement: B . ω cF OG = ω mFOG − ω E,z

(4.37)

Applying the proposed correction, it is possible to considerably reduce the drift of the device, substantially increasing its accuracy.

4.2 Position Estimation Filter This section is dedicated to the position estimation filter developed during the Ph.D. period. As stated at the beginning of this chapter, such filter is based on the UKF; it is thus necessary to derive a state-space model for the whole AUV system. The choice of a suitable vehicle model to adopt is indeed very delicate: the latter must constitute a smart and efficient trade-off between required computational load (since it must be used on board in real-time) and accuracy. In literature, AUV models can be usually classified into two main categories: complete dynamic models, which draw directly from (3.5), and kinematic models. While the former are capable of capturing more information about the evolution of the system, they inevitably require additional computational resources with respect to the latter; where hardware limitations prevail, it may be possible to resort to a completely kinematic model in favor of a reduced burden on the processing unit of the vehicle. In this work, a mixed kinematic and dynamic model has been adopted, suitably modifying (3.5) in order to take into account longitudinal dynamics only. This choice is justified by the fact that, for underwater vehicles, considerable dynamics takes place only on the direction of forward motion: most of the time AUVs move along the longitudinal direction (since it usually constitutes the direction of minimal resistance, this allows to reduce power consumption); this is especially true for torpedo-shaped vehicles, where lateral and vertical motion is strongly dampened by drag forces. Hence, the chosen model offers a satisfying level of accuracy, better describing the physics of a real vehicle compared with a kinematic only model, offering at the same time a reduction of the overall number of unknown parameters of the system and requiring less computational resources with respect to a complete dynamic model. The derived model is then used on board, within the position estimation filter of the vehicle; the latter is based on the UKF introduced in Sect. 3.2. The choice of an UKFbased position estimator is investigated as an alternative solution to the KF and the EKF, whose use in the marine field is quite common and extensively documented in literature. The UKF has been proposed in order to cope with the issues that may arise

4.2 Position Estimation Filter

61

with the two above-mentioned estimators: indeed, AUV models may be nonlinear and non-differentiable (as is the case of the adopted model), preventing the use of the KF and hindering the use of the EKF, since the latter requires the explicit computation of the derivatives of the equations of the state-space model of the system, which can be problematic in the case of functions with discontinuities and/or jumps. The UKF, on the other side, is completely derivative-free; in addition, the computational load it requires can be efficiently handled by the hardware which is today present on the majority of AUVs. In addition to position estimation, it is shown how the UKF-based navigation filter can be used to simultaneously estimate sea currents acting on the vehicle. Such operation is performed without the need for further instrumentation: indeed, Sect. 4.2.2 shows how the state-space model of the system, described in details in Sect. 4.2.1, can be suitably modified in order to take into account the effects of currents; in particular, the model is rewritten in terms of the relative velocity ν r , and a contribution proportional to current intensity is included into lateral dynamics. Then, current velocity components are added to the state vector and estimated on board, increasing at the same time the accuracy and the reliability of the estimated position.

4.2.1 AUV State-Space Model To describe the behavior of the AUV, the following state variables have been used:  x=

η1 ν1

 .

(4.38)

x is a vector in R6 composed of the NED frame position and of the body frame linear velocity of the vehicle. The state evolves according to a strictly causal, mixed kinematic and dynamic model, taking into account longitudinal dynamics: ⎡

R BN

   η 2 k−1 (ν 1 )k−1

⎤

  ⎢⎡ ⎤⎥ ⎢ τ (ν ,u ) Dx (ν1,x )k−1 (ν1,x )k−1 ⎥ 1,x k−1 k−1 ⎢ ⎥ + T ⎢⎢ − ⎥⎥ + wk−1 . m m k k−1 ⎣⎣ ⎦ ⎦ 0 0 (4.39) In (4.39), T is the (fixed) sampling time of the filter, and process noise w has been modeled as a zero mean stationary white noise. Furthermore, it is worth noting that, since attitude is accurately estimated by the dedicated filter presented in Sect. 4.1, there is no need to include it within the state vector of the system; instead, the orientation of the vehicle constitutes an input to the system evolution equation.



η1 ν1





η = 1 ν1



62

4 Navigation Filter

For what concerns longitudinal dynamics, the fourth equation of (4.39) derives from the complete dynamic model (3.5); additionally, gravitational, centripetal and Coriolis effects have been neglected, and only longitudinal force has been taken into account (to a first approximation, moments generated by the propellers and acting on the AUV can be neglected as their influence can be greatly reduced thanks to a smart mechanical design of the vehicle). The damping coefficient Dx has been computed as in (3.14), and drag coefficients C D,x have been identified for all the vehicles employed for the validation of the model resorting to experimental tests. For long and slender vehicles (such as MARTA AUV or Typhoon class vehicles), the area coefficient A f,x has been computed, according to [18], as the product of length and diameter of an equivalent cylinder representing the vehicle. Particular attention has to be given to the determination of the total force and moment acting on the AUV: such effects are a function of the thrust generated by the propellers of the vehicle, which depends on their rotational speed (i.e. the control input u to the system) and on their advance speed (which is, in turn, a function of the body frame velocity ν); the thrust exerted by all the propellers is then mapped onto total force and moment by τ (ν, u). Many literature sources provide the relation between thrust, rotational speed, and advance speed in the form of experimentally derived curves or look-up tables [5, 20]; additionally, it is not uncommon for these curves to be derived for particular operating conditions only (e.g., only for positive rotational and advance speeds). These relations, even if accurate, are hardly exploitable within a recursive estimation filter, where an analytical expression for such functions would be preferred. In this work, the propulsion system model presented in [6] has been adopted; such model provides indeed an analytical expression for the sought after relation, valid for all operating conditions, and constitutes a suitable trade-off between computational load and modeling accuracy. A sketch of the derivation of the chosen propulsion model is illustrated here below; for a detailed discussion, refer to [6] and references contained therein. Propulsion System Model The first step is the determination of the advance speed of a propeller. Let P Bp,i and V Bp,i denote, respectively, the position and the velocity of the i-th propeller, expressed in the body frame; then, the following equation holds:   V Bp,i = ν 1 + ν 2 × P Bp,i − O B ;

(4.40)

additionally, let n Bp,i indicate its line of action. The advance speed of the propeller is given by the projection of its velocity on n Bp,i :   Va,i = V Bp,i n Bp,i .

(4.41)

4.2 Position Estimation Filter

63

In particular, since an AUV usually moves in the direction of forward motion, the transverse velocity V Bp,i,t = V Bp,i − Va,i n Bp,i can be neglected for its rear propellers, and (4.40) is simplified as follows:   Va,i ∼ = V Bp,i  .

(4.42)

Let n i denote the rotational speed of the i-th propeller; then, once its advance speed is known, the exerted thrust is computed according to the following equation [1, 6]:      kb |n i | g sgn (n i ) Va,i 2 Ti n i , Va,i = sgn (n i ) kb n i − , pp

(4.43)

where sgn (·) is the sign function, and g (·) is a piecewise function defined as ⎧ ⎪ ⎨0 f or x ≤ 0 g (x) = x f or 0 < x ≤ |n i | p p ⎪ ⎩ |n i | p p f or x > |n i | p p

.

(4.44)

In (4.43)–(4.44), p p indicates the propeller pitch, while the bollard thrust coefficient kb is derived from: (4.45) Ti (n i , 0) = kb n i2 . As shown in Eq. (4.45), it is assumed that the relation between the rotational speed of a propeller and the thrust exerted at zero advance speed (bollard thrust) is (at least to a first approximation) quadratic; this behavior can be observed while performing suitable experimental test, which can be used to determine the value of kb . After the thrust exerted by each propeller of the vehicle has been computed, the resulting force and moment acting on the AUV can be determined. Let     U = T1 T2 · · · Ti · · · Tn p ∈ Rn p , Ti = Ti n i , Va,i

(4.46)

denote the vector collecting the thrust given by the n p thrusters of the vehicle; then, τ (ν, u) is given by:   τ (ν, u) = BU (ν, u) , u = n 1 n 2 · · · n p .

(4.47)

Matrix B ∈ R6×n p is constant and dependent on vehicle geometry, and is expressed as follows:   B1 , (4.48) B= B2 with       B1 = · · · n Bp,i · · · , B2 = · · · P Bp,i − O B × n Bp,i · · · .

(4.49)

64

4 Navigation Filter

For what concerns the available measurements, the proposed filter is able to exploit position measurements, them being either GPS fixes (used to initialize the filter while the vehicle is on surface) or position fixes coming from a dedicated underwater acoustic localization system (for instance, in the following equation the availability of an USBL system is assumed), and depth and velocity measurements; the measurement vector at the k-th time instant can be expressed as follows: yk =

   N  N  B   PU S B L d DS v DV L PGN P S . k

(4.50)

From (4.50), it is easy to note that y contains direct measurements of the components of the state vector; hence, the measurement equation is affine with respect to x: yk = Hk xk + vk ,

(4.51)

where matrix Hk contains only 1 or 0 elements. As in (4.39), measurement noise in (4.51) is assumed additive, stationary and with zero average value; additionally, initial state and process and measurement noises are assumed to have zero crosscorrelation. As a concluding remark, it is worth noting that the model in (4.39), together with (4.43)–(4.44), is nonlinear and non-differentiable (due to the presence of e.g., quadratic and sign functions), which may favor the adoption of a derivative-free estimation algorithm such as the UKF.

4.2.2 Sea Current Estimation Marine currents can considerably affect the motion of an underwater vehicle performing an autonomous mission, and they may even pose a serious threat to the success of the task itself. Several solutions exist to measure sea current, involving e.g., the use of dedicated instrument such as ADCPs. However, these devices are quite expensive and require adequate deployment, increasing the cost of operations at sea. Within this work, an alternative approach is proposed: currents are estimated in real-time by the vehicle, exploiting measurements which are already employed by the position estimation filter and the knowledge of the dynamic model of the vehicle, without requiring the purchase and the installation of additional devices. Additionally, the availability of a current estimate on board is likely to improve the accuracy of the computed position estimate as well. In particular, the state vector in (4.38) is augmented in order to include the first two components of ν cN (i.e. North and East current components); at this stage, the vertical component of the current has not been taken into account. The complete system model (taking currents into account) has been rewritten in terms of the relative velocity ν r , and evolves according to the following equations [3, 8]:

4.2 Position Estimation Filter

⎤ ⎡ ⎤ η1 η1 ⎢ν1 ⎥ ⎢ν1 ⎥ ⎢ N⎥ =⎢ N⎥ + ⎣u c ⎦ ⎣u c ⎦ N vc k vcN k−1 ⎡

65

⎡

R BN

   η 2 k−1 (ν 1 )k−1

⎤

  ⎢⎡ ⎤⎥ ⎢ τ1,x ((ν r )k−1 ,uk−1 ) Dx (ν1,x,r )k−1 (ν1,x,r )k−1 ⎥ ⎢ ⎥ − m m ⎢⎢ ⎥⎥   ⎢⎢ ⎥ ⎥ D ν ν ( ) ( ) y 1,y,r 1,y,r k−1 k−1 +T ⎢⎣ ⎦⎥ + wk−1 . − ⎢ ⎥ m ⎢ ⎥ 0 ⎢ ⎥  ⎣ ⎦ 0 0

(4.52)

As anticipated in Sect. 3.1.1, current has been considered constant (in the NED frame) over the operational area of the vehicle. A drag force contribution due to currents has been included into lateral dynamics; the lateral damping coefficient D y has been computed according to (3.14), with a resulting drag coefficient C D,y which is much higher than C D,x . To a first approximation, the current-related contribution J˙c ν cN has been neglected, since it is null when the vehicle reaches steady-state speed or, more generally, when the latter moves in a straight line; finally, an additive process noise (with the same spectral characteristics as those introduced for the other state variables) has been included in order to take into account slow current variations. The model in (4.52) is used within the same UKF-based navigation filter presented in previous sections, which is employed to simultaneously estimate the position of the vehicle and sea currents acting on the latter without a relevant computational cost increase (with respect to position only estimation), and without requiring any additional instrumentation. Observability of the System in the Presence of Sea Currents In order to justify the proposed current estimation strategy, the observability of the system in the presence of marine currents, exploiting the same measurements as the position estimator, is provided here below. For the ease of reading, the analysis is carried out on the continuous-time version (prior to discretization) of the model (4.52) with the measurement equation (4.50); additionally, the following assumptions have been made: 1. the magnitude of the current in the fixed frame is greater than zero; 2. the vehicle moves along the positive direction of its body x-axis, propelled by two rear thrusters (whose line of action is indeed x B ); they exert the same thrust, so that the only control input is their common positive rotational speed n t (conventionally, a positive n t implies a positive body-frame thrust); 3. the advance speed of the thrusters with respect to the current, expressed in body frame (equal to that of the vehicle, i.e. ν1,x,r ) is positive and less than n t p p ; together with the previous hypothesis, this implies that the propulsion model (4.43)–(4.44) is differentiable with respect to state components;

66

4 Navigation Filter

4. the orientation of the vehicle is not changing. Recalling that orientation is a timevarying input, this assumption equals to considering the system autonomous. Furthermore, it is assumed that roll and pitch are zero; 5. with respect to (4.50), despite their heterogeneity, GPS and USBL fixes are treated as generic position measurements. Additionally, it is worth reminding that it is assumed that absolute velocity measurements are available (i.e. obtained with a DVL with constant bottom lock). Finally, let us assume that the vehicle is not moving in the direction of the current or in a direction perpendicular to it, so that current can be experienced on both the body-frame x- and y-axis; this last hypothesis will then be relaxed. In light of assumptions 1–5, the state-space representation of the system is the following: ⎤ ⎡ ⎤ η˙ 1 f 1 (x, n t ) ⎢ ν˙ 1 ⎥ ⎢ ⎥ .. ⎥ x˙ = f (x, n t , w) = ⎢ ⎦+w= . ⎣u˙ cN ⎦ = ⎣ N f n x (x, n t ) v˙c   ⎡ ⎤ N R B η2 ν 1  ⎢ 2 k n 2 − kb n t ν1,x,r − ρA f,x C D,x ν 2 ⎥ ⎢m b t 1,x,r ⎥ pp 2m ⎢ ⎥ ρA f,y C D,y 2 ⎢ ⎥ ν =⎢ 1,y,r ⎥ + w ∈ Rn x , 2m ⎢ ⎥ 0 ⎢ ⎥ ⎣ ⎦ 0 ⎡

⎡ ⎢ y=⎣

⎤

(4.53)

0

h 1 (x, v)  ⎥  .. n ⎦ = I6×6 06×2 x + v ∈ R y ; .

(4.54)

h n y (x, v)

in the considered case, n x = 8 and n y = 6. For the purpose of the observability analysis, the sign of the fifth equation in (4.53) can be arbitrarily chosen (it has been assumed positive in this context); additionally, the x- and y-axis components of the relative velocity are given by: ν1,x,r = u − cψ u cN − sψ vcN ,

(4.55)

ν1,y,r = v + sψ u cN − cψ vcN ,

(4.56)

being cψ and sψ the sine and the cosine of the yaw angle. Due to the nonlinearity of the system, the observability analysis is carried out exploiting differential geometry as explained in [12]. In particular, a sufficient condition to guarantee the local weak observability of the system (4.53)–(4.54) (see [12] for the definition and the corresponding proof) is that: dim (d O) = n x ,

(4.57)

4.2 Position Estimation Filter

67

where  d O = span

∂Lkf(x,n t ,w) h j (x, v) ∂x

,

j = 1, . . . , n y , k ∈ N .

(4.58)

In (4.58), Lkf(x,n t ,w) h j (x, v) is the k-th order Lie derivative of the j-th measurement function with respect to the vector field f (x, n t , w), computed as follows: L0f(x,n t ,w) h j (x, v) = h j (x, v) ! " ∂h j (x, v)  f (x, n t , w) L1f(x,n t ,w) h j (x, v) = ∂x .. .  k−1  h v) ∂L (x, j ,w) f(x,n t Lkf(x,n t ,w) h j (x, v) = f (x, n t , w) . ∂x

(4.59)

In this context, it is equivalently shown that the matrix dO =



∂h 1 (x,v) ∂x

···

∂h n y (x,v) ∂L1f(x,n t ,w) h 4 (x,v) ∂L1f(x,n t ,w) h 5 (x,v) ∂x ∂x ∂x

 (4.60)

has full rank (i.e. eight linearly independent columns). From (4.54), it is easy to observe that:    I6×6 ∂L1f(x,nt ,w) h 4 (x,v) ∂L1f(x,nt ,w) h 5 (x,v) ; (4.61) dO = ∂x ∂x 02×6 this implies that, in addition to being linearly independent, the last two columns of d O must contain non-zero elements on the seventh and eighth rows in order to guarantee local weak observability. For the ease of reading, let us define Kx =

ρA f,x C D,x ρA f,y C D,y , Ky = ; 2m 2m

hence, explicitly computing the Lie derivatives yields: " ! ∂h 4 (x, v)  L1f(x,n t ,w) h 4 (x, v) = f (x, n t , w) = f 4 (x, n t ) = ∂x ! "   2 kb n t  2 kb n 2t − u − cψ u cN − sψ vcN − K x u − cψ u cN − sψ vcN = m pp

(4.62)

68

4 Navigation Filter

and !

∂h 5 (x, v) ∂x   N N 2 . = K y v + sψ u c − cψ vc

L1f(x,n t ,w) h 5 (x, v) =

"

f (x, n t , w) = f 5 (x, n t ) = (4.63)

Differentiating with respect to state components allows to obtain the last two columns of d O: ∂L1f(x,n t ,w) h 4 (x, v) ∂x

 2k n = 0 0 0 − mpb pt − 2K x ν1,x,r 0 0

2kb n t cψ mp p

+ 2K x cψ ν1,x,r

2kb n t sψ mp p

+ 2K x sψ ν1,x,r



(4.64)

and ∂L1f(x,n t ,w) h 5 (x, v) ∂x

 = 0 0 0 0 2K y ν1,y,r 0 2K y sψ ν1,y,r −2K y cψ ν1,y,r



.

(4.65)

Observing (4.64)–(4.65), it is easy to verify that rank loss of d O is associated with those points (in state space) that zero the determinant of the following matrix: # dO = given by:

2kb n t cψ mp p

+ 2K x cψ ν1,x,r 2K y sψ ν1,y,r

  d O  = −4K y ν1,y,r

!

2kb n t sψ mp p

+ 2K x sψ ν1,x,r −2K y cψ ν1,y,r

kb n t + K x ν1,r,x mp p

$ ,

(4.66)

" .

(4.67)

In light of the assumptions made at the beginning of the paragraph, the determinant of d O is always different than zero, thus guaranteeing the local weak observability of the current estimator. As anticipated, it is possible to relax the assumption that the vehicle is not moving in a direction that is parallel or perpendicular to the direction of the current. Let us now consider, for instance, the first case: the observability analysis is simplified due to the fact that the motion on the horizontal plane is constrained on a line parallel to the direction of the current, reducing the dimension of the system. In particular, without taking into account the depth of the vehicle (a direct measurement of it is obtained using the available sensors), observability can be evaluated on the following system:

4.2 Position Estimation Filter

69

⎡    ⎤ ⎡ ⎤ x˙ f1 x , nt     x˙  = ⎣ν˙  ⎦ = f x , n t , w = ⎣ f 2 x , n t ⎦ + w =    ν˙c f 3 x , n t ⎡ ⎤ ν     2⎥ ⎢ = ⎣ m2 kb n 2t − kpb n t νr − K x νr ⎦ + w p 0 ⎡ ⎤         x x , v h 1 0 0 ⎣ν  ⎦ + v , y = 1     + v = 010 h2 x , v  νc 

(4.68)

(4.69)



where K x has the same meaning as before, and νr = ν  − νc ; the state is composed of position, velocity, and current velocity along a line parallel to the direction of the current (recall that the latter is the only current component that needs to be estimated). Exploiting the same tools used for the previous case, the local weak observability of (4.68)–(4.69) is guaranteed by showing that the matrix 

dO =

 ∂h 1 (x ,v ) ∂h 2 (x ,v ) ∂x ∂x 



∂L1 f

(

x ,n t ,w

)

h 2 (x ,v ) 



∂x

(4.70)

has full rank.  Explicitly computing the elements of d O yields: ⎤ 10 0 ⎥ 2kb n t  ⎢ d O = ⎣0 1 − mp p − 2K x νr ⎦ ;  b nt 0 0 2k + 2K x νr mp p ⎡

(4.71)

hence, the system is locally weakly observable since the last element of the third column is always positive. Analogously, observability is guaranteed even if the vehicle moves in a direction perpendicular to that of the current. In this case, the system under study is the following: ⎡ ⊥  ⊥ ⎤ ⎤ f1 x x˙ ⊥  ⊥ ⊥  ⎢ ⊥  ⊥ ⎥ ⊥ ⊥ ⊥ x˙ = ⎣ν˙ ⎦ = f x , w = ⎣ f 2 x ⎦ + w⊥ =   ν˙c⊥ f 3⊥ x⊥ ⎡ ⎤ ν⊥   = ⎣ K y νr⊥ 2 ⎦ + w⊥ 0 ⎡ ⎤ #  $   x⊥ ⊥ ⊥ ⊥ h x , v 1 0 0 1 ⎣ν ⊥ ⎦ + v⊥ , y⊥ = ⊥  ⊥ ⊥  + v⊥ = 010 h2 x , v νc⊥ ⎡

(4.72)

(4.73)

70

4 Navigation Filter

where the used terms have obvious meaning. As before, the sign of the second equation in (4.72) is arbitrary. The condition (4.57) for the system (4.72)–(4.73) is verified evaluating the rank of the following matrix: ⊥

dO =

 ⊥ ⊥ ⊥ ⊥ ⊥ ∂h ⊥ 1 (x ,v ) ∂h 2 (x ,v ) ∂x⊥ ∂x⊥

∂L1⊥ f

h ⊥ x⊥ ,v⊥ ) (x⊥ ,w⊥ ) 2 ( ∂x⊥

 .

(4.74)

Indeed, computation of the Lie derivatives yields: ⎡

⎤ 10 0 d O = ⎣0 1 2K y νr⊥ ⎦ , 0 0 −2K y νr⊥ 

(4.75)

⊥

showing that d O has full rank since its last column has nonzero elements under assumptions 1–5, thus guaranteeing local weak observability.

References 1. 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. J Mechatron 39:185–195 2. Allotta B, Caiti A, Costanzi R, Fanelli F, Fenucci D, Meli E, Ridolfi A (2016) A new AUV navigation system exploiting unscented Kalman filter. J Ocean Eng 113:121–132 3. Allotta B, Costanzi R, Fanelli F, Monni N, Paolucci L, Ridolfi A (2017) Sea currents estimation during AUV navigation using Unscented Kalman Filter. In: Proceedings of the IFAC 2017 world congress, Toulouse (FR), July 9–14 4. Allotta B, Costanzi R, Fanelli F, Monni N, Ridolfi A (2015) Single axis FOG aided attitude estimation algorithm for mobile robots. J Mechatron 30:158–173 5. Carlton JS (2007) Marine propellers and propulsion, 2nd edn. Elsevier 6. Costanzi R (2015) Navigation systems for unmanned underwater vehicles. PhD Dissertation, University of Florence, Florence (IT) 7. 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 8. Costanzi R, Fanelli F, Ridolfi A, Allotta B (2016) Simultaneous navigation state and sea current estimation through augmented state Unscented Kalman Filter. In: Proceedings of the MTS/IEEE OCEANS’16 Monterey, Monterey (CA, US), Sept 19–22 9. Fitzgibbon A, Pilu M, Fisher RB (1999) Direct least square fitting of ellipses. IEEE Trans Pattern Anal Mach Intell 21(5): 10. Gebre-Egziabher D, Elkaim G, David Powell J, Parkinson B (2006) Calibration of strapdown magnetometers in magnetic field domain. J Aerosp Eng 19(2): 11. Halíˇr R, Flusser J (1998) Numerically stable direct least square fitting of ellipses. In: Proceedings of the WSCG international conference in central Europe on computer graphics and visualization 12. Hermann R, Krener AJ (1977) Nonlinear controllability and observability. IEEE Trans Autom Control 22(5):728–740 13. Hua M-D, Rudin K, Ducard G, Hamel T, Mahony R (2011) Nonlinear attitude estimation with measurement decoupling and anti-windup gyro-bias compensation. In: Proceedings of the 18th IFAC world congress 2011, Milan (IT), Aug 28–Sept 2, pp 2972–2978

References

71

14. Kok M, Schön TB (2014) Maximum likelihood calibration of a magnetometer using inertial sensors. In: Proceedings of the 19th world congress of the international federation of automatic control, Cape Town (ZA), Aug 24–29, pp 92–97 15. Mahony RE, Hamel T, Pflimlin JM (2008) Nonlinear complementary filters on the special orthogonal group. IEEE Trans Autom Control 53(5):1203–1218 16. Markovsky I, Kukush A, Van Huffel S (2004) Consistent least squares fitting of ellipsoids. Numerische Mathematik 98(1):177–194 17. Martin P, Salaün E (2010) Design and implementation of a low-cost observer-based attitude and heading reference system. Elsevier Control Eng Pract 18(7):712–722 18. Munson BR, Okiishi TH, Huebsch WW, Rothmayer AP (2012) Fundamentals of Fluid Mechanics, 7th edn. Wiley, USA 19. National Geophysics Data Center. www.ngdc.noaa.gov 20. Pivano L, Johansen TA, Smogeli ØN (2009) A four-quadrant thrust controller for marine propellers with loss estimation and anti-spin: theory and experiments. Mar Technol 46(4):229– 242 21. Siciliano B, Khatib O (2008) Handbook of robotics. Springer handbooks, Napoli and Stanford 22. Vasconcelos JF, Elkaim G, Silvestre C, Oliveira P, Cardeira B (2011) A geometric approach to strapdown magnetometer calibration in sensor frame. IEEE Trans Aerosp Electron Syst 47(2):1293–1306

Chapter 5

Results

This chapter collects the results of tests performed with the aim of validating and evaluating the performance of the developed navigation algorithms. These tests, executed throughout the whole Ph.D. period, involved all the AUVs of the MDM Lab, and consist in simulations using experimental data acquired at sea (to, e.g., test the correctness of the algorithms or to investigate the causes of unexpected or undesired behavior), preliminary experimental tests executed in controlled environments, and even complete online validation tests, where the developed algorithms were actively used within the GNC loop of the vehicles. All the unknown coefficients, tunable parameters, and gains introduced in previous chapters were identified or set, for each vehicle, according to the outcome of preliminary experimental tests in order to achieve the best possible behavior for the algorithms under test. Following the logic of previous chapters, results of tests regarding the developed attitude estimation filter are presented first (Sect. 5.1); indeed, the computation of an accurate orientation estimate is necessary in order to guarantee the availability of a precise and reliable input to the position estimator. Then, test results concerning the online validation of the latter are detailed in Sect. 5.2: the UKF-based filter presented in previous chapters is used online during suitable autonomous navigation missions. Finally, Sect. 5.3 reports results of simultaneous position and sea current estimation, highlighting the effectiveness of the solution illustrated in Sect. 4.2.2.

5.1 Attitude Estimator To evaluate the performance of the developed attitude estimation algorithm, several experimental tests have been performed; for these tests, TifOne and FeelHippo AUVs have been employed. It is worth remembering that both vehicles are equipped with an IMU and compass system manufactured by Xsens and a single-axis FOG by KVH, © Springer Nature Switzerland AG 2020 F. Fanelli, Development and Testing of Navigation Algorithms for Autonomous Underwater Vehicles, Springer Theses, https://doi.org/10.1007/978-3-030-15596-4_5

73

74

5 Results 0.4 Rotated magnetometer measurements Ellipse center Origin

0.3 0.2 0.1 0 -0.1 -0.2 -0.3 -0.4 -0.5

0

0.5

Fig. 5.1 Disturbance-free calibration test: rotated magnetic field measurements

mounted with its sensitive axis parallel to the IMU z-axis. In particular, in addition to outputting accelerometer, magnetometer and gyroscope data, the former possesses a proprietary magnetometer calibration algorithm and an internal attitude estimation filter which is able to compute a three-dimensional estimate of the orientation of the vehicle. Indeed, for these tests the output of such filter has been used both as a ground truth (in the case of magnetically undisturbed environments, both the proprietary and the developed filters shall correctly estimate the attitude of the vehicle), and at the same time it has been used to highlight the performance improvement obtained in the case of magnetically perturbed environments (since the internal filter is slower to react to such disturbances with respect to the proposed algorithm, corrupted magnetic field measurements are used by the former, resulting in a relevant drift of the yaw estimate from the true value). Preliminary tests have been executed to validate the magnetometer calibration procedure introduced in Sect. 4.1.1; the results of two of these experiments are reported here below. The first test was carried out in a disturbance-free environment, while before performing the second a metal object has been intentionally placed close to the sensor housing (with the sensor and the disturbance source moving together). For both tests, magnetic field measurements collected during a complete turn of the vehicle (the results shown here refer to FeelHippo AUV) about its z-axis are reported in Figs. 5.1 and 5.3 (in particular, horizontal measurements are shown), along with the same data after the mapping operated by the calibration algorithm (Figs. 5.2 and 5.4); it is worth remembering that, in the absence of magnetic disturbances, compass readings would lie on a circle centered at the origin of the xy plane.

5.1 Attitude Estimator

75

10 4 Calibrated magnetic field measurements Theoretical circle Origin

2 1.5 1 0.5 0 -0.5 -1 -1.5 -2 -3

-2

-1

0

1

2

3 10 4

Fig. 5.2 Disturbance-free calibration test: calibrated magnetic field measurements

Rotated magnetometer measurements Ellipse center Origin

0.5 0.4 0.3 0.2 0.1 0 -0.1 -0.2 -0.3 -0.4 -0.4

-0.2

0

0.2

0.4

0.6

0.8

Fig. 5.3 Magnetically perturbed calibration test: rotated magnetic field measurements

76

5 Results 10 4 Calibrated magnetic field measurements Theoretical circle Origin

2 1.5 1 0.5 0 -0.5 -1 -1.5 -2 -3

-2

-1

0

1

2

3 10 4

Fig. 5.4 Magnetically perturbed calibration test: calibrated magnetic field measurements

For the first test (Fig. 5.1), the measurement locus is close to the theoretical circle; only a minor Hard Iron effect is present, caused by electronics surrounding the compass housing (which in turn induce a slight shift of the centroid of the measurement locus from the origin). Concerning the second test instead (Fig. 5.3), the deviation from the theoretical locus is relevant (indicating the presence of both Hard and Soft Iron disturbances). Table 5.1 reports the characteristic parameters (coordinates of the center and length of the radii) of the ellipses shown in Figs. 5.1 and 5.3. For the first test, as stated before, only a minor Hard Iron disturbance is present, while the radii of the ellipse are very close (no Soft Iron effect); in this case, no significant correction is applied to the direction of the magnetometer readings.

Table 5.1 Measurement ellipse characteristics

Center coordinates [AU]

Radii length [AU]

Test 1

Test 2

C x : −0.0039

C x : 0.1873

C y : −0.0290

C y : 0.0801

Rx : 0.4139

Rx : 0.5546

R y : 0.4057

R y : 0.4450

5.1 Attitude Estimator

77

0.2 0 -0.2 0

5

10

15

20

25

30

35

0

5

10

15

20

25

30

35

0

5

10

15

20

25

30

35

0.2 0 -0.2

1 0 -1

di f f

Fig. 5.5 η 2

obtained during the first test

On the other side, the parameters obtained during the second calibration test indicate relevant shift and tilt, evidence of significant Hard and Soft Iron disturbances. Nonetheless, in both cases the proposed calibration algorithm is able to correctly compensate magnetic disturbances, mapping the measurements onto the theoretical locus, as visible from Figs. 5.2 and 5.4 (note that calibrated measurements are expressed in nT , while the compass outputs readings in arbitrary units; however, this conversion is irrelevant to the attitude estimator, since only the direction of the magnetic field is employed). After validating the calibration procedure, suitable attitude estimation tests were executed; at first, simpler estimation tests were performed in a controlled environment using TifOne AUV in order to check the proper functioning of the filter in a real experimental scenario and to highlight the achieved improvements over the IMU and compass system internal filter in the case of magnetically disturbed environments; then, the proposed filter was tested online during the execution of an autonomous navigation task. The first test, whose results are reported here below, aimed at comparing the attitude estimate computed by the proposed filter with that of the internal algorithm of the IMU and compass system. This very first test consists in a complete turn about the z-axis of the vehicle, executed in a disturbance-free environment. It is worth noting that the estimate of the proprietary filter exploits the proprietary compass calibration algorithm, while that computed by the proposed filter uses the calibration procedure described in Sect. 4.1.1.     Let η 2AE = φ AE θ AE ψ AE and η 2I C S φ I C S θ I C S ψ I C S denote, respectively, the attitude estimated by the two filters (where superscripts AE and I C S stand for attitude estimator and IMU and compass system), and let R BN ,AE and R BN ,I C S indicate the

78

5 Results ICS

150

AE

100 50 0 -50 -100 -150

0

5

10

15

20

25

30

35

Fig. 5.6 Yaw estimates computed during the first test

corresponding rotation matrices. Then, the roll, pitch, and yaw estimation differences di f f η 2 , extracted from the rotation matrix N ,di f f

RB

  = R BN ,AE R BN ,I C S ,

are reported in Fig. 5.5, while Fig. 5.6 shows the yaw estimated by both filters. It di f f can be noted that the magnitude of all the components of η 2 is very small; in particular, Fig. 5.6 highlights how the two yaw estimates almost overlap during the entire test. Hence, in the absence of magnetic disturbances, using the attitude estimated by a commercial device as ground truth, it has been shown that the developed filter is able to compute an accurate estimate of the complete orientation of the vehicle. Subsequent experimentation showed that this behavior is maintained even in a magnetically perturbed environment. In particular, the results of a second test are reported; such test is part of a series of validation experiments, all of them following the same structure: after initializing both filters, a source of magnetic disturbance (i.e. a metal object) is placed close to the compass housing while the vehicle is not moving; then, the AUV is rotated about its z-axis together with the disturbance source; finally, the metal object is removed and the vehicle is rotated back to its initial orientation. Figures 5.7, 5.8, 5.9 and 5.10 show the obtained results. di f f Similarly to the previous test, Fig. 5.7 shows the components of η 2 : while φdi f f and θdi f f remain close to zero, a relevant yaw difference is present. This is due to the proprietary filter not recognizing (and using) the corrupted magnetic field measurements, resulting in an incorrect attitude estimate. This is supported by Figs. 5.8 and 5.9: Fig. 5.8 shows that after the magnetic disturbance source gets close

5.1 Attitude Estimator

79

0.2 0 -0.2 0

20

40

60

80

100

120

140

160

180

0

20

40

60

80

100

120

140

160

180

0

20

40

60

80

100

120

140

160

180

0.2 0 -0.2

60 40 20 0

di f f

Fig. 5.7 η 2

obtained during the second test

Fig. 5.8 Yaw estimates computed during the second test

80

5 Results

Fig. 5.9 FOG angular rate measured during the second test 0.2 0.1 0 -0.1

0

20

40

60

80

100

120

140

160

180

0 0

20

40

60

80

100

120

140

160

180

20

40

60

80

100

120

140

160

180

50

30 20 10 0 0

Fig. 5.10 k2 and control angle values obtained during the second test

5.1 Attitude Estimator

81

to the sensor, ψ I C S deviates from the correct value (FOG measurements, reported in Fig. 5.9 and used as ground truth, indicate that the vehicle has not yet started rotating). On the contrary, the disturbance is correctly identified by the developed filter (the control angles in Fig. 5.10 rapidly exceed the threshold values, indicated by the black dashed lines, and gain k2 is reduced to zero; since no large accelerations were experienced during this test, gain k1 remained fixed at its initial value, thus it is not reported), and yaw is estimated integrating the FOG angular rate. Thanks to the precision of such device, the drift due to its bias can be neglected in the short term, and it can be assumed that the measured angular rate (taking into account Earth’s angular rate effect as illustrated in Sect. 4.1.2) is the true velocity of the instrument. Finally, after removing the disturbance source and rotating the vehicle back, ψ I C S slowly converges to the estimate computed by the proposed filter, while the latter is again using uncorrupted magnetic field measurements since the control angles have already fallen below their respective threshold values. As anticipated, this second set of tests show that the developed filter is able to compute an accurate attitude estimate even in the presence of unknown magnetic disturbances, which constitutes an undeniable benefit to navigation. Concerning the choice of the values of the gains k1 and k2 and of the corresponding thresholds, there is no predefined rule: indeed, the selected values must reflect the quality of the used sensors (especially for the two gains), the physical properties of the vehicle they are mounted on (for instance, a smaller vehicle is likely to be affected more by waves and currents, which may cause the control angles to exceed too small threshold values even if no magnetic disturbance is present), and must take into account any prior knowledge about the presence of external disturbances (which may come, e.g., from the knowledge of the type of the required task). For the results reported here, the maximum value of k2 was set to 0.1, maximum k1 was two orders of magnitude higher, and the two threshold values were set to 3◦ . Following the positive outcome of the tests aimed at validating the proposed strategy, the performance of the developed filter has been evaluated during prolonged navigation missions. The results reported here refer to one of the tests performed by FeelHippo AUV in the Gulf of Baratti (Livorno, Italy), executed in shallow waters. During the test, the vehicle could experience light sea current; the magnetic characteristics of the location (i.e. the presence of magnetic materials on the sea bottom) were unknown. The vehicle was required to autonomously follow a lawn mower path, exploiting the attitude estimate computed within its navigation filter. The test was executed on surface, in order to exploit the GPS signal as ground truth. Figure 5.11 shows the waypoints of the desired path and the GPS fixes received by the vehicle; Fig. 5.12, instead, shows the yaw estimated by the filter along with the orientation of the legs of the path (roll and pitch remained close to zero during the whole mission due to hydrostatic stability, thus they are not reported). It can be easily seen that the GPS fixes and the estimated yaw show good accordance, indicating that the proposed filter is able to correctly estimate the attitude of the AUV. In addition, even if it cannot be taken as a strict term of comparison due to the possible lateral motion of the vehicle, the maximum difference between the estimated yaw and the “GPS-based” yaw (computed numerically from the route between two subsequent sensor readings) never exceeds 5◦ .

82

5 Results 10

5

Waypoints Starting point GPS signal

0

-5 0

5

10

15

20

25

30

Fig. 5.11 FeelHippo AUV path 300

250

200

150

100

50

NE leg orientation SE leg orientation SW leg orientation

0 0

Fig. 5.12 Yaw estimate

50

100

150

200

250

300

350

400

450

5.1 Attitude Estimator

83

0.2 0.1 0 -0.1

0

50

100

150

200

250

300

350

400

450

50

100

150

200

250

300

350

400

450

50

100

150

200

250

300

350

400

450

50

0 0

20 10 0 0

Fig. 5.13 k2 and control angle values

For what concerns compass and FOG usage, Fig. 5.13 shows the values assumed 1 2 and αm and the corresponding gain k2 (the maximum by the two control angles αm values of the gains were the same of the previous test, while the two threshold values were set to 5◦ ; even in this case, gain k1 remained fixed at its maximum value and is not reported): depending on the position of the vehicle along the path, magnetometer readings are alternatively employed or discarded (as before, the black dashed lines indicate the two threshold values); indeed, in an environment with a-priori unknown magnetic characteristics, the AUV makes use of both sensors to accurately estimate its attitude. The positive results shown in this section, together with those, relative to other experiments performed throughout the Ph.D. period and not reported here for brevity reasons, indicate that the developed filter is able to compute an accurate attitude estimate in magnetically a-priori unknown environments, overcoming the problem of high susceptibility of commercial devices to magnetic disturbances, making it suitable for use during autonomous underwater navigation missions. Indeed, at the time of writing, the developed estimator is employed on all the AUVs of the MDM Lab, and its estimate is used as an input for the position estimation filter, as explained in Sect. 4.2.

5.2 Position Estimator This section reports the results obtained during sea tests whose main aim was the online validation of the UKF-based navigation filter introduced in previous chapters. Indeed, a preliminary evaluation of the performance of an UKF-based algorithm has

84

5 Results

Fig. 5.14 The lawn mower path followed by TifOne AUV during the sea tests near the Cala Minnola wreck (Levanzo, Aegadian Islands, Sicily, Italy)

been performed offline, exploiting navigation sensors data acquired during previous sea trials; the encouraging results obtained (which can be found, e.g., in [1, 2]) motivated the choice of including such filter within the GNC loop of the vehicles of the MDM Lab. In particular, the results obtained during two sea tests campaign are reported in this work; in both cases, TifOne AUV was employed. The first test campaign took place near the Cala Minnola wreck (Levanzo, Aegadian Islands, Sicily, Italy). TifOne AUV was required to autonomously follow the lawn mower path shown in Fig. 5.14. The path consisted of 27 waypoints (WP1 to WP27), covering an area of approximately 38 m per 55 m (for a total length equal to about 756 m); the GPS coordinates of the waypoint WP1 were 37.9891◦ N and 12.3545◦ E. The AUV, starting from an initial position approximately in the middle of the area to be covered, navigated underwater at a depth of about 25 m, maintaining a constant altitude of 2 m from the sea bottom, whereas the longitudinal speed reference was set to 0.5 m/s. Indeed, navigation at constant altitude is particularly useful for the mapping and the reconstruction of a surveyed area; in the considered case study, the wreck of Cala Minnola was a well-known archaeological site, and the data acquired by the payload of TifOne were used to create bi- and three-dimensional maps of the site itself. Finally, a non-negligible sea current was experienced on surface on the day of the trial, approximately directed in the North-South direction. An USBL modem was mounted on a support ship near the planned path (at 37.8607◦ N, 12.3127◦ E), as shown in Fig. 5.14, and was used to monitor the progression of the mission. Indeed, the position of the AUV measured by such sensor was not exploited by the online navigation filter; instead, it was used as ground truth to evaluate the performance of the latter.

5.2 Position Estimator

85

30 Waypoints Starting point Estimated position USBL receiver

20

10

0

-10

-20

-30

-40 -40

-30

-20

-10

0

10

20

Fig. 5.15 Comparison between the estimated vehicle position and the planned path during the Sicily test campaign, Italy

The estimated position of the AUV is reported in Fig. 5.15, where it is compared with the planned path. Although the sea current effect is visible for the first legs of the path, severely disturbing the GNC system of the vehicle (it is worth noting that no current estimators were available at the time), the AUV was able to recover from the disturbance and to return to the planned path, following it until the completion of the task, highlighting the goodness of the proposed navigation filter. From a quantitative point of view, Fig. 5.16 reports the error between the estimated position and the position measured by the USBL modem, computed in correspondence of each USBL measurement (for the ease of reading, the time axis of Fig. 5.16 has been shifted so as to start in correspondence of the first USBL fix). Despite the uncertainty affecting the USBL system itself (see for instance [3]) and the effect of sea currents (which may cause the orientation of the sensor to vary, affecting in turn the accuracy of its measurements), the USBL fixes are quite numerous and uniformly distributed over time; in addition, the estimation error remained limited throughout the whole task, confirming the satisfying performance of the proposed navigation filter. The second test whose results are reported in this work was performed in a sea basin in the harbor of La Spezia, Italy. The experimental setting was similar to that of the previous test: TifOne AUV autonomously followed a lawn mower path underwater, while an USBL modem was used to measure the position of the vehicle in order to evaluate the performance of the UKF-based navigation filter. Like the previous test, it is worth noting that USBL measurements were not actively employed by the filter.

86

5 Results 6

5

4

3

2

1

0 0

200

400

600

800

1000

1200

1400

  K F Fig. 5.16 Estimation error ηU − PUN S B L  during the Sicily test campaign, Italy 1

Fig. 5.17 The lawn mower path followed by TifOne AUV during the sea tests in La Spezia harbor, Italy

For this test, the planned path was composed of 8 waypoints (WP1 to WP8, see Fig. 5.17), for a total length of about 350 m over an area 80 m long and 10 m wide. The GPS coordinates of the waypoint WP1 were 44.0947◦ N and 9.8622◦ E, while the USBL modem was placed at 44.0950◦ N and 9.8615◦ E. In particular, the latter was mounted on a buoy equipped with an IMU and a GPS (in order to compute the

5.2 Position Estimator

87

100 90 80 70 60 50 40 30 20

Waypoints Starting point Estimated position USBL receiver

10 0 0

50

100

150

200

Fig. 5.18 Comparison between the estimated vehicle position and the planned path during the La Spezia test campaign, Italy

position of the AUV with respect to a NED frame), and transmitted data to shore using a WiFi access point on the buoy itself. The underwater mission was executed at the desired depth of 2 m, moving with a desired longitudinal speed of 0.5 m/s; no significant sea currents were observed within the harbor. Figure 5.18 compares the position estimated by the onboard filter with the planned path, showing good accordance between the two, while Fig. 5.19 reports the position error between the estimated position and USBL measurements. With the same assumptions about USBL uncertainty made for the previous test, and taking into account that the modem might have slightly moved during the test due to the involuntary, random motion of the buoy, the estimation error never exceeds 5.5 m. The satisfying results obtained during these two test campaigns, reported in this thesis, establish the validity of the proposed algorithm, which is able to accurately estimate the position of an AUV even in the presence of environmental disturbances which are very likely to affect its performance. In particular, these tests show how the developed UKF-based navigation filter can be suitably used with nonlinear and discontinuous AUV kinematic and dynamic models, constituting a valid choice in multiple applications.

5.3 Sea Current Estimator The last section of this chapter shows how, exploiting the UKF-based navigation filter presented in previous chapters, real-time sea current estimation can be achieved with

88

5 Results 6

5

4

3

2

1

0 0

200

400

600

800

1000

1200

  K F Fig. 5.19 Estimation error ηU − PUN S B L  during the La Spezia test campaign, Italy 1

Fig. 5.20 The lawn mower path followed by MARTA AUV during the sea tests in the Gulf of La Spezia, Italy

little to no computational load increase with respect to position only estimation. This section reports simulation results obtained exploiting real sensor data acquired during suitable sea trials. The aim of these tests is to validate the model reported in Eq. (4.52). The first set of results has been obtained exploiting data acquired by MARTA AUV during the execution of an autonomous task in the Gulf of La Spezia (Italy).

5.3 Sea Current Estimator

89

30

25

20

15

10

5

0

-5 -5

Starting point Estimated position Estimated current

0

5

10

15

20

25

Fig. 5.21 Estimated path and sea current during the sea tests in the Gulf of La Spezia, Italy

MARTA autonomously followed the lawn mower path shown in Fig. 5.20 (WP1 to WP24) at the constant depth of 6.5 m and at the desired speed of 0.5 m/s. The path was composed of twelve, 21 m long, and equally spaced (2.15 m between each pair) legs. The coordinates of the firs waypoint of the path were 44.0641117◦ N, 9.851056996◦ E. On test day, an extremely light current could be observed. To perform the desired simulations, GPS, depth, DVL, and attitude data (the latter output by the attitude estimator previously described in this work) acquired by the vehicle have been used. Figure 5.21 shows both the path and the sea current estimated by the UKF-based navigation filter; in particular, current is represented by red arrows pointing in the direction of the estimated current, ans whose magnitude is proportional to that of the latter. Figures 5.22 and 5.23 report, respectively, the magnitude and the bearing of the estimated current. The analysis of Figs. 5.21, 5.22 and 5.23 shows that, after an initial transient, current settles around an average value (in terms of both magnitude and direction), consistent with what could be experienced on-site. In addition to current estimation, the navigation filter is able at the same time to exploit such information in order to improve the accuracy of its position estimate, highlighting the overall goodness of the proposed strategy. Additional tests have been executed to further validate the current estimator; in particular, the data collected by the navigation sensors of TifOne AUV during the Sicily test campaign mentioned in Sect. 5.2 (as with the other test described in this section, GPS, depth, velocity, and attitude data) have been exploited to evaluate the performance of the developed algorithm.

90

5 Results 10 -3 18 16 14 12 10 8 6 4 2

0

200

400

600

800

1000

1200

Fig. 5.22 Magnitude of the estimated current during the sea tests in the Gulf of La Spezia, Italy 260 240 220 200 180 160 140 120 100 80 0

200

400

600

800

1000

1200

Fig. 5.23 Bearing of the estimated current during the sea tests in the Gulf of La Spezia, Italy

5.3 Sea Current Estimator

91

30 Starting point Estimated position 20 Estimated current

10

0

-10

-20

-30

-40 -40

-30

-20

-10

0

10

20

Fig. 5.24 Estimated path and sea current during the sea tests near the Cala Minnola wreck, Italy

As stated in Sect. 5.2, during the tests performed near the Cala Minnola wreck, light to moderate sea current could be observed on water surface. Indeed, even if underwater current would likely be weaker, the desired path was planned according to the direction of the experienced current, in order to reduce lateral currents on the vehicle as much as possible and to increase battery efficiency. Hence (compare Fig. 5.15), an estimated current directed in the North-South direction would prove the accuracy of the current estimator. Simulation results are reported in Figs. 5.24, 5.25, 5.26: the position of the vehicle and sea currents acting on it are simultaneously estimated. In particular, after an initial transient the current estimate exhibits stable behavior in terms of both magnitude and bearing, remaining between 0.15 and 0.2 m/s for the former (Fig. 5.25) and settling around the average value of 185◦ for the latter (red arrows in Figs. 5.24, and 5.26). Indeed, these values are coherent with what could be witnessed on the day of the experiment. The promising simulation results shown in this section highlight the suitability of the developed algorithm, which can be efficiently used to estimate sea currents acting on an autonomous vehicle in real-time, while the latter is navigating. Additionally, the acquired information could be used on board for different purposes; for instance, it could allow for online path replanning in order to exploit favorable environmental characteristics or, on the other hand, to avoid undesirable drag, improving in turn battery life. At the time of writing, additional tests are scheduled to be carried out in the near future in order to fully validate the developed filter exploiting an independent ground

92

5 Results 0.22 0.2 0.18 0.16 0.14 0.12 0.1 0.08 0.06 0.04 0.02 0

500

1000

1500

2000

Fig. 5.25 Magnitude of the estimated current during the sea tests near the Cala Minnola wreck, Italy

250

200

150

100

50

0

500

1000

1500

2000

Fig. 5.26 Bearing of the estimated current during the sea tests near the Cala Minnola wreck, Italy

5.3 Sea Current Estimator

93

truth. Specifically, an ADCP will be placed in an area of interest, and one of the AUVs of the fleet of the MDM Lab will be used to perform an autonomous navigation task in the same area in order to compare the onboard estimate with the current measured by the external device. Alternatively, since the DVL mounted on FeelHippo AUV, (which recently replaced the previously equipped LinkQuest DVL), manufactured by Nortek, possesses current measurement functionalities, such vehicle (paired, if necessary, with another AUV) might be used to obtain a quantitative assessment of the performance of the current estimator.

References 1. Allotta B, Caiti A, Chisci L, Costanzi R, Di Corato F, Fanelli F, Fantacci C, Fenucci D, Meli E, Ridolfi A (2015) A comparison between EKF-based and UKF-based navigation algorithms for AUVs localization. In: Proceedings of the MTS/IEEE OCEANS 2015, May 18–21, 2015, Genova, Genova (IT) 2. Allotta B, Caiti A, Costanzi R, Fanelli F, Fenucci D, Meli E, Ridolfi A (2016) A new AUV navigation system exploiting unscented Kalman filter. J Ocean Eng 113:121–132 3. Christensen L, Fritsche M, Albiez J, Kirchner F (2010) USBL pose estimation using multiple responders. In: Proceedings of the MTS/IEEE OCEANS 2010, May 24-27, 2010, Sidney, Sidney (AU)

Chapter 6

Conclusion

This work collects the results of the research activity on underwater robotics carried out at the Mechatronics and Dynamic Modeling Laboratory of the Department of Industrial Engineering of the University of Florence during the years 2014–2017. The activity focused on the study of navigation algorithms for autonomous underwater vehicles, starting from the analysis of state-of-the-art solutions and leading to the improvement of existing algorithms or to the development of new ones. Underwater self-localization and pose estimation constituted the main subject of the research, since the difficulties (often related to the unavailability of instruments and technologies commonly employed in other domains) that accompany operations below the surface represent one of the major obstacles for the success of autonomous missions; in particular, the focus was given to the development of accurate attitude and position estimation algorithms for AUVs. Concerning attitude estimation, starting from an already existing solution an attitude estimator fusing inertial and magnetic field measurements was developed; several design changes were applied to the original formulation of the algorithm in order to increase its suitability for use in the underwater field. The main feature of the resulting filter is the ability to promptly identify and react to the presence of external sources of magnetic disturbance, which would otherwise severely affect the performance of a vehicle’s GNC system, precluding the success of any performed task. Regarding position estimation, on the other hand, an UKF-based filter exploiting position (either GPS fixes on surface or acoustic position measurements, if available, underwater), depth, and velocity data, together with an accurate attitude estimate (computed indeed by the above-mentioned filter) was developed. In addition, the possibility of including real-time estimation of sea currents within such filter (without adding further instrumentation and relying only on a modification of the mathematical vehicle model used within the filter itself and on already available sensor data) was explored. © Springer Nature Switzerland AG 2020 F. Fanelli, Development and Testing of Navigation Algorithms for Autonomous Underwater Vehicles, Springer Theses, https://doi.org/10.1007/978-3-030-15596-4_6

95

96

6 Conclusion

A preliminary laboratory research and testing phase was paired with a significant share of experimental activity at sea, which allowed to identify those issues that could not have been identified without a practical confrontation with the characteristics of the underwater environment. All the vehicles of the MDM Lab (presented in Chap. 2) were employed to validate the algorithms developed during the Ph.D. period and introduced in Chap. 4 of this work. Both attitude and position estimators were tested online during the execution of suitable autonomous missions, while the performance of the current estimator was evaluated by means of simulations exploiting real navigation data acquired during previous campaigns (with its online validation scheduled for the near future). The satisfying results obtained, presented in Chap. 5, highlight the goodness of the derived solutions, which may constitute a suitable alternative to existing pose estimation strategies. Nonetheless, there is still room for improvement: for instance, smart initialization procedures for the attitude estimation filter described in Sect. 4.1 may prove useful in the case of severely perturbed environments; furthermore, depending on the available onboard processing units, more elaborate vehicle and current mathematical models could be used within the filter introduced in Sect. 4.2 in order to increase its accuracy. These issues, whose resolution constitutes a natural continuation of the research activity carried out thus far, will be subjected to further investigation.

Appendix

Author’s Biography

Dr. Francesco Fanelli is currently working as a Software Engineer at the National Oceanography Centre (NOC), Southampton, United Kingdom, within the Marine Autonomous and Robotic Systems (MARS) group. Prior to his current role, Francesco spent the past four years at the University of Florence, as a Ph.D. student first, and then as a Postdoctoral Researcher, conducting his research activity within the Mechatronics and Dynamic Modeling Laboratory (MDM Lab) of the Department of Industrial Engineering of the University of Florence (DIEF), led by Prof. Benedetto Allotta. During his research activity he worked in the field of underwater robotics, with special attention given to the development and the field testing of navigation strategies for Autonomous Underwater Vehicles (AUVs). This book collects the main outcomes of his activity, which resulted in multiple publications in international journals and conference papers, and in the awarding of his Ph.D. degree in Industrial and Reliability Engineering in April 2018. His current interests and activity focus on the development of software for AUVs.

© Springer Nature Switzerland AG 2020 F. Fanelli, Development and Testing of Navigation Algorithms for Autonomous Underwater Vehicles, Springer Theses, https://doi.org/10.1007/978-3-030-15596-4

97