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

3D mapping is one of the most significant abilities for Autonomous Underwater Vehicles (AUV). This paper proposes a 3D mapping 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-looking MSS is used for 2D SLAM (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 .