- Email: [email protected]
Geometric Bounding Techniques for Underwater Navigation
Proceedings of the 15th IFAC Symposium on System Identification Saint-Malo, France, July 6-8, 2009
Geometric Bounding Techniques for Underwater Navigation ⋆ Colin Morice ∗ Sandor Veres ∗∗ ∗
Department of Electromechanical Engineering, University of Southampton, Southampton, England, SO17 1BJ (e-mail: [email protected]). ∗∗ Department of Electromechanical Engineering, University of Southampton, Southampton, England, SO17 1BJ (e-mail: [email protected]). Abstract: This paper describes the application of geometric bounding techniques to navigation of an underwater vehicle. A technique is described for obtaining a fix of submersible position based on measurements of range from a supporting ship and the propagation of dead-reckoning errors. Also described is the estimation of bounds on a constant drift rate and the use of these bounds in the determination of velocity sensor misalignment angle. An assessment is made of the accuracy to which navigational parameters can be estimated using these methods. 1. INTRODUCTION Typically, Autonomous Underwater Vehicles (AUVs) navigate in the underwater domain using dead-reckoning navigation systems such as bottom-lock acoustic doppler systems and inertial navigation systems (INS). For navigation to remain accurate over long periods of time, position estimates from dead-reckoning systems must be updated periodically with measurements of absolute position, usually requiring the submersible to surface to obtain a GPS position fix. It is, however, undesirable for a submersible to routinely depart from its mission to obtain GPS fixes as surfacing is time consuming and because the accuracy of dead-reckoning navigation is much reduced while sensors are not in range of the seabed. These issues are greatly exacerbated in deep diving vehicles and so an alternative source of absolute position fix is required while navigating in the underwater domain. As a substitute for onboard GPS, acoustic localization techniques are increasingly being used for acquisition of geo-referenced position fixes while underwater. This paper describes techniques that have been developed for analysing navigational error in the deep diving autonomous underwater vehicle Autosub 6000 [McPhail, 2008], shown in Figure 1, using an acoustic localization system. Range-and-bearing acoustic positioning systems allow three dimensional positioning of a submersible but require careful calibration and are sensitive to inaccurate sound speed information. Alternatively, range-only sensors can be used to obtain position fixes with much reduced calibration issues and are less sensitive to sound speed errors. If a range-only system consisting of a single transponder mounted on the submersible and a transceiver mounted on a supported ship is to be used to obtain position fixes, range-only data must be fused with submersible dead⋆ This work is funded by the Underwater Systems Laboratory, National Oceanography Centre, Southampton, School of Engineering Sciences, University of Southampton, Southampton and the Engineering and Physical Sciences Research Council (EPSRC).
978-3-902661-47-0/09/$20.00 © 2009 IFAC
Fig. 1. Autosub 6000 being recovered during sea trials on board the RSS Discovery. reckoning positioning. In this case multiple temporally spaced range measurements are required to produce a single range-only position fix. Careful planning of the trajectories of both the ship and submersible is required if there is to be no ambiguity in derived position fixes. Position ambiguity can be dealt with by enforcing tight constraints on the trajectory of the submersible, as is a requirement seen in some optimization based techniques [Scherbatyuk, 1995]. Such tight constraints are not enforced by Kalman filter based methods [Larsen, 2000, Baccou and Jouvencel, 2003] which can result in a casual approach to mission planning leading to poor navigational accuracy. Further, the Kalman filter technique requires linearization of measurement errors around the submersible’s dead reckoning position estimate, which may give rise to large navigational errors if there is a large drift in dead-reckoning navigation. In this paper techniques will be outlined for obtaining error bounds on navigational parameters by modelling error bounds on acoustic range measurements and dead reckoning navigation. Errors in all sensor measurements will be taken into account, based on the assumption
1469
10.3182/20090706-3-FR-2004.0242
15th IFAC SYSID (SYSID 2009) Saint-Malo, France, July 6-8, 2009 that sensor errors lie within known bounds. While it cannot be claimed that no assumptions have been made of error distributions, given conservative error bounds we can be sure that the true values of estimated parameters are contained within computed solution sets. Bounding methods for state estimating, model parameter bounding and control problems have been developed in many papers [Walter, 1990, Kurzhanski and Veliov, 1994, Milanese et al., 1996]. In mobile robot positioning set inversion has previously been used by Jaulin et al. [2000] as a method for initializing the position of a mobile robot in a known environment using a series of range sensors. A set membership technique has been used by Caiti et al. [2005] for position estimation in underwater localization using multiple acoustic beacons. We will begin by extending the set membership approach described by Caiti et al. [2005] so that dead-reckoning position estimates are fused with acoustic range measurements from a single beacon. This allows the refinement of deadreckoning position estimation using a single transceivertransponder pair. We then proceed to develop a technique for localization using acoustic ranging and velocity measurements in the absence of initial position information. We will show how a set containing the initial position can be calculated exactly using bounding techniques. We will then develop techniques for estimation of unmeasured drift and relate this drift to dead-reckoning sensor misalignment. The advantage of the techniques presented here over those in which error statistics are either approximated or ignored altogether is that the techniques presented here yield solution sets that are guaranteed to contain the true values of parameters being estimated. By this method one gains information of the ranges of the estimated parameters that are not feasible given available sensory data and one is not mislead about the accuracy of estimates as can be the case with statistical approximations of errors in point estimates, especially when the number of data samples is relatively small for the law of large numbers to take effect. 2. METHODS We will demonstrate the use of geometric bounding techniques to solve three problems. Firstly we will address the problem of using acoustic ranging measurements to refine a dead-reckoning based estimate of the current position of an underwater vehicle. Secondly, we will derive a technique for obtaining bounds on the initial position of an underwater vehicle by taking range measurements while it undertakes a prescribed maneuver. Thirdly, we derive a method using range measurements for estimating a constant drift velocity which is undetected by onboard dead-reckoning systems. This drift rate is used to estimate bounds on an angle of misalignment of the velocity measurement sensor. 2.1 The use of range measurements to bound errors in vessel position We wish to use range measurements to refine a deadreckoning based estimate of the submersibles position. First we define the dead-reckoning based estimate as ˆ k = X0 ⊕ XR,k X (1)
ˆk where ⊕ denotes Minkowski addition as defined in (2), X is an estimated bounding set containing the submersible’s current position, X0 is a set containing the submersible’s initial position, XR,k is a bounding set containing the relative change in position from the initial position and k is a periodic sampling time index. (1) is equivalent to ˆ = {x0 + xR | x0 ∈ X0 , xR ∈ XR,k } X (2) In (1) the set XR,k , describing the position of the submersible relative to the initial position X0 , is defined as XR,k = Xv,k ⊕ Xd,k (3) where Xv,k and Xd,k are defined as follows: Xv,k = (V1 ⊕ V2 ⊕ . . . ⊕ Vk−1 ⊕ Vk )∆t (4) Xd,k = (D1 ⊕ D2 ⊕ . . . ⊕ Dk−1 ⊕ Dk )∆t (5) Here Vk is the Minkowski addition of a vector vk , given by the measured velocity at instant k rotated into a world coordinate frame, with a polytope Ev,k which describes the error in vk . Dk is defined by an unmeasured drift velocity dk with a bounding error Ed,k . If the velocity sensor is tracking the sea floor and is properly calibrated then this drift component of XR,k can be ignored. Vk = vk ⊕ Ev,k (6) Dk = dk ⊕ Ed,k (7) (1) therefore completely describes all possible positions of the submersible through propagation of errors in deadreckoning navigation. This set can be refined by restricting the position of the submersible to be within a range bound of an observing ship. A range measurement with bounded errors can be described by a set Rk . We know that the ˆ up,k must be range updated location of the submersible X bounded by the range measurement Rk and the deadˆk reckoning navigation estimate X ˆ up,k = Rk ∩ X ˆk X (8) and so a refined estimate of the submersible’s position can be calculated from Equations (1), (3) and (8) as ˆ up,k = Rk ∩ (X0 ⊕ Xv,k ⊕ Xd,k ) X (9) For a series of range measurements made at instants k = p1 . . . pN the position correction should be used for future dead reckoning updates. A position update at instant k = pn then becomes ˆp ˆ up,p = Rp ∩ (X (10) ⊕ Xv,p ⊕ Xd,p ) X n
n
n−1
n
n
where Xv,pn and Xd,pn are the addition of measured position changes and unmeasured drifts from instants k = p(n−1) to k = pn such that Xv,pn = (Vp(n−1) ⊕Vp(n−1) +1 ⊕. . .⊕Vpn −1 ⊕Vpn )∆t (11) Xd,pn = (Dp(n−1) ⊕Dp(n−1) +1 ⊕. . .⊕Dpn −1 ⊕Dpn )∆t (12) It should be noted that for highly vertical geometries (submersible depth much greater than the horizontal shipsubmersible distance) the range measurement set can be approximated as a spherical shell centred on the observing ship corrected for average sound speed using standard tables such as Carter’s tables Carter [1980]. If this is not the case then the range measurement must be constructed using a sound propagation model using a sound speed profile obtained in situ. The influence of GPS errors on the range measurement set can be included in calculations by Minkowski addition of a set describing uncertainty of ship position to the range set.
1470
15th IFAC SYSID (SYSID 2009) Saint-Malo, France, July 6-8, 2009 2.2 The use of range measurements to bound the initial position of a vessel
Noting (14), (20) can therefore be written ˆ d,k = Rk ⊕ (X− ⊕ X− ) X 0
To determine a bounding set for the submersible’s initial ˆ 0 encompassing all possible position we wish to find a set X initial positions for which a solution to (9) exists. Formally this is described by (13) {x0 | Rk ∩ (x0 ⊕ Xv,k ⊕ Xd,k ) 6= ∅} (13) Now we note that Minkowski addition can be described in an alternative form to that of (2) as A ⊕ B = {z | A ∩ (z − B) 6= ∅} (14) Bearing (14) in mind we see that (13) can be rewritten as ˆ 0,k = {x0 | Rk ∩ (x0 − X− ) 6= ∅} X (15) R,k
where X− R,k = {−xR,k | xR,k ∈ (Xv,k ⊕ Xd,k )} (13) can therefore be expressed as ˆ 0,k = Rk ⊕ X− X
R,k
(16) (17)
ˆ 0,k for the initial position based on the The best bound X range measurement at instant k is given by (15). When range measurements are available at a number of different ˆ 0,k for instants k the true initial condition is bounded by X all k. The true initial position can therefore be bounded by K \ ˆ0 = (Rk ⊕ X− (18) X R,k ) k=1
Further, if a prior bound Xp0 to X0 is known then ˆ 0 = Xp X 0
K \ \ ( (Rk ⊕ X− R,k ))
(19)
k=1
(19) allows estimation of the initial position of a submersible based on ship-submersible range measurements and dead-reckoning velocity information. In practice (19) is the form that will be used where Xp0 can describe an initial position derived from the onboard navigational equipment, depth bounds from a pressure sensor or the full region bounded by the sea floor and surface.
− Xd,k = {xd,k | Rk ∩ (xd,k − (X− 0 ⊕ Xv,k ) 6= ∅}
where
X− 0
is defined as X− 0 = {−x0 | x0 ∈ X0 }
and
X− v,k
(21) (22)
ˆ d,k on a constant in which case we may derive bounds V drift velocity vd using the range measurement at instant k as ˆ d,k = { xd,k | xd,k ∈ X ˆ d,k } V (26) k∆t Given that the true drift rate must be bounded by all ˆ d,k then the true drift rate must be bounded estimates V by K \ ˆd = ˆ d,k V V (27) k=1
Using (24), (26) and (27) we are therefore able to obtain an estimate of a constant drift velocity based on deadreckoning measurements, ship-submersible range measurements and a conservative estimate of the initial position of the submersible. We can modify this technique for estimation of constant drift to allow estimation of misalignment angle of the dead reckoning system. We will assume that if the submersible is bottom tracking a so capable of maintaining a constant velocity relative to the sea floor. Under the condition that the submersible is traveling at a constant speed s and bearing θ for all k we can equate the estimated drift ˆ d to constant velocity offset V ˆ a due to sensor velocity V misalignment so that K \ ˆ d,k ˆa = V (28) V k=1
A misalignment of the velocity measurement system by an angle of φ will produce a velocity error in the submersible body frame of
sˆbx
= {−xv,k | xv,k ∈ Xv,k }
ebx = sˆbx . cos(φ) + sˆby . sin(φ) − sˆbx
(29)
eby = sˆby . cos(φ) − sˆbx . sin(φ) − sˆby
(30)
sˆby
where and are the measured forward and starboard components of velocity and ebx and eby are errors in velocity measurements in the forward and starboard directions due to sensor misalignment. Given that we know the bearing θ at which the submersible is traveling we can rotate the estimated drift around −θ to obtain an estimate of the velocity error caused by sensor misalignment in the body reference frame. ˆ b = R′ .V ˆa V (31) a θ Following this rotation, for a submersible maintaining a constant measured forward velocity and zero starboard velocity so that sˆbx = sˆ and sˆby = 0 we find that the velocity errors in the body frame are
is defined as X− v,k
(24)
ˆ d,k is equivalent to the drift One may observe that X velocity integrated over time. Given the assumption of a ˆ d,k is equivalent to constant drift rate Vd it is clear that X ˆ an estimated Vd,k scaled by a factor k∆t so that ˆ d,k = {kˆ ˆ d,k } ˆd ∈ V X vd ∆t | v (25)
2.3 The use of range measurements to bound estimates of constant drift rate and sensor alignment angle. In practice it is likely that a conservative estimate of the initial position of the submersible will be available while drift velocities will not be. It would therefore be prudent to use the available initial position estimate, dead-reckoning data and range measurements to estimate drift velocities. The unmeasured drift Xd,k must be such that a solution to (9) exists. Formally this condition is expressed as {xd,k | Rk ∩ (xd,k ⊕ X0 ⊕ Xv,k ) 6= ∅} (20) From (14) we observe that this can be expressed as
v,k
(23)
1471
ebx = sˆ. cos(φ) − sˆ
(32)
eby
(33)
= −ˆ s. sin(φ)
15th IFAC SYSID (SYSID 2009) Saint-Malo, France, July 6-8, 2009 Table 1. Errors used in simulations.
2500 AUV path Range measurment Ship position
Simulation Errors ADCP velocity error ± 0.3 % distance traveled INS error < 0.02 deg (negligible) Range error ± 0.4 m Ship GPS error ± 1.23 m Depth error ±1m
y − direction (m)
2000
We can therefore obtain bounds on the misalignment angle ˆ b. φ from (34) and (35) for all [ebx eby ]T in V a ebx − sˆ ) sˆ −eby φ = arcsin( ) sˆ φ = arccos(
1500
1000
(34) (35)
3. SIMULATION OF GEOMETRIC TECHNIQUES USING ARTIFICIAL DATA The techniques described in Section 2 have been applied to simulated navigational data using the Geometric Bounding Toolbox [Veres, 2003]. Modeled error bounds in measurements and real world parameters take values that the author deems to be consistent with those that would be encountered in real world operation (see Table 1). Values quoted are consistent with those stated by Kinsey et al. [2006]. As this paper represents part of a project to develop navigational tools for the deep diving underwater vehicle, Autosub 6000, error parameters have been chosen that most accurately model errors in Autosub’s sensors. Autosub 6000, being equipped with an IXSEA PHINS INS, can be expected to experience a heading error of up to 0.02 deg. This corresponds to a positioning error that is two orders of magnitude smaller than can be expected of the RDI Workhorse Acoustic Doppler Current Profiler (ADCP) used by Autosub 6000 to measure velocity. Because of this, errors in INS heading measurements have been neglected. The range error quoted is that stated by LinkQuest for the TrackLink 10000 system that is installed on Autosub 6000. The bounds in GPS position fixes are taken at 99.7 % confidence intervals of fix accuracy seen in GPS data. Depth measurements are calculated from an onboard pressure sensor. 3.1 The use of range measurements to bound the initial position of a vessel Simulations have been carried out to estimate the initial position based on (19). An example solution for the box trajectory shown in Figure 2 is shown in Figure ˆ 0,k 3. A number of estimates of the initial position X are computed as rings (sections of spheres limited by bounds on the depth measurement). Each of these rings describes a set containing all possible initial positions of the submersible that are consistent with available sensor measurements at time k. The true initial position must be consistent with all sensor measurements and so must lie within the intersection of each of these rings. For practical application of the system we are interested in the expected performance under various real world conditions. For initial position estimation, a ray tracing model has been used to simulate acoustic range measurements
500 −500
0
500 x − direction (m)
1000
1500
Fig. 2. An example trajectory used for localization. The submersible begins at position (500,500) and navigates a box of sidelength 1000 m around the ship, located at the origin, while ship-to-submersible range is measured acoustically. for sound speed profiles that can be expected in North Atlantic and in Antarctic Regions. These sound speed profiles represent the extremes of sound speed variation that Autosub is likely to encounter in operation. The sound speed profile will affect the size of the observed solution set by affecting the angle at which acoustic wavefronts arrive at the submersible and will also have a small affect on the size on the bounds of the range shell. It is desirable to have a measure of the accuracy that can be expected for a given box size at a given depth. To this end, navigational boxes similar to that shown in Figure 2 have been simulated for boxes of side length ranging from 250 m to 1500 m, and at depths ranging from 500 m to 4000 m. Error parameters used in simulations are those shown in Table 1. For all simulations the resulting initial position sets contain the true initial positions. The sizes of the initial position set calculated in each simulation are shown in Figure 4. These sizes are the lengths of the maximum dimension across the initial position set calculated in each simulation. It can be seen that the solution size is relatively insensitive to sound speed profile so long as sound speed information is accurate, with a difference between solution size observed in the North Atlantic and Antarctic waters of less than 0.5 m in all simulations. The size of solution sets is a trade off between the growth of error bounds in deadreckoning navigation increasing with larger box size and decreasing angle of acoustic wavefronts to the horizontal with smaller box sizes. It can be seen that solution sizes in Figure 4 are sufficiently small for practical use in bounding initial position. 3.2 The use of range measurements to bound estimate of constant drift rate In this section we will look at the problem of estimation of an unmeasured drift velocity, which can in turn be used to estimate an angle by which the velocity sensor is misaligned. This system requires an estimate of initial position, which may be taken from the vehicles dead-
1472
15th IFAC SYSID (SYSID 2009) Saint-Malo, France, July 6-8, 2009
Fig. 3. Initial position estimation for the box maneuver. Left Panel: each shaded ring is a solution for X0,k corresponding to range measurement Rk and dead-reckoning information up to instant k. Right Panel: the intersection of all ˆ 0. solutions of X0,k giving the estimate of initial position X the initial position of the submersible is presumed to be known to within ± 20 m.
largest distance across initial position solution (m)
28 500 m depth 1000 m depth 2000 m depth 3000 m depth 4000 m depth
26 24
Although one can expect two discrete sets of velocity solutions when the submersible navigates a linear trajectory, in the case of bearing estimation the correct solution can be determined by considering the range of acceptable drift rates that can be observed for an acceptable range of misalignment angles. Using (34) the range of drift velocities depicted in Figure 5 are consistent with a sensor misalignment in the range of 0.22 deg and 2.21 deg. While this result does encompass the true misalignment angle of 1 deg the accuracy of the result is not sufficient for this technique to be preferable over others available. This technique may however be of use in post processing situations when navigational data is corrupted by a poorly calibrated sensor.
22 20 18 16 14 12 10 8
0
500 1000 side length of navigational box (m)
1500
4. CONCLUDING REMARKS
ˆ 0 corresponding for Fig. 4. Size of initial position solution X boxes with sides of length 250 m to 1500 m calculated in 250 m steps at depths of 500 m to 4000 m. Results for antarctic sound speed conditions are shown as a solid line while North Atlantic conditions are shown as dashed lines. reckoning position estimate so long as error bounds in the position estimate are large enough to account for the unmeasured drift. Velocity estimation has been applied to simulated data for a submersible maintaining a constant heading with a velocity measurement system misaligned by 1 deg. If the submersible maintains a constant velocity relative to the sea floor then any velocity error can be considered to be due to the misaligned sensor and will be a constant component of the measured velocity. Using (24) this component has been estimated (Figure 5). To produce these results
The techniques described in Section 2 are advantageous over others described in the literature in a number of ways. Firstly, the full range of possible solutions that are consistent with measurement errors are calculated, rather than the single most likely solution. Secondly, the computed solution sets may be disparate and so can show multiple discrete solutions. Thirdly, no statistical assumptions are made about sensor measurements other than that they lie within acceptable tolerances. At present there are, however, some limitations introduced by the computational techniques used to produce the results shown in Section 3. As calculations are currently based on vertex and polytope facet calculations, the input and output sets can only be described as straight edged, polygonal sets. There is therefore some approximation of the range measurement sets, which are inherently curved. The computational techniques currently used are also limited to application in short term estimation of navigational
1473
15th IFAC SYSID (SYSID 2009) Saint-Malo, France, July 6-8, 2009
Fig. 5. Sensor misalignment estimation for a bottom tracking dead reckoning sensor that is misaligned by 1 deg. Left panel: each shaded ring indicates the range of drift rates that are consistent with a range measurement. Values of drift rate in the intersection of these rings are consistent with all measurements. Right panel: the range of drift rates, shown rotated into the body reference frame, that are consistent with all sensor measurements and consistent with a heading misalignment of 0.22 to 2.21 deg. parameters, as solution sets become increasingly complex as additional range measurement information is incorporated. The geometric techniques are also highly sensitive to outliers. Techniques for dealing with outliers in geometric computation are, however, well documented [Bai et al., 2002]. While the techniques described here have been applied to problems in underwater navigation they can equally well be applied to problems in terrestrial navigation systems such as the initialization of a mobile robot location. In this setting bounds on the initial location of a robot can be computed exactly based on computation of the equations described in Section 2, without introducing any approximations to the solution set. We believe that the techniques described here have potential as useful tools both in their own right and for validation of navigational parameters estimated by other methods. It is hoped that the results shown in Section 3 will serve as useful guidelines to the accuracy that can be achieved in single-transponder pair range-only navigation. REFERENCES P. Baccou and B. Jouvencel. Simulation results, postprocessing experimentations and comparisons results for navigation, homing and multiple vehicles operations with a new positioning method using on transponder. Intelligent Robots and Systems, 2003.(IROS 2003). Proceedings. 2003 IEEE/RSJ International Conference on, 1, 2003. E.W. Bai, H. Cho, R. Tempo, and Y. Ye. Optimization with few violated constraints for linear bounded errorparameter estimation. Automatic Control, IEEE Transactions on, 47(7):1067–1077, 2002. A. Caiti, A. Garulli, F. Livide, and D. Prattichizzo. Localization of autonomous underwater vehicles by floating
acoustic buoys: a set-membership approach. Oceanic Engineering, IEEE Journal of, 30(1):140–152, 2005. D.J.T. Carter. Echo-sounding correction tables. Hydrographic Department, Ministry of Defence, Taunton, UK, NP, 139:150, 1980. L. Jaulin, E. Walter, O. Leveque, and D. Meizel. Set inversion for chi-algorithms, with application to guaranteed robot localization. Mathematics and Computers in Simulation, 52(3):197–210, 2000. J.C. Kinsey, R.M. Eustice, and L.L. Whitcomb. A survey of underwater vehicle navigation: recent advances and new challenges. proc. 7th Conference on Maneuvering and Control of Marine Craft (MCMC’2006), IFAC, Lisbon, 2006. A.B. Kurzhanski and V.M. Veliov, editors. Modeling Techniques for Uncertain Systems, Progress in Systems & Control Theory. Birkh¨ auser/IIASA, Boston, 1994. M.B. Larsen. Synthetic long baseline navigation of underwater vehicles. OCEANS 2000 MTS/IEEE Conference and Exhibition, 3, 2000. S.D. McPhail. RRS Discovery Cruise D323, 19 September3 October 2007. First. Technical report, National Oceanography Centre, Southampton, 2008. M. Milanese, J. Norton, H. Piet-Lahanier, and E. Walter, editors. Bounding Approaches to System Identification. Plenum Press. New York, NY, USA, 1996. A.P. Scherbatyuk. The AUV positioning using ranges from one transponder LBL. OCEANS’95. MTS/IEEE.’Challenges of Our Changing Global Environment’. Conference Proceedings., 3, 1995. S.M. Veres. Geometric Bounding Toolbox (GBT) for Matlab. Official website: http://www. sysbrain. com, 2003. E. Walter. Special issue on parameter identification with error bounds. Mathematics and Computers in Simulation, 32(5&6):447–607, 1990.
1474
Geometric Bounding Techniques for Underwater Navigation ⋆ Colin Morice ∗ Sandor Veres ∗∗ ∗
Department of Electromechanical Engineering, University of Southampton, Southampton, England, SO17 1BJ (e-mail: [email protected]). ∗∗ Department of Electromechanical Engineering, University of Southampton, Southampton, England, SO17 1BJ (e-mail: [email protected]). Abstract: This paper describes the application of geometric bounding techniques to navigation of an underwater vehicle. A technique is described for obtaining a fix of submersible position based on measurements of range from a supporting ship and the propagation of dead-reckoning errors. Also described is the estimation of bounds on a constant drift rate and the use of these bounds in the determination of velocity sensor misalignment angle. An assessment is made of the accuracy to which navigational parameters can be estimated using these methods. 1. INTRODUCTION Typically, Autonomous Underwater Vehicles (AUVs) navigate in the underwater domain using dead-reckoning navigation systems such as bottom-lock acoustic doppler systems and inertial navigation systems (INS). For navigation to remain accurate over long periods of time, position estimates from dead-reckoning systems must be updated periodically with measurements of absolute position, usually requiring the submersible to surface to obtain a GPS position fix. It is, however, undesirable for a submersible to routinely depart from its mission to obtain GPS fixes as surfacing is time consuming and because the accuracy of dead-reckoning navigation is much reduced while sensors are not in range of the seabed. These issues are greatly exacerbated in deep diving vehicles and so an alternative source of absolute position fix is required while navigating in the underwater domain. As a substitute for onboard GPS, acoustic localization techniques are increasingly being used for acquisition of geo-referenced position fixes while underwater. This paper describes techniques that have been developed for analysing navigational error in the deep diving autonomous underwater vehicle Autosub 6000 [McPhail, 2008], shown in Figure 1, using an acoustic localization system. Range-and-bearing acoustic positioning systems allow three dimensional positioning of a submersible but require careful calibration and are sensitive to inaccurate sound speed information. Alternatively, range-only sensors can be used to obtain position fixes with much reduced calibration issues and are less sensitive to sound speed errors. If a range-only system consisting of a single transponder mounted on the submersible and a transceiver mounted on a supported ship is to be used to obtain position fixes, range-only data must be fused with submersible dead⋆ This work is funded by the Underwater Systems Laboratory, National Oceanography Centre, Southampton, School of Engineering Sciences, University of Southampton, Southampton and the Engineering and Physical Sciences Research Council (EPSRC).
978-3-902661-47-0/09/$20.00 © 2009 IFAC
Fig. 1. Autosub 6000 being recovered during sea trials on board the RSS Discovery. reckoning positioning. In this case multiple temporally spaced range measurements are required to produce a single range-only position fix. Careful planning of the trajectories of both the ship and submersible is required if there is to be no ambiguity in derived position fixes. Position ambiguity can be dealt with by enforcing tight constraints on the trajectory of the submersible, as is a requirement seen in some optimization based techniques [Scherbatyuk, 1995]. Such tight constraints are not enforced by Kalman filter based methods [Larsen, 2000, Baccou and Jouvencel, 2003] which can result in a casual approach to mission planning leading to poor navigational accuracy. Further, the Kalman filter technique requires linearization of measurement errors around the submersible’s dead reckoning position estimate, which may give rise to large navigational errors if there is a large drift in dead-reckoning navigation. In this paper techniques will be outlined for obtaining error bounds on navigational parameters by modelling error bounds on acoustic range measurements and dead reckoning navigation. Errors in all sensor measurements will be taken into account, based on the assumption
1469
10.3182/20090706-3-FR-2004.0242
15th IFAC SYSID (SYSID 2009) Saint-Malo, France, July 6-8, 2009 that sensor errors lie within known bounds. While it cannot be claimed that no assumptions have been made of error distributions, given conservative error bounds we can be sure that the true values of estimated parameters are contained within computed solution sets. Bounding methods for state estimating, model parameter bounding and control problems have been developed in many papers [Walter, 1990, Kurzhanski and Veliov, 1994, Milanese et al., 1996]. In mobile robot positioning set inversion has previously been used by Jaulin et al. [2000] as a method for initializing the position of a mobile robot in a known environment using a series of range sensors. A set membership technique has been used by Caiti et al. [2005] for position estimation in underwater localization using multiple acoustic beacons. We will begin by extending the set membership approach described by Caiti et al. [2005] so that dead-reckoning position estimates are fused with acoustic range measurements from a single beacon. This allows the refinement of deadreckoning position estimation using a single transceivertransponder pair. We then proceed to develop a technique for localization using acoustic ranging and velocity measurements in the absence of initial position information. We will show how a set containing the initial position can be calculated exactly using bounding techniques. We will then develop techniques for estimation of unmeasured drift and relate this drift to dead-reckoning sensor misalignment. The advantage of the techniques presented here over those in which error statistics are either approximated or ignored altogether is that the techniques presented here yield solution sets that are guaranteed to contain the true values of parameters being estimated. By this method one gains information of the ranges of the estimated parameters that are not feasible given available sensory data and one is not mislead about the accuracy of estimates as can be the case with statistical approximations of errors in point estimates, especially when the number of data samples is relatively small for the law of large numbers to take effect. 2. METHODS We will demonstrate the use of geometric bounding techniques to solve three problems. Firstly we will address the problem of using acoustic ranging measurements to refine a dead-reckoning based estimate of the current position of an underwater vehicle. Secondly, we will derive a technique for obtaining bounds on the initial position of an underwater vehicle by taking range measurements while it undertakes a prescribed maneuver. Thirdly, we derive a method using range measurements for estimating a constant drift velocity which is undetected by onboard dead-reckoning systems. This drift rate is used to estimate bounds on an angle of misalignment of the velocity measurement sensor. 2.1 The use of range measurements to bound errors in vessel position We wish to use range measurements to refine a deadreckoning based estimate of the submersibles position. First we define the dead-reckoning based estimate as ˆ k = X0 ⊕ XR,k X (1)
ˆk where ⊕ denotes Minkowski addition as defined in (2), X is an estimated bounding set containing the submersible’s current position, X0 is a set containing the submersible’s initial position, XR,k is a bounding set containing the relative change in position from the initial position and k is a periodic sampling time index. (1) is equivalent to ˆ = {x0 + xR | x0 ∈ X0 , xR ∈ XR,k } X (2) In (1) the set XR,k , describing the position of the submersible relative to the initial position X0 , is defined as XR,k = Xv,k ⊕ Xd,k (3) where Xv,k and Xd,k are defined as follows: Xv,k = (V1 ⊕ V2 ⊕ . . . ⊕ Vk−1 ⊕ Vk )∆t (4) Xd,k = (D1 ⊕ D2 ⊕ . . . ⊕ Dk−1 ⊕ Dk )∆t (5) Here Vk is the Minkowski addition of a vector vk , given by the measured velocity at instant k rotated into a world coordinate frame, with a polytope Ev,k which describes the error in vk . Dk is defined by an unmeasured drift velocity dk with a bounding error Ed,k . If the velocity sensor is tracking the sea floor and is properly calibrated then this drift component of XR,k can be ignored. Vk = vk ⊕ Ev,k (6) Dk = dk ⊕ Ed,k (7) (1) therefore completely describes all possible positions of the submersible through propagation of errors in deadreckoning navigation. This set can be refined by restricting the position of the submersible to be within a range bound of an observing ship. A range measurement with bounded errors can be described by a set Rk . We know that the ˆ up,k must be range updated location of the submersible X bounded by the range measurement Rk and the deadˆk reckoning navigation estimate X ˆ up,k = Rk ∩ X ˆk X (8) and so a refined estimate of the submersible’s position can be calculated from Equations (1), (3) and (8) as ˆ up,k = Rk ∩ (X0 ⊕ Xv,k ⊕ Xd,k ) X (9) For a series of range measurements made at instants k = p1 . . . pN the position correction should be used for future dead reckoning updates. A position update at instant k = pn then becomes ˆp ˆ up,p = Rp ∩ (X (10) ⊕ Xv,p ⊕ Xd,p ) X n
n
n−1
n
n
where Xv,pn and Xd,pn are the addition of measured position changes and unmeasured drifts from instants k = p(n−1) to k = pn such that Xv,pn = (Vp(n−1) ⊕Vp(n−1) +1 ⊕. . .⊕Vpn −1 ⊕Vpn )∆t (11) Xd,pn = (Dp(n−1) ⊕Dp(n−1) +1 ⊕. . .⊕Dpn −1 ⊕Dpn )∆t (12) It should be noted that for highly vertical geometries (submersible depth much greater than the horizontal shipsubmersible distance) the range measurement set can be approximated as a spherical shell centred on the observing ship corrected for average sound speed using standard tables such as Carter’s tables Carter [1980]. If this is not the case then the range measurement must be constructed using a sound propagation model using a sound speed profile obtained in situ. The influence of GPS errors on the range measurement set can be included in calculations by Minkowski addition of a set describing uncertainty of ship position to the range set.
1470
15th IFAC SYSID (SYSID 2009) Saint-Malo, France, July 6-8, 2009 2.2 The use of range measurements to bound the initial position of a vessel
Noting (14), (20) can therefore be written ˆ d,k = Rk ⊕ (X− ⊕ X− ) X 0
To determine a bounding set for the submersible’s initial ˆ 0 encompassing all possible position we wish to find a set X initial positions for which a solution to (9) exists. Formally this is described by (13) {x0 | Rk ∩ (x0 ⊕ Xv,k ⊕ Xd,k ) 6= ∅} (13) Now we note that Minkowski addition can be described in an alternative form to that of (2) as A ⊕ B = {z | A ∩ (z − B) 6= ∅} (14) Bearing (14) in mind we see that (13) can be rewritten as ˆ 0,k = {x0 | Rk ∩ (x0 − X− ) 6= ∅} X (15) R,k
where X− R,k = {−xR,k | xR,k ∈ (Xv,k ⊕ Xd,k )} (13) can therefore be expressed as ˆ 0,k = Rk ⊕ X− X
R,k
(16) (17)
ˆ 0,k for the initial position based on the The best bound X range measurement at instant k is given by (15). When range measurements are available at a number of different ˆ 0,k for instants k the true initial condition is bounded by X all k. The true initial position can therefore be bounded by K \ ˆ0 = (Rk ⊕ X− (18) X R,k ) k=1
Further, if a prior bound Xp0 to X0 is known then ˆ 0 = Xp X 0
K \ \ ( (Rk ⊕ X− R,k ))
(19)
k=1
(19) allows estimation of the initial position of a submersible based on ship-submersible range measurements and dead-reckoning velocity information. In practice (19) is the form that will be used where Xp0 can describe an initial position derived from the onboard navigational equipment, depth bounds from a pressure sensor or the full region bounded by the sea floor and surface.
− Xd,k = {xd,k | Rk ∩ (xd,k − (X− 0 ⊕ Xv,k ) 6= ∅}
where
X− 0
is defined as X− 0 = {−x0 | x0 ∈ X0 }
and
X− v,k
(21) (22)
ˆ d,k on a constant in which case we may derive bounds V drift velocity vd using the range measurement at instant k as ˆ d,k = { xd,k | xd,k ∈ X ˆ d,k } V (26) k∆t Given that the true drift rate must be bounded by all ˆ d,k then the true drift rate must be bounded estimates V by K \ ˆd = ˆ d,k V V (27) k=1
Using (24), (26) and (27) we are therefore able to obtain an estimate of a constant drift velocity based on deadreckoning measurements, ship-submersible range measurements and a conservative estimate of the initial position of the submersible. We can modify this technique for estimation of constant drift to allow estimation of misalignment angle of the dead reckoning system. We will assume that if the submersible is bottom tracking a so capable of maintaining a constant velocity relative to the sea floor. Under the condition that the submersible is traveling at a constant speed s and bearing θ for all k we can equate the estimated drift ˆ d to constant velocity offset V ˆ a due to sensor velocity V misalignment so that K \ ˆ d,k ˆa = V (28) V k=1
A misalignment of the velocity measurement system by an angle of φ will produce a velocity error in the submersible body frame of
sˆbx
= {−xv,k | xv,k ∈ Xv,k }
ebx = sˆbx . cos(φ) + sˆby . sin(φ) − sˆbx
(29)
eby = sˆby . cos(φ) − sˆbx . sin(φ) − sˆby
(30)
sˆby
where and are the measured forward and starboard components of velocity and ebx and eby are errors in velocity measurements in the forward and starboard directions due to sensor misalignment. Given that we know the bearing θ at which the submersible is traveling we can rotate the estimated drift around −θ to obtain an estimate of the velocity error caused by sensor misalignment in the body reference frame. ˆ b = R′ .V ˆa V (31) a θ Following this rotation, for a submersible maintaining a constant measured forward velocity and zero starboard velocity so that sˆbx = sˆ and sˆby = 0 we find that the velocity errors in the body frame are
is defined as X− v,k
(24)
ˆ d,k is equivalent to the drift One may observe that X velocity integrated over time. Given the assumption of a ˆ d,k is equivalent to constant drift rate Vd it is clear that X ˆ an estimated Vd,k scaled by a factor k∆t so that ˆ d,k = {kˆ ˆ d,k } ˆd ∈ V X vd ∆t | v (25)
2.3 The use of range measurements to bound estimates of constant drift rate and sensor alignment angle. In practice it is likely that a conservative estimate of the initial position of the submersible will be available while drift velocities will not be. It would therefore be prudent to use the available initial position estimate, dead-reckoning data and range measurements to estimate drift velocities. The unmeasured drift Xd,k must be such that a solution to (9) exists. Formally this condition is expressed as {xd,k | Rk ∩ (xd,k ⊕ X0 ⊕ Xv,k ) 6= ∅} (20) From (14) we observe that this can be expressed as
v,k
(23)
1471
ebx = sˆ. cos(φ) − sˆ
(32)
eby
(33)
= −ˆ s. sin(φ)
15th IFAC SYSID (SYSID 2009) Saint-Malo, France, July 6-8, 2009 Table 1. Errors used in simulations.
2500 AUV path Range measurment Ship position
Simulation Errors ADCP velocity error ± 0.3 % distance traveled INS error < 0.02 deg (negligible) Range error ± 0.4 m Ship GPS error ± 1.23 m Depth error ±1m
y − direction (m)
2000
We can therefore obtain bounds on the misalignment angle ˆ b. φ from (34) and (35) for all [ebx eby ]T in V a ebx − sˆ ) sˆ −eby φ = arcsin( ) sˆ φ = arccos(
1500
1000
(34) (35)
3. SIMULATION OF GEOMETRIC TECHNIQUES USING ARTIFICIAL DATA The techniques described in Section 2 have been applied to simulated navigational data using the Geometric Bounding Toolbox [Veres, 2003]. Modeled error bounds in measurements and real world parameters take values that the author deems to be consistent with those that would be encountered in real world operation (see Table 1). Values quoted are consistent with those stated by Kinsey et al. [2006]. As this paper represents part of a project to develop navigational tools for the deep diving underwater vehicle, Autosub 6000, error parameters have been chosen that most accurately model errors in Autosub’s sensors. Autosub 6000, being equipped with an IXSEA PHINS INS, can be expected to experience a heading error of up to 0.02 deg. This corresponds to a positioning error that is two orders of magnitude smaller than can be expected of the RDI Workhorse Acoustic Doppler Current Profiler (ADCP) used by Autosub 6000 to measure velocity. Because of this, errors in INS heading measurements have been neglected. The range error quoted is that stated by LinkQuest for the TrackLink 10000 system that is installed on Autosub 6000. The bounds in GPS position fixes are taken at 99.7 % confidence intervals of fix accuracy seen in GPS data. Depth measurements are calculated from an onboard pressure sensor. 3.1 The use of range measurements to bound the initial position of a vessel Simulations have been carried out to estimate the initial position based on (19). An example solution for the box trajectory shown in Figure 2 is shown in Figure ˆ 0,k 3. A number of estimates of the initial position X are computed as rings (sections of spheres limited by bounds on the depth measurement). Each of these rings describes a set containing all possible initial positions of the submersible that are consistent with available sensor measurements at time k. The true initial position must be consistent with all sensor measurements and so must lie within the intersection of each of these rings. For practical application of the system we are interested in the expected performance under various real world conditions. For initial position estimation, a ray tracing model has been used to simulate acoustic range measurements
500 −500
0
500 x − direction (m)
1000
1500
Fig. 2. An example trajectory used for localization. The submersible begins at position (500,500) and navigates a box of sidelength 1000 m around the ship, located at the origin, while ship-to-submersible range is measured acoustically. for sound speed profiles that can be expected in North Atlantic and in Antarctic Regions. These sound speed profiles represent the extremes of sound speed variation that Autosub is likely to encounter in operation. The sound speed profile will affect the size of the observed solution set by affecting the angle at which acoustic wavefronts arrive at the submersible and will also have a small affect on the size on the bounds of the range shell. It is desirable to have a measure of the accuracy that can be expected for a given box size at a given depth. To this end, navigational boxes similar to that shown in Figure 2 have been simulated for boxes of side length ranging from 250 m to 1500 m, and at depths ranging from 500 m to 4000 m. Error parameters used in simulations are those shown in Table 1. For all simulations the resulting initial position sets contain the true initial positions. The sizes of the initial position set calculated in each simulation are shown in Figure 4. These sizes are the lengths of the maximum dimension across the initial position set calculated in each simulation. It can be seen that the solution size is relatively insensitive to sound speed profile so long as sound speed information is accurate, with a difference between solution size observed in the North Atlantic and Antarctic waters of less than 0.5 m in all simulations. The size of solution sets is a trade off between the growth of error bounds in deadreckoning navigation increasing with larger box size and decreasing angle of acoustic wavefronts to the horizontal with smaller box sizes. It can be seen that solution sizes in Figure 4 are sufficiently small for practical use in bounding initial position. 3.2 The use of range measurements to bound estimate of constant drift rate In this section we will look at the problem of estimation of an unmeasured drift velocity, which can in turn be used to estimate an angle by which the velocity sensor is misaligned. This system requires an estimate of initial position, which may be taken from the vehicles dead-
1472
15th IFAC SYSID (SYSID 2009) Saint-Malo, France, July 6-8, 2009
Fig. 3. Initial position estimation for the box maneuver. Left Panel: each shaded ring is a solution for X0,k corresponding to range measurement Rk and dead-reckoning information up to instant k. Right Panel: the intersection of all ˆ 0. solutions of X0,k giving the estimate of initial position X the initial position of the submersible is presumed to be known to within ± 20 m.
largest distance across initial position solution (m)
28 500 m depth 1000 m depth 2000 m depth 3000 m depth 4000 m depth
26 24
Although one can expect two discrete sets of velocity solutions when the submersible navigates a linear trajectory, in the case of bearing estimation the correct solution can be determined by considering the range of acceptable drift rates that can be observed for an acceptable range of misalignment angles. Using (34) the range of drift velocities depicted in Figure 5 are consistent with a sensor misalignment in the range of 0.22 deg and 2.21 deg. While this result does encompass the true misalignment angle of 1 deg the accuracy of the result is not sufficient for this technique to be preferable over others available. This technique may however be of use in post processing situations when navigational data is corrupted by a poorly calibrated sensor.
22 20 18 16 14 12 10 8
0
500 1000 side length of navigational box (m)
1500
4. CONCLUDING REMARKS
ˆ 0 corresponding for Fig. 4. Size of initial position solution X boxes with sides of length 250 m to 1500 m calculated in 250 m steps at depths of 500 m to 4000 m. Results for antarctic sound speed conditions are shown as a solid line while North Atlantic conditions are shown as dashed lines. reckoning position estimate so long as error bounds in the position estimate are large enough to account for the unmeasured drift. Velocity estimation has been applied to simulated data for a submersible maintaining a constant heading with a velocity measurement system misaligned by 1 deg. If the submersible maintains a constant velocity relative to the sea floor then any velocity error can be considered to be due to the misaligned sensor and will be a constant component of the measured velocity. Using (24) this component has been estimated (Figure 5). To produce these results
The techniques described in Section 2 are advantageous over others described in the literature in a number of ways. Firstly, the full range of possible solutions that are consistent with measurement errors are calculated, rather than the single most likely solution. Secondly, the computed solution sets may be disparate and so can show multiple discrete solutions. Thirdly, no statistical assumptions are made about sensor measurements other than that they lie within acceptable tolerances. At present there are, however, some limitations introduced by the computational techniques used to produce the results shown in Section 3. As calculations are currently based on vertex and polytope facet calculations, the input and output sets can only be described as straight edged, polygonal sets. There is therefore some approximation of the range measurement sets, which are inherently curved. The computational techniques currently used are also limited to application in short term estimation of navigational
1473
15th IFAC SYSID (SYSID 2009) Saint-Malo, France, July 6-8, 2009
Fig. 5. Sensor misalignment estimation for a bottom tracking dead reckoning sensor that is misaligned by 1 deg. Left panel: each shaded ring indicates the range of drift rates that are consistent with a range measurement. Values of drift rate in the intersection of these rings are consistent with all measurements. Right panel: the range of drift rates, shown rotated into the body reference frame, that are consistent with all sensor measurements and consistent with a heading misalignment of 0.22 to 2.21 deg. parameters, as solution sets become increasingly complex as additional range measurement information is incorporated. The geometric techniques are also highly sensitive to outliers. Techniques for dealing with outliers in geometric computation are, however, well documented [Bai et al., 2002]. While the techniques described here have been applied to problems in underwater navigation they can equally well be applied to problems in terrestrial navigation systems such as the initialization of a mobile robot location. In this setting bounds on the initial location of a robot can be computed exactly based on computation of the equations described in Section 2, without introducing any approximations to the solution set. We believe that the techniques described here have potential as useful tools both in their own right and for validation of navigational parameters estimated by other methods. It is hoped that the results shown in Section 3 will serve as useful guidelines to the accuracy that can be achieved in single-transponder pair range-only navigation. REFERENCES P. Baccou and B. Jouvencel. Simulation results, postprocessing experimentations and comparisons results for navigation, homing and multiple vehicles operations with a new positioning method using on transponder. Intelligent Robots and Systems, 2003.(IROS 2003). Proceedings. 2003 IEEE/RSJ International Conference on, 1, 2003. E.W. Bai, H. Cho, R. Tempo, and Y. Ye. Optimization with few violated constraints for linear bounded errorparameter estimation. Automatic Control, IEEE Transactions on, 47(7):1067–1077, 2002. A. Caiti, A. Garulli, F. Livide, and D. Prattichizzo. Localization of autonomous underwater vehicles by floating
acoustic buoys: a set-membership approach. Oceanic Engineering, IEEE Journal of, 30(1):140–152, 2005. D.J.T. Carter. Echo-sounding correction tables. Hydrographic Department, Ministry of Defence, Taunton, UK, NP, 139:150, 1980. L. Jaulin, E. Walter, O. Leveque, and D. Meizel. Set inversion for chi-algorithms, with application to guaranteed robot localization. Mathematics and Computers in Simulation, 52(3):197–210, 2000. J.C. Kinsey, R.M. Eustice, and L.L. Whitcomb. A survey of underwater vehicle navigation: recent advances and new challenges. proc. 7th Conference on Maneuvering and Control of Marine Craft (MCMC’2006), IFAC, Lisbon, 2006. A.B. Kurzhanski and V.M. Veliov, editors. Modeling Techniques for Uncertain Systems, Progress in Systems & Control Theory. Birkh¨ auser/IIASA, Boston, 1994. M.B. Larsen. Synthetic long baseline navigation of underwater vehicles. OCEANS 2000 MTS/IEEE Conference and Exhibition, 3, 2000. S.D. McPhail. RRS Discovery Cruise D323, 19 September3 October 2007. First. Technical report, National Oceanography Centre, Southampton, 2008. M. Milanese, J. Norton, H. Piet-Lahanier, and E. Walter, editors. Bounding Approaches to System Identification. Plenum Press. New York, NY, USA, 1996. A.P. Scherbatyuk. The AUV positioning using ranges from one transponder LBL. OCEANS’95. MTS/IEEE.’Challenges of Our Changing Global Environment’. Conference Proceedings., 3, 1995. S.M. Veres. Geometric Bounding Toolbox (GBT) for Matlab. Official website: http://www. sysbrain. com, 2003. E. Walter. Special issue on parameter identification with error bounds. Mathematics and Computers in Simulation, 32(5&6):447–607, 1990.
1474