一种基于动态实时视觉SLAM方法的移动机器人

一种基于动态实时视觉slam方法的移动机器人
技术领域
1.本发明涉及人工智能技术领域，具体为一种基于动态实时视觉slam方法的移动机器人。


背景技术：

2.现有的移动机器人技术中，为了能够构建出工作环境的高精度地图，通常采取的是同时定位与地图构建(simultaneous localization and mapping，slam)技术，通过采集外部传感器数据来进行机器人位姿状态的估计，并对周围环境进行地图构建；然而，传统的动态视觉slam算法大致可以分为两类，一类以几何检测(多视几何、几何聚类、光流等)为主，另一类以图像分割(mask r-cnn、segnet等)为主。基于几何检测的动态视觉slam算法虽在运行效率上存在优势，但在高动态场景下却无法准确分离动态对象，定位精度不够理想；基于深度学习的动态视觉slam算法是目前动态slam算法的主流方向，它以图像分割算法为主、几何检测为辅，但由于图像分割网络本身十分耗时，在动态环境下无法实时地完成机器人的位姿估计，所以无法在环境复杂且需实时定位的场景中应用，从而导致移动机器人在现实生活中的应用受到了限制。


技术实现要素：

3.为了解决现有基于slam技术的移动机器人在动态环境下无法实时准确地完成机器人的位姿估计的问题，本发明提供一种基于动态实时视觉slam方法的移动机器人，其不仅能够实现对未知的环境进行感知与理解，且能够在动态环境下进行实时位姿估计，使移动机器人能够更灵活的适用于不同的应用场景。
4.本发明的技术方案是这样的：一种基于动态实时视觉slam方法的移动机器人，其包括：图像采集模块、动态视觉slam模块和控制器，其特征在于：
5.所述动态视觉slam模块包括：视觉slam模块和图像分割模块；
6.所述视觉slam模块、所述图像分割模块分别与图像采集模块通信连接；
7.所述控制器通过串口通信连接所述动态视觉slam模块；
8.所述图像采集模块获取环境的rgb图像信息和深度信息后，将rgb图像信息和深度信息以rgb图像序列和深度图像序列的方式进行传输，记作待处理数据；所述图像采集模块将所述待处理数据分别发送到所述视觉slam模块和所述图像分割模块中；
9.所述待处理数据送入所述视觉slam模块中进行识别操作；所述视觉slam模块中对待处理数据的处理操作流程与所述图像分割模块中对所述待处理数据的操作流程并行进行；
10.所述视觉slam模块包括：顺序连接的lucas-kanade稀疏光流模块、二次剔除模块、特征匹配位姿估计模块；
11.所述图像分割模块中对所述待处理数据中的rgb图像帧通过语义分割网络进行目标检测，得到动态对象的语义分割图像后，再进行阈值分割处理，最终得到得到只有动态对
象的动态图像帧，并将所述动态图像帧送入到所述视觉slam模块中的所述二次剔除操作中；
12.所述lucas-kanade稀疏光流模块模接收所述图像采集模块输入的当前rgb图像帧提取orb特征点后，基于lucas-kanade稀疏光流法进行异常特征点的剔除后，将所述待处理数据的orb特征点送入所述二次剔除模块中，进行二次剔除操作；
13.所述二次剔除模块中设置动态特征点容器，在进行所述二次剔除操作前，先确认是否接收到了所述图像分割模块发出的新的动态图像帧；
14.如果接收到了，则将新接收的动态图像帧记作：newimg，使用newimg对所述动态特征点容器进行更新，再使用所述动态特征点容器对所述待处理的orb特征点进行二次剔除操作；
15.如果没有接收到动态图像帧更新，则直接利用所述动态特征点容器中已有的特征点，对所述待处理数据的orb特征点进行所述二次剔除操作；
16.所述二次剔除模块将进行所述二次剔除操作之后的所述待处理数据的orb特征点送入到所述特征匹配位姿估计模块中；
17.在所述特征匹配位姿估计模块中，对所述待处理数据的orb特征点进行特征匹配，进行机器人的位姿估计得到位姿估计，并构建出环境的三维点云地图；
18.将所述位姿估计结果、所述三维点云地图发送给所述控制器，所述控制器基于所述位姿估计结果、所述三维点云地图，进行二维占据栅格地图的构建，根据期望目标位置，完成机器人的轨迹规划，并控制机器人运动。
19.其进一步特征在于：
20.所述lucas-kanade稀疏光流模块中进行的异常特征点的剔除流程，具体包括以下步骤：
21.a1：所述lucas-kanade稀疏光流模块接收所述图像采集模块输入的当前rgb图像帧，记作previmg；
22.a2：所述lucas-kanade稀疏光流模块对待处理的图像帧previmg进行orb特征点的检测，提取得到 orb特征点，完成稀疏光流点的初始化，并将previmg的图像帧与特征点的坐标数据进行保存；
23.a3：接收previmg的下一组所述rgb图像帧，记作：nextimg；
24.a4：基于lucas-kanade稀疏光流法，对previmg和nextimg计算光流，进行orb特征点跟踪，得到每个orb特征点的跟踪状态status；
25.a5：计算每个所述orb特征点的误差值errors；
26.将每一个误差值errors与预设的误差值阈值errors_value进行比较；
27.如果所述orb特征点跟踪成功，且误差值errors大于误差值阈值errors_value，则所述orb特征点为设置异常特征点；删除所述异常特征点；
28.直至nextimg的orb特征点都参与过计算，执行步骤a6；
29.a6：比较nextimg剩余的所述orb特征点的数量和预设的特征点数量阈值，
30.所述特征点数量阈值表示在进行orb特征点跟踪时必须满足的特征点数量；
31.当nextimg剩余的所述orb特征点的数量小于所述特征点数量阈值时，对nextimg进行orb特征点检测，将得到的orb特征点放入nextimg的特征点集合中；
32.a7：将nextimg对应的orb特征点送入所述二次剔除模块中；
33.a8：将nextimg定义为previmg，循环执行步骤a3～a7；
34.所述二次剔除模块中进行的二次剔除操作，具包括以下步骤：
35.b1：接收所述lucas-kanade稀疏光流模块送入的所述待处理数据的orb特征点；
36.b2：确认是否接收到了所述图像分割模块发出的新的动态图像帧；
37.如果接收到了，则将新接收的动态图像帧newimg，对动态图像帧newimg进行orb特征提取，将提取到的特征点记作：neworb，执行步骤b3；
38.否则，直接执行步骤b6；
39.b3：读取所述二次剔除模块中现存的动态特征点，记作：curorb；
40.计算neworb和curorb之间的汉明距离d：
[0041][0042]
其中，d(a,b)表示a和b两特征点之间的汉明距离；ai和bi分别为a和b两特征点第i对点对的二进制描述符；
[0043]
b4：将neworb中每个特征点对应的汉明距离d与预设的距离阈值d_value进行比较；
[0044]
当d《d_value时，判定对应的特征点与现存动态特征点重复，从neworb中删除；
[0045]
b5：将剩余的neworb放入curorb中；
[0046]
b6：利用动态特征点容器对所述待处理数据的orb特征点进行二次剔除；
[0047]
所述二次剔除的过程中，计算动态特征点容器中特征点与所述待处理数据的orb特征点的汉明距离，记作d_c，将d_c与预设的距离阈值d_cvalue进行比较；
[0048]
其中，距离阈值d_cvalue，设置为二次剔除操作中剔除掉20％的所述待处理数据的orb特征点的值；
[0049]
b7：将二次剔除后的待处理数据的orb特征点，送入所述特征匹配位姿估计模块中；
[0050]
所述图像分割模块中对所述待处理数据中的rgb图像帧进行分割的过程，包括以下步骤：
[0051]
c1：基于mask r-cnn模型构建语义分割网络模型；
[0052]
c2：接收所述图像采集模块发送的对所述待处理数据中的rgb图像帧，记作：待处理图像帧；
[0053]
c3：将所述待处理图像帧送入所述语义分割网络模型中进行目标检测，得到动态对象的语义分割图像；
[0054]
c4：获取所述语义分割图像的阈值分割图；
[0055]
c5：将所述阈值分割图与对应的所述待处理图像帧进行图像的与运算，对非动态区域进行掩膜遮盖，即得到仅存动态对象的所述动态图像帧。
[0056]
本发明提供的一种基于动态实时视觉slam方法的移动机器人，其通过设置图像分割模块和视觉 slam模块，将图像采集模块获取的环境图像数据分别输入到图像分割模块和视觉slam模块中，将图像分割处理和位姿估计操作并行处理，通过在lucas-kanade稀疏光流模块、二次剔除模块中分别进行异常点的剔除，图像分割模块输出的动态图像帧在二
次剔除模块中参与第二次异常点剔除的操作，不但提高识别准确率，而在确保位姿估计精度的基础上，极大地提高了处理效率，确保本发明技术方案能够在动态环境下进行实时位姿估计，使移动机器人能够更灵活的适用于不同的应用场景。
附图说明
[0057]
图1为本发明移动机器人的模块示意图；
[0058]
图2为动态实时视觉slam算法框架图；
[0059]
图3为动态图像帧阈值分割示意图；
[0060]
图4为二次剔除模块流程示意图；
[0061]
图5为算法耗时实验中的跟踪线程单帧耗时图。
具体实施方式
[0062]
本发明提供一种基于动态实时视觉slam方法的移动机器人，如图1所示，其包括：图像采集模块、动态视觉slam模块、控制器和供电模块。动态视觉slam模块包括：视觉slam模块和图像分割模块。视觉slam模块、图像分割模块通过ros话题通信与图像采集模块相连接；控制器通过串口通信连接动态视觉slam模块。供电模块为其他所有模块供电。其他机器人部件基于现有技术中的模块实现。
[0063]
如图2所示，图像采集模块获取环境的rgb图像信息和深度信息后，将rgb图像信息和深度信息以 rgb图像序列和深度图像序列的方式进行传输，记作待处理数据；图像采集模块将待处理数据分别发送到视觉slam模块和图像分割模块中。
[0064]
在视觉slam模块中，动态实时视觉slam算法的主要工作步骤，包括：依次进行的跟踪线程、局部建图、回环检测和构建三维点云地图。
[0065]
待处理数据送入视觉slam模块中进行识别操作；视觉slam模块中对待处理数据的处理操作流程与图像分割模块中对待处理数据的操作流程并行进行；视觉slam模块包括：顺序连接的lucas-kanade 稀疏光流模块、二次剔除模块、特征匹配位姿估计模块。如图2所示，图像采集模块、顺序连接的 lucas-kanade稀疏光流模块、二次剔除模块和特征匹配位姿估计模块的操作，构成了一个完整的跟踪线程，将图像分割模块独立于跟踪线程之外，跟踪线程无需等待图像分割模块对当前帧的分割结果，直接使用已有的动态特征点进行匹配，极大减少了跟踪线程的耗时，提高系统的实时性。
[0066]
图像分割模块中对待处理数据中的rgb图像帧通过语义分割网络进行目标检测，得到动态对象的语义分割图像后，再进行阈值分割处理，最终得到得到只有动态对象的动态图像帧，并将动态图像帧送入到跟踪线程中的二次剔除操作中。
[0067]
在跟踪线程中，lucas-kanade稀疏光流模块模接收图像采集模块输入的当前rgb图像帧提取orb 特征点后，基于lucas-kanade稀疏光流法进行异常特征点的剔除后，将待处理数据的orb特征点送入二次剔除模块中，进行二次剔除操作。
[0068]
二次剔除模块中设置动态特征点容器，在进行二次剔除操作前，先确认是否接收到了图像分割模块发出的新的动态图像帧；
[0069]
如果接收到了，则将新接收的动态图像帧记作：newimg，使用newimg对动态特征点容器进行更新，再使用动态特征点容器对待处理的orb特征点进行二次剔除操作；
[0070]
如果没有接收到动态图像帧更新，则直接利用动态特征点容器中已有的特征点，对待处理数据的orb 特征点进行二次剔除操作；
[0071]
二次剔除模块将进行二次剔除操作之后的待处理数据的orb特征点送入到特征匹配位姿估计模块中。
[0072]
在特征匹配位姿估计模块中，对待处理数据的orb特征点进行特征匹配，进行机器人的位姿估计得到位姿估计，并构建出环境的三维点云地图；将位姿估计结果、三维点云地图发送给控制器，控制器基于位姿估计结果，进行二维占据栅格地图的构建，根据期望目标位置，完成机器人的轨迹规划，并控制机器人运动。
[0073]
lucas-kanade稀疏光流法基于三大假设：一是亮度恒定，即同一目标在不同帧的亮度不会变化；二是运动微小，即相邻帧的运动较小；三是空间一致，即目标区域内的像素点有相似的运动。lk稀疏光流法通过对连续帧图像中特征点进行跟踪，计算各特征点光流矢量，由此判断其中是否存在异常特征点。
[0074]
依据亮度恒定与运动微小两个假设，得到图像的约束方程为：
[0075]ix
u+iyv+i
t
＝0
[0076]
其中，u、v分别为像素点沿x和y轴的速度矢量；i
x
、iy分别为图像在x和y轴方向的梯度；i
t
为时间方向的梯度。
[0077]
根据空间一致性假设，即邻域内的光流为固定值，邻域内像素点具有相似的运动，选取的邻域范围，邻域内所有像素点可由下式表示：
[0078][0079]
式中，p为邻域内像素点；
[0080]
本发明技术方案中选取的跟踪点为orb特征点，orb特征点为变化明显的角点，可以有效避免出现孔径问题，进而避免无法判断位移点方向的问题发生；通过最小二乘法进行拟合优化，最终可以求解得速度矢量为：
[0081][0082]
lucas-kanade稀疏光流模块中进行的异常特征点的剔除流程，具体包括以下步骤：
[0083]
a1：lucas-kanade稀疏光流模块接收图像采集模块输入的当前rgb图像帧，记作previmg；
[0084]
a2：lucas-kanade稀疏光流模块对待处理的图像帧previmg进行orb特征点的检测，提取得到orb 特征点，完成稀疏光流点的初始化，并将previmg的图像帧与特征点的坐标数据进行保存；
[0085]
a3：接收previmg的下一组rgb图像帧，记作：nextimg；
[0086]
a4：基于lucas-kanade稀疏光流法，调用calcopticalflowpyrlk()函数，对previmg和nextimg计算光流，进行orb特征点跟踪，得到每个orb特征点的跟踪状态status；
[0087]
a5：计算每个orb特征点的误差值errors；
[0088]
将每一个误差值errors与预设的误差值阈值errors_value进行比较；
[0089]
如果orb特征点跟踪成功，且误差值errors大于误差值阈值errors_value，则orb特征点为设置异常特征点；删除异常特征点；
[0090]
直至nextimg的orb特征点都参与过计算，执行步骤a6；
[0091]
a6：比较nextimg剩余的orb特征点的数量和预设的特征点数量阈值，
[0092]
特征点数量阈值表示在进行orb特征点跟踪时必须满足的特征点数量；
[0093]
当nextimg剩余的orb特征点的数量小于特征点数量阈值时，对nextimg进行orb特征点检测，将得到的orb特征点放入nextimg的特征点集合中；
[0094]
a7：将nextimg对应的orb特征点送入二次剔除模块中；
[0095]
a8：将nextimg定义为previmg，循环执行步骤a3～a7。
[0096]
在lucas-kanade稀疏光流模块中，由于每次对输入图像帧进行异常特征点的剔除后，图像都会损失一部分orb特征点，使得特征点数目随着跟踪时间逐渐减少，最终因过少的特征点导致跟踪丢失、路径不完整等。为此本技术方案在异常特征点剔除之后，设置了特征点数量阈值，当提出异常特征点的图像帧特征点数量低于特征点数量阈值时，就对当前图像帧再次进行orb特征点检测，更新当前帧图像的特征点集合，保证充足的特征点用于跟踪。
[0097]
本发明的技术方案中，基于orb特征点与lucas-kanade稀疏光流法，利用opencv库 calcopticalflowpyrlk()函数，实现了异常特征点的剔除，并将其融入跟踪线程之中，以实现潜在动态特征点的初步剔除。
[0098]
本发明技术方案中，二次剔除模块是利用图像分割模块的语义分割结果，对输入图像帧的潜在动态特征点进行二次剔除；如图4所示，二次剔除模块首先判断是否接收到图像分割模块的动态图像帧，如果没有动态图像帧更新，则直接利用已有的动态图像帧进行二次剔除；如果存在动态图像帧更新，则对接收到的动态图像帧进行处理后，再进行后续的二次剔除。
[0099]
二次剔除模块中进行的二次剔除操作，具体包括以下步骤：
[0100]
b1：接收lucas-kanade稀疏光流模块送入的待处理数据的orb特征点；
[0101]
b2：确认是否接收到了图像分割模块发出的新的动态图像帧；
[0102]
如果接收到了，则将新接收的动态图像帧newimg，对动态图像帧newimg进行orb特征提取，将提取到的特征点记作：neworb，执行步骤b3；
[0103]
否则，直接执行步骤b6；
[0104]
b3：读取二次剔除模块中现存的动态特征点，记作：curorb；
[0105]
计算neworb和curorb之间的汉明距离d：
[0106][0107]
其中，d(a,b)表示a和b两特征点之间的汉明距离；ai和bi分别为a和b两特征点第i对点对的二进制描述符；
[0108]
b4：将neworb中每个特征点对应的汉明距离d与预设的距离阈值d_value进行比较；
[0109]
当d《d_value时，判定对应的特征点与现存动态特征点重复，从neworb中删除；
[0110]
通过距离阈值d_value，将距离过小的特征点，即相似的动态特征点进行剔除，降低整体计算量；
[0111]
b5：将剩余的neworb放入curorb中；
[0112]
b6：利用动态特征点容器对待处理数据的orb特征点进行二次剔除；
[0113]
二次剔除的过程中，计算动态特征点容器中特征点与待处理数据的orb特征点的汉明距离，记作d_c，将d_c与预设的距离阈值d_cvalue进行比较；
[0114]
其中，距离阈值d_cvalue，设置为二次剔除操作中剔除掉20％的待处理数据的orb特征点的值，确保将异常的特征点剔除，又能够有足够的特征点参与后续位姿估计的计算；
[0115]
b7：将二次剔除后的待处理数据的orb特征点，送入特征匹配位姿估计模块中。
[0116]
如图3所示，图像分割模块中对待处理数据中的rgb图像帧进行分割的过程，包括以下步骤：
[0117]
c1：基于mask r-cnn模型构建语义分割网络模型；
[0118]
c2：接收图像采集模块发送的对待处理数据中的rgb图像帧，记作：待处理图像帧；
[0119]
c3：将待处理图像帧送入语义分割网络模型中进行目标检测，得到动态对象的语义分割图像；
[0120]
c4：获取语义分割图像的阈值分割图；
[0121]
将语义分割图像的动态区域像素值置0，其余区域像素值置255，得到动态区域为白色、其余区域为黑色的阈值分割图；
[0122]
c5：将阈值分割图与对应的待处理图像帧进行图像的与运算，对非动态区域进行掩膜遮盖，即得到仅存动态对象的动态图像帧；
[0123]
调用opencv库的cv2.bitwise_and()函数，将阈值分割图与输入图像帧进行图像的与运算，对非动态区域进行掩膜遮盖，得到仅存动态对象的动态图像帧，图像二值化运算如下式：
[0124][0125]
其中，i
input
、i
output
和i
mask
分别为输入图像帧、动态图像帧和阈值分割图中目标位置像素值。
[0126]
本发明技术方案中的图像分割模块中构建的语义分割网络模型基于mask r-cnn实例分割网络，确保语义分割模型具有高准确率、简单直观、易于使用等优点。具体实现时，mask r-cnn网络在coco数据集上进行预训练，该数据集包含八十余类动态对象，满足实验要求。基于本发明技术方案中的语义分割网络模型实例分割可以预测像素级的语义标签，并为输入图像的动态对象生成掩膜。
[0127]
为了验证本发明技术方案的有效性和可行性，选取tum数据集进行实验，与现有技术中的 orb-slam2、refusion、dyna-slam几种视觉识别方法进行了比较，从精度和运行帧率等方面进行定量分析。
[0128]
实验平台为搭载ubuntu16.04操作系统的笔记本，运行内存8gb，处理器型号：i7-8550u，主频 1.8ghz，64位操作系统，使用一张nvidia geforce mx 150显卡。tum数据集提供序列对齐的rgb图像和深度图像，可直接进行点云分割、位姿估计与三维重建等操作，该
数据图像分辨率为640
×
480。实验选取了tum rgb-d freiburg3数据集下的walking_static、walking_xyz、walking_rpy、walking_halfsphere、 sitting_xyz、sitting_halfsphere六组子数据集，其中walking和sitting分别表示高动态和低动态场景的数据集。实验选取绝对轨迹误差作为评判标准，对比实验的具体结果如下面表1所示。
[0129]
表1绝对轨迹误差(单位：m)
[0130][0131]
由表1可知：本发明技术方案相比于orb-slam2算法，在低动态和高动态环境中，相机定位误差分别减少了22.14％和95.14％；与refusion算法相比，在低动态和高动态环境中，定位误差分别减少了74.32％和78.14％；相比于dyna-slam算法，实验效果接近；orb-slam2和refusion算法在低动态场景下相机的绝对轨迹误差较小，在高动态环境下误差较大，甚至出现丢帧情况；dyna-slam与本发明技术方案在两种动态环境下能尽可能地保留静态特征点，保证相机定位的精度，保持估计轨迹具有高吻合度。数据集实验表明本发明技术方案在动态环境下具有较好的定位精度和鲁棒性。
[0132]
除定位精度外，slam算法的实时性也是评价其系统优劣的重要性能指标，图5表示的是四种算法在 tum rgb-d各子数据集下，跟踪线程每帧图片的耗时情况。由图5可以看出，orb-slam2算法耗时最低，基于深度学习的dyna-slam算法耗时最高，在动态场景下耗时波动大，本发明的算法在耗时方面与 orb-slam2接近。下面表2记录了四种算法下跟踪线程在tum rgb-d各子数据集下的平均耗时情况。
[0133]
表2跟踪线程平均耗时(单位：ms)
[0134][0135]
由表2可知，本发明技术方案对跟踪线程进行改进，使得跟踪线程耗时受图像分割算法的影响小，能够有效减少算法跟踪线程的耗时，在保证定位精度的情况下，提高算法的实时性。
[0136]
在本发明的技术方案的动态实时视觉slam方法以orb-slam2算法为基础，与lucas-kanade稀疏光流法、mask r-cnn目标检测算法相结合，对环境中存在的动态潜在对象进行检测，实现对环境动态对象的感知，可以确保机器人能够更准确地感知与理解环境；对跟踪线程进行算法改进，对动态特征点进行实时剔除，准确高效地完成机器人位姿估计和三维点云地图的构建；控制器基于机器人位姿估计信息、三维点云地图可以更准确的完成路径规划；本专利的技术方案，可以确保机器人能够更加灵活的使用在各种存在动态对象的复杂场景中。