This work presents a six degrees of freedom probabilistic scan matching method for registration of 3D underwater sonar scans. Unlike previous works, where local submaps are built to overcome measurement sparsity, our solution develops scan matching directly from the raw sonar data. Our method, based on the probabilistic Iterative Correspondence (pIC), takes measurement uncertainty into consideration while developing the registration procedure. A new probabilistic sensor model was developed to compute the uncertainty of each scan measurement individually. Initial displacement guesses are obtained from a probabilistic dead reckoning approach, also detailed in this document. Experiments, based on real data, demonstrate superior robustness and accuracy of our method with respect to the popular ICP algorithm. An improved trajectory is obtained by integration of scan matching updates in the localization data fusion algorithm, resulting in a substantial reduction of the original dead reckoning drift.
Keywords: AUV; Coda Echoscope 3D; ICP; localization; probabilistic scan matching; registration; underwater.