This thesis presents the development of a localization and mapping algorithm for an autonomous underwater vehicle (AUV). It is based on probabilistic scan matching of raw sonar scans within a pose-based simultaneous localization and mapping (SLAM) framework.
To address the motion-induced distortions affecting the generation of full sector scans, an extended Kalman filter (EKF) is used to estimate the robot motion during that scan. The filter uses a constant velocity model with acceleration noise for motion prediction.Velocities from Doppler velocity log (DVL) and heading measurements from attitude and heading reference system (AHRS) are fed asynchronously and update the state. The scan is undistorted by compounding the relative robot position in the scan, with the range and bearing measurements of the beams gathered by the sonar. Assuming Gaussian noise, the algorithm is able to estimate the uncertainty of the sonar measurements with respect to a frame located at the center of the scan.
For estimating the global trajectory of the vehicle, a second filter, an augmented state EKF (ASEKF), stores the pose of the vehicle where each full scan was completed. Each new full scan is cross registered with all the previous scans that are in a certain range and a modified probabilistic iterative correspondence (pIC) algorithm is applied. This technique has a twofold effect: first, we obtain a better estimate of the vehicle’s displacement that is then used to update the ASEKF, and second, loop-closing events are updated automatically and simultaneously.In addition, we present a closed form method for estimating the uncertainty of the scan matching result.
The proposed method is well suited for confined environments including but not limited to: geological folds, boulder areas, caves, man-made walls and structures, where horizontal scanning sonar can constantly detect and distinguish its surroundings over most of the vehicle’s trajectory.
The method was tested with three real-world datasets: one obtained in an abandoned marina during an engineering test mission, and two additional ones in the natural environment of underwater cavern systems. In the marina dataset, the results show the quality of our algorithm by comparing it to the ground truth from a global positioning system (GPS) receiver and to other previously published algorithms. For the cavern datasets, the results are compared against fixed ground truth points that the vehicle visits twice along the trajectory it travels. In all the experiments the trajectory correction is notable and the unoptimized algorithm execution time is much faster than the experiment time, indicating the potential of the algorithm for real-time on-board AUV operation.
________________________
Aquesta tesis presenta el desenvolupament d’un algoritme de localització i mapeig per un vehicle submarí autònom (AUV).
L’algoritme es basa en tècniques probabilístiques de scan matching utilitzant scans de sonar dins un sistema de localització i construcció simultània de mapes (SLAM) basat en posició.
S’utilitza un filtre de Kalman extès (EKF) per estimar el moviment del robot durant l’adquisició de l’scan i així solucionar les distorsions causades pel moviment al llarg de l’adquisició d’un scan complet.
Aquest filtre utilitza un model de velocitat constant amb l’acceleració com a soroll per predir el moviment.
Velocitats provinents d’un Doppler velocity log (DVL) i mesures d’una unitat d’orientació s’introdueixen al filtre asíncronament per tal d’actualitzar l’estat. La distorsió de l’scan s’elimina gràcies a la composició de la posició relativa del robot dins l’scan amb les mesures de rang i la orientació dels feixos del sónar. Assumint un soroll Gaussià, l’algoritme és capaç d’estimar la incertesa de les mesures del sónar respecte a un sistema de coordenades situat al centre de l’scan.
Per estimar la trajectòria global del vehicle, un segon filtre, en aquest cas un EKF d’estat augmentat (ASEKF), guarda la posició del vehicle on es finalitza l’adquisició de cada scan complet. Cada nou scan es registra amb tots els scans previs localitzats dins un rang determinat i s’aplica una modificació de l’algoritme probabilistic iterative correspondence (pIC).
Aquesta tècnica té dos propòsits: primer, obtenir una millor estimació del desplaçament del vehicle que posteriorment s’utilitzarà per actualitzar l’estat del ASEKF, i segon, actualitzar automàticament i simultàniament events de tancament de bucles. A més a més, es presenta un mètode de formulació tancada per estimar la incertesa del resultat de l’scan matching.
El mètode proposat és adequat per entorns confinats, com per exemple: plecs geològics, zones rocoses, coves, parets artificials i estructures on un sonar d’escaneig horitzontal pugui constantment detectar i distingir el seu voltant al llarg de la major part de la trajectòria del vehicle.
El mètode s’ha provat en tres datasets adquirits en entorns reals: un obtingut en una marina abonadonada i dos més en l’entorn natural d’un sistema de coves submarí. Els resultats presentats pel dataset de la marina demostren la qualitat de l’algoritme comparant-lo amb mesures d’un sistema de posicionament global (GPS) i amb dos algoritmes prèviament publicats. Pel què fa els datasets de la cova, els resultats es comparen amb punts de referència coneguts i fixes que el vehicle revisita al llarg de la trajectòria. En tots els experiments la correcció de la trajectòria és notable i el temps d’execució de l’algoritme és molt més ràpid que el temps de l’experiment, indicant així el potencial de l’algoritme per operar en temps real des d’un AUV.
