CN114488164B - 水下航行器同步定位与建图方法及水下航行器 - Google Patents

Abstract

本发明提供一种水下航行器同步定位与建图方法及水下航行器，方法包括：S1：采用多波束图像声呐获取被测区域的传感数据，所述多波束图像声呐设置在水下航行器上；S2：根据所述传感数据估计所述多波束图像声呐的实时的姿态信息；S3：采用非线性优化算法对所述实时的姿态信息进行整合得到所述多波束图像声呐的第一轨迹和第一地图。以多波束图像声呐为传感器基础，建立适用于实际海洋环境的SLAM技术及其算法，实现航行器在自身位置不确定的条件下，在完全未知环境中创建地图，同时利用地图进行自主定位和导航的问题。

Description

水下航行器同步定位与建图方法及水下航行器

技术领域

本发明涉及水下定位与地图构建技术领域，尤其涉及一种水下航行器同步定位与建图方法及水下航行器。

背景技术

水下航行器在广袤海域中航行，获取准确的定位信息是关键。目前，较为成熟的水下定位方式主要有以下几类：第一类是依托惯性传感器(IMU)，通过测量物体三轴姿态角以及加速度来计算定位信息，该类方法的优势是仅凭借IMU而无需外源信息辅助即可实现定位，劣势是IMU的惯性传感器会有误差累计效应，随着时间的增长，定位误差增加，精度漂移较大。第二类是依托声学基线系统，主要有长、短、超短基线，一般通过水下事先安装有应答基阵，航行器载体上安装有发射基阵，通过声学信号传播时延差，列出解算方程，最终确定被测载体的三维位置坐标。第三类是依托多普勒速度仪(DVL)，通过声学多普勒效应来测量航行器对海底的相对速度，以此来实现定位。第四类是依托重力、磁场等无源定位的方式，适用于水下环境的物理场变化较为显著的区域，当航行体行驶至该目标区域时，可以使用外部相关信息来及时修正定位误差，但是缺乏普适性，无法适用于绝大多数的海域情况中。

在海洋这样广袤和复杂的巨大环境中，缺少预期作业区域的地图或地形信息的情况极为普遍，SLAM技术具有较好的普适性。航行器所工作的海洋环境往往是未知的海域，不便事先大量布置海底信标，这时传统的定位方式便存在局限性，需要更加智能的SLAM技术来完成航行器自主定位和智能决策。在海洋环境中，水下环境的情况和地面环境的情况差异极大。传统的光学感知传感器如光学摄像头、红外传感器和激光雷达，由于水下电磁波衰减剧烈，工作范围十分受限。

以上背景技术内容的公开仅用于辅助理解本发明的构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创造性。

发明内容

本发明为了解决现有的问题，提供一种水下航行器同步定位与建图方法及水下航行器。

为了解决上述问题，本发明采用的技术方案如下所述：

一种水下航行器同步定位与建图方法，包括如下步骤：S1：采用多波束图像声呐获取被测区域的传感数据，所述多波束图像声呐设置在水下航行器上；S2：根据所述传感数据估计所述多波束图像声呐的实时的姿态信息；S3：采用非线性优化算法对所述实时的姿态信息进行整合得到所述多波束图像声呐的第一轨迹和第一地图。

优选地，所述传感数据包括声呐图像，将所述声呐图像中每一个像素的深度值化为相应的灰度值得到灰度值矩阵。

优选地，对所述灰度值矩阵进行如下预处理：对所述灰度值矩阵进行滤波得到滤波后的灰度值矩阵；增强所述滤波后的灰度值矩阵的对比度并进行线性拟合得到预处理后的灰度值矩阵。

优选地，采用快速特征点提取和描述的算法对相邻两次采样得到的所述预处理后的灰度值矩阵中特征点的灰度值变化实时得到所述多波束图像声呐的所述姿态信息。

优选地，采用非线性优化算法对所述实时的姿态信息进行整合得到所述第一轨迹和所述第一地图还包括：同步采用多普勒流速仪、惯性导航单元、深度计获取第二传感数据。

优选地，设置传感数据的置信度权重，通过卡尔曼融合算法将所述第二传感数据和所述姿态信息进行融合校正得到所述多波束图像声呐的坐标和姿态角度；基于所述坐标的连续变化信息，经过非线性优化整合得到所述第一轨迹和所述第一地图。

优选地，还包括如下步骤：S4：在整合所述实时的姿态信息过程中实时检测所述第一轨迹和所述第一地图，实时回环检测，若匹配到历史轨迹数据和已建地图信息则校正此时所述多波束图像声呐的所述坐标；S5：依据校正后的所述坐标调整得到全局一致的第二轨迹和第二地图。

优选地，所述校正包括：第一个灰度矩阵中特征点坐标为(xn,yn)，第二个灰度矩阵中所述特征点坐标为(un,vn)

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

本发明还提供一种水下航行器，包括多波束图像声呐和处理器；所述多波束图像声呐设置在所述水下航行器的主体的至少一个面上；所述处理器，用于执行如上任一所述的水下航行器同步定位与建图方法。

本发明再提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上任一所述方法的步骤。

本发明的有益效果为：提供一种水下航行器同步定位与建图方法及水下航行器，通过多波束图像声呐在水下进行定位，具有更远的视距，能够在漆黑无光的海底水域工作，能够探测到海底沉积物之下的地形地貌特征和人造结构物特征，更适应海洋环境，具有普适性；以多波束图像声呐为传感器基础，建立适用于实际海洋环境的SLAM技术及其算法，实现航行器在自身位置不确定的条件下，在完全未知环境中创建地图，同时利用地图进行自主定位和导航的问题。

附图说明

图1是本发明实施例中一种水下航行器同步定位与建图方法的示意图。

图2是本发明实施例中一种水下航行器同步定位与建图融合算法的流程示意图。

图3是本发明实施例中一种水下航行器同步定位与建图融合算法的硬件示意图。

图4是本发明实施例中又一种水下航行器同步定位与建图方法的示意图。

图5是本发明实施例中一种水下航行器同步定位与建图流程的示意图。

图6是本发明实施例中又一种水下航行器同步定位与建图流程的示意图。

图7是本发明实施例中一种水下航行器的结构示意图。

图8是本发明实施例中又一种水下航行器的结构示意图。

图9是本发明实施例中一种水下航行器同步定位与建图方法的效果示意图。

具体实施方式

为了使本发明实施例所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

需要说明的是，当元件被称为“固定于”或“设置于”另一个元件，它可以直接在另一个元件上或者间接在该另一个元件上。当一个元件被称为是“连接于”另一个元件，它可以是直接连接到另一个元件或间接连接至该另一个元件上。另外，连接既可以是用于固定作用也可以是用于电路连通作用。

需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多该特征。在本发明实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

目前，其他较为成熟的水下定位替代方案主要有以下三类：第一类是依托惯性传感器(IMU)，通过测量物体三轴姿态角以及加速度来计算定位信息，该类方法的优势是仅凭借IMU而无需外源信息辅助即可实现定位，劣势是IMU的惯性传感器会有误差累计效应，随着时间的增长，定位误差累积，精度漂移较大。第二类是依托声学基线系统，主要有长、短、超短基线，一般通过水下事先安装有应答基阵，航行器载体上安装有发射基阵，通过声学信号传播时延差，列出解算方程，最终确定被测载体的三维位置坐标。第三类是依托多普勒速度仪(DVL)，通过声学多普勒效应来测量航行器对海底和海流的相对速度，以此来实现定位。第四类是依托重力、磁场等无源定位的方式，适用于水下环境的物理场变化较为显著的区域，当航行体行驶至该目标区域时，可以使用外部相关信息来及时修正定位误差，但是缺乏普适性，无法适用于绝大多数的海域情况中。

如图1所示，一种水下航行器同步定位与建图方法，包括如下步骤：

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

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

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

本发明以多波束图像声呐为传感器基础，建立适用于实际海洋环境的SLAM技术及其算法，解决航行器需要在自身位置不确定的条件下，在完全未知环境中创建地图，同时利用地图进行自主定位和导航的问题。

在现有水下SLAM技术中，没有使用过多波束图像声呐传感器，本发明创新性地使用多波束图像声呐作为传感器，其技术将远不同于使用光学传感器的传统水下SLAM技术。多波束图像声呐相当于光学相机，能够在水下感知更大范围内的环境特征，其水下视距远大于光学相机，能获取较大面积的范围感知信息，解决水下环境水下的特征不明显的问题。同时水下环境与地面环境中较为明显的特征情况相比是有较大差异的，同时存在着的大量的干扰以及声呐噪声，加大了水下声呐图像处理的难度和效果。

多波束图像声呐拥有发射基阵和接收基阵，基阵由换能器构成。多波束图像声呐向被测区域发射带有特定信息的声信号，该信号在海水中传播时遇到障碍物即声呐目标物时，就会产生回声信号，回声信号被声呐的接收换能器接收后，再将声信号转换为相应的电信号，根据这些信息(包括声学反射强度信息、发射接收时间延迟信息和相位差异信息)形成的图像序列，并可直接得到目标的距离、方位的物理属性。按照发射波束和接收波束的先后顺序和序列，按其强度值设置相应灰度值，按照显示像素进行排列即可得到图像数据。这些传感数据，通过网络接口标准，通过网线传输至边缘计算板中，从而实现数据的读取。

多波束图像声呐的发声部分是“T”型阵列，一个是发声装置，一个是接收装置；基阵主要由直线阵列组成发声部分由多个连续排列的声源组成，声波会叠加，从而实现了声波的定向发射，电子扫描实现接收预成多波束，实现二维成像，获得二维声图。其可获取在海洋环境中获取较远距离的水声图像和探测障碍物的距离信息，并可据此传感信息为后续实现SLAM中的前端和后端提供传感数据基础，这是一种全新的水下SLAM技术思路。

在本发明的一种实施例中，传感数据包括声呐图像，将声呐图像中每一个像素的深度值化为相应的灰度值得到灰度值矩阵。具体地，多波束图像声呐传感的原始数据是许多单个波束发射所返回的时间值大小的集合(距离信息)，其会反馈为一张声呐图像。本发明将反馈得到的声呐图像中的每一个像素分离开来，依据每一个像素的深度信息将其转化为相应的灰度值，距离越小，灰度值越小，距离越大，灰度值越大，用此方法将整个图像转化为一个灰度值矩阵。多波束图像声呐获取的水下图像存在大量的畸变数据，这是由于声音在不均匀水质中传播返回的过程中会发生折射，这在图像转化为灰度值矩阵的过程中会造成失真。本发明对所述灰度值矩阵进行如下预处理：

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

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

具体地，通过对多波束图像声呐的反馈图像，转化为灰度值矩阵后进行滤波，去除明显的误差影区的灰度值数据，然后对其进行对比度上的增强(即在灰度值变化比较剧烈的像素点与像素点之间，通过乘上一定的系数来加大他们之间的差值的方法，从而实现对比度上的增强)，最后对灰度值较为接近且像素位置较为接近的像素值进行曲线拟合。通过上述方法来减少水下声学折射对感知数据的失真。

像素点的灰度值的判别，设像素点坐标为(x,y)，则有Gray(x,y)＝[R(x,y)+G(x,y)+B(x,y)]/3；若Gray(x,y)同时满足Gray(x,y)<Gray(x-1,y)且Gray(x,y)<Gray(x+1,y)且Gray(x,y)<Gray(x,y-1)且Gray(x,y)<Gray(x,y+1)且Gray(x,y)<Gray(x+1,y+1)且Gray(x,y)<Gray(x+1,y-1)且Gray(x,y)<Gray(x-1,y+1)且Gray(x,y)<Gray(x-1,y-1)；则有Gray1(x,y)＝Gray(x,y)*0.8；Gray1(x±1,y±1)＝Gray(x±1,y±1)*1.2；同理反之亦然。最后对灰度值较为接近且像素位置较为接近的像素值进行曲线拟合。通过上述方法来减少水下声学折射对感知数据的失真。

在视觉里程计部分，也即前端处理算法中，具体思路是：首先建立携带多波束图像声呐的航行器的运动方程，然后建立多波束图像声呐的观测方程，在建立完运动方程和观测方程后，对多波束图像声呐的成像模型进行标定。

在建立了图像声呐的成像模型并标定之后，可将多波束图像声呐所获取的任意前后两帧之间的特征点进行匹配并计算姿态。特征点匹配上使用ORB算法(图像的特征点时图像中比较显著显著的点，如轮廓点，较暗区域中的亮点，较亮区域中的暗点等。ORB采用FAST算法来检测特征点。这个定义基于特征点周围的图像灰度值，检测候选特征点周围一圈的像素值，如果候选点周围领域内有足够多的像素点与该候选点的灰度值差别够大，则认为该候选点为一个特征点。

具体地，得到了初步处理后的灰度值矩阵后，其每一次传感采样都会得到一个全新的灰度值矩阵。将灰度值矩阵中的特征点用ORB算法进行框定，在一个灰度值矩阵中最终会框定有若干特征点。然后在下一次相邻的采样中得到的灰度值矩阵中，同样用ORB算法对特征点进行框定，也会得到若干特征点，这些特征点用同样的ORB算法框定，在前后两个灰度值矩阵的差异并不很大，框定的特征点绝大多数就是上一个灰度值矩阵中的特征点，可以近似认为这些灰度值特征点的地球坐标是相同的。因此，通过比较前后两次采样得到灰度值特征点的变化，结合多波束图像声呐本身的成像数学关系，可以解算出前后两次采样之间，声呐位姿的变化情况。通过类似的方法，可以实现解算很长时间范围内，较为连续的位姿变化情况。在获取特征点后，进行位姿求解，可以采用对极几何法。

通过上述步骤，采用快速特征点提取和描述的算法对相邻两次采样得到的所述预处理后的灰度值矩阵中特征点的灰度值变化实时得到所述多波束图像声呐的所述姿态信息。

在后端处理算法。在前端已经通过视觉里程计算法得到了初步的位姿估计。但是该SLAM系统中，同时拥有其他水下定位的传感器，也能辅助提供一定参考意义的定位数据(都转化为世界坐标系下的定位数据)，提供卡尔曼滤波算法，并赋予不同的置信度的权重，实现数据融合。

如图2和图3所示，具体地，在本发明的一种实施例中，采用非线性优化算法对所述实时的姿态信息进行整合得到所述第一轨迹和所述第一地图还包括：

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

航行器上装有海洋中专有的定位辅助传感器，如多普勒流速仪、惯性导航单元、深度计等，将它们的数据设置一定的置信度权重，通过卡尔曼融合算法，将其和前述整合的初步姿态信息进行融合校正。然后，采用非线性优化算法对所述实时的姿态信息进行整合得到第一轨迹和第一地图。

设置传感数据的置信度权重，通过卡尔曼融合算法将所述第二传感数据和所述姿态信息进行融合校正得到所述多波束图像声呐的坐标和姿态角度；

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

具体地，设置一定的置信度权重k(多普勒流速仪)＝0.1；k(惯性导航单元)＝0.1；k(深度计)＝0.1；k(声呐视觉里程计)＝0.7；

通过卡尔曼融合算法，将所述第二传感数据和所述姿态信息进行融合校正，即姿态信息中相同类型的数据融合算法为坐标：

(x,y,z)＝k(多普勒流速仪)*(x,y,z)(多普勒流速仪)+k(惯性导航单元)*(x,y,z)(惯性导航单元)+k(深度计)*(x,y,z)(深度计)+k(声呐视觉里程计)*(x,y,z)(声呐视觉里程计)；

姿态角度：

θ＝k(多普勒流速仪)*θ(多普勒流速仪)+k(惯性导航单元)*θ(惯性导航单元)+k(深度计)*θ(深度计)+k(声呐视觉里程计)*θ(声呐视觉里程计)；

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

在本发明的一种实施例中，在整合所述实时的姿态信息过程中实时检测所述第一轨迹和所述第一地图，实时回环检测，一旦匹配到历史轨迹数据和已建地图信息(即携带多波束图像声呐的航行器第二次经过曾经到达过的相同地点时)，立刻校正此时的坐标，以此来减少和消除定位和建图的累计误差。

如图4所示，本发明的水下航行器同步定位与建图方法还包括如下步骤：

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

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

具体地，所述校正包括：

携带多波束图像声呐的航行器下水时的地球坐标是已知的，而且是准确的，即起点坐标是已知且准确的。下水后，本发明的同步定位与建图方法便开始工作。在起点坐标位置正确的基础上，航行器在航行过程中用上述已经说明的方法不断计算实时的姿态，并实时根据姿态变化计算坐标并更新轨迹。在上述方法中，姿态的估计是通过获取灰度值矩阵中特征点位置的变化而计算得到的，当航行器上的多波束图像声呐扫描获取的图像，转化为灰度值矩阵，发现转化得到的灰度值矩阵中特征点的位置与前述已经扫描得到过的灰度值矩阵中的特征点位置匹配度(相似度)很高，即设两个矩阵中特征点中，第一个灰度矩阵中特征点坐标为(xn,yn)，第二个灰度矩阵中所述特征点坐标为(un,vn)

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

图5是本发明实施例中一种水下航行器同步定位与建图流程的示意图。

图6是本发明实施例中又一种水下航行器同步定位与建图流程的示意图。

如图7和图8所示，一种水下航行器，其特征在于，包括多波束图像声呐和处理器；

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

所述处理器，用于执行如上任一所述的水下航行器同步定位与建图方法。

如图9所示，通过在AUV艏部上搭载多波束图像声呐，在方形海洋工程水池中用该方法完成了航迹定位和水池壁面的初步建模，能够实现方形水池样式的建图效果和定位位置的效果。

在本发明的一种实施例中，水下航行器上还包括IMU(惯性导航单元)、DVL(多普勒对底流速仪)、深度计(水深压力计)辅助传感器；边缘计算板和PC等组成。其安装方式为：多波束图像声呐固定安装于航行器载体的前侧，IMU、DVL、深度计等辅助传感器均安装于航行器本体上，它们产生的数据均先传回航行器中的边缘计算板，并在其上进行SLAM计算，或将它们的数据通过脐带缆线传回地面PC，再进行SLAM计算。

与传统的水下航行器的各类定位方法相比，本发明通过多波束声呐对海洋环境进行感知，通过SLAM智能算法，综合处理声呐获取的海洋环境信息，进行海洋环境地图构建和定位。它可以实现将一个水下航行器放入未知环境中的未知位置，使其机器人边移动边逐步描绘出此环境完全的地图，进行定位与建图。在海洋这样广袤和复杂的巨大环境中，缺少预期作业区域的地图或地形信息的情况极为普遍，SLAM技术具有较好的普适性。航行器所工作的海洋环境往往是未知的海域，也是不便事先大量布置海底信标的区域，这时传统的定位方式便存在局限性，需要更加智能的SLAM技术来完成航行器自主定位和智能决策。而传统的水下定位方式中IMU的惯性传感器会有误差累计效应，声学基线系统要事先布置海底声学信标，而依托重力、磁场等无源定位的方式，局限性较大，无法适用于绝大多数的海域情况中。

多波束图像声呐优势在于：相较于光学摄像头，多波束图像声呐在水下具有更远的视距，能够在漆黑无光的海底水域工作，能够探测到海底沉积物之下的地形地貌特征和人造结构物特征，更适应海洋环境。

基于多波束声呐的水下航行器SLAM方法在工作时，将感知传感器多波束图像声呐搭载于水下航行器上，对多波束图像声呐信息进行读取和预处理，同时对惯性传感器、DVL等信息进行读取和同步。前端基于声呐图像的视觉里程计估算相邻声呐图像间声呐载体位姿的运动，以及局部地图的样子。后端接受不同时刻视觉里程计测量的声呐载体位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。回环检测判断水下航行器是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。最后，根据估计的轨迹，建立与任务要求对应的地图，并据此实现定位的功能。

在本发明的一种实施例中，利用光学传感器作为传感来源的水下航行器同步定位与建图方法。所采用的感知传感器一般为光学摄像头、红外传感器。然而，水下环境的情况和地面环境的情况差异极大。首先，在水下电磁波衰减剧烈，不同频率的光照射的深度并不一样，红光透射的最浅大约只有几米左右，橙黄色的光能通过10-30米的深度，蓝绿光可超过100米。因此，光学传感器的工作范围十分受限，在水深30米以上的区域中，一般自然光已经十分微弱，光学传感器已经难以发挥作用。其次，水下环境中存在大量的杂质(漂浮有鱼群、藻类、沙石等)，也有由于水体折射和散射造成的大量环境噪声，对于沉积物下方的地形特征难以提取等。另外，海底地形地貌特征与地面地形特性的差异性也较大，传统的光学摄像头、红外传感器也难以实现较好的感知。在水下SLAM中，通过使用多波束图像声呐作为感知传感器相较之是更加合适的选择。多波束图像声呐利用声波在水中传播和反射来获取海底的三维地形地貌数据，相较于光学摄像头，多波束图像声呐具有更远的视距，能够在漆黑无光的海底水域工作，能够探测到海底沉积物之下的地形地貌特征和人造结构体特征。因此，多波束图像声呐更适合在海洋环境中使用。

在本发明中基于多波束图像声呐传感的水下航行器同步定位与建图方法中，仅在一个方向中使用了多波束图像声呐进行扫描。在改进方案中可以在航行器的六个面六个方向上均设置多波束图像声呐，这样能够获取更加全面的外界传感信息，增强水下航行器同步定位与建图方法准确性和鲁棒性。

本申请实施例还提供一种控制装置，包括处理器和用于存储计算机程序的存储介质；其中，处理器用于执行所述计算机程序时至少执行如上所述的方法。

本申请实施例还提供一种存储介质，用于存储计算机程序，该计算机程序被执行时至少执行如上所述的方法。

本申请实施例还提供一种处理器，所述处理器执行计算机程序，至少执行如上所述的方法。

所述存储介质可以由任何类型的易失性或非易失性存储设备、或者它们的组合来实现。其中，非易失性存储器可以是只读存储器(ROM，Read Only Memory)、可编程只读存储器(PROM，Programmable Read-Only Memory)、可擦除可编程只读存储器(EPROM，ErasableProgrammable Read-Only Memory)、电可擦除可编程只读存储器(EEPROM，ElectricallyErasable Programmable Read-Only Memory)、磁性随机存取存储器(FRAM，FerromagneticRandom Access Memory)、快闪存储器(Flash Memory)、磁表面存储器、光盘、或只读光盘(CD-ROM，Compact Disc Read-Only Memory)；磁表面存储器可以是磁盘存储器或磁带存储器。易失性存储器可以是随机存取存储器(RAM，Random Access Memory)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(SRAM，Static Random Access Memory)、同步静态随机存取存储器(SSRAM，SynchronousStatic Random Access Memory)、动态随机存取存储器(DRAM，Dynamic Random AccessMemory)、同步动态随机存取存储器(SDRAM，Synchronous Dynamic Random AccessMemory)、双倍数据速率同步动态随机存取存储器(DDRSDRAM，Double Data RateSynchronous Dynamic Random Access Memory)、增强型同步动态随机存取存储器(ESDRAMEnhanced Synchronous Dynamic Random Access Memory)、同步连接动态随机存取存储器(SLDRAM，Sync Link Dynamic Random Access Memory)、直接内存总线随机存取存储器(DRRAM，Direct Rambus Random Access Memory)。本发明实施例描述的存储介质旨在包括但不限于这些和任意其它适合类型的存储器。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统和方法，可以通过其它的方式实现。以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，如：多个单元或组件可以结合，或可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的各组成部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或单元的间接耦合或通信连接，可以是电性的、机械的或其它形式的。

上述作为分离部件说明的单元可以是、或也可以不是物理上分开的，作为单元显示的部件可以是、或也可以不是物理单元，即可以位于一个地方，也可以分布到多个网络单元上；可以根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。

另外，在本发明各实施例中的各功能单元可以全部集成在一个处理单元中，也可以是各单元分别单独作为一个单元，也可以两个或两个以上单元集成在一个单元中；上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

或者，本发明上述集成的单元如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分。而前述的存储介质包括：移动存储设备、ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。

本申请所提供的几个方法实施例中所揭露的方法，在不冲突的情况下可以任意组合，得到新的方法实施例。

本申请所提供的几个产品实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的产品实施例。

本申请所提供的几个方法或设备实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的方法实施例或设备实施例。

以上内容是结合具体的优选实施方式对本发明所做的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (7)

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

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

S2：根据所述传感数据估计所述多波束图像声呐的实时的姿态信息，其中，采用快速特征点提取和描述的算法对相邻两次采样得到的预处理后的灰度值矩阵中特征点的灰度值变化实时得到所述多波束图像声呐的所述姿态信息；

S3：采用非线性优化算法对所述实时的姿态信息进行整合得到所述多波束图像声呐的第一轨迹和第一地图，包括：同步采用多普勒流速仪、惯性导航单元、深度计获取第二传感数据，设置传感数据的置信度权重，通过卡尔曼融合算法将所述第二传感数据和所述姿态信息进行融合校正得到所述多波束图像声呐的坐标和姿态角度；基于所述坐标的连续变化信息，经过非线性优化整合得到所述第一轨迹和所述第一地图。

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

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

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

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

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

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

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

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

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

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

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

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

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

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