3D Mapping by a Robotic Fish with Two Mechanical Scanning Sonars

3D mapping is one of themost significant abilities for autonomous underwater vehicles (AUV). This paper proposes a 3Dmapping algorithm for a robotic fish using two mechanical scanning sonars (MSSs) with one being forward-looking and the other downward-looking. Combined with inertial measurement unit (IMU), the forward-lookingMSSis used for 2DSLAM(simultaneous localization and mapping) by which the 2D poses of the vehicle are optimally obtained by applying a pose-based GraphSLAM. Based on the estimated 2D poses, depth and orientation, the measurements from the downward-looking sonar are used to build the 3D map by adapting 3D mapping algorithm Octomap while taking into account the pose uncertainty. The effectiveness of the proposed algorithm is verified by extensive simulations which also show that it can generate more informative 3D map than the scenario where no uncertainty of poses is considered.