一种基于特征点法的清洁机器人视觉SLAM系统的制作方法

一种基于特征点法的清洁机器人视觉slam系统
技术领域
1.本发明涉及计算机视觉技术领域，尤其涉及一种基于特征点法的清洁机器人视觉slam系统。


背景技术：

2.近年来，slam(simultaneous localizationand mapping)即同步定位与建图技术，在清洁机器人上发挥了至关重要的作用，视觉slam系统根据提取信息方式的不同可以将其分为基于特征点法和直接法的视觉slam系统，现有的基于特征点法的视觉slam系统追踪的特征点较少，导致会有较大的累计误差，局部建图的精准性、slam系统的精度和鲁棒性不够优秀，不能广泛运用。


技术实现要素：

3.本发明的目的在于提供一种基于特征点法的清洁机器人视觉slam系统，旨在解决现有的视觉slam系统累计误差大、定位精准性不足的问题。
4.为实现上述目的，本发明提供了一种基于特征点法的清洁机器人视觉slam系统，包括视觉特征提取模块、特征匹配模块、相机位姿估计模块、回环检测模块和动态避障模块；
5.所述视觉特征提取模块、所述特征匹配模块、所述相机位姿估计模块、所述回环检测模块和所述动态避障模块依次连接；
6.所述视觉特征提取模块用于提取相机拍摄图像的图像特征点；
7.所述特征匹配模块用于对所述图像特征点进行特征匹配，得到两张图像上的匹配结果；
8.所述相机位姿估计模块用于根据所述匹配点估计相机位姿；
9.所述回环检测模块用于进行回环检测，根据回环信息对相机位姿进行纠正，得到全局一致的姿态估计；
10.所述动态避障模块用于规划移动路径。
11.其中，所述动态避障模块包括全局规划单元和局部规划单元；所述全局规划单元和所述局部规划单元连接；
12.所述全局规划单元用于规划全局移动路径；
13.所述局部规划单元用于基于所述全局移动路径，规划局部移动路径。
14.其中，所述视觉特征提取模块采用卷积神经网络结构提取图像特征点。
15.其中，所述特征匹配模块采用orb算法提取相机拍摄图像的图像特征点。
16.其中，所述相机位姿估计模块采用pnp算法求解相机位姿。
17.本发明的一种基于特征点法的清洁机器人视觉slam系统，利用卷积神经网络结构进行视觉特征的提取和匹配，以epnp算法和gauss-newtonba求解方法进行位姿估计与局部优化；采用位姿图方法进行全局ba优化；利用dbow3执行基于字典模型的回环检测，根据图
像间向量的差异来描述图像间的相似性，保证视觉里程计追踪轨迹的精准性，slam系统根据视觉里程计数据生成融合位姿，提高导航与定位数据的可靠性。dwa算法通过不断更新局部地图和代价，保证局部避障的精准性，与a*算法相结合，实现全局最优路径的评价与规划，模拟其下一步移动可能趋于的方向，从而使全向移动机器人在运动过程中顺利避开障碍物，增强其稳定性和安全性，使得整个系统更加稳定可靠。
附图说明
18.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
19.图1是本发明的一种基于特征点法的清洁机器人视觉slam系统的结构示意图。
20.1-视觉特征提取模块、2-特征匹配模块、3-相机位姿估计模块、4-回环检测模块、5-动态避障模块、51-全局规划单元、52-局部规划单元。
具体实施方式
21.下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
22.请参阅图1，本发明提供一种基于特征点法的清洁机器人视觉slam系统：包括视觉特征提取模块1、特征匹配模块2、相机位姿估计模块3、回环检测模块4和动态避障模块5；
23.所述视觉特征提取模块1、所述特征匹配模块2、所述相机位姿估计模块3、所述回环检测模块4和所述动态避障模块5依次连接；
24.所述视觉特征提取模块1用于提取相机拍摄图像的图像特征点；
25.所述特征匹配模块2用于对所述图像特征点进行特征匹配，得到两张图像上的匹配结果；
26.所述相机位姿估计模块3用于根据所述匹配点估计相机位姿；
27.所述回环检测模块4用于进行回环检测，根据回环信息对相机位姿进行纠正，得到全局一致的姿态估计；
28.所述动态避障模块5用于规划移动路径。
29.在本实施方式中，所述视觉特征提取模块1引入全卷积神经网络，利用sojka算法添加一个过滤器，计算图像上点到灰度值边缘(多边形的边)的距离，并使用高斯滤波函数进行对灰度边缘进行处理，将距离大于阈值的区域排除，以此排除在提取边或者角点上的特征点时非多边形角点以外像素的干扰，在光线变化以及视角差异下依然可以很好的获取图像特征点，有效的解决由于特征点匹配度不足导致的里程累积误差较大问题，同时机器人环境适配度更高。
30.所述特征匹配模块2采用orb算法提取相机拍摄图像的图像特征点，orb中biref描述子本身是一组二进制编码，brief能够显著加速匹配过程；所述相机位姿估计模块3采用pnp算法求解相机位姿，pnp根据已知n个3d点及其在像素坐标中的投影位置，来对相机位姿
进行估计，用ba的方法进行求解，通过构建投影点与匹配点之间的误差函数，以此作为目标进行非线性优化；orb全称为oriented fast and rotated brief，结合了fast角点和brief描述子两个元素；已有匹配点的情况下，还需要对相机的运动进行估计，pnp负责完成将一对3d点向2d坐标进行运动估计，求解方法使用ba方法，其是一种非线性优化方法，算法通过构建最小二乘问题进行迭代求解位置投影的最小误差；ba的目标是：在已知n个三维空间点p及其投影p的情况下，计算相机的位姿r,t。对于pi＝[xi,yi,zi]
t
及其投影像素ui＝[ui,vi,1]
t
，满足如下关系：
[0031]
sui＝ktpi[0032]
投影误差：
[0033]
t是相机位姿r,t的李群表示。ba中的投影关系与dlt方法中的类似，差别在于此处等式右边未使用齐次坐标，而左边的像素坐标ui是非齐次坐标表示，因此在等式右边存在齐次到非齐次的转变，k是相机内参矩阵(3
×
3的矩阵)，实际做法是取出tpi的前三个维度，变换为非齐次的三维空间坐标，即：
[0034]
p
′
＝(tp)
1:3
＝[x
′
,y
′
,z
′
]
t
[0035]
则有：
[0036]
sui＝kp
′
[0037]
构建误差：
[0038][0039]
根据相机内参k:
[0040][0041]
可以得到上述非齐次坐标的误差向量：
[0042][0043]
由于投影误差函数时离散的，在进行优化前还需要对其进行线性化处理：
[0044]
e(x δx)≈e(x) j
t
δx
[0045]jt
为一个2
×
6的雅可比矩阵，x为6维的相机位姿向量，在计算时对t左乘扰动量δξ，而后对上述线性化后的误差函数求扰动量的导数：
[0046][0047]
由上述误差向量及投影的非齐次坐标得到：
[0048]
[0049]
进一步地，利用p
′
自身的叉积，最终得到：
[0050][0051]
需要说明的是，此处的误差是由观测到的像素坐标与预测得到的像素坐标得到的，上述的2
×
6的雅可比矩阵描述的是投影误差关于相机位姿李代数的一阶线性变化关系，回代到上述的线性函数中即可对相机位姿进行优化处理。
[0052]
在实际中，工程中往往还需要对特征点的空间位置p进行优化，线性化方法和上述对位姿优化时类似，仍基于误差函数对位置p进行求导，即：
[0053][0054]
在上述过程中已计算出，而p
′
＝rp t，即得到：
[0055][0056]
上述与即为投影误差关于相机位姿与特征点的偏导项，在非线性优化的求解过程中提供了重要的梯度信息，决定了算法迭代的过程与结果。
[0057]
所述回环检测模块4运用g2o优化算法，结合dbow3执行基于字典模型的回环检测，根据回环提供的信息对相机位姿进行纠正，得到全局一致的姿态估计。前端负责对短时间内的相机轨迹进行跟踪并构建局部地图，但是由于误差的不断累积，获取的局部地图是不准确的，因此，slam系统的后端需要对长时间内的轨迹和地图进行优化，以ba为主的非线性方法则是考虑了slam过程中所有时刻之间的关系，通过相机的投影模型构建ba的代价函数，并利用矩阵的稀疏性结构加速ba求解。位姿图方法是简化版的ba方法，在非线性优化的过程中只对位姿进行优化，而路标点则在完成初始估计后被固定，这种方法大大降低了后端的计算量，对于降低前端里程计的累计误差、提升slam系统的整体跟踪和建图效果具有重要意义，使得slam系统的应用场景进一步拓宽；回环检测既可以保证视觉里程计追踪轨迹及局部建图的精准性，也能够方便相机利用回环提供的数据关联进行重定位，其对整个slam系统的精度和鲁棒性具有显著的提升，在orb-slam工程中，其回环检测线程采用了词袋模型，本质是利用图像中的特征生成单词来构建字典，并用向量表示图像所包含的字典中的单词，根据图像间向量的差异来描述图像间的相似性。
[0058]
tf-idf是一种文本加权计算方法，适用于词袋模型，其中，tf用以描述特征在图像中出现的频率，对应所属单词的区分度，若图像中某单词ωi出现了ni次，该图像中单词总出现次数为n，则tfi＝ni/n；idf用以描述某单词ωi对应特征数量占所有特征数量的比例，若有m个特征，而ωi对应的特征数为mi，则idfi＝log(m/mi)。用tf与idf之积对描述ωi的权重ηi，故ηi＝tfi×
idfi，最终得到图像对应的词袋：
[0059][0060]
计算图像a和b之间的相似度s(v
a-vb)：
[0061][0062]
在对a和b的相似度值进行判断后，回环检测模块4可以得出回环的检测结果，如果发生回环，则将当前的位姿作为新的节点加入到后端的位姿图中，进行重新估计，消除前端里程计产生的累计误差。
[0063]
所述动态避障模块5采用a*算法，在给定全局环境条件地图的情形下，在当前地点和目标点中间计算出一条最佳路线，从而使机器人可以安全、无冲突地通过各种障碍物；沿着全局路线移动的整个过程中，动态障碍随时会出现，这给机器人在朝着目标地点前进的过程中增加了不确定因素。要想实现全向移动机器人的避障功能，还必须采取局部的路径规划方法使得移动机器人能够实时地识别周围环境的变化情况并采取行动，局部路径规划采取动态窗口算法，即dwa算法，dwa算法过程是：首先，对机器人的速度进行采样，包括x、y方向的线速度和角速度；其次，以采样速度、障碍物距离、目标距离作为机器人的初始状态输入，对机器人的运动轨迹进行模拟；最终，以模拟结果中得分最高的轨迹控制底盘进行移动；重复上述步骤，直至到达全局路径的终点；通过不断更新局部地图和代价，保证局部避障的精准性，具体做法是：以全局路径中第一个离开局部地图的点作为全局目标；用队列方式实现栅格地图的膨胀描述，设置目标点和路径上的点的代价为0，偏移邻接点对应的代价会增加，膨胀会累增；将动态窗口法与a*算法相结合，实现了全局最优路径的评价与规划，还可以实时避障，使得路由速度比较均匀，曲率变换也保持连续，提供的运动控制参数也更适合于移动机器人的动态控制。
[0064]
将本发明的一种基于特征点法的清洁机器人视觉slam系统部署在清洁机器人上，利用卷积神经网络结构进行视觉特征的提取和匹配，以epnp算法和gauss-newtonba求解方法进行位姿估计与局部优化；采用位姿图方法进行全局ba优化；利用dbow3执行基于字典模型的回环检测，slam系统向路径规划系统输入视觉里程计信息，由于相机固定在机器人上，因此只需要通过坐标转换信息，机器人底盘即可利用视觉里程计信息进行辅助定位，并根据视觉里程计数据生成融合位姿，提高导航与定位数据的可靠性，解决现有的视觉slam系统累计误差大的问题。
[0065]
进一步的，所述动态避障模块5包括全局规划单元51和局部规划单元52；所述全局规划单元51和所述局部规划单元52连接；
[0066]
所述全局规划单元51用于规划全局移动路径；
[0067]
所述局部规划单元52用于基于所述全局移动路径，规划局部移动路径。
[0068]
在本实施方式中，所述全局规划单元51采用a*算法，在给定全局环境条件地图的情形下，在当前地点和目标点中间计算出一条最佳路线，从而使机器人可以安全、无冲突地通过各种障碍物；所述局部规划单元52采取动态窗口算法，即dwa算法，将动态窗口法与a*算法相结合，实现了全局最优路径的评价与规划，可以实时避障，使得路由速度比较均匀，曲率变换也保持连续，提供的运动控制参数也更适合于移动机器人的动态控制。
[0069]
进一步的，所述视觉特征提取模块1采用卷积神经网络结构提取图像特征点。
[0070]
进一步的，所述特征匹配模块2采用orb算法提取相机拍摄图像的图像特征点。
[0071]
进一步的，所述相机位姿估计模块3采用pnp算法求解相机位姿。
[0072]
以上所揭露的仅为本发明一种较佳实施例而已，当然不能以此来限定本发明之权利范围，本领域普通技术人员可以理解实现上述实施例的全部或部分流程，并依本发明权利要求所作的等同变化，仍属于发明所涵盖的范围。