蛇形水下机器人多传感器融合的实时三维环境建模系统

本申请涉及一种蛇形水下机器人多传感器融合的实时三维环境建模系统。所述系统包括：传感器模块，用于通过传感器获取多模态数据；I MU位姿模块，用于对所述多模态数据中的所述惯性导航数据进行校正，得到I MU高频位姿数据；声呐模块，用于对所述多模态数据中的所述声呐数据进行补偿，得到声呐点云数据；视觉模块，用于对所述多模态数据中的所述视觉数据进行重建，得到视觉图像数据；融合模块，用于将所述I MU高频位姿数据、声呐点云数据及视觉图像数据输入紧耦合因子图模型进行计算，得到分层融合点云地图；建模模块，用于基于所述分层融合点云地图建模，得到三维环境建模。采用本系统能够实时三维建模，提高环境建模质量。