视觉定位恢复方法、装置、机器人和可读存储介质与流程

1.本发明涉及机器人视觉领域，具体而言，涉及一种视觉定位恢复方法、装置、机器人和可读存储介质。


背景技术：

2.现有的机器人视觉导航针对跟踪失败时，采用重定位技术进行恢复，要求机器人回到已生成地图的区域，再次获得机器人之前地图中的位姿，才能执行重定位。而现有的重定位过程中，机器人只能盲目的运动以尝试重新定位成功，因此重定位的时间较长，并且成功率较低。


技术实现要素：

3.鉴于上述问题，本发明提供了一种视觉定位恢复方法、装置、机器人和可读存储介质，以缩短重定位的时间，提高重定位的成功率。
4.为了实现上述目的，本发明采用如下的技术方案：
5.一种视觉定位恢复方法，包括：
6.获取视觉图像计算位姿以及相应的位姿评分，将所述位姿以及所述位姿评分关联存储至轨迹集；
7.当确定定位跟踪失败时，按照预设规则从所述轨迹集中选取一个位姿作为新导航目标；
8.以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图，将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位；
9.在重定位成功后，将所述恢复地图与全局地图合并为新的全局地图，并恢复初始导航目标，在所述新的全局地图上进行视觉定位导航。
10.优选地，所述的视觉定位恢复方法中，所述按照预设时间间隔获取视觉图像计算当前位姿以及相应的位姿评分包括：
11.获取当前视觉图像，对所述当前视觉图像进行特征点提取，获得多个图像特征点；
12.利用所述多个图像特征点与全局地图的地图特征点进行匹配运算，获得当前位姿以及相应的位姿评分。
13.优选地，所述的视觉定位恢复方法中，所述按照预设规则从所述轨迹集中选取一个位姿作为新导航目标包括：
14.从所述轨迹集中筛选出位姿评分大于预设阈值的至少一个初筛位姿；
15.根据位姿距离算法从至少一个所述初筛位姿中筛选出与所述恢复地图的原点的位姿距离最小的位姿，作为所述新导航目标。
16.优选地，所述的视觉定位恢复方法中，所述位姿距离算法的算式包括：
17.dist(t
i
,t
j
)＝α|x
i
‑
x
j
,y
i
‑
y
j
,z
i
‑
z
j
|2 β|a
i
‑
a
j
,b
i
‑
b
j
,c
i
‑
c
j
|2；
18.式中，dist()为位姿距离，t
i
为第i个位姿，t
j
为第j个位姿；x
i
、y
i
、z
i
为t
i
位姿的坐
标，a
i
、b
i
、c
i
为t
i
位姿的欧拉角；x
j
、y
j
、z
j
为t
j
位姿的坐标，a
j
、b
j
、c
j
为t
j
位姿的欧拉角；α为直线距离权重，β为转动角度权重。
19.优选地，所述的视觉定位恢复方法中，还包括：
20.启动视觉定位导航后，创建所述全局地图并接收所述导航目标，在所述全局地图上向所述导航目标进行视觉定位导航。
21.优选地，所述的视觉定位恢复方法中，所述将所述新导航目标映射至所述恢复地图的算式包括：
22.recover
goal
＝t
‑
1last
t
recover
；
23.式中，t
last
为所述上一位姿，t
recover
为所述新导航目标，recover
goal
为映射后所述新导航目标。
24.优选地，所述的视觉定位恢复方法中，还包括：
25.通过运动追踪传感器获取上一位姿与当前位姿之间的位姿变换，利用所述上一位姿与所述位姿变换计算所述坐标原点，根据所述坐标原点创建所述恢复地图；
26.所述坐标原点的计算算式包括：t＝t
last
t
diff
，式中，t为所述坐标原点，t
last
为所述上一位姿，t
diff
为所述位姿变换。
27.本发明还提供一种视觉定位恢复装置，包括：
28.位姿计算模块，用于获取视觉图像计算位姿以及相应的位姿评分，将所述位姿以及所述位姿评分关联存储至轨迹集；
29.导航目标选取模块，用于当确定定位跟踪失败时，按照预设规则从所述轨迹集中选取一个位姿作为新导航目标；
30.重定位模块，用于以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图，将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位；
31.导航恢复模块，用于在重定位成功后，将所述恢复地图与全局地图合并为新的全局地图，并恢复初始导航目标，在所述新的全局地图上进行视觉定位导航。
32.本发明还提供一种机器人，包括存储器以及处理器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行所述的视觉定位恢复方法。
33.本发明还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行所述的视觉定位恢复方法。
34.本发明提供一种视觉定位恢复方法，该视觉定位恢复方法包括：获取视觉图像计算位姿以及相应的位姿评分，将所述位姿以及所述位姿评分关联存储至轨迹集；当确定定位跟踪失败时，按照预设规则从所述轨迹集中选取一个位姿作为新导航目标；以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图，将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位；在重定位成功后，将所述恢复地图与全局地图合并为新的全局地图，并恢复初始导航目标，在所述新的全局地图上进行视觉定位导航。本发明的视觉定位恢复方法，在视觉导航计算出当前机器人的位姿的同时，还计算出相应的位姿评分进行关联存储至轨迹集，在定位追踪失败时，可以通过获取最优的历史位姿进行重定位路线的导航，从而缩短重定位的时间，提高重定位的成功率。
35.为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
36.为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。
37.图1是本发明实施例1提供的一种视觉定位恢复方法的流程图；
38.图2是本发明实施例2提供的一种计算位姿及评分的流程图；
39.图3是本发明实施例3提供的一种轨迹集中选取位姿的流程图；
40.图4是本发明实施例4提供的一种视觉定位恢复方法的流程图；
41.图5是本发明实施例4提供的一种视觉定位恢复装置的结构示意图。
具体实施方式
42.下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
43.通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
44.在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
45.此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
46.除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。
47.实施例1
48.图1是本发明实施例1提供的一种视觉定位恢复方法的流程图，该方法包括如下步骤：
49.步骤s11：获取视觉图像计算位姿以及相应的位姿评分，将所述位姿以及所述位姿评分关联存储至轨迹集。
50.本发明实施例中，在机器人启动视觉定位导航后，可以创建全局地图并接收导航目标，在全局地图上向导航目标进行视觉定位导航，即通过视觉同步定位与建图系统进行视觉导航。在通过视觉同步定位与建图系统进行视觉导航的场景中，可以按照预设时间间隔获取机器人行驶时前方的视觉图像，输入至系统，以计算出机器人在全局地图上的当前位姿。在计算出当前位姿的同时，还可以对该当前位姿进行评分，例如通过当前位姿的特征
点与全局地图中的地图特征点进行匹配，其匹配的程度越高，则当前的位姿评分越高。
51.本发明实施例中，在通过视觉图像计算出机器人的当前位姿以及相应的位姿评分后，可以关联存储至预先设定的轨迹集中，通过轨迹集来表示机器人在视觉导航行驶过程中的轨迹，为重定位提供所需的位姿。
52.步骤s12：当确定定位跟踪失败时，按照预设规则从所述轨迹集中选取一个位姿作为新导航目标。
53.本发明实施例中，视觉导航在复杂的环境中可能难以获得机器人的当前位姿，即所谓的定位跟踪失败。在定位跟踪失败后，可以从轨迹集中按照预设规则选取出一个最优的位姿作为新导航目标，以便通过视觉导航在导航至该新导航目标期间使用重定位功能，使视觉导航重新定位回全局地图。
54.步骤s13：以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图，将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位。
55.本发明实施例中，由于定位跟踪失败，将导致当前的全局地图可能处于失效状态，在重定位过程中为了避免出现更大的误差，因此以上一位姿作为坐标原点创建恢复地图，由于相邻两位姿之间的时间差非常小，因此重定位过程中带来的误差也非常小，对于重定位的影响非常小。
56.本发明实施例中，由于轨迹集中的位姿是以初始的全局地图的坐标生成的，因此在应用在恢复地图时，需要进行坐标的映射。所述将所述新导航目标映射至所述恢复地图的算式包括：
57.recover
goal
＝t
‑
1last
t
recover
；
58.式中，t
last
为所述上一位姿，t
recover
为所述新导航目标，recover
goal
为映射后所述新导航目标。其中，在机器人中可以预先设置有基于上述算式的应用程序，在获取到新导航目标以及上一位姿后，可以输入至该应用程序，以获得在恢复地图上映射的新导航目标。
59.步骤s14：在重定位成功后，将所述恢复地图与全局地图合并为新的全局地图，并恢复初始导航目标，在所述新的全局地图上进行视觉定位导航。
60.本发明实施例中，在恢复地图上通过视觉导航向新导航目标前进的过程中，同时开启重定位功能，以便找到在之前全局地图中的位姿。一旦找到在之前全局地图中的位姿，也即说明恢复地图与初始的全局地图已经出现重合部分，这时可以将恢复地图与全局地图合并为新的全局地图，在更新地图后恢复初始导航目标，重新在全局地图上向初始导航目标进行导航。
61.本发明实施例中，在视觉导航计算出当前机器人的位姿的同时，还计算出相应的位姿评分进行关联存储至轨迹集，在定位追踪失败时，可以通过获取最优的历史位姿进行重定位路线的导航，从而缩短重定位的时间，提高重定位的成功率。
62.实施例2
63.图2是本发明实施例2提供的一种计算位姿及评分的流程图，包括如下步骤：
64.步骤s21：获取当前视觉图像，对所述当前视觉图像进行特征点提取，获得多个图像特征点。
65.步骤s22：利用所述多个图像特征点与全局地图的地图特征点进行匹配运算，获得当前位姿以及相应的位姿评分。
66.本发明实施例中，在机器人中可以预先设置有进行匹配运算的应用程序，在获取到当前视觉图像的图像特征点后，可以将该图像特征点以及全局地图的地图特征点输入至该应用程序，以获得相应的位姿评分。
67.实施例3
68.图3是本发明实施例3提供的一种轨迹集中选取位姿的流程图，包括如下步骤：
69.步骤s31：从所述轨迹集中筛选出位姿评分大于预设阈值的至少一个初筛位姿。
70.本发明实施例中，从轨迹集中选取作为新导航目标的位姿时，首先从轨迹集中筛选出所有位姿评分大于预设阈值的相应的位姿，作为初筛位姿，初筛位姿可能会存在多个，因此需要根据位姿距离进行进一步的筛选。其中，上述筛选的过程也即位姿评分的对比过程，例如可以从轨迹集中按逆时间顺序获取位姿评分与预设阈值进行对比，在确定位姿评分大于预设阈值时从轨迹集中提取出相应的初筛位姿。
71.步骤s32：根据位姿距离算法从至少一个所述初筛位姿中筛选出与所述恢复地图的原点的位姿距离最小的位姿，作为所述新导航目标。
72.本发明实施例中，所述位姿距离算法的算式包括：
73.dist(t
i
,t
j
)＝α|x
i
‑
x
j
,y
i
‑
y
j
,z
i
‑
z
j
|2 β|a
i
‑
a
j
,b
i
‑
b
j
,c
i
‑
c
j
|2；
74.式中，dist()为位姿距离，t
i
为第i个位姿，t
j
为第j个位姿；x
i
、y
i
、z
i
为t
i
位姿的坐标，a
i
、b
i
、c
i
为t
i
位姿的欧拉角；x
j
、y
j
、z
j
为t
j
位姿的坐标，a
j
、b
j
、c
j
为t
j
位姿的欧拉角；α为直线距离权重，β为转动角度权重。其中，在获得多个初筛位姿后，将利用每个初筛位姿分别于恢复地图的原点的位姿按照上述位姿距离算法的算式计算出相应的位姿距离，在筛选出位姿距离最小的相应的初筛位姿，作为最终新导航目标。其中，该恢复地图的原点的位姿即轨迹集中最近的上一位姿。
75.实施例4
76.图4是本发明实施例4提供的一种视觉定位恢复方法的流程图，该方法包括如下步骤：
77.步骤s41：按照预设时间间隔获取视觉图像计算位姿以及相应的位姿评分，将所述位姿以及所述位姿评分关联存储至轨迹集。
78.此步骤与上述步骤s11一致，在此不再赘述。
79.步骤s42：当无法计算当前位姿时确定定位跟踪失败时，按照预设规则从所述轨迹集中选取一个位姿作为新导航目标。
80.此步骤与上述步骤s12一致，在此不再赘述。
81.步骤s43：通过运动追踪传感器获取上一位姿与当前位姿之间的位姿变换，利用所述上一位姿与所述位姿变换计算所述坐标原点，根据所述坐标原点创建所述恢复地图，将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位。
82.本发明实施例中，所述坐标原点的计算算式包括：t＝t
last
t
diff
，式中，t为所述坐标原点，t
last
为所述上一位姿，t
diff
为所述位姿变换。
83.本发明实施例中，上述运动追踪传感器包括惯性传感器和轮式里程计中至少一种传感器。
84.步骤s44：在重定位成功后，将所述恢复地图与全局地图合并为新的全局地图，并恢复初始导航目标，在所述新的全局地图上进行视觉定位导航。
85.此步骤与上述步骤s14一致，在此不再赘述。
86.实施例5
87.图5是本发明实施例4提供的一种视觉定位恢复装置的结构示意图。
88.该视觉定位恢复装置500包括：
89.位姿计算模块510，用于获取视觉图像计算位姿以及相应的位姿评分，将所述位姿以及所述位姿评分关联存储至轨迹集；
90.导航目标选取模块520，用于当确定定位跟踪失败时，按照预设规则从所述轨迹集中选取一个位姿作为新导航目标；
91.重定位模块530，用于以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图，将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位；
92.导航恢复模块540，用于在重定位成功后，将所述恢复地图与全局地图合并为新的全局地图，并恢复初始导航目标，在所述新的全局地图上进行视觉定位导航。
93.本发明实施例中，上述各个模块更加详细的功能描述可以参考前述实施例中相应部分的内容，在此不再赘述。
94.此外，本发明还提供了一种机器人，该机器人包括存储器和处理器，存储器可用于存储计算机程序，处理器通过运行所述计算机程序，从而使机器人执行上述方法或者上述视觉定位恢复装置中的各个模块的功能。
95.存储器可包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据机器人的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
96.本发明还提供了一种计算机存储介质，用于储存上述机器人中使用的计算机程序。
97.在本技术所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
98.另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。
99.所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计
算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read
‑
only memory)、随机存取存储器(ram，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质。
100.以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。