移动机器人初始位姿估计方法、系统、计算机及机器人与流程

1.本发明涉及移动机器人技术领域，特别涉及一种移动机器人初始位姿估计方法、系统、计算机及机器人。


背景技术：

2.现有的基于ros navigation功能包来实现导航的移动机器人大多数使用amcl(adaptive monte carlo localization)算法进行粒子滤波，从而对自身在已知的地图中进行概率定位。现有的移动机器人在首次启动时，通常要求用户手动输入当前机器人的初始化位姿，然而，用户手动输入的初始化位姿不够准确，且机器人不移动时也不会自动更新初始化位姿。
3.现有技术大部分通过将上述amcl粒子分布至整张地图、以及强制粒子更新等手段来对移动机器人的初始姿态进行估计，然而，通过将amcl粒子分布至整张地图得出的移动机器人的初始位姿的精度较差，并且强制粒子更新会降低当前移动机器人后续定位的精度。
4.因此，针对现有技术的不足，提供一种能够提升移动机器人初始位姿估计精度的方法很有必要。


技术实现要素：

5.基于此，本发明的目的是提供一种移动机器人初始位姿估计方法、系统、计算机及机器人，以实现移动机器人初始位姿估计。
6.本发明实施例第一方面提出了一种移动机器人初始位姿估计方法，所述方法包括：
7.当初始化ros完成时，启用相机并通过所述相机发布rgb图像流话题节点；
8.获取所述rgb图像流话题节点中的图像，并计算出所述图像中的orb特征的第一关键点与第一描述子；
9.读取模板图像集并加载出实验区图像词典；
10.计算出所述模板图像集中的orb特征的关键点集与描述子集，所述描述子集包括若干第二描述子，所述关键点集包括若干第二关键点；
11.根据所述实验区图像词典将所述第一描述子和所述第二描述子均转换成对应的特征向量，并计算出所述特征向量之间的相似度得分；
12.从所述模板图像集中提取出匹配度最高的图像及对应的位姿，并计算出所述相机获取的图像与所述模板图像集中的匹配度最高的图像的旋转矩阵与平移矩阵；
13.根据匹配度最高的图像对应的位姿计算出当前位置的空间姿态，并发布ros初始化位置消息。
14.本发明的有益效果是：本技术首次提出使用视觉图像信息来辅助移动机器人开展初始位姿估计，具体的，通过视觉slam技术获取先验关键帧及其姿态信息，基于视觉词典的
回环检测技术匹配相似度最高模板图像，根据对极约束使用八点法求解当前帧与模板帧变换矩阵，与先验姿态处理后得移动机器人位姿估计。算法能够在较短时间内部署在基于ros控制的移动机器人上，于已知占用地图区域内可有效估计移动机器人位姿，并且对算法的算力要求低，适用于大范围的推广与使用。
15.优选的，所述获取所述rgb图像流话题节点中的图像，并计算出所述图像中的orb特征的第一关键点与第一描述子的步骤包括：
16.基于linux/ubuntu操作系统环境，通过cv::bridge在所述rgb图像流话题节点中获取到移动机器人当前位置的正方向；
17.按照sensor_msgs::image格式二进制图像转化成常用的cv::mat类型的图像数据，并使用opencv内嵌的图像orb特征提取算法计算出所述图像数据对应的所述第一描述子与所述第一关键点。
18.优选的，所述模板图像集包括基于orb-slam3算法生成的当前移动机器人运行区域的关键帧图像集，以及相对于当前移动机器人地图原点的空间姿态，所述空间姿态由坐标x、y、z与四元数wx、wy、wz、z表示；
19.所述实验区图像词典由所述关键帧图像集利用dbow3库以k叉树、l层深度、tf_idf权重计算方式、l1范数评分方式并使用kmeans 聚类生成；
20.其中，所述tf_idf权重计算方式具体包括：
21.tf_idf权重计算方式使用的公式如下：
22.ηi＝tfi×
idfi；
23.其中，tfi是指某个特征在单幅图像中出现的频率，ni是指某个特征在图像中出现的次数，n是指图像中的总特征数，idfi是指出现某个特征的图像数量占图像总数的比例，n表示图像词典中的总图像数，ni表示出现某个特征的图象数，ηi表示某一特征在所述实验区图像词典中的权重；
24.其中，所述l1范数评分方式具体包括：
25.使用l1范数计算特征向量之间的匹配程度；
[0026][0027]
其中，v1表示第一张图像的特征向量，v2表示第二张图像的特征向量，s表示两个向量之间的匹配度得分。
[0028]
优选的，所述根据所述实验区图像词典将所述第一描述子和所述第二描述子均转换成对应的特征向量，并计算出所述特征向量之间的相似度得分的步骤包括：
[0029]
对所述模板图像集中的全部图像提取出orb特征并获取到对应的关键点集与描述子集；
[0030]
将所述第一描述子以及所述描述子集中的各个第二描述子均通过所述实验区图像词典转化为对应的特征向量；
[0031]
根据所述l1范数特征向量间的匹配程度计算出对应的匹配得分；
[0032]
其中，描述子到特征向量的转换过程通过查询所述实验区图像词典完成，且特征向量描述式如下：
[0033]
v＝(n1，n2，
…
，n
k*l-1
，n
k*l
)；
[0034]
其中，v表示图像在所述实验区图像词典空间下的特征向量描述形式，ni表示所述实验区图像词典空间下的某一特征，向量维数由词典分叉数k与层数l的积决定，当描述子中存在所述实验区图像词典中的某个特征ni时，记录对应维度下的特征出现频数，否则当前维度为0。
[0035]
优选的，所述从所述模板图像集中提取出匹配度最高的图像及对应的位姿，并计算出所述相机获取的图像与所述模板图像集中的匹配度最高的图像的旋转矩阵与平移矩阵的步骤包括：
[0036]
根据相似度得分的高低确定出匹配度最高的图像，并分别获取到匹配得分最高、次高对应的图像在所述模板图像集中的位置index
first
以及index
second
；
[0037]
通过判断|index
first-index
second
|≤10的结果来对匹配进行时间一致性约束，以消除误匹配对移动机器人带来的风险；
[0038]
在确定出匹配度最高的图像有效之后，读取当前匹配度最高的图像的图像数据，并进行orb特征提取，以获取到第二关键点keypoints
tem
；
[0039]
基于所述第一关键点keypomts以及所述相机的相机焦距、相机光心，根据对极几何约束出本质矩阵e＝t
×
r，并根据所述本质矩阵e恢复出两个视角的旋转矩阵r与平移矩阵t。
[0040]
优选的，所述根据匹配度最高的图像对应的位姿计算出当前位置的空间姿态，并发布ros初始化位置消息的步骤包括：
[0041]
基于所述旋转矩阵r以及所述平移矩阵t构造出变换矩阵t；
[0042]
读取出匹配度最高的图像空间的坐标及四元数，并将所述坐标和所述四元数转换成对应的变换矩阵t
tem
；
[0043]
将两变换矩阵t
tem
做矩阵乘法得到当前位置图像对应的地图坐标原点变换矩阵t
final
；
[0044]
将所述t
final
转换成四元数，构造geometry_msgs：：posewithcovariancestamped格式、名为initialpose的ros初始化位置消息。
[0045]
优选的，所述当初始化ros完成时，启用相机并通过所述相机发布rgb图像流话题节点的步骤包括：
[0046]
将摄像头布置于移动机器人的前部，并朝所述移动机器人的正方向呈平视状态；
[0047]
通过usb type-c接口将所述摄像头与所述移动机器人的主控制器电性连接；
[0048]
将所述移动机器人的主控制器在linux/ubuntu操作系统环境下开启ros系统，并打开所述相机的ros节点，以发布名为/camera/color/image_raw的rgb图像流。
[0049]
本发明实施例第二方面提出了一种移动机器人初始位姿估计系统，所述系统包括：
[0050]
启动模块，用于当初始化ros完成时，启用相机并通过所述相机发布rgb图像流话题节点；
[0051]
第一计算模块，用于获取所述rgb图像流话题节点中的图像，并计算出所述图像中
的orb特征的第一关键点与第一描述子；
[0052]
读取模块，用于读取模板图像集并加载出实验区图像词典；
[0053]
第二计算模块，用于计算出所述模板图像集中的orb特征的关键点集与描述子集，所述描述子集包括若干第二描述子，所述关键点集包括若干第二关键点；
[0054]
转换模块，用于根据所述实验区图像词典将所述第一描述子和所述第二描述子均转换成对应的特征向量，并计算出所述特征向量之间的相似度得分；
[0055]
匹配模块，用于从所述模板图像集中提取出匹配度最高的图像及对应的位姿，并计算出所述相机获取的图像与所述模板图像集中的匹配度最高的图像的旋转矩阵与平移矩阵；
[0056]
第三计算模块，用于根据匹配度最高的图像对应的位姿计算出当前位置的空间姿态，并发布ros初始化位置消息。
[0057]
其中，上述移动机器人初始位姿估计系统中，所述第一计算模块具体用于：
[0058]
基于linux/ubuntu操作系统环境，通过cv::bridge在所述rgb图像流话题节点中获取到移动机器人当前位置的正方向；
[0059]
按照sensor_msgs：：image格式二进制图像转化成常用的cv：：mat类型的图像数据，并使用opencv内嵌的图像orb特征提取算法计算出所述图像数据对应的所述第一描述子与所述第一关键点。
[0060]
其中，上述移动机器人初始位姿估计系统中，所述模板图像集包括基于orb-slam3算法生成的当前移动机器人运行区域的关键帧图像集，以及相对于当前移动机器人地图原点的空间姿态，所述空间姿态由坐标x、y、z与四元数wx、wy、wz、z表示；
[0061]
所述实验区图像词典由所述关键帧图像集利用dbow3库以k叉树、l层深度、tf_idf权重计算方式、l1范数评分方式并使用kmeans 聚类生成；
[0062]
其中，所述tf_idf权重计算方式具体包括：
[0063]
tf_idf权重计算方式使用的公式如下：
[0064]
ηi＝tfi×
idfi；
[0065]
其中，tfi是指某个特征在单幅图像中出现的频率，ni是指某个特征在图像中出现的次数，n是指图像中的总特征数，idfi是指出现某个特征的图像数量占图像总数的比例，n表示图像词典中的总图像数，ni表示出现某个特征的图象数，ηi表示某一特征在所述实验区图像词典中的权重；
[0066]
其中，所述l1范数评分方式具体包括：
[0067]
使用l1范数计算特征向量之间的匹配程度；
[0068][0069]
其中，v1表示第一张图像的特征向量，v2表示第二张图像的特征向量，s表示两个向量之间的匹配度得分。
[0070]
其中，上述移动机器人初始位姿估计系统中，所述转换模块具体用于：
[0071]
对所述模板图像集中的全部图像提取出orb特征并获取到对应的关键点集与描述子集；
[0072]
将所述第一描述子以及所述描述子集中的各个第二描述子均通过所述实验区图像词典转化为对应的特征向量；
[0073]
根据所述l1范数特征向量间的匹配程度计算出对应的匹配得分；
[0074]
其中，描述子到特征向量的转换过程通过查询所述实验区图像词典完成，且特征向量描述式如下：
[0075]
v＝(n1，n2，
…
，n
k*l-1
，n
k*l
)；
[0076]
其中，v表示图像在所述实验区图像词典空间下的特征向量描述形式，ni表示所述实验区图像词典空间下的某一特征，向量维数由词典分叉数k与层数l的积决定，当描述子中存在所述实验区图像词典中的某个特征ni时，记录对应维度下的特征出现频数，否则当前维度为0。
[0077]
其中，上述移动机器人初始位姿估计系统中，所述匹配模块具体用于：
[0078]
根据相似度得分的高低确定出匹配度最高的图像，并分别获取到匹配得分最高、次高对应的图像在所述模板图像集中的位置index
first
以及index
second
；
[0079]
通过判断|index
first-index
second
|≤10的结果来对匹配进行时间一致性约束，以消除误匹配对移动机器人带来的风险；
[0080]
在确定出匹配度最高的图像有效之后，读取当前匹配度最高的图像的图像数据，并进行orb特征提取，以获取到第二关键点keypoints
ten
；
[0081]
基于所述第一关键点keypoints以及所述相机的相机焦距、相机光心，根据对极几何约束出本质矩阵e＝t
×
r，并根据所述本质矩阵e恢复出两个视角的旋转矩阵r与平移矩阵t。
[0082]
其中，上述移动机器人初始位姿估计系统中，所述第三计算模块具体用于：
[0083]
基于所述旋转矩阵r以及所述平移矩阵t构造出变换矩阵t；
[0084]
读取出匹配度最高的图像空间的坐标及四元数，并将所述坐标和所述四元数转换成对应的变换矩阵t
tem
；
[0085]
将两变换矩阵t
tem
做矩阵乘法得到当前位置图像对应的地图坐标原点变换矩阵t
final
；
[0086]
将所述t
final
转换成四元数，构造geometry_msgs::posewithcovariancestamped格式、名为initialpose的ros初始化位置消息。
[0087]
其中，上述移动机器人初始位姿估计系统中，所述启动模块具体用于：
[0088]
将摄像头布置于移动机器人的前部，并朝所述移动机器人的正方向呈平视状态；
[0089]
通过usb type-c接口将所述摄像头与所述移动机器人的主控制器电性连接；
[0090]
将所述移动机器人的主控制器在linux/ubuntu操作系统环境下开启ros系统，并打开所述相机的ros节点，以发布名为/camera/color/image_raw的rgb图像流。
[0091]
本发明实施例第三方面提出了一种计算机，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上面所述的汽车负载模拟测试方法。
[0092]
本发明实施例第四方面提出了一种机器人，包括上面所述的移动机器人初始位姿
估计系统。
[0093]
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
[0094]
图1为本发明第一实施例提供的移动机器人初始位姿估计方法的流程图；
[0095]
图2为本发明第三实施例提供的移动机器人初始位姿估计系统的结构框图。
[0096]
如下具体实施方式将结合上述附图进一步说明本发明。
具体实施方式
[0097]
为了便于理解本发明，下面将参照相关附图对本发明进行更全面的描述。附图中给出了本发明的若干实施例。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容更加透彻全面。
[0098]
需要说明的是，当元件被称为“固设于”另一个元件，它可以直接在另一个元件上或者也可以存在居中的元件。当一个元件被认为是“连接”另一个元件，它可以是直接连接到另一个元件或者可能同时存在居中元件。本文所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只是为了说明的目的。
[0099]
除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。
[0100]
请参阅图1，所示为本发明第一实施例提供的移动机器人初始位姿估计方法，本实施例提供的移动机器人初始位姿估计方法能够在较短时间内部署在基于ros控制的移动机器人上，于已知占用地图区域内可有效估计移动机器人位姿，并且对算法的算力要求低，适用于大范围的推广与使用。
[0101]
具体的，本实施例提供的移动机器人初始位姿估计方法具体包括以下步骤：
[0102]
步骤s10，当初始化ros完成时，启用相机并通过所述相机发布rgb图像流话题节点；
[0103]
在本实施例中，首先需要说明的是，本实施例提供的移动机器人初始位姿估计方法具体应用在基于ros系统驱动的移动机器人上。并且会预先在该移动机器人上安装有适用于ros驱动的相机，该相机设置在当前移动机器人的前端，用于获取当前移动机器人前方的图像。
[0104]
因此，在本步骤中，当移动机器人启动时，需要首先完成对当前移动机器人内部有的ros的初始化操作，并且当初始化ros完成时，再启用相机并通过当前相机实时采集图像并发布rgb图像流话题节点。
[0105]
步骤s20，获取所述rgb图像流话题节点中的图像，并计算出所述图像中的orb特征的第一关键点与第一描述子；
[0106]
进一步的，在本步骤中，当通过相机获取到当前移动机器人前方的图像时，当前移动机器人内部的控制器能够识别出当前图像对应的rgb图像流话题节点，并进一步获取到
当前rgb图像流话题节点中的图像，并通过预设程序计算出当前图像中的orb特征的第一关键点与第一描述子，并执行步骤s30。
[0107]
步骤s30，读取模板图像集并加载出实验区图像词典；
[0108]
进一步的，在本步骤中，当控制器获取到当前图像中的orb特征对应的第一关键点以及第一描述子时，该控制器会立即读取当前移动机器人内部预存好的模板图像集，并对应加载出实验区图像词典。
[0109]
步骤s40，计算出所述模板图像集中的orb特征的关键点集与描述子集，所述描述子集包括若干第二描述子，所述关键点集包括若干第二关键点；
[0110]
进一步的，在本步骤中，控制器会基于上述步骤s30读取出的模板图像集对应计算出当前模板图像集中的orb特征的关键点集与描述子集，具体的，上述描述子集包括若干第二描述子，上述关键点集包括若干第二关键点。
[0111]
步骤s50，根据所述实验区图像词典将所述第一描述子和所述第二描述子均转换成对应的特征向量，并计算出所述特征向量之间的相似度得分；
[0112]
更进一步的，在本步骤中，控制器会基于上述步骤s30读取出的实验区图像词典将上述第一描述子与第二描述子均转换成对应的特征向量，并对应计算出各个特征向量之间的相似度得分。
[0113]
步骤s60，从所述模板图像集中提取出匹配度最高的图像及对应的位姿，并计算出所述相机获取的图像与所述模板图像集中的匹配度最高的图像的旋转矩阵与平移矩阵；
[0114]
在本步骤中，需要说明的是，控制器会对上述步骤s30获取到的模板图像集以及实验区图像词典进行二次处理，具体的，控制器从当前模板图像集中提取出匹配度最高的图像及对应的位姿，并计算出上述相机获取的图像与当前模板图像集中的匹配度最高的图像的旋转矩阵与平移矩阵。
[0115]
步骤s70，根据匹配度最高的图像对应的位姿计算出当前位置的空间姿态，并发布ros初始化位置消息。
[0116]
最后，在本步骤中，当通过上述步骤s60在当前模板图像集中提取出匹配度最高的图像及对应的位姿时，本步骤会立即根据匹配度最高的图像对应的位姿计算出当前移动机器人的当前位置的空间姿态，并发布对应的ros初始化位置消息，从而能够在较短时间内部署在基于ros控制的移动机器人上，于已知占用地图区域内可有效估计移动机器人位姿，并且对算法的算力要求低，适用于大范围的推广与使用。
[0117]
使用时，通过视觉slam技术获取先验关键帧及其姿态信息，基于视觉词典的回环检测技术匹配相似度最高模板图像，根据对极约束使用八点法求解当前帧与模板帧变换矩阵，与先验姿态处理后得移动机器人位姿估计。算法能够在较短时间内部署在基于ros控制的移动机器人上，于已知占用地图区域内可有效估计移动机器人位姿，并且对算法的算力要求低，适用于大范围的推广与使用。
[0118]
需要说明的是，上述的实施过程只是为了说明本技术的可实施性，但这并不代表本技术的移动机器人初始位姿估计方法只有上述唯一一种实施流程，相反的，只要能够将本技术的移动机器人初始位姿估计方法实施起来，都可以被纳入本技术的可行实施方案。
[0119]
综上，本发明上述实施例当中的移动机器人初始位姿估计方法能够在较短时间内部署在基于ros控制的移动机器人上，于已知占用地图区域内可有效估计移动机器人位姿，
并且对算法的算力要求低，适用于大范围的推广与使用。
[0120]
另外，本发明第二实施例也提供了一种移动机器人初始位姿估计方法，本实施例提供的移动机器人初始位姿估计方法具体包括以下步骤：
[0121]
本实施例提供的移动机器人初始位姿估计方法同样运用在基于ros系统驱动的移动机器人上。
[0122]
步骤s11，将摄像头布置于移动机器人的前部，并朝所述移动机器人的正方向呈平视状态；通过usb type-c接口将所述摄像头与所述移动机器人的主控制器电性连接；将所述移动机器人的主控制器在linux/ubuntu操作系统环境下开启ros系统，并打开所述相机的ros节点，以发布名为/camera/color/image_raw的rgb图像流。
[0123]
具体的，在本实施例中，首先需要说明的是，本实施例会首先将相机布置于移动机器人的前部，并朝当前移动机器人的正方向呈平视状态。进一步的，通过usb type-c接口将当前相机中的摄像头与当前移动机器人的主控制器电性连接，以使当前相机接入当前移动机器人的ros系统中。
[0124]
进一步的，将当前移动机器人的主控制器在linux/ubuntu操作系统环境下开启ros系统，并打开所述相机的ros节点，以发布名为/camera/color/image_raw的rgb图像流，该rgb图像流包括若干rgb图像流话题节点。
[0125]
步骤s21，基于linux/ubuntu操作系统环境，通过cv：：bridge在所述rgb图像流话题节点中获取到移动机器人当前位置的正方向；按照sensor_msgs：：image格式二进制图像转化成常用的cv：：mat类型的图像数据，并使用opencv内嵌的图像orb特征提取算法计算出所述图像数据对应的所述第一描述子与所述第一关键点。
[0126]
进一步的，在本步骤中，会基于linux/ubuntu操作系统环境对获取到的rgb图像流中的若干rgb图像流话题节点进行处理，即控制器会通过cv：：bridge在当前rgb图像流话题节点中获取到移动机器人当前位置的正方向，并按照sensor_msgs：：image格式二进制图像转化成常用的cv；：mat类型的图像数据，并使用opencv内嵌的图像orb特征提取算法计算出当前图像数据对应的第一描述子与第一关键点。
[0127]
具体的，在本实施例中，需要说明的是，模板图像集包括基于orb-slam3算法生成的当前移动机器人运行区域的关键帧图像集，以及相对于当前移动机器人地图原点的空间姿态，所述空间姿态由坐标x、y、z与四元数wx、wy、wz、z表示；
[0128]
所述实验区图像词典由所述关键帧图像集利用dbow3库以k叉树、l层深度、tf_idf权重计算方式、l1范数评分方式并使用kmeans 聚类生成；
[0129]
其中，所述tf_idf权重计算方式具体包括：
[0130]
tf_idf权重计算方式使用的公式如下：
[0131]
ηi＝tfi×
idfi；
[0132]
其中，tfi是指某个特征在单幅图像中出现的频率，ni是指某个特征在图像中出现的次数，n是指图像中的总特征数，idfi是指出现某个特征的图像数量占图像总数的比例，n表示图像词典中的总图像数，ni表示出现某个特征的图象数，ηi表示某一特征在所述实验区图像词典中的权重；
[0133]
其中，所述l1范数评分方式具体包括：
[0134]
使用l1范数计算特征向量之间的匹配程度；
[0135][0136]
其中，v1表示第一张图像的特征向量，v2表示第二张图像的特征向量，s表示两个向量之间的匹配度得分。
[0137]
步骤s31，读取模板图像集并加载出实验区图像词典；
[0138]
具体的，在本步骤中，当控制器获取到当前图像中的orb特征对应的第一关键点以及第一描述子时，该控制器会立即读取当前移动机器人内部预存好的模板图像集，并对应加载出实验区图像词典，其中，模板图像集和实验区图像词典都是在当前移动机器人中已经预设好的程序，控制器可以直接调出并使用，以执行步骤s41。
[0139]
步骤s41，计算出所述模板图像集中的orb特征的关键点集与描述子集，所述描述子集包括若干第二描述子，所述关键点集包括若干第二关键点；
[0140]
进一步的，在本步骤中，控制器会基于上述步骤s31读取出的模板图像集对应计算出当前模板图像集中的orb特征的关键点集与描述子集，具体的，上述描述子集包括若干第二描述子，上述关键点集包括若干第二关键点。
[0141]
步骤s51，对所述模板图像集中的全部图像提取出orb特征并获取到对应的关键点集与描述子集；将所述第一描述子以及所述描述子集中的各个第二描述子均通过所述实验区图像词典转化为对应的特征向量；根据所述l1范数特征向量间的匹配程度计算出对应的匹配得分；
[0142]
其中，描述子到特征向量的转换过程通过查询所述实验区图像词典完成，且特征向量描述式如下：
[0143]
v＝(n1，n2，
…
，n
k*l-1
，n
k*l
)；
[0144]
其中，v表示图像在所述实验区图像词典空间下的特征向量描述形式，ni表示所述实验区图像词典空间下的某一特征，向量维数由词典分叉数k与层数l的积决定，当描述子中存在所述实验区图像词典中的某个特征ni时，记录对应维度下的特征出现频数，否则当前维度为0。
[0145]
具体的，在本步骤中，通过对上述模板图像集中的全部图像提取出orb特征并获取到对应的关键点集与描述子集，进一步的，将上述第一描述子以及上述描述子集中的各个第二描述子均通过上述实验区图像词典转化为对应的特征向量，最后根据上述l1范数特征向量间的匹配程度即可有效的计算出对应的匹配得分。
[0146]
步骤s61，根据相似度得分的高低确定出匹配度最高的图像，并分别获取到匹配得分最高、次高对应的图像在所述模板图像集中的位置index
first
以及index
second
；
[0147]
通过判断|index
first-index
second
|≤10的结果来对匹配进行时间一致性约束，以消除误匹配对移动机器人带来的风险；
[0148]
在确定出匹配度最高的图像有效之后，读取当前匹配度最高的图像的图像数据，并进行orb特征提取，以获取到第二关键点keypoints
tem
；
[0149]
基于所述第一关键点keypomts以及所述相机的相机焦距、相机光心，根据对极几何约束出本质矩阵e＝t
×
r，并根据所述本质矩阵e恢复出两个视角的旋转矩阵r与平移矩阵t。
[0150]
具体的，在本步骤中，会进一步对获取到的图像进行处理，具体的，在本步骤中，会根据上述步骤s51计算出的相似度得分的高低确定出匹配度最高的图像，并对应分别获取到匹配得分最高、次高对应的图像在所述模板图像集中的位置，并且在确定出匹配度最高的图像有效之后，读取当前匹配度最高的图像的图像数据，并进行orb特征提取，以获取到第二关键点keypomts
tem
；
[0151]
基于所述第一关键点keypoints以及所述相机的相机焦距、相机光心，根据对极几何约束出本质矩阵e＝t
×
r，并根据所述本质矩阵e恢复出两个视角的旋转矩阵r与平移矩阵t，从而能够有效的获取到旋转矩阵r与平移矩阵t。
[0152]
步骤s71，基于所述旋转矩阵r以及所述平移矩阵t构造出变换矩阵t；读取出匹配度最高的图像空间的坐标及四元数，并将所述坐标和所述四元数转换成对应的变换矩阵t
tem
；将两变换矩阵t
tem
做矩阵乘法得到当前位置图像对应的地图坐标原点变换矩阵t
final
；将所述t
final
转换成四元数，构造geometry_msgs：：posewithcovariancestamped格式、名为initialpose的ros初始化位置消息。
[0153]
最后，在本步骤中，通过计算出的旋转矩阵r与平移矩阵t，再通过矩阵乘法可以有效的将当前移动机器人的当前位置转换至当前移动机器人的占用地图原点，以准确的发布ros初始化位置消息，进而可以快速、精准地估计移动机器人初始位姿。
[0154]
需要指出的是，本发明第二实施例所提供的方法，其实现原理及产生的一些技术效果和第一实施例相同，为简要描述，本实施例未提及之处，可参考第一实施例中相应内容。
[0155]
综上，本发明上述实施例当中的移动机器人初始位姿估计方法能够在较短时间内部署在基于ros控制的移动机器人上，于已知占用地图区域内可有效估计移动机器人位姿，并且对算法的算力要求低，适用于大范围的推广与使用。
[0156]
请参阅图2，所示为本发明第三实施例提供的移动机器人初始位姿估计系统，该移动机器人初始位姿估计系统具体包括：
[0157]
启动模块12，用于当初始化ros完成时，启用相机并通过所述相机发布rgb图像流话题节点；
[0158]
第一计算模块22，用于获取所述rgb图像流话题节点中的图像，并计算出所述图像中的orb特征的第一关键点与第一描述子；
[0159]
读取模块32，用于读取模板图像集并加载出实验区图像词典；
[0160]
第二计算模块42，用于计算出所述模板图像集中的orb特征的关键点集与描述子集，所述描述子集包括若干第二描述子，所述关键点集包括若干第二关键点；
[0161]
转换模块52，用于根据所述实验区图像词典将所述第一描述子和所述第二描述子均转换成对应的特征向量，并计算出所述特征向量之间的相似度得分；
[0162]
匹配模块62，用于从所述模板图像集中提取出匹配度最高的图像及对应的位姿，并计算出所述相机获取的图像与所述模板图像集中的匹配度最高的图像的旋转矩阵与平移矩阵；
[0163]
第三计算模块72，用于根据匹配度最高的图像对应的位姿计算出当前位置的空间姿态，并发布ros初始化位置消息。
[0164]
需要说明的是，上述各个模块可以是功能模块也可以是程序模块，既可以通过软件来实现，也可以通过硬件来实现。对于通过硬件来实现的模块而言，上述各个模块可以位于同一处理器中；或者上述各个模块还可以按照任意组合的形式分别位于不同的处理器中。
[0165]
其中，上述移动机器人初始位姿估计系统中，所述第一计算模块22具体用于：
[0166]
基于linux/ubuntu操作系统环境，通过cv::bridge在所述rgb图像流话题节点中获取到移动机器人当前位置的正方向；
[0167]
按照sensor_msgs::image格式二进制图像转化成常用的cv::mat类型的图像数据，并使用opencv内嵌的图像orb特征提取算法计算出所述图像数据对应的所述第一描述子与所述第一关键点。
[0168]
其中，上述移动机器人初始位姿估计系统中，所述模板图像集包括基于orb-slam3算法生成的当前移动机器人运行区域的关键帧图像集，以及相对于当前移动机器人地图原点的空间姿态，所述空间姿态由坐标x、y、z与四元数wx、wy、wz、z表示；
[0169]
所述实验区图像词典由所述关键帧图像集利用dbow3库以k叉树、l层深度、tf_idf权重计算方式、l1范数评分方式并使用kmeans 聚类生成；
[0170]
其中，所述tf_idf权重计算方式具体包括：
[0171]
tf_idf权重计算方式使用的公式如下：
[0172]
ηi＝tfi×
idfi；
[0173]
其中，tfi是指某个特征在单幅图像中出现的频率，ni是指某个特征在图像中出现的次数，n是指图像中的总特征数，idfi是指出现某个特征的图像数量占图像总数的比例，n表示图像词典中的总图像数，ni表示出现某个特征的图象数，ηi表示某一特征在所述实验区图像词典中的权重；
[0174]
其中，所述l1范数评分方式具体包括：
[0175]
使用l1范数计算特征向量之间的匹配程度；
[0176][0177]
其中，v1表示第一张图像的特征向量，v2表示第二张图像的特征向量，s表示两个向量之间的匹配度得分。
[0178]
其中，上述移动机器人初始位姿估计系统中，所述转换模块52具体用于：
[0179]
对所述模板图像集中的全部图像提取出orb特征并获取到对应的关键点集与描述子集；
[0180]
将所述第一描述子以及所述描述子集中的各个第二描述子均通过所述实验区图像词典转化为对应的特征向量；
[0181]
根据所述l1范数特征向量间的匹配程度计算出对应的匹配得分；
[0182]
其中，描述子到特征向量的转换过程通过查询所述实验区图像词典完成，且特征向量描述式如下：
[0183]
v＝(n1，n2，l，n
k*l-1
，n
k*l
)；
[0184]
其中，v表示图像在所述实验区图像词典空间下的特征向量描述形式，ni表示所述实验区图像词典空间下的某一特征，向量维数由词典分叉数k与层数l的积决定，当描述子中存在所述实验区图像词典中的某个特征ni时，记录对应维度下的特征出现频数，否则当前维度为0。
[0185]
其中，上述移动机器人初始位姿估计系统中，所述匹配模块62具体用于：
[0186]
根据相似度得分的高低确定出匹配度最高的图像，并分别获取到匹配得分最高、次高对应的图像在所述模板图像集中的位置index
first
以及index
second
；
[0187]
通过判断|index
first-index
second
|≤10的结果来对匹配进行时间一致性约束，以消除误匹配对移动机器人带来的风险；
[0188]
在确定出匹配度最高的图像有效之后，读取当前匹配度最高的图像的图像数据，并进行orb特征提取，以获取到第二关键点keypoints
tem
；
[0189]
基于所述第一关键点keypoints以及所述相机的相机焦距、相机光心，根据对极几何约束出本质矩阵e＝t
×
r，并根据所述本质矩阵e恢复出两个视角的旋转矩阵r与平移矩阵t。
[0190]
其中，上述移动机器人初始位姿估计系统中，所述第三计算模块72具体用于：
[0191]
基于所述旋转矩阵r以及所述平移矩阵t构造出变换矩阵t；
[0192]
读取出匹配度最高的图像空间的坐标及四元数，并将所述坐标和所述四元数转换成对应的变换矩阵t
tem
；
[0193]
将两变换矩阵t
tem
做矩阵乘法得到当前位置图像对应的地图坐标原点变换矩阵t
final
；
[0194]
将所述t
final
转换成四元数，构造geometry_msgs：：posewithcovariancestamped格式、名为initialpose的ros初始化位置消息。
[0195]
其中，上述移动机器人初始位姿估计系统中，所述启动模块12具体用于：
[0196]
将摄像头布置于移动机器人的前部，并朝所述移动机器人的正方向呈平视状态；
[0197]
通过usb type-c接口将所述摄像头与所述移动机器人的主控制器电性连接；
[0198]
将所述移动机器人的主控制器在linux/ubuntu操作系统环境下开启ros系统，并打开所述相机的ros节点，以发布名为/camera/color/image_raw的rgb图像流。
[0199]
本发明第四实施例提供了一种计算机，其包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述第一实施例或者第二实施例提供的汽车负载模拟测试方法。
[0200]
本发明第五实施例提供了一种机器人，其包括上述第三实施例提供的移动机器人初始位姿估计系统。
[0201]
综上所述，本发明上述实施例当中的移动机器人初始位姿估计方法、系统、计算机及机器人能够在较短时间内部署在基于ros控制的移动机器人上，于已知占用地图区域内可有效估计移动机器人位姿，并且对算法的算力要求低，适用于大范围的推广与使用。
[0202]
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用
于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。
[0203]
计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(ram)，只读存储器(rom)，可擦除可编辑只读存储器(eprom或闪速存储器)，光纤装置，以及便携式光盘只读存储器(cdrom)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
[0204]
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(pga)，现场可编程门阵列(fpga)等。
[0205]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0206]
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。