基于视觉传感器的多移动机器人协作建图方法及系统

1.本发明涉及移动机器人定位与地图构建技术领域，更具体的说，涉及一种基于视觉传感器的多移动机器人协作建图方法及系统。


背景技术：

2.在面对一些大规模场景或者场景相对复杂的作业环境，由于单个移动机器人只能根据其观测到的局部信息做出决策，并且还可能由于其自身传感器故障导致整个系统的瘫痪，因此，单个移动机器人在上述作业环境方面存在局限性，需要多机器人协作共同完成任务，以弥补单机器人的能力限制，进而提高整个系统的效率和鲁棒性。
3.在面向未知大范围环境探索、人员搜救、军事侦察等场景下，采用多机器人协作方式能够大大提高感知效率，从而克服单个移动机器人的感知局限，为后续的自主决策与路径规划奠定基础。


技术实现要素：

4.本发明的目的是提供一种基于视觉传感器的多移动机器人协作建图方法及系统，解决现有移动机器人在面对未知大范围环境下协同建图精度差、效率低的问题。
5.为了实现上述目的，本发明提供了一种基于视觉传感器的多移动机器人协作建图方法，包括以下步骤：
6.步骤s1、相机标定，获取相机内参数及畸变参数；
7.步骤s2、各子机器人分别独立运行视觉slam算法，获得自身位姿和局部地图；
8.步骤s3、各子机器人采用多机器人通讯机制将自身数据传输至服务端，所述多机器人通讯机制基于ros系统设计实现；
9.步骤s4、服务端基于公共区域相似性原理进行地图重叠区域检测，建立各子机器人的局部地图之间的数据关联；
10.步骤s5、服务端构建pnp问题，计算得出局部地图和全局地图之间的变换矩阵；
11.步骤s6、服务端将局部地图融合转换至全局地图，并进行全局集束优化；
12.步骤s7、服务端采用多机器人通讯机制将全局地图数据发送至各子机器人；
13.步骤s8、子机器人接收全局地图数据并进行解析，更新子机器人的本地局部地图数据。
14.在一实施例中，所述步骤s1，进一步包括：
15.采用ros系统中的相机标定工具包获取相机内参数以及相机畸变参数。
16.在一实施例中，所述步骤s2，进一步包括：
17.子机器人搭载rgb-d相机对输入图像提取orb特征点，在位姿跟踪线程中进行恒速模型估计、参考关键帧估计以及重定位步骤，初步计算出相机位姿，获得自身位姿数据；
18.子机器人在局部建图线程中根据所新建的关键帧，增加局部地图点数目，关键帧之间重新进行特征匹配，将未匹配的特征点插入到全局地图中，生成新的地图点，获得局部
点云地图。
19.在一实施例中，所述步骤s2，进一步包括：
20.子机器人在闭环检测线程中对所有的图像关键帧进行位姿优化。
21.在一实施例中，所述步骤s3，进一步包括：
22.各子机器人发布自身的关键帧和局部地图数据至服务端；
23.服务端通过订阅关键帧和地图数据的话题接收各子机器人发布数据。
24.在一实施例中，所述步骤s4，进一步包括：
25.步骤s41、服务端加载视觉词袋模型，初始化全局地图数据；
26.步骤s42、基于公共区域相似性原理确定各子机器人的局部地图之间的相似关键帧和候选关键帧。
27.在一实施例中，所述步骤s41，进一步包括：
28.服务端在初始化全局地图数据时加载视觉词袋模型；
29.检查当前地图数据是否为空，直到接收到第一帧由子机器人端传输过来的地图数据时确定全局坐标系，全局坐标系与所接收到第一帧地图数据对应的子机器人端的局部坐标系保持一致。
30.在一实施例中，所述步骤s42，进一步包括：
31.服务端对接收到的所有关键帧迭代遍历，并在存储全局地图关键帧的字典树中进行挑选；
32.选择一定数量的与当前全局地图关键帧具有相同特征的关键帧作为相似关键帧，在相似关键帧基础上根据地图点数目确定候选关键帧。
33.在一实施例中，所述步骤s5，进一步包括：
34.服务端遍历候选关键帧集合，对每个候选关键帧与当前全局地图关键帧之间构建pnp问题，计算出当前候选关键帧在全局坐标系下的位姿，结合候选关键帧在局部坐标下的位姿，计算出局部地图与全局地图之间的变换矩阵。
35.在一实施例中，所述步骤s5，进一步包括：采用epnp算法对pnp问题进行求解。
36.在一实施例中，所述步骤s6，进一步包括：
37.利用步骤s5获得的变换矩阵，将局部地图中的所有关键帧和地图点数据转换到全局地图坐标系下；
38.对全局地图中所有关键帧和所有地图点进行全局集束优化。
39.在一实施例中，所述步骤s7，进一步包括：
40.服务端将全局地图中的关键帧数据和地图点数据进行序列化，并进行数据包的拆分；
41.服务端基于多机器人通讯机制发布数据包的话题。
42.在一实施例中，所述步骤s8，进一步包括：
43.各个子机器人基于多机器人通讯机制进行接收地图数据；
44.各子机器人端对接收的地图数据进行反序列化从而得到对应的格式数据；
45.各子机器人利用变换矩阵将全局地图数据添加到自己局部地图的参考坐标系下，实现本地局部地图的实时更新。
46.为了实现上述目的，本发明提供了一种基于视觉传感器的多移动机器人协作建图
系统，其特征在于，包括多个子机器人和服务端：
47.所述多个子机器人，分别与服务端通信进行数据交互，
48.所述多个子机器人和服务端，用于实现如上述任一项所述的方法。
49.本发明提供的基于视觉传感器的多移动机器人协作建图方法及系统，从实际应用角度出发，实现局部地图向全局地图的高效融合及地图更新，可实现移动机器人在未知大范围环境下的自主地图构建，克服了单移动机器人建图效率低下的问题，提高在未知大范围环境下基于视觉传感器的环境感知效率，系统可用性及鲁棒性强，可应用于大规模作业场景下的环境感知任务。
附图说明
50.本发明上述的以及其他的特征、性质和优势将通过下面结合附图和实施例的描述而变的更加明显，在附图中相同的附图标记始终表示相同的特征，其中：
51.图1揭示了根据本发明一实施例的基于视觉传感器的多移动机器人协作建图方法流程图；
52.图2揭示了根据本发明一实施例中的基于视觉传感器的多移动机器人协作建图流程框图；
53.图3揭示了根据本发明一实施例的服务端全局地图融合算法流程图；
54.图4揭示了根据本发明一实施例的服务端地图重叠区域检测流程图；
55.图5揭示了根据本发明一实施例的根据本发明一实施例的基于视觉传感器的多移动机器人协作建图系统示意图；
56.图6揭示了根据本发明一实施例的仿真环境下的多移动机器人协作建图结果图。
57.图中各附图标记的含义如下：
58.11子机器人；
59.12子机器人；
60.1n子机器人；
61.20服务端。
具体实施方式
62.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释发明，并不用于限定发明。
63.本发明提出的一种基于视觉传感器的多移动机器人协作建图方法，应用于无人搜救和环境探索等领域，用于移动机器人自主定位与地图构建，对移动机器人在大范围未知环境下的环境感知具有推动意义。
64.图1揭示了根据本发明一实施例中的基于视觉传感器的多移动机器人协作建图方法流程图，图2揭示了根据本发明一实施例中的基于视觉传感器的多移动机器人协作建图流程框图，如图1和如2所示，本发明提出的基于视觉传感器的多移动机器人协作建图方法，采用集中式方案，具体包括以下步骤：
65.步骤s1、相机标定，获取相机内参数及畸变参数；
66.步骤s2、各子机器人分别独立运行视觉slam算法，获得自身位姿和局部地图；
67.步骤s3、各子机器人采用多机器人通讯机制将自身数据传输至服务端，所述多机器人通讯机制基于ros系统设计实现；
68.步骤s4、服务端基于公共区域相似性原理进行地图重叠区域检测，建立各子机器人的局部点云地图之间的数据关联；
69.步骤s5、服务端构建pnp问题，计算得出局部地图和全局地图之间的变换矩阵；
70.步骤s6、服务端将局部地图融合转换至全局地图，并进行全局集束优化；
71.步骤s7、服务端采用多机器人通讯机制将全局地图数据发送至各子机器人；
72.步骤s8、子机器人接收全局地图数据并进行解析，更新子机器人的本地局部地图数据。
73.本发明提出的一种基于视觉传感器的多移动机器人协作建图方法，采用集中式的多机器人协作架构，基于ros实现了多机器人节点数据通信，单台移动机器人搭载相同的视觉slam系统，分工协作完成自身局部子地图构建；服务器端基于公共区域相似性原理并采用视觉词袋模型技术建立局部地图间的数据关联，之后构建pnp问题求解局部地图到全局地图间的坐标变换矩阵从而实现局部地图融合，进一步对地图数据进行全局集束优化，以降低全局地图误差。
74.下文将对这些步骤进行详细描述。应理解，在本发明范围内，本发明的上述各技术特征和在下文(如实施例)中具体描述的各技术特征之间都可以互相组合，相互关联，从而构成优选的技术方案。
75.步骤s1、相机标定，获取相机内参数及畸变参数。
76.在本实施例中，对rgb-d相机标定，采用ros系统中开源的相机标定工具包获取相机内参数以及相机畸变参数。
77.相机内参数具体包括相机内参数f
x
，fy，c
x
，cy；
78.相机畸变参数具体包括径向畸变参数k1，k2，k3和切向畸变系数p1，p2。
79.步骤s2、各子机器人分别独立运行视觉slam算法，获得自身位姿和局部地图。
80.各子机器人独立运行自身视觉slam算法，获得其自身位姿和局部点云地图。
81.定位与建图(simultaneous localization and mapping，slam)是指载有特定传感器的主体在没有先验信息的情况下，通过计算特定传感器获取图像帧来估计自身运动的轨迹，并建立周围环境的地图，其广泛应用于机器人、无人机、自动驾驶、增强现实、虚拟现实等应用中。
82.本实施例中，子机器人端运行orb-slam2视觉slam算法。
83.如图2所示，子机器人11，...，子机器人1n分别独立运行视觉slam算法，对输入图像进行特征点提取与匹配，利用局部建图线程进行局部跟踪和后段优化，获得局部稀疏点云地图，同时利用闭环检测线程来进行全局位姿优化，更正漂移误差。
84.如图2所示，更进一步的，所述步骤s2，进一步包括：
85.子机器人搭载rgb-d相机对输入图像提取orb(oriented fast and rotated brief，快速导向与简要旋转)特征点，在位姿跟踪线程中进行恒速模型估计、参考关键帧估计以及重定位步骤，初步计算出相机位姿，获得自身位姿数据；
86.子机器人在局部建图线程中根据所新建的关键帧，增加局部地图点数目，同时关
键帧之间重新进行特征匹配，将未匹配的特征点插入到全局地图中，以此生成新的地图点，获得局部点云地图。
87.更进一步的，子机器人在闭环检测线程中对所有的图像关键帧进行位姿优化，以减少相机运动过程中所产生的累计和漂移误差。
88.步骤s3、各子机器人采用多机器人通讯机制将自身数据传输至服务端，所述多机器人通讯机制基于ros系统设计实现。
89.基于ros系统设计实现多机器人通讯机制，子机器人端传输数据给服务端。
90.如图2所示，子机器人11，...，子机器人1n与服务端20之间基于ros系统中的发布订阅话题的通讯方式传输数据包。
91.所述ros系统是用于编写机器人软件程序的一种分布式的软件架构，可提供多节点之间的通信服务。
92.更具体的说为，基于ros系统中的发布订阅话题的通讯方式为：
93.整个ros系统由ros主节点进行管理发布者节点和订阅者节点，发布者与订阅者基于rpc协议向ros主节点注册其节点信息以及要发布和订阅的话题信息，ros主节点会将相同话题的节点绑定在一起，使订阅者能够发现发布者的地址信息。发布者和订阅者之间基于tcp协议建立网络连接通道，订阅者便可订阅由发布者发送的消息数据，实现数据通信。
94.子机器人端发布自身的关键帧和地图数据给服务端，服务端通过订阅关键帧和地图数据的话题接收子机器人端的数据。
95.在图2所示的实施例中，子机器人11，...，子机器人1n发布自身的关键帧和地图数据给服务端20，服务端20通过订阅关键帧和地图数据的话题接收子机器人11，...，子机器人1n的数据。
96.图3揭示了根据本发明一实施例的服务端全局地图融合算法流程图，图4揭示了根据本发明一实施例的服务端地图重叠区域检测流程图，结合图3和图4详细说明本发明中的步骤s4-步骤s6。
97.步骤s4、服务端基于公共区域相似性原理进行地图重叠区域检测，建立各子机器人的局部地图之间的数据关联。
98.各子机器人在单独进行slam算法构建局部地图过程中，会形成所有关键帧地图点的集合。如果局部地图与全局地图之间有重复观测过的环境区域，那么此场景下的orb特征点会有较大的相似度。服务端为每个局部地图以及全局地图建立关键帧数据库，数据库中的索引存储视觉单词的标识。
99.如图4所示，所述步骤s4，进一步包括以下步骤：
100.s41、全局地图数据初始化。
101.服务端在初始化时加载视觉词袋模型，首先检查当前地图数据是否为空，直到接收到第一帧由子机器人端传输过来的地图数据，此时，便可确定全局坐标系，其坐标系与所接收到地图数据对应的子机器人端的局部地图坐标系保持一致。
102.s42、确定相似关键帧和候选关键帧。
103.随着接收到的地图数据越来越多，当子机器人曾经过公共区域，服务端即可进行地图重建区域检测，其对接收到的局部地图的所有关键帧迭代遍历，并选择出与当前帧具有一定数量和相同特征的关键帧作为相似关键帧，在相似关键帧基础上根据地图点数据确
定候选关键帧。
104.在本实施例中，服务端对接收到的局部地图的所有关键帧迭代遍历，并在存储全局地图关键帧的字典树中进行挑选，选择出与当前帧具有一定数量和相同特征的关键帧作为相似关键帧并存储到对应集合中，进一步在此集合中依据匹配地图数量超过20的原则确定候选关键帧。
105.在本实施例中，服务端在进行特征库中的遍历时，通过使用基于视觉词袋模型，可以高效地查询关键帧数据库，从而找到公共区域。
106.所述视觉词袋模型为对已有图像进行orb特征提取并通过聚类算法将重复的特征聚集在一起进而形成词袋库，词袋通过树形结构进行存储，其查询的时间复杂度为log(n)，之后利用聚类算法生成字典树。
107.采用tf-idf(term frequency-inverse document frequency)指数作为每个叶子节点的权重值，具有很好的区分能力。包含视觉单词越少的图像，其idf值就越大。
108.对于第k个节点的tf和idf的计算如下式所示：
[0109][0110][0111]
tf-idf＝tfk×
idfk[0112]
其中，tf表示单词频率，idf表示逆向文本频率。
[0113]
步骤s5、构建pnp问题，计算得出局部地图和全局地图之间的变换矩阵。
[0114]
如图4所示，遍历候选关键帧集合，对每个候选关键帧与当前帧之间建立pnp(perspective-n-point)问题，通过求解器计算出当前帧在全局坐标系下的位姿tn，结合候选关键帧在局部坐标下的位姿to，便可计算出局部地图与全局地图间的变换矩阵
[0115]
在本实施例中，所述pnp问题可描述为在已知关键帧k
α
与候选关键帧kg共同匹配到的地图点p在全局坐标下的位姿以及地图点p在已知关键帧k
α
中的二维投影位置，需要求解已知关键帧k
α
在全局地图坐标系下的位姿。
[0116]
在本实施例中，通过采用epnp算法求解pnp问题。
[0117]
所述epnp算法包括以下步骤：
[0118]
首先，选择4个控制点构成质心坐标系，计算世界坐标系下每个3d点用4个控制点线性表达时的系数α；
[0119]
其次，根据系数α与3d-2d匹配对中的2d点，计算出控制点在相机坐标系下的3d坐标；
[0120]
最后，根据3d坐标与之前的2d-3d匹配对内的3d坐标，构建3d-3d的求解问题，用迭代最近点icp算法进行求解。
[0121]
步骤s6、服务端将局部地图融合转换至全局地图，并进行全局集束优化。
[0122]
服务端进行局部地图融合，并进行全局集束优化。
[0123]
利用步骤s5获得的变换矩阵可将局部地图中的所有关键帧和地图点数据转换到全局地图坐标系下；
[0124]
对全局地图中所有关键帧和所有地图点进行全局集束优化，从而降低全局地图误差。
[0125]
所述全局集束优化，是指每一个匹配好的特征点建立方程，联立形成超定方程并求解得到最优的位姿矩阵和三维空间点坐标。
[0126]
步骤s7、服务端采用多机器人通讯机制将全局地图数据发送至各子机器人。
[0127]
服务端分发关键帧数据和地图点数据。
[0128]
服务端将全局地图中的关键帧数据和地图点数据进行序列化，进行数据包的拆分，之后服务端基于ros系统的多机器人通讯机制发布数据包的话题。
[0129]
步骤s8、子机器人接收全局地图数据并进行解析，更新子机器人的本地局部地图数据。
[0130]
子机器人端接收地图数据并进行解析，更新子机器人所维护的地图数据。
[0131]
各子机器人可订阅这个话题进行接收数据。
[0132]
各子机器人端接收完服务端广播的地图数据后，对数据进行反序列化从而得到对应的格式数据。
[0133]
各子机器人利用先前计算出的变换矩阵将全局地图数据添加到自己局部地图的参考坐标系下，实现本地局部地图的实时更新。
[0134]
图5揭示了根据本发明一实施例的根据本发明一实施例的基于视觉传感器的多移动机器人协作建图系统示意图，如图5所示的实施例中，采用一台计算资源丰富的处理器作为服务端20，子机器人11和子机器人12作为客户端。
[0135]
如图5所示的集中式方案中，各子机器人11和子机器人12之间相互独立，仅和作为计算中心的服务端20进行信息数据交互，在服务端20对接收到的数据包进行更复杂的操作，有利于构建全局一致的地图，在系统搭建过程中复杂度低，易于数据融合的计算与结果分发。
[0136]
子机器人11、子机器人12和服务端20用于实现如图1至图4所示的基于视觉传感器的多移动机器人协作建图方法。
[0137]
总体来说，采用集中式的多机器人协作技术方案在实际应用中更加易于实现，且在数据通信实施以及数据一致性方面更具有优势。
[0138]
图6揭示了根据本发明一实施例的仿真环境下的多移动机器人协作建图结果图，如图6所示的实施例中，同样采用子机器人11、子机器人12和服务端20。
[0139]
如图6所示，在仿真环境下子机器人11、子机器人12协作建图之后服务端20的全局地图结果，以及子机器人11、子机器人12各自所维护的地图结果。
[0140]
服务端20中的两条灰色实线分别为两台子机器人的实际运动轨迹，黑色点云为服务端20所维护的全局点云地图，包含了子机器人11和子机器人12所维护的地图数据。
[0141]
同时，子机器人11、子机器人12也维护了其自身建立的局部地图与从服务端20接收到更新后的点云地图数据，子机器人11、子机器人12与服务端20共同拥有全局一致的环境点云地图。
[0142]
本发明提供的基于视觉传感器的多移动机器人协作建图方法及系统，从实际应用角度出发，实现局部地图向全局地图的高效融合及地图更新，可实现移动机器人在未知大范围环境下的自主地图构建，克服了单移动机器人建图效率低下的问题，提高在未知大范
围环境下基于视觉传感器的环境感知效率，系统可用性及鲁棒性强，可应用于大规模作业场景下的环境感知任务。
[0143]
尽管为使解释简单化将上述方法图示并描述为一系列动作，但是应理解并领会，这些方法不受动作的次序所限，因为根据一个或多个实施例，一些动作可按不同次序发生和/或与来自本文中图示和描述或本文中未图示和描述但本领域技术人员可以理解的其他动作并发地发生。
[0144]
如本技术和权利要求书中所示，除非上下文明确提示例外情形，“一”、“一个”、“一种”和/或“该”等词并非特指单数，也可包括复数。一般说来，术语“包括”与“包含”仅提示包括已明确标识的步骤和元素，而这些步骤和元素不构成一个排它性的罗列，方法或者设备也可能包含其他的步骤或元素。
[0145]
上述实施例是提供给熟悉本领域内的人员来实现或使用本发明的，熟悉本领域的人员可在不脱离本发明的发明思想的情况下，对上述实施例做出种种修改或变化，因而本发明的保护范围并不被上述实施例所限，而应该是符合权利要求书提到的创新性特征的最大范围。