[0040] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0041] 本发明公开了一种悬浮式水下自主航行器的多航次地图定位和导航方法，其具体实施例如下所述。
[0042] (1)建立单航次图像序列数据库
[0043] 一实施例中，HAUV以如图1所示的预设航行路径进行巡航，获取该水下区域地形的照片，到达导航终点后，沿方形涡状线向终点附近区域搜索，最终回到起点，并记录导航终点到起点的距离d。所获照片按照如下表所示的示意性格式存于数据库中，其中，相机位置可以通过HAUV自带的惯性测量单元进行航位推算得到，航向角可以通过电子罗盘进行测量。
[0044]
[0045] 获取单航次的图像序列(也可以称之为照片序列)后，对图像(即照片)进行以下两方面的分析处理：a)寻找闭环图像，优化图像位置；b)分析图像显著性。
[0046] 1)闭环检测
[0047] 通过基于位置约束的词袋法实现闭环检测。为了排除小特征点的干扰，先对图像进行模糊化处理。然后，建立一个特征词典，将每一幅图像中新出现的特征加入特征词典中，在地图探索的过程中，特征词典中词汇的数量是不断增加的。可以用特征词汇直方图向量(nw1,nw2...nwk)来表征图像(nwk表示特征词汇wk在图像中出现的次数)，假设在某一时刻，第i幅图像的特征直方图向量为xi，第j幅图像的特征词汇直方图向量为xj，则两幅图像的相似程度可以用余弦相似度来表示：
[0048]
[0049] 闭环检测过程中，先从数据库中找出跟当前位置的距离在设定值以内的图像，逐一比较当前图像跟它们的余弦相似度，如果余弦相似度高于设定值，则进行特征匹配。如果特征匹配成功，则根据闭环图像间的单应矩阵计算当前图像所对应的相机位置，从而消除闭环点之前那段航程的累积误差。如果多于一幅图像匹配成功，则选取余弦相似度较高的图像进行位置推算。
[0050] 2)位置解算
[0051] 由闭环检测获得相互匹配的图像后，通过以下公式计算图像所对应的相机相对位置关系：
[0052]
[0053] 上式中，(Δx，Δy)是相机中心在世界坐标系中的坐标变化，T是航行器体坐标系到世界坐标系的转换矩阵(这里假定航行器体坐标系跟图像中心坐标系相重合)，是当前时刻航行器的航向，K是简化的相机参数矩阵(以相机高度h(由高度计测得)跟相机x，y方向的像素焦距fx和fy的比值来反映成像平面到海底平面的尺寸转换，矩阵中忽略了相机成像的畸变)，H是前一刻图像到当前图像的单应矩阵(由于相机的朝向基本垂直海底平面，且相机的高度近似不变，该变换可以近似为等距变换；H矩阵由特征点匹配求解得到(已有现成算法，故不再赘述)；θ表示图像的相对旋转角，tx、ty表示图像的相对位移，其单位是像素)，P矩阵的作用是将图像平面的坐标原点移动到图像中心，W是图像宽度，H是图像高度，其单位均是像素。
[0054] 通过闭环图像间的相对位置计算，能够对后序图像的位置进行校正，从而提高图像序列的位置精度。
[0055] 3)显著性检测
[0056] 用显著性衡量图像的质量(即衡量照片中特征的有效性)。首先，对图片进行模糊化处理，这是为了排除细小而繁多的特征点，只保留较大的特征。图像显著性通过信息熵E来衡量，其计算公式如下：
[0057]
[0058] 其中，wk表示图像中的一个特征词汇，假设其总数为n，p(wk)是wk词汇在图像中出现的频率。从该公式可以看出：特征词汇种类越多，分布越均匀，信息熵就越大，越能体现出图像的显著性。
[0059] 通过以上步骤获得的特征词汇直方图向量、闭环图片、显著性等信息也可以存入数据库中。
[0060] (2)多航次地图优化
[0061] 1)单航次地图精度评价
[0062] 为了从多次探索中筛选出精度更高的地图，设计精度评价函数对地图精度进行评价，拟采用的精度评价函数如下：
[0063] fv＝k1·ta+k2·s(t)+k3·d
[0064] 其中，ta表示所有闭环图像的序号间隔，s(t)表示闭环图像的序号间隔的方差，d表示导航终点到起点的距离，k1、k2和k3是加权系数。运用精度评价函数对单次探索的地图精度进行评价，闭环事件(闭环事件是指同一地形的两幅图片成功匹配，余弦相似度是初步筛选，筛选完成后进行图像匹配)的次数越多、分布越均匀、导航终点距离起点越近，精度评价函数值fv就越小，也表明地图精度越高。
[0065] 2)多航次地图融合优化
[0066] HUAV通过多次航行，可以得到多组地图，然后通过融合算法从中筛选出有效信息，提升地图精度。多地图的融合算法拟分为两步：同向路径地图融合和反向路径地图优化。
[0067] 首先，比较同向路径地图的评价指标，选出精度评价指标fv数值最小的地图作为后续优化的基本地图(即基准地图)。
[0068] 然后，进行地图间匹配。假设图像a在基本地图i的位置坐标为(xa,ya)(这里称之为基本位置或者基准位置)，在另一组地图j上找到了该图像的匹配图像b(采用跟闭环检测相同的方法进行搜寻)，其位置坐标为(xb,yb)，通过从图像a到图像b的单应距阵，可以求得两图像的相对位置(Δx,Δy)，则图像a在地图i上的坐标和它在地图j上的坐标的差值为(xa-xb+Δx,ya-yb+Δy)。设Δxij＝xa-xb+Δx，Δyij＝ya-yb+Δy，通过Δxij和Δyij可以知道该图片在其他地图上的投影位置跟其基本位置的相对关系。分析图2所示的三种情况：如图2的左图所示，如果投影位置比较均匀的分布在基本位置周围，且间距较小，则基本位置可不作改变；如图2的中图所示，如果投影位置分布在基本位置的一个方向，则调整基本位置的相应坐标(该情况下是增大y坐标)；如图2的右图所示，如果有投影位置距离基本位置较远(大于阈值)，则去除该投影位置，以其他位置进行优化。此外，也可能出现有多个投影位置在阈值外的情况，此时将该基本位置标记为不确定性大的位置，等待进一步优化。
[0069] 基本位置初步优化完成后，按照投影位置到基本位置的距离平均值da和阈值外位置的数量ne，以及图片的序号s计算该基本位置的不确定性指数fl：
[0070] fl＝k11·da+k12·ne+k13·s
[0071] 上式中，k11、k12和k13是加权系数。平均距离越大、无效投影位置越多、在图像序列中越靠后，其不确定性指数越大，该基本位置的不确定性也越大。
[0072] 经过上述程序，基本地图得到了优化，并且基本地图的每一幅图像都有对应的不确定性指数。在此基础上，进一步地，进行反向路径巡航，此时，基本地图里编号靠后的图像排在了靠前的位置，其不确定性较小。通过闭环检测算法获取基本图像在新路径(即反向路径)的位置坐标(xk，yk)后，可以通过以下公式对基本路径的坐标进行优化：
[0073]
[0074] 上式中，st表示基本图像总数，sk是新路径图像编号，k21是权值系数，(x′a，y′a)和(x″a，y″a)分别表示优化前和优化后的基本位置。该公式的本质是通过序号较小因而具有较大确定性的图像位置对不确定性较大的基本位置进行校正。在一具体实施例中，如果条件允许，可以进行多次反向路径巡航，并用同向路径优化法获得反向路径中基本图像的优化位置及其不确定指数flk，flk可以代替上式中的(st-sk)进行基本地图优化。
[0075] 通过同向路径地图融合和反向路径地图优化，得到探索区域的最终地图，该区域被标记为已知区域，然后继续进行未知区域的探索。
[0076] (3)多区域地图获取
[0077] 某一区域完成后，HAUV从基站出发，前往更远的区域进行探索，并将该区域的地图加入已知地图之中。在此过程中，需要确保HAUV对未知区域的起点和终点有充分精确的定位，不至于由于定位误差迷失方位，无法回到基站，并能够较为平顺的拼合新探索区域和已知区域地图。
[0078] 首先，在HAUV安装一个视觉靶标投放装置，在新探索区域的起点投放视觉靶标，其结构并不复杂，例如，以绘有显眼图案的铁片作为视觉靶标。也可以在HAUV上安装一个竖直向下的电磁铁(即靶标回收装置)，在地图探索完成后，还可以回收视觉靶标。当HAUV航行到未知区域的起点后，投放装载在其上的视觉靶标，然后以其作为起点进行地图探索，具体探索路径如图3所示。每完成一定次数的探索，HAUV就回到基站充电并进行数据处理。重复进行多次探索，直至未知区域的地图优化完成，HAUV回收视觉靶标，用于下一个未知区域的探索。
[0079] (4)基于地图的路径规划和导航
[0080] 经过多次地图探索和地图优化，最后形成区域地图数据库。当HAUV正式作业时，以地图为参照进行定位和导航。
[0081] 首先，根据地图图像的显著性指数和目标位置进行综合分析，选取一条途径显著性区域较多、且路线相对较短的路径(如图4中的灰色折线)，如图4(图中灰度越大，表示显著性越好)所示。这里的显著性区域是指由多张相邻的显著性较大的图像所组成的区域。如果以单张显著性图像作为途经点，由于地图误差和定位误差的存在，HAUV可能无法准确到达图像所在位置，因此，采用显著性区域作为途经点，以确保HAUV能够到达该区域。
[0082] 其次，在HAUV航行的过程中，通过上述闭环检测的方法进行地图匹配和位置校正，从而获得高精度的实时位置。
[0083] 综上，将本发明实施例归纳总结为下述步骤(参考图5)：
[0084] S1预设航行器在一航行区域的航行路径，航行器按照预设的航行路径航行，并使用设置在航行器上的竖直向下的相机获取水下地形的照片序列以存储于数据库；
[0085] S2对照片序列进行图像数据处理，通过闭环检测搜寻同一地形的照片；
[0086] S3对同一地形的照片进行匹配并计算出照片所对应的相机位置的变化，以对数据库中的照片序列所对应的位置进行优化；
[0087] S4计算照片序列中每一幅照片的显著性，以衡量照片中特征的有效性；
[0088] S5对单航次所获地图的数据库的精度进行评价；
[0089] S6对同一航行区域进行多次同向和反向巡航，获取多组地图，进行同向路径的地图融合和反向路径的地图优化，以得到区域地图的数据库；
[0090] S7投放靶标，重复实施步骤S1-S6，探索其他航行区域，以获取经区域拓展的地图的数据库；
[0091] S8利用步骤S7中获取的地图的数据库，结合显著性区域优先的路径规划算法，实施所述航行器的定位和导航。
[0092] 应当理解，本文所述的示例性实施例是说明性的而非限制性的。尽管结合附图描述了本发明的一个或多个实施例，本领域普通技术人员应当理解，在不脱离通过所附权利要求所限定的本发明的精神和范围的情况下，可以做出各种形式和细节的改变。
