An IMU and USBL-aided buoy for underwater localization

Design and OptimizationCONTRIBUTED SESSIONSAn IMU and USBL-aided buoy for underwater localization VII International Conference on Computational Methods in Marine Engineering MARINE 2017 M. Visonneau, P. Queutey and D. Le Touz´e (Eds) AN IMU AND USBL-AIDED BUOY FOR UNDERWATER LOCALIZATION B. Allotta∗, M. Bianchi∗, F. Fanelli∗, J. Gelli∗, N. Monni∗, M. Pagliai∗, N. Palma∗and A. Ridolfi∗ ∗Department of Industrial Engineering (DIEF), University of Florence Via di Santa Marta 3, 50139, Florence, Italy e-mails: [email protected], [email protected], m.pag[email protected] Key words: Underwater Robotics, Underwater Localization Abstract. Autonomous underwater navigation remains, as of today, a challenging task. The marine environment limits the number of sensors available for precise localization, hence Autonomous Underwater Vehicles (AUVs) usually rely on inertial and velocity sensors to obtain an estimate of their position either through dead reckoning or by means of more sophisticated navigation filters (such as Kalman filters and its extensions [1]). On the other hand, acoustic localization makes possible the determination of a reliable vehicles pose estimate exploiting suitable acoustic modems [3]; such estimate can even be integrated within the navigation filter of the vehicle in order to increase its accuracy. In this paper, the authors discuss the development and the performance of an Ultra-Short BaseLine (USBL)-aided buoy to improve the localization of underwater vehicles. At first, the components and the physical realization of the buoy will be discussed; then, the procedure to compute the position of the target will be analyzed. The following part of the paper will be focused on the development of a recursive state estimation algorithm to process the measurements computed by the buoy; specifically, Extended Kalman Filter [4] has been adopted to deal with the nonlinearities of the sensors housed on the buoy. A validation of the measurement filtering through experimental tests is also proposed. 1 INTRODUCTION Undersea operations are an example of how robotics can replace humans: working underwater is, indeed, both dangerous and difficult. The birth of the underwater robotics is due to military purposes (e.g. seabed mines clearing), but with years the field of application has widened to a vast category of scientific and commercial tasks. An example is the digital reconstruction of the seabed, exploiting a proper set of sensors mounted on board an underwater vehicle, like waterproof cameras and multibeam echosounders. Due to the heterogeneity of tasks that can be performed, many types of underwater vehicles exist: among them it is possible to find Remotely Operated Vehicles (ROVs), i.e. tethered underwater mobile devices operated by a crew aboard a vessel and Autonomous Underwater Vehicles (AUVs), capable of travelling underwater without requiring input from an operator. By using such vehicles the need of a proper localization system 1 449 B. Allotta, M. Bianchi, F. Fanelli, J. Gelli, N. Monni, M. Pagliai, N. Palma and A. Ridolfi arises [5]. In fact, radio waves, exploited by Global Positioning System (GPS) are quickly absorbed by the water, hence the need to rely on different instruments for the positioning. Such instruments are usually based on acoustic waves because of their property to propagate in the water even for long distances; in this sense, the localization techniques mainly used in the underwater environment are Long BaseLine (LBL) and Ultra Short BaseLine (USBL). In Long BaseLine localization, different transmitter modems are placed underwater in well known positions. Periodically, pings are generated by these modems, as response to acoustic signals coming from the target to locate (i.e. the underwater vehicle), and are received back by the modem mounted on the vehicle. Once responses from all the transmitters are available, they are used to retrieve the position of the vehicle, by triangulation or position search algorithms. LBL can reach an accuracy of a few centimetres, a precision that does not depend on the distance between the vehicle and the fixed transmitters. On the other hand, LBL systems require a not negligible amount of work for the installation of the baseline stations; this procedure, indeed, often requires vessels and proper equipments. The Ultra Short BaseLine localization, instead, is based on a single device integrating both the acoustic transceiver (needed for communicating with the compatible acoustic modems) and a series of transducers (typically 5), placed in known relative positions (normally in the order of few tens of centimetres). Transducers are capable of acquiring the signal transmitted by a modem and, once the signal reaches them, it is acquired and processed by a dedicated unit. A typical installation for the USBL device is on the bottom of a surface vessel [5], with a considerable saving on the costs of the infrastructures that have to be installed in the operational area compared to the LBL solution. However, the positioning accuracy guaranteed by such a system degrades with the relative distance and additional sensors (e.g. GPS, gyroscopes or electronic compass) are required, in order to compensate for the changing position and orientation of the surface vessel the device is mounted on. Despite these drawbacks, USBL solutions are widely employed for tasks where flexibility and time saving in the installation are mandatory. In this paper underwater target localization is discussed, exploiting a self-moving buoy, built by the Mechatronics and Dynamic Modelling Laboratory (MDM Lab) of the University of Florence, housing an USBL device. Section 2 provides the hardware features of the buoy and a description of the procedure used for locating the target. In Section 3 the development of an Extended Kalman Filter (EKF) to improve the measurements computed by the buoy is discussed. The solution proposed has been validated through data obtained from experimental tests on stationary and moving targets; the results are presented in Section 4. 2 DESCRIPTION OF THE BUOY 2.1 Hardware features The buoy used for underwater localization is shown in Figure 1: it consists of a wooden board fixed on a life buoy and a case located on top. The case is certified with the IP67 (International Protection) rating, meaning that the inside is impermeable on condition that the immersion into the water is temporary, not deeper than 1 meter and not longer than 30 minutes. Cases with higher IP ratings can be found in commerce, but the IP67 rating is suitable for this purpose, being the case located over the water level. Almost all of the water-sensitive (electronic) components of the buoy are placed inside the IP67-rated case and connected to the 2 450 B. Allotta, M. Bianchi, F. Fanelli, J. Gelli, N. Monni, M. Pagliai, N. Palma and A. Ridolfi (a) (b) Figure 1: The buoy used to locate the underwater target (Figure 1a) and the IMU-USBL subsystem (Figure 1b). outside by using impermeable cables and cable glands suitable for aquatic environment. Two thrusters can also be installed in order for the buoy to reach or maintain a desired position. Regarding the electronic and electrical features, the following components are installed on the buoy: •Six cells, 8000 mAh (Ampere-hour) Lithium-ion Polimer (Li-Po) battery; •24V-5V DC-DC converter; •Odroid-XU onboard computer; •Xsens MTi-100 Inertial Measurement Unit (IMU); •EvoLogics S2CR 18/34 USBL transceiver; •SwiftNav Piksi Differential GPS (DGPS) receiver; •PicoStation Ubiquiti wireless module; •Polulu Micro Maestro 6-channel USB servo controller. To guarantee the impermeability of the unit, the Xsens IMU is located inside a watertight aluminum cylinder which is rigidly fixed on the USBL transceiver; the USBL transceiver is mounted on a stainless steel pole rigidly connected to the wooden board of the buoy. Both IMU and USBL transceiver are positioned underwater. Thanks to the rigid connection between the IMU-USBL system and the buoy, every variation in the orientation of this latter is reflected in a same variation in the one of the IMU-USBL system and it will be detected by the IMU; this 3 451 B. Allotta, M. Bianchi, F. Fanelli, J. Gelli, N. Monni, M. Pagliai, N. Palma and A. Ridolfi Figure 2: A scheme of the connections of the buoy components. information is then used to properly process the measurement provided by the USBL sensor. The Piksi DGPS receiver communicates with a base station, located in a known position on the mainland and provides the position of the buoy with an accuracy of few centimetres. Regarding the electronic components, the 8000 mAh Li-Po battery provides 24V voltage to supply the onboard computer and, consequently, some of the components that are plugged to it. A 24V-5V DC-DC converter has been inserted in order to avoid overvoltages for the Odroid-XU computer. Both the wireless module and the DGPS receiver are fixed on a tinier wooden board on top of the main one and connected to it by means of two 50 cm threaded rods. This solution has been adopted in order to limit interferences in the radio signals sent and received by the buoy. The Polulu Micro Maestro 6-channel USB servo controller provides an USB interface for the two thrusters. In order to power on the onboard computer of the buoy, a magnetic activation switch is used; this solution guarantees easy emergency shutdowns when buoy components safety may be at risk. 2.2 Localization procedure The procedure to compute the position of an underwater vehicle involves an Earth-fixed frame, located in a given position in the sea and whose axes are aligned according to the NED (North East Down) convention; in this context, the Earth-fixed frame origin is coincident with the first position of the buoy measured by the DGPS. In order to detect variations in the pose (i.e. position and orientation) of the buoy with respect to the Earth-fixed frame, the IMU and the DGPS are used. Let us consider a buoy-fixed reference frame; of course, at first such frame will be coincident with the Earth-fixed frame, but it will change its pose according to the movements of the buoy. The DGPS measures the position of the buoy-fixed frame origin (OB) with respect to another reference frame, fixed on the base station located on the mainland; let us indicate such measurement with OS B. By calling 4 452 B. Allotta, M. Bianchi, F. Fanelli, J. Gelli, N. Monni, M. Pagliai, N. Palma and A. Ridolfi OS Wthe position of the origin of the Earth-fixed frame (measured by the DGPS and referred to the base station) it is trivial to compute the relative positioning of the buoy with respect to the Earth-fixed frame (PW B) as: PW B=OS B−OS W.(1) Regarding the orientation, the IMU measures the relative orientation between the buoy-fixed and the Earth-fixed frame, expressed with the RPY (Roll Pitch Yaw) Euler angles ϕ,ϑ,ψ. Therefore, given the position of the target measured by the USBL PU Tand known the relative orientation of the USBL sensor with respect to the IMU, expressed by the mounting angles ϕm, ϑm,ψm, it is possible to compute the position of the target with respect to the current position of the buoy (PB T) as: PB T=RB IRI UPU T,(2) where RB Iand RI Uare, respectively, the rotation matrices describing the relative orientation between the IMU and the buoy and between the IMU and the USBL device. The expression of RB Ican be computed starting from the RPY angles ϕ,ϑ,ψby using the rotation matrices composition rule; the same holds for RI Uand the mounting angles ϕm,ϑm,ψm. The position of the target referred to the Earth-fixed frame can then be calculated as: PW T=PB T+PW B,(3) Known PW Tand the absolute position (latitude, longitude and altitude) of the Earth-fixed frame origin LLAOW, the absolute position of the target can be computed using standard conversion functions as follows: LLAPT=fc(LLAOW,PW T).(4) where fc(·,·) is the function performing the conversion from relative to absolute coordinates. To compute the position of the vehicle, measurements obtained from the sensors mounted on the buoy (i.e. IMU, USBL and DGPS) are processed using Robot Operating System (ROS) [7]. Specifically, for each sensor a ROS node implements the interface by publishing data on a proper ROS topic: the content of such topics is then read by another node that computes the position of the underwater target as discussed. 3 MEASUREMENTS FILTERING To improve the underwater localization, a recursive state estimation algorithm can be used; such an algorithm relies on a model of the target to locate and of the sensors exploited to compute its position. Given the nonlinearities of the sensors involved, the Extended Kalman Filter (EKF) [4] has been used. 3.1 Underwater target motion model Two different motion models need to be considered: one describing a stationary target and another referred to a target moving at constant speed. These are, indeed, the two main situations 5 453 B. Allotta, M. Bianchi, F. Fanelli, J. Gelli, N. Monni, M. Pagliai, N. Palma and A. Ridolfi occurring in the practice: usually an AUV navigates at the cruise speed along its minimal resistance direction or is required to maintain a fixed position (i.e. hovering). The mathematical description exploits the relative coordinates of the target referred to the Earth-fixed frame, but considering only the North and East components; the depth (Down axis component) is not relevant for this topic as it is usually known or can be measured properly by the depth sensor housed on board the vehicle. For these reasons, the considered state vector is x=[xW T,y W T]Tfor the stationary target and x=[xW T,˙xW T,y W T,˙yW T]Tfor the target travelling at constant speed. 3.1.1 Stationary target state space representation The state space representation describing the behaviour of a stationary target can be obtained directly at discrete time. Ideally, the equation modelling the evolution of a quantity xthat does not change over time is: xt+1 =xt.(5) However, in order the model to be more realistic, a white noise wt, having minimal standard deviation σ, is added: xt+1 =xt+wt,wt∼wn(0,σ2I).(6) 3.1.2 Moving target state space representation To model the behaviour of a target moving at constant speed polynomial kinematic models described in [6] are adopted. Specifically, a White Noise Acceleration (WNA) model is used. Such a model is derived starting from a continuous time motion model and then discretized using the ZOH (Zero Order Hold) technique for the discretization. Let us consider the components of the target motion x=[xW T,˙xW T,y W T,˙yW T]T=[x1,x 2,x 3,x 4]T; assuming different acceleration fluctuations (σcx and σcy) along the two directions (x1and x3), it is possible to model a target moving at constant speed with the following equations:           x1 k+1 x2 k+1 x3 k+1 x4 k+1           =�Ak02×2 02×2Ak� ��� � Ak           x1 k x2 k x3 k x4 k           +wk, wk∼wn(0,Qk),Qk=�σ2 cxQk02×2 02×2σ2 cyQk� (7) where: 6 454 B. Allotta, M. Bianchi, F. Fanelli, J. Gelli, N. Monni, M. Pagliai, N. Palma and A. Ridolfi Ak=�1Tk 01 �,Qk=  T3 k 3 T2 k 2 T2 k 2Tk (8) with Tk=tk+1−tkbeing the offset between the k-th and k+1-th time samples. It is important to note that Tkvaries over time, because the working frequency of the USBL sensor is not fixed. This is caused by the time required from the acoustic signal to travel from the USBL transceiver to the acoustic modem and back to the USBL transceiver, which varies depending on the water conditions and the distance between the two devices. 3.2 Measurement equations As it has been discussed earlier, the underwater localization procedure is based on data provided by IMU, USBL and DGPS. However, the angles measured by the IMU are already compensated by its inner estimation algorithm. The DGPS, instead, measures the position of the buoy; if the absolute position of the base station is reliable, the DGPS measurements are accurate. On the other hand, the USBL transceiver provides a position of the underwater target that can be inaccurate for many reasons (e.g. buoy position perturbations or bad water conditions): the filtering is then aimed to the compensation of the USBL measurements errors. Let us consider the case of the stationary target (for the moving one the procedure is similar); let us indicate with xW B,y W Bthe North and the East coordinate of the buoy at time twith respect to the Earth-fixed frame. Considering that the USBL transceiver exploits a spherical positioning system, the characteristic of the sensor set used to locate the target can be modelled as: yt=ht(xt)+vt= �(x1 t−xW B)2+(x2 t−yW B)2 atan2(x1 t−xW B,x 2 t−yW B) +vt(9) where vtis a zero-mean Gaussian noise depending on the USBL transceiver technical features. The atan2(·,·) function in Equation (9) provides an information about the azimuth angle of the target and is piecewise continuous and differentiable; when differentiable, its partial derivatives are: ∂atan2(x,y) ∂x =−y x2+y2,∂atan2(x,y) ∂y =x x2+y2.(10) The points where atan2(x, y) is discontinuous (x=0∧y�= 0) or undefined (x=0∧y= 0) never occur in practice, hence it is always possible to compute its partial derivatives; the range measurement characteristic h1 t(xt), instead, is always differentiable. It is then possible to use the Extended Kalman Filter to process the measurements provided by the buoy. Let us note that the choice to adopt the Earth-fixed frame as reference frame leads to a time-variant measurement equation: the coordinates of the buoy xW Band yW B, indeed, vary over time. However, this is not an issue, because the expression of the partial derivatives of ht(xt) remains the same, therefore it can be computed only once, offline, and applied online, reducing the computational load required from the EKF. The expression of the Jacobian matrix Ctto be used within the filter is then: 7 455 B. Allotta, M. Bianchi, F. Fanelli, J. Gelli, N. Monni, M. Pagliai, N. Palma and A. Ridolfi Figure 3: Satellite image of the piers of Roffia Lake (from Google Maps): the piers are labelled as ‘P1’, ‘P2’, ‘P3’, ‘P4’, and ‘P5’. In the performed tests the buoy has been located at the end of P1 and two acoustic modems are moored at different piers (P2 and P5). Ct=   ex √(e2 x+e2 y) ey √(e2 x+e2 y) −ey e2 x+e2 y ex e2 x+e2 y   (11) where: ex= (ˆx1 t|t−1−xW B),e y= (ˆx2 t|t−1−yW B) (12) being ˆx t|t−1= [ˆx1 t|t−1,ˆx2 t|t−1]Tthe predicted state estimate at time t. 4 EXPERIMENTAL RESULTS To evaluate the effects of the measurements filtering on the underwater target localization several experimental tests were carried out at Roffia Lake (San Miniato, PI). Stationary and moving target localization have been performed exploiting, respectively, two EvoLogics 18/34 acoustic modems moored in known positions and MARTA AUV (MArine Robotic Tool for Archaeology [2], Figure 5a). The measurements have been collected online, while the filtering has been applied offline, using MATLAB, in order to find a proper tuning of the EKF parameters. 4.1 Stationary target localization In the stationary target localization (see Figure 3), two EvoLogics 18/34 acoustic modems have been moored at the piers P2 and P5 of Roffia Lake at a given depth, while the self-moving buoy has been placed near pier P1. In the experiments in exam various angular perturbations have been induced on the buoy, so that it has been possible to verify the robustness of both the raw measurements and the filtered ones. Figures 4a and 4b show the result of the localization of the targets moored at P2 and P5, respectively. The measurements computed by the buoy have been compared with the position of the target measured on surface with a GPS. It is possible to note how the localization of the target results in a wide circular sector; this is due to the high yaw angular rates caused by the perturbations voluntarily induced on the buoy. By applying the filtering of the USBL measures, the localization performance is improved. In fact, as visible in Figure 4d, the azimuth angle errors obtained after the filtering are considerably concentrated with respect to those of the raw 8 456 B. Allotta, M. Bianchi, F. Fanelli, J. Gelli, N. Monni, M. Pagliai, N. Palma and A. Ridolfi measurements. It is important to note that the errors mean is not zero: however, this is given by the inaccuracy of the target GPS measure (usually around 3 meters) used as benchmark. 4.2 Moving target localization The localization of a moving target exploiting the buoy has been tested using MARTA AUV. Specifically, different paths have been performed by the vehicle, in order to fully evaluate the performance of the localization filter: in the following the results obtained executing a lawnmower path (Figure 5b) are discussed. For every line composing the path, a steady speed is reached after an initial transient: the lawn-mower path is then, ideally, composed of subsequent uniform linear motions along different lines. On the other hand, the moving target model is referred to an uniform linear motion along a single line, hence a major importance has been given to the EKF update step rather than the prediction one through a suitable tuning of the EKF parameters, in order to trust more the measurements with respect to the target model. The target positions and velocities estimated by the filter have then been evaluated. Specifically, the estimated speeds have been compared with the measurements of the Doppler Velocity Log (DVL) sensor housed on board the vehicle. As for the estimated positions, in this context it has not been possible to use the position of the target provided by the GPS as benchmark, being such sensor unavailable underwater. The estimated positions, then, have been evaluated in terms of smoothness, because the path described by the vehicle is usually regular; indeed, the water strongly dampens the vehicle motion. In Figure 6a it is possible to note that the filtered positions are more regular than the raw ones, hence more consistent with the trajectory followed by the target. As for the velocities, the comparison with data obtained from DVL is shown in Figure 6b. It can be noted that the velocities estimated by the EKF present a slight time delay with respect to those measured on board the vehicle. The authors believe that this behaviour is caused by the time needed by the acoustic waves to propagate underwater, which produces a time offset between the position computed by the buoy at a given time and the actual position of the vehicle. Also, an initial transient for the speed estimate is present, depending on the initial state estimate the filter is initialized with. Despite these drawbacks, the results obtained through the use of the moving target model within the EKF are satisfying. 5 CONCLUSIONS This paper focuses on underwater target localization exploiting an USBL transmitter housed on a self-moving buoy, aided with an IMU and a DGPS. Firstly, the hardware features of the buoy have been introduced; then, the procedure used by the buoy to compute the position of the underwater target has been presented. To improve the quality of the measurements computed by the buoy, a recursive state estimation algorithm has been applied: in particular, Extended Kalman Filter has been used to deal with the nonlinearity of the USBL characteristic. Regarding the state transition equation of the filter, two different models have been used. The first one results sufficiently accurate to describe the behaviour of a stationary target, while the second one models a moving target and has been obtained by using a White Noise Acceleration model, suitable for the case of an uniform linear motion. Such a strategy has been validated through 9 457