Post mission trajectory smoothing for the Isurus AUV

Post Mission Trajectory Smoothing for the Isurus AUV Aníbal Matos, Nuno Cruz and Fernando Lobo Pereira Faculdade de Engenharia da Universidade do Porto Instituto de Sistemas e Robótica R. Dr. Roberto Frias 4200-465 Porto Portugal {anibal, nacruz, flp}@fe.up.pt Abstract – In this paper we describe an algorithm that produces a post mission estimate of the spatial evolution of the Isurus AUV. To make this post processing possible, the navigation system records on the vehicle logging system all the navigation data received during the mission execution. The data comprise the depth of the vehicle, the outputs of the tilt sensors and digital compass, the angular velocity of the propeller, as well as acoustic range measurements to a set of transponders. After mission completion, the logged data is then processed to produce the estimate of the evolution of the vehicle. The algorithm used to process this data is based on a fixed interval nonlinear stochastic smoothing scheme and produces an estimate that evolves continuously in time. For each instant of time, the post mission position estimate is based on all the information collected during the mission, as opposed to real time estimates that can only take into account past data. I. INTRODUCTION Isurus is an AUV operated and partially developed at the Underwater Systems and Technology Laboratory (LSTS) from Porto University. This vehicle has been performing several missions for the last few years in different operational scenarios, mainly collecting CTD and bathymetric data. After the execution of each mission, the collected data is processed according to the final purpose of the mission. In almost all the cases, it is necessary to locate the collected data both in time and in space. If time location is a trivial task (the vehicle carries a real time clock), the spatial location requires information related to the navigation system of the vehicle. The main function of the navigation system of an AUV is to provide an estimate of the vehicle position in real time. In the case of Isurus, the navigation system receives depth data from a pressure cell, attitude data from an integrated set of tilt sensors and digital compass, and velocity data from an encoder coupled to the propeller. Absolute positioning data is provided by time of flight based range measurements to a set of acoustic transponders deployed in the operation area. This system employs a Kalman filter based procedure to obtain a real time position estimate. This estimate is computed by time integrating the vehicle velocity and correcting the integrated value with range measurements from the acoustic system. Although this estimate is accurate enough for the guidance and control of the vehicle, it is not well suited for spatially locating the data collected by the AUV. In fact, the discrete nature of the correction mechanism gives rise to discontinuities in the evolution of the estimate, which is a major drawback for spatially locating the collected data. This paper presents an algorithm that processes the data collected by the vehicle navigation system during the execution of a mission and produces an estimate of the trajectory described by the vehicle more suited for spatially locating the oceanographic data gathered. This estimate evolves continuously in time and is computed using techniques based on a nonlinear stochastic smoothing scheme. The paper is organized as follows. In section II we describe the Isurus AUV. Then we describe the Isurus navigation algorithm that fuses in real time the dead reckoning data together with absolute position information. In section IV we present the post mission trajectory smoothing algorithm. Finally, we present experimental results that show that the post-mission smoothing algorithm can greatly improve the position estimate given by the on-board navigation system, mainly when the vehicle moves in turns or when range measurements are scarce. II. THE ISURUS AUV Autonomous Underwater Vehicles constitute powerful and effective tools for underwater data gathering. These vehicles operate with no external direct control, carrying a set of relevant sensors to characterize the underwater environment and to locate themselves. Isurus (Fig. 1) is a REMUS (Remote Environment Measuring UnitS) class AUV, built by the Woods Hole Oceanographic Institution, MA, USA, in 1997. These vehicles are low cost, lightweight AUVs specially designed for coastal waters monitoring [1]. The reduced weight and dimensions makes them extremely easy to handle, requiring no special equipment for launching and recovery. For the past 6 years, the LSTS has been customizing Isurus, and performing oceanographic and environmental monitoring missions with 1234 it in different operational scenarios, including estuaries, river dams and coastal ocean waters [2,3]. Fig. 1 – The Isurus AUV at the surface Isurus has a diameter of 20 cm and is about 1.5 meters long, weighting about 35 kg in air. Inside the hull, several subsystems have been improved or specifically developed at LSTS, contributing to the continuous enhancement of the vehicle performance and reliability. The maximum forward speed of the vehicle is 4 knots, however the best energy efficiency is achieved at about 2 knots. At this velocity, the energy provided by a set of rechargeable Lithium-Ion batteries may last for over 20 hours (i.e., over 40 nautical miles). Although small in size, this vehicle can accommodate a wide range of oceanographic sensors, such as CTD, altimeter, sidescan sonar, and optical backscatter. Each of these sensors can be independently installed on board, enabling the use of the vehicle in different configurations according to the requirements of each mission. The navigation system of the Isurus AUV estimates the position of the vehicle based on data provided from several devices and systems. The vertical coordinate is obtained from a depth cell installed on the vehicle. To estimate the vehicle horizontal position, the navigation system fuses together dead reckoning data with absolute positioning data. The dead reckoning data is composed by the vehicle attitude, obtained from a digital compass and a set of tilt sensors, and also by the vehicle velocity with respect to the water, obtained from an encoder that measures the propeller rotation speed. The absolute positioning data consists in range measurements to acoustic beacons deployed in the operation area. These beacons (Fig. 2) are multi-frequency transponders and were developed at the LSTS [4]. Each beacon can be configured to reply with a signal of given frequency when interrogated by another signal of a possibly different frequency. Fig. 2 – Multi-frequency transponder To obtain a range measurement to a particular beacon, the vehicle has to send an interrogation signal and then wait for and detect the beacon reply signal. The range is computed from the overall time of flight of the acoustic signals exchanged [5]. III. NAVIGATION ALGORITHM Since the vehicle depth is obtained directly from the depth cell, the major task of the navigation algorithm is the fusion of the dead reckoning data with the range measurements to estimate the horizontal position. The instantaneous velocity with respect to the water is obtained by measuring the propeller rotation speed and the vehicle heading, pitch and roll. Velocity measurements are fused together with range measurements by a Kalman filter based algorithm [6], taking advantage of the characteristics of each type of data. On one hand, the vehicle velocity is available at a high rate, but its integration leads to a drift in the estimated position. On the other, range measurements, available at a lower rate, can be noisy but do not drift over time. The algorithm updates the estimate of the vehicle position at the same rate the velocity is measured, and corrects it whenever a new range measurement is available, giving the best estimated position in real-time. The horizontal position of the vehicle is defined by the north (x) and east (y) deviations from a base point defined for each operation. The filtering algorithm is based on a simple model that relates the propeller speed (ω), the vehicle pitch (θ) and heading (ψ) angles to the time derivatives of the x and y coordinates, defined by the following equations cos( )cos( ) x x kw = ωθ ψ+  (1) cos( )sin( ) y yk w = ωθ ψ +  (2) This model roughly characterizes the vehicle motion in a straight line at a constant speed. In the above equations, wx and wy represent the north and east components of the water current velocity, respectively, and k is a constant of proportionality obtained from a linearization of the relationship between propeller rotation speed and vehicle longitudinal velocity in steady state. The navigation algorithm is based on a continuous discrete Kalman filter, with the vehicle motion characterized by the differential equations (1) and (2). The filter has a state of dimension 4, corresponding to the estimates of the north and east coordinates of the vehicle position and also to the north and east components of the water speed. To keep the notation simple the filter state will be represented by [] x y Exyww = . The filter also keeps a matrix with the covariance of the estimation error (P) that is used to assess the quality of the position estimate. Between the receptions of two consecutive range measurements, the evolution of E and P is ruled by the differential equations 1235 cos( )cos( ) cos( )sin( ) (,,,) 0 0 x x kw k EFE ωθ ψ+   ωθ ψ+  =ωθψ=     w   (3) T FF EE PPP ∂∂ ∂∂ =⋅+⋅ + Q (4) where Q is a, possibly time varying, symmetric semi-positive definite matrix that represents the rate of increase of the estimation error, due to the imperfect modeling of the vehicle motion. Whenever a new range measurement is received and is validated, the state E and the covariance matrix P are corrected according to the expressions )( * rrKEE −⋅+= −+ (5) E P −−+ ⋅⋅−= P H K P P (6) where E − and P − are the values of the E and P before the correction and E+ and P+ are their values after the correction; r is the measured range and r* is the expected range to the transponder. The matrices H and K are, respectively, the jacobian of the observation with respect to the state, and the Kalman gain [5,6]. IV. TRAJECTORY SMOOTHING The navigation algorithm presented above provides at each moment an estimate of the vehicle position. This estimate is updated by two different mechanisms with quite different features. The first depends on the dead reckoning data and is described by the differential equations (3) and (4). It is a sampled continuous process, due to the sampling of the dead reckoning data (performed at 10 Hz). The other updating mechanism acts whenever a new range measurement is available and updates the position estimate according to the correction equations (5) and (6). It is, therefore, a discrete event process. Due to the discrete time nature of the correction mechanism, the position estimate given by the navigation algorithm does not evolve continuously (even if the dead reckoning were performed at an infinite sampling rate). It can have discontinuities at the moments of corrections motivated by reception of range measurements. Such discontinuities are inherent to the navigation algorithm employed as it provides at each instant the best estimate of the vehicle position at that instant, based on all the information collected up to that moment. While these discontinuities in the estimate of the vehicle position are acceptable as far as navigation is concerned, they are a major drawback whenever the vehicle is collecting data that have to be spatially located, as in the case of bathymetry missions. To overcome such difficulty we implemented an algorithm to post process the data collected by the navigation system during the execution of the mission. The algorithm is applied upon the execution of the mission and computes the spatial evolution of the vehicle. The new position estimates are based on the information collected during all the mission, as opposed to the real-time estimates that can only take into account past data. The smoothing algorithm, described below, is based on the Rauch-Tung-Striebel nonlinear smoother [6]. The smoother has the same state of the real time filter. Let { with i} i t0,...N = , be the instants of time at which the dead reckoning data were collected during the mission. To simplify the presentation, we are assuming here that the range measurements can only occur at the time instants ti. The goal of the smoothing algorithm is to compute the estimate of the state E and of the error covariance matrix P, at the time instants ti, based on all the data collected. Such estimates will be denoted by and , respectively. These values are computed in two steps. The first step is essentially the same as the real time Kalman filter. This step computes a sequence of the estimates of the state E and of the error covariance matrix P, at the time instants t |iN 1] |iN i, each one based only on past data. These estimates, denoted by Ei and Pi, respectively, are obtained sequentially according to the following procedure. The differential equations (3) and (4) are integrated in the interval [, ii tt + with initial data Ei and Pi, respectively, to obtain 1i E − + and 1i P − + . If there is no new range measurement at the instant t1i + , then 1i E1i E − + + = and 1ii PP 1 − + = 1i t+ + . If there is a new range measurement at the instant , then 11ii EE + + + = and 1i1i PP + + + =, where 1i E + + and are computed according to (5) and (6). 1i P+ + In the second step of the smoothing algorithm, and are computed from and (and using the values from the first step), according to the equations |iN E|iN P 1|iN E+1|iN P+ |1| ( iN i i i N i i i EEKE Eb +) = +− φ − (7) 1 ( iiiiii i KP P Q) − =φφφ+ TT (8) |1| ( iN i i i N i i i i i PPKP PQK + =+ −φφ− T ) T (9) The initial conditions are and |NN N EE=|NN N PP = . In the above equations, is the value at t of the solution of the differential equation i b1i+ F E F ∂ ∂ =+ F E Ebb with the initial condition ∂  ∂ − () 0 i bt = , 1 (, i i t ii t Qt + ) ()(, ) i Qtd = φτ ∫τφτ τ T, and 1 (, ii t +) i t φ =φ , where (,) φ ⋅⋅ is the transition matrix associated to the linearized vehicle dynamics, i.e., F E zz ∂ ∂ = . V. EXPERIMENTAL RESULTS Both the navigation and the post mission smoothing algorithms described here have been extensively tested different scenarios of operation. The testing program enabled 1236 us to fine tune the parameters of both algorithms for optimal performance. Currently, these algorithms are employed in virtually all the missions performed with Isurus. We present below some results illustrating the behavior of both the real time navigation and the post mission smoothing algorithms. The data was collected during a 2 hours mission that took place 2 km off the portuguese coast near Aveiro. In this mission, Isurus collected CTD and bathymetric data in an area about 100 m by 200 m at several depths. Fig. 3 shows the real-time estimate of the vehicle position computed by the navigation algorithm (red line) as well as the desired trajectory (black line). The discontinuous nature of the estimated trajectory can be readily observed in this figure, since there were large corrections of the estimated position. Typically, these large corrections arise just after the vehicle being without receiving range measurements for a long time (typically more than 10 seconds). -100 -50 050 100 -140 -120 -100 -80 -60 -40 -20 0 east (m) north (m) Fig. 3 – Real time trajectory estimation It should be referred here that the lateral offset between the two trajectories in the east-west lines is not the result of a poor performance of the navigation algorithm, but is a direct consequence of the strong water current from the north that was present during the mission. In fact, the heading reference generated by the vehicle guidance system does not integrate the off track error, giving rise to large off track errors when there are large water currents perpendicular to the desired trajectory. -100 -50 050 100 -140 -120 -100 -80 -60 -40 -20 0 east (m) north (m) Fig. 4 – Post mission trajectory estimation Figure 4 presents the estimated trajectory produced by the smoothing algorithm. As expected, this trajectory does not present discontinuities, since each correction due to a range measurement is distributed along the mission interval, and is not accounted for at a single instant of time. A detail of the two estimates of the vehicle trajectory is shown in fig. 5, were the red line corresponds to the real time estimate and the blue one to the post mission estimate. The two estimates are very similar when the vehicle is going in a straight line or is turning slowly. When the vehicle makes tight turns or when there are large corrections in the real time estimate, the two estimates can be a few meters away. Since the dynamic model used here does not describe with sufficient accuracy the vehicle motion during turns, larger corrections in the real time estimate are expected when the vehicle is turning. 55 60 65 70 75 80 85 90 95 100 105 110 115 -80 -75 -70 -65 -60 -55 -50 -45 -40 -35 east (m) north (m) Fig. 5 – Comparison of the two estimates One advantage of using stochastic filtering techniques, like the Kalman filter used in the real time estimation or the smoothing filter used the post mission estimation, is the information provided by the error covariance matrix P. The 2×2 sub-matrix of P, denoted Pxy, and corresponding to the x and y components of the filter state E, can be used to measure the quality of the estimate of the vehicle position. The matrix Pxy defines an uncertainty ellipse in the plane that characterizes the error of the position estimate. This ellipse in defined by 3 parameters: the lengths of its two principal axes and the orientation of its major axis. Figures 6 and 7 show the evolution of the lengths of the axes of the uncertainty ellipse along the mission for the real time and the post mission estimates, respectively. The higher values of uncertainty occur either when the vehicle is turning or when range measurements are scarce. In the real time case the lengths of the axes are mostly between 1 and 2 meters, while in the post mission case these lengths are almost always below 0.8 meters. This illustrates the superior accuracy of the post mission trajectory estimate, as expected. 1237 01000 2000 3000 4000 5000 6000 7000 0 1 2 3 4 5 6 7 time (s) uncertainty (m) Fig. 6 – Real time uncertainty ellipse axes 01000 2000 3000 4000 5000 6000 7000 0 0.4 0.8 1.2 1.6 2 time (s) uncertainty (m) Fig. 7 – Post mission uncertainty ellipse axes Finally, figure 8 shows the evolution of the lengths of the major axes of the uncertainty ellipses for both the real time estimate (red line) and the post mission estimate (blue line), during a short period of the mission. The real time uncertainty presents a typical saw-tooth behavior: the uncertainty grows as the dead reckoning data is integrated over time and is “reset” to a low value whenever a new range measurement is received by the navigation algorithm. On the contrary, the uncertainty of the post mission estimate varies smoothly, reflecting the continuity properties of this estimate. Nonetheless, the two uncertainties evolve in a correlated way, since their higher values and their lower values occur at almost the same instants of time. 6050 6100 6150 6200 6250 6300 0 0.5 1 1.5 2 2.5 3 3.5 time (s) uncertainty (m) Fig. 8 – Comparison of the two uncertainties VI. CONCLUSIONS Both the real time and the post mission trajectory estimation algorithms described in this paper have been successfully implemented and tested under several conditions. In a first stage, the performances of both algorithms have been evaluated with data taken from previous missions. After a few adjustments in the parameters of the algorithms, they were finally validated in real applications. The results obtained so far also show that the post mission trajectory smoothing algorithm can greatly improve the position estimate given by the on-board navigation system, mainly when the vehicle moves in turns or when range measurements are scarce. This is particularly relevant in the characterization of underwater features, as collected data has to be spatially located with the maximum possible accuracy. Although Isurus is not equipped with sophisticated dead reckoning navigation systems such as inertial measurement units or Doppler based acoustic velocimeters, the performance of the post mission algorithm seems to be very good, even when the vehicle is turning. REFERENCES [1] C. von Alt, B. Allen, T. Austin, and R. Stokey, “Remote Environmental Measuring Units”, Proceedings of the Autonomous Underwater Vehicle's 94 Conference, Cambridge, USA, 1994. [2] N. Cruz, J. Sousa, F. L. Pereira, A. Matos, E. Silva, J. Coimbra, and E. Dias, “PISCIS: Multiple Autonomous Underwater Vehicles for Environmental and Oceanographic Field Studies”, Proceedings Environment 2010 Conference, Porto, Portugal, 2003. [3] N. Cruz, A. Matos, A. Martins, J. Silva, D. Santos, D. Boutov, D. Ferreira and F. L. Pereira, “Estuarine Environment Studies with Isurus, a REMUS class AUV”, Proceedings of the MTS/IEEE Oceans’99 Conference. Seattle, USA, 1999. [4] N. Cruz, L. Madureira, A. Matos, and F. Lobo Pereira, “A Versatile Acoustic Beacon for Navigation and Remote Tracking of Multiple Underwater Vehicles”, Proceedings of the MTS/IEEE Oceans’01 Conference, Honolulu, USA, 2001. [5] A. Matos, N. Cruz, A. Martins and F. Lobo Pereira, “Development and Implementation of a Low-Cost LBL Navigation System for an AUV”, Proceedings of the MTS/IEEE Oceans’99 Conference, Seattle, USA, 1999. [6] A. Gelb, Applied Optimal Estimation, MIT Press, 1989. 1238