CN108592916B - 悬浮式水下自主航行器的多航次地图定位和导航方法 - Google Patents

Abstract

本发明公开了一种浮式水下自主航行器的多航次地图定位和导航方法，包括：S1航行器沿预设航行路径巡航，获取水下地形的照片序列；S2图像数据处理并进行闭环检测；S3特征匹配以优化照片序列；S4计算照片的显著性；S5地图精度评价；S6多航次巡航，以进行同向路径地图融合和反向路径地图优化；S7拓展地图数据库；S8显著性区域路径规划以实时定位和导航。本发明是一种针对海底基站的HAUV应用环境，通过对同一区域进行多航次探索实现地图优化，进而实现水下精确定位和导航的方法。该方法可以有效解决定位精度和运算效率之间的矛盾，实现HAUV在水下的实时高精度导航。

Description

悬浮式水下自主航行器的多航次地图定位和导航方法

技术领域

本发明属于水下航行器领域，具体涉及一种悬浮式水下自主航行器的多航次地图定位和导航方法。

背景技术

悬浮式水下航行器(Hover Autonomous Underwater Vehicle，简称HAUV)是一种具备强大机动能力的自主水下航行器。HAUV具备多个推进器，以及具备能够在水层中悬浮、具备强大的垂直机动能力、能够贴近海底航行等多项优点。

在HAUV中采用声学定位、航位推算定位等传统水下定位方法的使用成本高，且存在声学定位的有效距离有限，航位推算具有误差累积问题等缺陷。现阶段，基于海底地形的机器视觉方法是解决HAUV水下高精度定位和导航问题的有效手段。

发明内容

本发明要解决的技术问题是提供一种针对海底基站的HAUV应用环境，通过对同一区域进行多航次探索实现地图优化，进而实现水下精确定位和导航的方法。该方法可以有效解决定位精度和运算效率之间的矛盾，实现HAUV在水下的实时高精度导航。

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

一种悬浮式水下自主航行器的多航次地图定位和导航方法，所述方法包括：

S1预设所述航行器在一航行区域的航行路径，所述航行器按照预设的航行路径航行，并使用设置在所述航行器上的竖直向下的相机获取水下地形的照片序列以存储于数据库；

S2对所述照片序列进行图像数据处理，通过闭环检测搜寻同一地形的照片；

S3对同一地形的照片进行匹配并计算出照片所对应的相机位置的变化，以对数据库中的所述照片序列所对应的位置进行优化；

S4计算所述照片序列中每一幅照片的显著性，以衡量照片中特征的有效性；

S5对单航次所获地图的数据库的精度进行评价；

S6对同一航行区域进行多次同向和反向巡航，获取多组地图，进行同向路径的地图融合和反向路径的地图优化，以得到区域地图的数据库；

S7投放靶标，重复实施步骤S1-S6，探索其他航行区域，以获取经区域拓展的地图的数据库；

S8利用步骤S7中获取的地图的数据库，结合显著性区域优先的路径规划算法，实施所述航行器的定位和导航。

一优选实施例中，所述闭环检测采用基于位置约束的词袋法。

一优选实施例中，闭环检测过程中，从数据库中选择与当前照片对应的位置在预设距离值以内的照片，逐一比较当前照片与所选照片的余弦相似度，若余弦相似度高于设定值，则进行对同一地形的照片进行特征匹配。

一优选实施例中，使用信息熵来衡量照片的显著性。

一优选实施例中，使用精度评价函数对单航次所获地图的数据库的精度进行评价。

一优选实施例中，同向路径的地图融合和反向路径的地图优化包括下述步骤：

S61在同向路径多航次获取的多幅地图中选择精度评价函数值最小的地图作为基准地图；

S62对基准地图与同向路径的其他地图进行匹配，定义基准地图中每一幅照片的基准位置；

S63针对反向路径实施步骤S1-S5；

S64针对反向路径实施步骤S61-S62；

S65使用反向路径的基准位置对同向路径的基准位置进行优化。

一优选实施例中，所述航行器设有靶标投放装置和收回装置。

一优选实施例中，所述显著性区域为由多张相邻的度量其显著性的信息熵超过设定值的照片所对应的位置组成的区域。

一优选实施例中，所述航行器沿弓字形路径在未知区域进行探索性巡航以获取该区域的地图，且在路径巡航的末段，所述航行器沿方形涡状线搜寻水下地形目标，所述水下地形目标包括基站或靶标。

一优选实施例中，采用单应矩阵计算两张照片所对应的相对位置关系。

采用本发明具有如下的有益效果：

1、本发明采用航行器进行区域巡航的方式拍摄获取地图数据库，从而以基于海底地形的机器视觉方法来解决定位精度和运算效率之间的矛盾。

2、本发明采用闭环检测的方式，以余弦相似度高于设定值为标准，来判断是否进行图像间的特征匹配。根据闭环图像间的单应矩阵计算当前图像所对应的相机位置，从而消除闭环点之前那段航程的累积误差。通过闭环图像间的相对位置计算，能够对后序图像的位置进行校正，从而提高图像序列的位置精度。

3、本发明对图像显著性以及地图精度提出了衡量标准，从另一个角度解决定位精度和运算效率之间的矛盾。

4、本发明结合单航次地图精度评价与多航次地图融合优化的处理方法，对航行器多次航行获取的多组地图，采用融合算法从中筛选出有效信息，提升地图精度。

5、本发明在航行器正式作业时，采用著性区域作为途径点(即结合显著性区域优先的路径规划算法)，以确保航行器的定位和导航精度。

附图说明

图1为航行器的预设航行路径；

图2为基准位置和投影位置的相互关系类型示意图；

图3为未知区域地图探索策略示意图；

图4为以地图为参考的导航路径示意图；

图5为航行器的多航次地图定位和导航方法步骤示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明公开了一种悬浮式水下自主航行器的多航次地图定位和导航方法，其具体实施例如下所述。

(1)建立单航次图像序列数据库

一实施例中，HAUV以如图1所示的预设航行路径进行巡航，获取该水下区域地形的照片，到达导航终点后，沿方形涡状线向终点附近区域搜索，最终回到起点，并记录导航终点到起点的距离d。所获照片按照如下表所示的示意性格式存于数据库中，其中，相机位置可以通过HAUV自带的惯性测量单元进行航位推算得到，航向角可以通过电子罗盘进行测量。

获取单航次的图像序列(也可以称之为照片序列)后，对图像(即照片)进行以下两方面的分析处理：a)寻找闭环图像，优化图像位置；b)分析图像显著性。

1)闭环检测

通过基于位置约束的词袋法实现闭环检测。为了排除小特征点的干扰，先对图像进行模糊化处理。然后，建立一个特征词典，将每一幅图像中新出现的特征加入特征词典中，在地图探索的过程中，特征词典中词汇的数量是不断增加的。可以用特征词汇直方图向量(n_w1,n_w2...n_wk)来表征图像(n_wk表示特征词汇wk在图像中出现的次数)，假设在某一时刻，第i幅图像的特征直方图向量为x_i，第j幅图像的特征词汇直方图向量为x_j，则两幅图像的相似程度可以用余弦相似度来表示：

闭环检测过程中，先从数据库中找出跟当前位置的距离在设定值以内的图像，逐一比较当前图像跟它们的余弦相似度，如果余弦相似度高于设定值，则进行特征匹配。如果特征匹配成功，则根据闭环图像间的单应矩阵计算当前图像所对应的相机位置，从而消除闭环点之前那段航程的累积误差。如果多于一幅图像匹配成功，则选取余弦相似度较高的图像进行位置推算。

2)位置解算

由闭环检测获得相互匹配的图像后，通过以下公式计算图像所对应的相机相对位置关系：

上式中，(Δx，Δy)是相机中心在世界坐标系中的坐标变化，T是航行器体坐标系到世界坐标系的转换矩阵(这里假定航行器体坐标系跟图像中心坐标系相重合)，

是当前时刻航行器的航向，K是简化的相机参数矩阵(以相机高度h(由高度计测得)跟相机x，y方向的像素焦距f_x和f_y的比值来反映成像平面到海底平面的尺寸转换，矩阵中忽略了相机成像的畸变)，H是前一刻图像到当前图像的单应矩阵(由于相机的朝向基本垂直海底平面，且相机的高度近似不变，该变换可以近似为等距变换；H矩阵由特征点匹配求解得到(已有现成算法，故不再赘述)；θ表示图像的相对旋转角，t_x、t_y表示图像的相对位移，其单位是像素)，P矩阵的作用是将图像平面的坐标原点移动到图像中心，W是图像宽度，H是图像高度，其单位均是像素。

通过闭环图像间的相对位置计算，能够对后序图像的位置进行校正，从而提高图像序列的位置精度。

3)显著性检测

用显著性衡量图像的质量(即衡量照片中特征的有效性)。首先，对图片进行模糊化处理，这是为了排除细小而繁多的特征点，只保留较大的特征。图像显著性通过信息熵E来衡量，其计算公式如下：

其中，w_k表示图像中的一个特征词汇，假设其总数为n，p(w_k)是w_k词汇在图像中出现的频率。从该公式可以看出：特征词汇种类越多，分布越均匀，信息熵就越大，越能体现出图像的显著性。

通过以上步骤获得的特征词汇直方图向量、闭环图片、显著性等信息也可以存入数据库中。

(2)多航次地图优化

1)单航次地图精度评价

为了从多次探索中筛选出精度更高的地图，设计精度评价函数对地图精度进行评价，拟采用的精度评价函数如下：

f_v＝k₁·t_a+k₂·s(t)+k₃·d

其中，t_a表示所有闭环图像的序号间隔，s(t)表示闭环图像的序号间隔的方差，d表示导航终点到起点的距离，k₁、k₂和k₃是加权系数。运用精度评价函数对单次探索的地图精度进行评价，闭环事件(闭环事件是指同一地形的两幅图片成功匹配，余弦相似度是初步筛选，筛选完成后进行图像匹配)的次数越多、分布越均匀、导航终点距离起点越近，精度评价函数值f_v就越小，也表明地图精度越高。

2)多航次地图融合优化

HUAV通过多次航行，可以得到多组地图，然后通过融合算法从中筛选出有效信息，提升地图精度。多地图的融合算法拟分为两步：同向路径地图融合和反向路径地图优化。

首先，比较同向路径地图的评价指标，选出精度评价指标f_v数值最小的地图作为后续优化的基本地图(即基准地图)。

然后，进行地图间匹配。假设图像a在基本地图i的位置坐标为(x_a,y_a)(这里称之为基本位置或者基准位置)，在另一组地图j上找到了该图像的匹配图像b(采用跟闭环检测相同的方法进行搜寻)，其位置坐标为(x_b,y_b)，通过从图像a到图像b的单应距阵，可以求得两图像的相对位置(Δx,Δy)，则图像a在地图i上的坐标和它在地图j上的坐标的差值为(x_a-x_b+Δx,y_a-y_b+Δy)。设Δx_ij＝x_a-x_b+Δx，Δy_ij＝y_a-y_b+Δy，通过Δx_ij和Δy_ij可以知道该图片在其他地图上的投影位置跟其基本位置的相对关系。分析图2所示的三种情况：如图2的左图所示，如果投影位置比较均匀的分布在基本位置周围，且间距较小，则基本位置可不作改变；如图2的中图所示，如果投影位置分布在基本位置的一个方向，则调整基本位置的相应坐标(该情况下是增大y坐标)；如图2的右图所示，如果有投影位置距离基本位置较远(大于阈值)，则去除该投影位置，以其他位置进行优化。此外，也可能出现有多个投影位置在阈值外的情况，此时将该基本位置标记为不确定性大的位置，等待进一步优化。

基本位置初步优化完成后，按照投影位置到基本位置的距离平均值d_a和阈值外位置的数量n_e，以及图片的序号s计算该基本位置的不确定性指数f_l：

f_l＝k₁₁·d_a+k₁₂·n_e+k₁₃·s

上式中，k₁₁、k₁₂和k₁₃是加权系数。平均距离越大、无效投影位置越多、在图像序列中越靠后，其不确定性指数越大，该基本位置的不确定性也越大。

经过上述程序，基本地图得到了优化，并且基本地图的每一幅图像都有对应的不确定性指数。在此基础上，进一步地，进行反向路径巡航，此时，基本地图里编号靠后的图像排在了靠前的位置，其不确定性较小。通过闭环检测算法获取基本图像在新路径(即反向路径)的位置坐标(x_k，y_k)后，可以通过以下公式对基本路径的坐标进行优化：

上式中，s_t表示基本图像总数，s_k是新路径图像编号，k₂₁是权值系数，(x′_a，y′_a)和(x″_a，y″_a)分别表示优化前和优化后的基本位置。该公式的本质是通过序号较小因而具有较大确定性的图像位置对不确定性较大的基本位置进行校正。在一具体实施例中，如果条件允许，可以进行多次反向路径巡航，并用同向路径优化法获得反向路径中基本图像的优化位置及其不确定指数f_lk，f_lk可以代替上式中的(s_t-s_k)进行基本地图优化。

通过同向路径地图融合和反向路径地图优化，得到探索区域的最终地图，该区域被标记为已知区域，然后继续进行未知区域的探索。

(3)多区域地图获取

某一区域完成后，HAUV从基站出发，前往更远的区域进行探索，并将该区域的地图加入已知地图之中。在此过程中，需要确保HAUV对未知区域的起点和终点有充分精确的定位，不至于由于定位误差迷失方位，无法回到基站，并能够较为平顺的拼合新探索区域和已知区域地图。

首先，在HAUV安装一个视觉靶标投放装置，在新探索区域的起点投放视觉靶标，其结构并不复杂，例如，以绘有显眼图案的铁片作为视觉靶标。也可以在HAUV上安装一个竖直向下的电磁铁(即靶标回收装置)，在地图探索完成后，还可以回收视觉靶标。当HAUV航行到未知区域的起点后，投放装载在其上的视觉靶标，然后以其作为起点进行地图探索，具体探索路径如图3所示。每完成一定次数的探索，HAUV就回到基站充电并进行数据处理。重复进行多次探索，直至未知区域的地图优化完成，HAUV回收视觉靶标，用于下一个未知区域的探索。

(4)基于地图的路径规划和导航

经过多次地图探索和地图优化，最后形成区域地图数据库。当HAUV正式作业时，以地图为参照进行定位和导航。

首先，根据地图图像的显著性指数和目标位置进行综合分析，选取一条途径显著性区域较多、且路线相对较短的路径(如图4中的灰色折线)，如图4(图中灰度越大，表示显著性越好)所示。这里的显著性区域是指由多张相邻的显著性较大的图像所组成的区域。如果以单张显著性图像作为途经点，由于地图误差和定位误差的存在，HAUV可能无法准确到达图像所在位置，因此，采用显著性区域作为途经点，以确保HAUV能够到达该区域。

其次，在HAUV航行的过程中，通过上述闭环检测的方法进行地图匹配和位置校正，从而获得高精度的实时位置。

综上，将本发明实施例归纳总结为下述步骤(参考图5)：

S1预设航行器在一航行区域的航行路径，航行器按照预设的航行路径航行，并使用设置在航行器上的竖直向下的相机获取水下地形的照片序列以存储于数据库；

S2对照片序列进行图像数据处理，通过闭环检测搜寻同一地形的照片；

S3对同一地形的照片进行匹配并计算出照片所对应的相机位置的变化，以对数据库中的照片序列所对应的位置进行优化；

S4计算照片序列中每一幅照片的显著性，以衡量照片中特征的有效性；

S5对单航次所获地图的数据库的精度进行评价；

S6对同一航行区域进行多次同向和反向巡航，获取多组地图，进行同向路径的地图融合和反向路径的地图优化，以得到区域地图的数据库；

S7投放靶标，重复实施步骤S1-S6，探索其他航行区域，以获取经区域拓展的地图的数据库；

S8利用步骤S7中获取的地图的数据库，结合显著性区域优先的路径规划算法，实施所述航行器的定位和导航。

应当理解，本文所述的示例性实施例是说明性的而非限制性的。尽管结合附图描述了本发明的一个或多个实施例，本领域普通技术人员应当理解，在不脱离通过所附权利要求所限定的本发明的精神和范围的情况下，可以做出各种形式和细节的改变。

Claims (10)

1.一种悬浮式水下自主航行器的多航次地图定位和导航方法，其特征在于，所述方法包括：

S1预设所述航行器在一航行区域的航行路径，所述航行器按照预设的航行路径航行，并使用设置在所述航行器上的竖直向下的相机获取水下地形的照片序列以存储于数据库；

S2对所述照片序列进行图像数据处理，通过闭环检测搜寻同一地形的照片；

S3对同一地形的照片进行匹配并计算出照片所对应的相机位置的变化，以对数据库中的所述照片序列所对应的位置进行优化；

S4计算所述照片序列中每一幅照片的显著性，以衡量照片中特征的有效性；

S5对单航次所获地图的数据库的精度进行评价；

S6对同一航行区域进行多次同向和反向巡航，获取多组地图，进行同向路径的地图融合和反向路径的地图优化，以得到区域地图的数据库；

S7投放靶标，重复实施步骤S1-S6，探索其他航行区域，以获取经区域拓展的地图的数据库；

S8利用步骤S7中获取的地图的数据库，结合显著性区域优先的路径规划算法，实施所述航行器的定位和导航。

2.如权利要求1所述的悬浮式水下自主航行器的多航次地图定位和导航方法，其特征在于，所述闭环检测采用基于位置约束的词袋法。

3.如权利要求1或2所述的悬浮式水下自主航行器的多航次地图定位和导航方法，其特征在于，闭环检测过程中，从数据库中选择与当前照片对应的位置在预设距离值以内的照片，逐一比较当前照片与所选照片的余弦相似度，若余弦相似度高于设定值，则进行对同一地形的照片进行特征匹配。

4.如权利要求1所述的悬浮式水下自主航行器的多航次地图定位和导航方法，其特征在于，使用信息熵来衡量照片的显著性。

5.如权利要求1所述的悬浮式水下自主航行器的多航次地图定位和导航方法，其特征在于，使用精度评价函数对单航次所获地图的数据库的精度进行评价。

6.如权利要求5所述的悬浮式水下自主航行器的多航次地图定位和导航方法，其特征在于，同向路径的地图融合和反向路径的地图优化包括下述步骤：

S61在同向路径多航次获取的多幅地图中选择精度评价函数值最小的地图作为基准地图；

S62对基准地图与同向路径的其他地图进行匹配，定义基准地图中每一幅照片的基准位置；

S63针对反向路径实施步骤S1-S5；

S64针对反向路径实施步骤S61-S62；

S65使用反向路径的基准位置对同向路径的基准位置进行优化。

7.如权利要求1所述的悬浮式水下自主航行器的多航次地图定位和导航方法，其特征在于，所述航行器设有靶标投放装置和收回装置。

8.如权利要求1所述的悬浮式水下自主航行器的多航次地图定位和导航方法，其特征在于，所述显著性区域为由多张相邻的度量其显著性的信息熵超过设定值的照片所对应的位置组成的区域。

9.如权利要求1或7所述的悬浮式水下自主航行器的多航次地图定位和导航方法，其特征在于，所述航行器沿弓字形路径在未知区域进行探索性巡航以获取该区域的地图，且在路径巡航的末段，所述航行器沿方形涡状线搜寻水下地形目标，所述水下地形目标包括基站或靶标。

10.如权利要求1所述的悬浮式水下自主航行器的多航次地图定位和导航方法，其特征在于，采用单应矩阵计算两张照片所对应的相对位置关系。

Images