一种针对水下视觉退化场景的机器人在线定位和建图方法

本发明公开了一种针对水下视觉退化场景的机器人在线定位和建图方法，该方法包括在线外参标定；利用相机图像数据进行初始化，如失败则利用声纳图像数据初始化；读取成像声纳、双目相机的图像数据，得到匹配的声纳图像特征点、相机图像特征点并判断是否发生视觉退化，若发生则利用声纳图像特征点进行声纳位姿估计以优化IMU传播估计得到的位姿；读取IMU传感器数据并进行IMU预积分，利用预积分量进行IMU传播估计，得到机器人当前时刻的IMU相对于世界坐标系的位姿；基于光束平差法进行机器人位姿的优化求解，并进行建图。本发明通过融合双目相机，IMU，成像声纳的数据来提升视觉退化场景下更准确、更鲁棒的水下机器人定位和建图能力。