基于半一致拓扑度量地图的视觉导航方法及系统

本发明涉及视觉导航，更具体地说，涉及一种基于半一致拓扑度量地图的视觉导航方法及系统。

背景技术：

1、目前，自主视觉导航是机器人重要功能之一。自主视觉导航领域中主流导航方式是对环境构建高精度的几何度量地图，根据传感器信息估计自身在几何度量地图的全局高精度三维位姿，基于高精度定位结果进行运动规划。

2、但是，高精度的几何度量地图构建运算复杂，地图的存储需求高，同时仅有少量单位企业有高精地图绘制资质，这一定程度上制约了视觉导航市场的发展。

3、因此，现阶段如何摆脱对高精地图的依赖，使无室外高精地图绘制资质或使用条件的无人系统仍然拥有鲁棒性，是现今自主视觉导航领域中研究的主要方向之一。

技术实现思路

1、本发明的目的在于克服现有技术中的缺点与不足，提供一种基于半一致拓扑度量地图的视觉导航方法，该方法可摆脱对高精地图的依赖，使无室外高精地图绘制资质或使用条件的无人系统仍然拥有鲁棒性，从而可提高自主视觉导航的精度。

2、本发明的第二个目的在于提供一种基于半一致拓扑度量地图的视觉导航系统。

3、为了达到上述目的，本发明通过下述技术方案予以实现：一种基于半一致拓扑度量地图的视觉导航方法，其特征在于：包括以下步骤：

4、关键帧间距参数计算步骤：采集有定位真值的图像数据集，计算测试图像与参考图像的位置误差，统计误差分布，计算地图构建步骤所需的关键帧间距参数；

5、半一致拓扑度量地图构建步骤：包含两个进程，第一个进程是局部建图进程，提取输入图像的全局和局部特征，从连续输入图像中选择图像作为关键帧，最终保存关键帧信息，第二个进程是回环检测进程，对关键帧搜索历史关键帧是否有匹配关键帧，若有匹配关键帧则记录两帧关键帧的回环信息，两个进程共同构建出半一致拓扑度量地图；

6、基于半一致拓扑度量地图的视觉定位步骤：提取输入图像的全局和局部特征，利用图像全局特征计算粗定位从而找出地图中最相似关键帧，利用局部特征计算输入图像与最相似关键帧的细定位；

7、基于半一致拓扑度量地图的规划步骤：利用地图的拓扑和度量信息进行路径搜索，使用细定位结果和路径搜索结果计算局部目的地，输入局部目的地至避障模块执行。

8、所述关键帧间距参数计算步骤包括：

9、图像数据采集步骤：在可获取图像定位真值的场景中，围绕不同坐标点利用双目相机采集一张处于中心的参考图像以及一定范围内随机地点的若干测试图像，以及图像对应的定位真值；

10、位置误差计算步骤：对所采集的图像数据，使用基于半一致拓扑度量地图的视觉定位步骤中的细定位方法计算测试图像与参考图像的位置误差；

11、误差分布统计步骤：按测试图像与参考图像的真实距离差值划分不同距离区间，统计属于不同距离区间的图像的数目以及平均计算位置误差；

12、参数计算步骤：利用导航鲁棒性模型公式，结合误差分布统计结果，计算半一致拓扑度量地图构建步骤所需的关键帧间距参数。

13、所述导航鲁棒性模型公式为：

14、em+eo+ea≤rm                                   (1)

15、式中，

16、rm表示两帧图像可匹配的最大距离阈值，简称可匹配范围；

17、em表示两幅图像匹配后计算所得的相对位置与真值的误差，简称图像匹配位置误差；

18、eo表示机器人的里程计估计的移动距离与真值的误差，简称里程计误差；

19、ea表示机器人移动动作结束后实际停留的位置与期望到达位置的误差，简称移动误差；

20、其中eo的简化模型为eo＝ηdo，do为移动距离，η为误差比例，则可得关键帧间距最大值的表达式为：

21、

22、式中，dmksct表示半一致拓扑度量地图中关键帧所需设置的距离最大值，dmksct作为关键帧间距参数。

23、所述半一致拓扑度量地图构建步骤包括：

24、提取图像的全局和局部特征步骤：使用深度神经网络模型对输入图像提取每个像素及其周边像素组合而成每个像素位置的局部特征，通过池化把局部特征聚合为图像的全局特征；

25、选择关键帧步骤：计算当前输入图像与上一个添加的关键帧的全局特征向量、相对移动距离和相对旋转角度差异，若三个差值任意一个超过对应阈值，则把当前输入图像选择为关键帧；

26、回环检测步骤：对早于待检测关键帧添加的历史关键帧，依次计算历史关键帧与待检测关键帧的全局特征差异和对极几何约束条件，若某个历史关键帧通过两个检测条件，则待检测关键帧与该历史关键帧回环，记录回环信息。

27、所述提取图像的全局和局部特征步骤和选择关键帧步骤为：

28、(1)每当有输入图像及其对应里程计位姿，计算输入图像的里程计位姿与最近添加的关键帧的里程计位姿的差值，位姿的差值包含位置差值向量δt∈r3×3以及姿态差值矩阵δr∈so(3)，其中姿态差值矩阵可以转换为三个欧拉角：翻滚角、俯仰角和偏航角，位置差值和姿态差值表示当前相对上一帧添加的关键帧的移动距离和旋转角度；

29、(2)若位置差值或姿态差值超过预设值，则把当前输入图像记录为新关键帧，记录新关键帧与上一帧关键帧在里程计中测算出的位姿差值，其中位置差值的阈值可使用步骤一计算出的关键帧间距最大值dmksct，或取该值的一半以提高定位时图像匹配的成功率，姿态差值的阈值可设为π/6弧度；

30、所述回环检测步骤为：

31、(1)对于待检测关键帧i，根据待检关键帧i与历史关键帧的图像全局特征差异，找出若干最相似的历史关键帧；

32、采用深度神经网络模型hf-net提取图像的全局特征向量zg和局部特征点的特征向量zl(i)以及其像素坐标ql(i)。通过求全局特征向量的余弦计算两帧图像相似度df(*,*)：

33、

34、式中，

35、z1和z2表示待求相似度的两帧图像；

36、||*||2表示求向量的2范数；

37、(2)若选出最相似的历史关键帧中存在某帧与待检关键帧i通过了对极几何检测，则该历史关键帧与待检测关键帧i回环，记录两个关键帧的相互回环信息。

38、本发明的半一致拓扑度量地图包括：

39、关键帧：建图采集的图像中作为地点锚点的重要图像，同时地图构建步骤会记录前后关键帧的相对距离和方向；

40、拓扑图：用于表示环境结构的、带节点间相对距离和方向的拓扑图，拓扑图中每个节点包含距离相近而方向不同的关键帧，需要指出，拓扑图的所有节点不在同一个欧式坐标系，节点间的距离和方向仅在该节点附近的欧式坐标系有效；

41、状态：用于简化表示无人系统在半一致拓扑度量地图地点和方向的概念，其内涵为所处拓扑节点以及其朝向的拓扑节点。

42、所述基于半一致拓扑度量地图的视觉定位步骤：

43、粗定位步骤：计算输入图像与所构建半一致拓扑度量地图中所有关键帧的全局特征差值，根据全局特征差值使用贝叶斯定位计算后验定位概率，找出概率最大节点中与输入图像最相似的关键帧；

44、细定位步骤：利用输入图像与粗定位步骤所得最相似关键帧的特征点计算两帧图像的对极几何约束，并通过求解输入的双目图像与最相似关键帧组成的三角形获取输入图像相对最相似关键帧的三维位姿。

45、在粗定位步骤中，采用马尔可夫定位算法并通过贝叶斯公式计算地图中每个状态的后验定位概率：

46、pp(st|zt,at-1)＝ηpo(zt|st)pt(st|st-1,at-1)pp(st-1|zt-1,at-2)   (4)

47、式中，

48、pp表示所有状态的后验定位概率向量；

49、po表示所有状态的观测概率向量；

50、pt表示状态间的转移概率矩阵；

51、st表示t时刻的状态；

52、zt表示从0到t时刻所有观测的图像全局特征集合；

53、at表示从0到t时刻所有移动动作集合；

54、at＝(μt,σt)表示t时刻移动动作，假设其满足高斯分布，即移动动作具体由移动的期望距离μt和移动距离方差σt组成；

55、其中观测概率与图像相似度相关：

56、

57、式中，

58、df表示待计算的图像特征与地图所有状态的关键帧的相似度向量；

59、从一个状态k出发执行动作at后落在某个状态k’的概率使用高斯分布的累计密度函数计算：

60、

61、式中，

62、lf(k,k’)表示从状态k移动至状态k’与其前方状态中点的距离；

63、lb(k,k’)表示从状态k移动至状态k’与其后方状态中点的距离；

64、fcdf(*)表示高斯分布的累计密度函数；

65、通过粗定位计算出的后验定位概率中，概率最高的状态可以找出与当前输入图像最相似的关键帧。

66、所述细定位步骤包括以下步骤：

67、第一步，分别计算左右目图像与最相似关键帧的本征矩阵el∈r3×3和er∈r3×3，若计算失败则退出；

68、第二步，分别用本征矩阵计算左右目图像与最相似关键帧的归一化相对旋转平移矩阵tl’∈se(3)和tr’∈se(3)；

69、第三步，用三角形三条边的向量计算三角形的两个角：

70、

71、式中，

72、jc表示最相似关键帧点；

73、上标或下标的l和r分别表示左目和右目点；

74、tba表示点a指向点b的向量；

75、<*,*>表示通过余弦法求两个向量的夹角；

76、∠表示三个字母组成的夹角；

77、第四步，计算三角形左目相对最相似关键帧的距离：

78、

79、第五步，整合可得左目与最相似关键帧的相对转换矩阵，即细定位的结果tljc：

80、

81、式中，

82、rljc’来自tl’的旋转矩阵；

83、tljc’来自tl’的平移向量。

84、所述基于半一致拓扑度量地图的规划步骤包括：

85、路径搜索步骤：使用视觉定位的粗定位结果得到的关键帧对应的最大节点作为起点节点，外部指定终点节点，在半一致拓扑度量地图中用迪杰斯特拉算法搜索最短路径；

86、局部目的地计算步骤：根据细定位计算的相对最相似关键帧的三维位姿以及地图中最相似关键帧相对其相邻关键帧的位姿，计算相邻关键帧相对自身的三维位姿作为局部目的地。

87、一种基于半一致拓扑度量地图的视觉导航系统，其特征在于：包括

88、关键帧间距参数计算模块：采集有定位真值的图像数据集，计算测试图像与参考图像的位置误差，统计误差分布，计算地图构建步骤所需的关键帧间距参数；

89、半一致拓扑度量地图构建模块：包含两个子模块，第一个进程是局部建图模块，提取输入图像的全局和局部特征，从连续输入图像中选择图像作为关键帧，最终保存关键帧信息，第二个进程是回环检测模块，对关键帧搜索历史关键帧是否有匹配关键帧，若有匹配关键帧则记录两帧关键帧的回环信息，两个进程共同构建出半一致拓扑度量地图；

90、基于半一致拓扑度量地图的视觉定位模块：提取输入图像的全局和局部特征，利用图像全局特征计算粗定位从而找出地图中最相似关键帧，利用局部特征计算输入图像与最相似关键帧的细定位；

91、基于半一致拓扑度量地图的规划模块：利用地图的拓扑和度量信息进行路径搜索，使用细定位结果和路径搜索结果计算局部目的地，输入局部目的地至避障模块执行。

92、与现有技术相比，本发明具有如下优点与有益效果：

93、1、本发明可以摆脱视觉导航系统对于高精地图的依赖，使无室外高精地图绘制资质或使用条件的无人系统仍然拥有鲁棒的视觉导航功能。

94、2、本发明中半一致拓扑度量地图的构建不需要进行全局位姿优化，可以有效降低视觉建图的运算复杂度。

95、3、本发明中半一致拓扑度量地图可以显著减少视觉地图中所需保存的关键帧数量，从而有效降低地图存储需求。