[发明专利]水下航行器同步定位与建图方法及水下航行器

权利要求书

1.一种水下航行器同步定位与建图方法，其特征在于，包括如下步骤：

S1：采用多波束图像声呐获取被测区域的传感数据，所述多波束图像声呐设置在水下航行器上；

S2：根据所述传感数据估计所述多波束图像声呐的实时的姿态信息；

S3：采用非线性优化算法对所述实时的姿态信息进行整合得到所述多波束图像声呐的第一轨迹和第一地图。

2.如权利要求1所述的水下航行器同步定位与建图方法，其特征在于，所述传感数据包括声呐图像，将所述声呐图像中每一个像素的深度值化为相应的灰度值得到灰度值矩阵。

3.如权利要求2所述的水下航行器同步定位与建图方法，其特征在于，对所述灰度值矩阵进行如下预处理：

对所述灰度值矩阵进行滤波得到滤波后的灰度值矩阵；

增强所述滤波后的灰度值矩阵的对比度并进行线性拟合得到预处理后的灰度值矩阵。

4.如权利要求3所述的水下航行器同步定位与建图方法，其特征在于，采用快速特征点提取和描述的算法对相邻两次采样得到的所述预处理后的灰度值矩阵中特征点的灰度值变化实时得到所述多波束图像声呐的所述姿态信息。

5.如权利要求4所述的水下航行器同步定位与建图方法，其特征在于，采用非线性优化算法对所述实时的姿态信息进行整合得到所述第一轨迹和所述第一地图还包括：

同步采用多普勒流速仪、惯性导航单元、深度计获取第二传感数据。

6.如权利要求5所述的水下航行器同步定位与建图方法，其特征在于，设置传感数据的置信度权重，通过卡尔曼融合算法将所述第二传感数据和所述姿态信息进行融合校正得到所述多波束图像声呐的坐标和姿态角度；

基于所述坐标的连续变化信息，经过非线性优化整合得到所述第一轨迹和所述第一地图。

7.如权利要求6所述的水下航行器同步定位与建图方法，其特征在于，还包括如下步骤：

S4：在整合所述实时的姿态信息过程中实时检测所述第一轨迹和所述第一地图，实时回环检测，若匹配到历史轨迹数据和已建地图信息则校正此时所述多波束图像声呐的所述坐标；

S5：依据校正后的所述坐标调整得到全局一致的第二轨迹和第二地图。

8.如权利要求7所述的水下航行器同步定位与建图方法，其特征在于，所述校正包括：

第一个灰度矩阵中特征点坐标为(xn,yn)，第二个灰度矩阵中所述特征点坐标为(un,vn)

若有则认为两个灰度值矩阵中的所述特征点位置匹配度高，认定为再次出现于同一位置上；则所述特征点坐标校正成时间顺序上最早得到的坐标。

9.一种水下航行器，其特征在于，包括多波束图像声呐和处理器；

所述多波束图像声呐设置在所述水下航行器的主体的至少一个面上；

所述处理器，用于执行如权利要求1-8任一所述的水下航行器同步定位与建图方法。

10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-8任一所述方法的步骤。