一种基于地形特征匹配的水下地形匹配导航方法

摘要

本发明提供一种基于地形特征匹配的水下地形匹配导航方法，处理先验地图，求出高度的梯度值，利用梯度的大小筛选出实际为较为陡峭的点作为特征点，将这些特征点的梯度进行霍夫变换得到特征的长度及位置，写入实际特征信息库。处理多波束声纳发回的数据，求出对应点的高度值，对其求梯度，将梯度的模长进行霍夫变换，得到样本特征的长度和与机器人的相对位置。利用这些特征的长度、对应深度等信息与之前构建的实际特征信息库进行匹配，得到与每一块特征区域所匹配的先验地图的区域。分别利用这些区域的相对位置信息得出水下机器人的位置，对这些位置进行分析得到机器人的精确位置。本发明可相对快速的地形匹配定位与导航任务。

说明书

技术领域

本发明涉及一种定位与导航，尤其涉及一种基于地形特征匹配的水下地形匹配导航方法，属于水下定位与导航技术。

背景技术

水下机器人由于其成本低廉、可拓展性强、可以完成各种信息采集与利用等任务深受各国家研究机构重视，由于在水下环境中，高频信号衰减快，所以GPS等常规定位方式并不适用，由于定位与导航是水下机器人能否安全返航的重要技术因素，所以找到可行的定位与导航技术成为了水下机器人领域的重点技术。

现阶段水下机器人导航技术主要有惯性导航、水声学导航、航位推算导航以及地形匹配导航。惯性导航与航位推算导航由于精度有限以及需要定期上浮利用GPS修正等因素受到限制，不利于水下机器人长期水下作业，而水声学导航需要声学基阵，实际操作过程需要辅助才能完成。

发明内容

本发明的目的是为了提供一种基于地形特征匹配的水下地形匹配导航方法。

本发明的目的是这样实现的：第一步：处理已知的先验地图，计算先验地图中的每一点的高度的梯度通过对高度的梯度grad(x,y)的大小进行筛选，选出梯度的模长||grad(x,y)||＞阈值Α的点作为先验地图的特征点；

第二步：将第一步得到的所有先验地图的特征点的梯度的模长||grad(x,y)||做霍夫变换，识别出每一个特征的拟合线段，并记录这些线段的起始点坐标位置(x_s,y_s),(x_e,y_e)以及每条线段对应的高度作为实际特征信息存入实际特征信息库；

第三步：启动多波束声纳，采集水下机器人所在位置及周边的实时水深数据，记录得到的每一个数据点的坐标位置(x^*，y^*)以及对应数据点的深度h^*；

第四步：计算第三步中的每一个数据点水深的梯度通过对水深的梯度grad(x^*,y^*)的大小进行筛选，选出梯度的模长||grad(x^*,y^*)||＞Α的点作为多波束声纳数据的特征点；

第五步：将第四步得到的多波束声纳数据的特征点的梯度的模长||grad(x,y)||做霍夫变换，并识别出每一个特征的拟合线段，记录这些线段的起始点坐标位置(x_s,y_s),(x_e,y_e)以及每条线段对应的高度信息、与机器人的相对位置信息，作为样本特征信息；

第六步：逐条读取样本特征信息，判断实际特征信息库中是否存在高度相匹配的特征：若存在则进行下一步，若不存在，则读取下一条特征，重复此步骤；

第七步：判断经第六步得到的特征中是否存在长度相一致的特征，若存在则进行下一步，若不存在，则读取下一条特征，重复第六步；

第八步：读取第七步得到的相匹配的特征的端点坐标位置(x^*_s,y^*_s),(x^*_e,y^*_e)，找到能将样本特征端点坐标(x_s,y_s),(x_e,y_e)转换为匹配特征端点(x^*_s,y^*_s),(x^*_e,y^*_e)的坐标变换；

第九步：根据第八步中的坐标变换以及第五步中与机器人的相对位置信息得到机器人此时的位置(x₁,y₁)，(x₂,y₂)…(x_n,y_n)；

第十步：根据第九步得到的位置，得到最终确定的精确坐标位置

第十一步：将(x_match,y_match)反馈给主控计算机，完成地形匹配定位。

与现有技术相比，本发明的有益效果是：本发明使用梯度进行判断，可克服涨落潮差等因素影响，由于只对筛选出的特征进行匹配，很大程度上减小了匹配过程的计算量。

本发明通过提取探测到地形的特征(实际为较为陡峭的点)信息来与已知地形进行快速匹配，在不依赖于GPS、初始位置等信息的情况下进行相对快速的地形匹配定位与导航任务。

附图说明

图1为本发明的流程图；

图2为本发明提取特征点的流程图；

图3为本发明匹配操作的流程图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

本发明涉及的是一种基于地形梯度特征匹配的水下地形匹配方法，包括已知的地形的处理来提取特征点和通过处理探测到的地形得到特征点并与上一步得到的已知特征点进行匹配来确定自身位置。

结合图1说明整体的实现方式，本发明处理先验地图数据，提取特征点，将特征点的梯度进行霍夫变换，得到特征的长度、端点位置，将这些信息存储为实际特征点信息库，用作后续匹配。将多波束声纳传回的数据进行处理，也应用算法提取出特征，将这些特征信息进行整合，得到样本特征信息。将样本特征信息与之前生成的实际特征点信息库进行匹配，得到对应的位置信息，进行处理得到此时机器人的精确位置坐标。

结合图2说明提取特征的过程，首先将先验地图高度数据/探测到的高度数据进行处理，由下式计算出高度的梯度grad(x,y)/grad(x^*,y^*)。

在此过程后判断高度的梯度大小是否大于阈值A即判断||grad(x,y)||＞Α，若||grad(x,y)||≤Α则认为此点不是特征点，若||grad(x,y)||＞Α，则认为此点是特征点，记录这个点的梯度等信息。

判断是否数据读取完毕，若读取完毕则进行下一步，否则读取下一点数据重复上述步骤。

将所有的特征点的梯度的模长||grad(x,y)||进行霍夫变换，得到各个特征所对应的线段，记录线段的端点(x_s,y_s),(x_e,y_e)，以及每一条线段所对应位置的高度，得到实际特征信息存入实际特征点信息库/样本特征点信息。

结合图3说明匹配操作的实现方式，逐条读取样本特征信息，判断实际特征信息中是否存在高度相匹配的特征，若不存在则读取下一条数据重新判断；若存在则判断这些特征中是否存在长度相一致的特征，若相同则认为找到相匹配的特征点，读取端点坐标(x^*_s,y^*_s),(x^*_e,y^*_e)进行下一步操作，若不存在，则读取下一条数据重新重复上述步骤。找到能将样本特征端点坐标(x_s,y_s),(x_e,y_e)转换为匹配特征端点(x^*_s,y^*_s),(x^*_e,y^*_e)的旋转变换，据此得到机器人此时的位置(x_match,y_match)。

本发明的具体流程如下：

1、处理先验地图，计算每一点的高度的梯度通过对高度的梯度grad(x,y)的大小进行筛选，选出梯度的模长||grad(x,y)||＞阈值Α的点作为特征点。

2、将这些特征点的梯度的模长||grad(x,y)||做霍夫变换，识别出每一个特征的拟合线段，也即得到各个特征所对应的线段，记录这些线段的起始点坐标(x_s,y_s),(x_e,y_e)，以及每条线段对应的高度信息，作为实际特征信息存入实际特征点信息库。

3、启动多波束声纳，采集水下机器人所在位置及周边的实时水深数据，记录得到的每一个数据点的位置(x^*，y^*)，以及对应点的深度h^*。

4、计算每一个测量点水深的梯度通过对水深的梯度grad(x^*,y^*)的大小进行筛选，选出梯度的模长||grad(x^*,y^*)||＞Α的点作为特征点。

5、将这些特征点的梯度的模长||grad(x,y)||做霍夫变换，识别出每一个特征的拟合线段，记录这些线段的起始点坐标(x_s,y_s),(x_e,y_e)，以及每条线段对应的高度信息、与机器人的相对位置信息，作为样本特征信息。

6、通过对比特征的大小及对应高度，在实际特征点信息库中筛选出与之相匹配的特征，找到两者的空间位置关系，也即逐条读取样本特征信息，判断实际特征信息中是否存在高度相匹配的特征，若存在则进行下一步，若不存在，则读取下一条特征，重复此步骤。

7、判断这些特征中是否存在长度相一致的特征，若存在则进行下一步，若不存在，则读取下一条特征，重复步骤(6)。

8、此时认为找到相匹配的特征，读取端点坐标(x^*_s,y^*_s),(x^*_e,y^*_e)，找到能将样本特征端点坐标(x_s,y_s),(x_e,y_e)转换为匹配特征端点(x^*_s,y^*_s)，(x^*_e,y^*_e)的旋转变换。

9、根据步骤(8)中的坐标变换，以及步骤(5)中特征与机器人的相对位置信息，也即根据样本特征与实际特征的空间关系以及样本特征与机器人的相对位置信息得到机器人此时的位置(x₁,y₁)，(x₂,y₂)…(x_n,y_n)。

10、处理这些定位位置，得到最终确定的精确定位点

11、将(x_match,y_match)反馈给主控计算机，完成地形匹配定位。

具体的说本发明的提取给定区域的特征包括：

(1)计算每一点的高度的梯度

(2)判断每一点高度的梯度的模长||grad(x,y)||是否大于给定的值阈值A；

(3)若梯度的模长小于等于阈值A，即||grad(x,y)||≤Α，则此点不是特征点；

(4)若梯度的模长大于阈值A，即||grad(x,y)||＞Α，则此点视为特征点；

(5)将所有的特征点的梯度的模长||grad(x,y)||进行霍夫变换，得到各个特征所对应的线段；

(6)记录线段的端点(x_s,y_s),(x_e,y_e)，以及每一条线段所对应位置的高度，作为实际特征信息存入实际特征点信息库/样本特征点信息用来匹配。