Advanced Robotics, Vol. 15, No. 5, pp. 533– 549 (2001)ÓVSP and Robotics Society of Japan 2001.Full paperTowards terrain-aided navigation for underwater roboticsSTEFAN WILLIAMS¤, GAMINI DISSANAYAKEand HUGH D URRANT-WHYTEAustralian Centre for Field Robotics, Department of Mechanical and Mechatronic Engineering,University of Sydney, NSW 2 006, AustraliaReceived 27 July 2000; accepted 19 November 2000Abstract—This paper describes an approach to autonomous navigation for an undersea vehicle thatuses information f rom a scanning sonar to generate navigation estimates based on a simultaneouslocalization and mapping algorithm. Development of low-speed platform models for vehicle controland the theoretical and practical details of mapping and position e stimation using sonar are provided.An implementation of these techniques on a small submersible vehicle ‘Oberon’ are presented.Keywords: Terrain-aided navigation; localization; ma pping; uncertainty; autonomous underwatervehicle.1. INTRODUCTIONCurrent work on undersea vehicles at the Australian Centre for F ield Roboticsconcentrates on the development of terrain-aided navigation techniques, sensorfusion and vehicle control architectures for real-time platform control. Positionand attitud e estimation algorithms that use information from scanning sonar tocomplement a vehicle dynamic model and unobservable environmen tal disturbancesare invaluable in the subsea environment. Key elements of the current researchwork include the developmen t of sonar feature models, the tracking and use ofthese models in mapping an d position estimation, and the development of low-speedplatform models for vehicle control.While many land-based robots use GPS or maps of the environment to provideaccurate position updates for navigation, a robot operating underwater does nottypically have access to this type of information. In underwater scienti c missions,a priori maps are seldom availab le and other methods for localisation must beconsidered. Many underwater robotic systems rely on  xed acoustic tran spondersthat are su rveyed into the robot’s work area [1]. These transponders are then¤To whom correspondence should be addressed. E-mail: [email protected]. Williamset al.interrogated to trian gulate the position of the vehicle. The surveying of thesetransponders can be a costly and time consuming affair — especially at the depths atwhich these vehicles often o perate and their performance can vary with conditionswithin the water co lumn in which the vehicle is operating.As an alternative to beacon-based navigation, a vehicle can use its o n-boardsensors to extract terrain information from the environment in which it is operating.One of the key technologies being developed in the context of this work is analgorithm for Simultaneous Localization and Map Building (SLAM) to estimate theposition of an underwater vehicle. SLAM is the process of concurrently buildingup a feature based map of the environment and using this map to obtain estimatesof the location of the vehicle [2– 6]. In essence, the vehicle relies on its abilityto extract useful navigation informatio n from the data returned by its sensors.The robot typically starts at an u nknown location with no a priori knowledge oflandmark locations. From relative observations of landmarks, it simultaneouslycomputes an estimate of vehicle location and an estimate of landmark locations.While continuing in motion, the robot builds a complete map of landmarks and usesthese to provide continu ous estimates of the vehicle location. The potential for thistype of navigation system for su bsea robots is enormous considering the dif cultiesinvolved in localization in underwater environments.This paper presents the results of the application of SLAM to estimate the motionof an underwater vehicle. This work represen ts the  rst instance of a deployableunderwater implementation of the SLAM algorithm. Section 2 introduces theOberon submersible vehicle developed at the Centre and b rie y describes thesensors and actuators used. Section 3 summ arizes the stochastic mapping algorithmused for SLAM, while Section 4 presents the feature extraction and data associationtechniques used to generate the observations for the SLAM algorithm. In Section 5,a series of trials are described and the results of applying SLAM durin g  eld trials ina natural terrain environment along Sydney’s coast are p resented. Finally, Section6 concludes the paper by summarizing the results and discussing future researchtopics as well as on-going work.2. THE OBERON VEHICLEThe experimental platform used for the work reported in this paper is a mid-sizesubmersible robotic vehicle called Oberon designed and built at the Australian Cen-tre for Field Robotics (see Fig. 1). The vehicle is equipped with two scanninglow-frequency terrain-aiding sonars and a color CCD camera, togeth er with bathy-ometric depth sensors, a  ber optic gyroscope and a magneto-inductive compasswith integrated two-axis tilt sensor [7]. This vehicle is intended primarily as a re-search platform upon which to test novel sensing strategies and control methods.Autonomous navigation usin g the information provided by the vehicle’s on-boardsensors represen ts one of the ultimate goals of the project [8].Towards terrain-aided navigation535Figure 1. Oberon at sea.3. FEATURE-BASED POSITION ESTIMATIONThis section presents a feature based localisation and mapping technique used forgenerating vehicle position estimates. By tracking the relative position between thevehicle and identi able features in the environment, both the position of the vehicleand the position of the features can be estimated simu ltaneously. The correlationinformation between the estimates of the vehicle and feature locations is maintainedto ensure that consistent estimates of these states are generated.3.1. The estimation processThe localization and map building process consists of a recursive, three-stageprocedure comprising p rediction, observation and u pdate steps using an extendedKalman  lter (EKF) [3]. The Kalman  lter is a recursive, least-squares estimatorand produces at timeka minimum mean-squared error estimate Ox.kjk/of the statex.k/given a series of observations, ZkD [z.1/ : : : z.k/]:Ox.kjk/DE£xjZk¤:(1)The  lter fuses a predicted state estimate Ox.kjk¡ 1/with an observation z.k/ofthe state x.k/to produce the updated estimateOx.kjk/. For the SLAM