一种残膜回收机防缠绕挑膜装置的制 一种秧草收获机用电力驱动行走机构

一种基于Kinect的SLAM优化方法及装置与流程

2022-05-26 23:01:49 来源:中国专利 TAG:

一种基于kinect的slam优化方法及装置
技术领域
1.本技术属于数据处理技术领域,具体涉及一种基于kinect的slam优化方法及装置。


背景技术:

2.slam(simultaneous localization and mapping),同步定位与建图,即机器人在陌生房间里自主移动,探测根据采集到的数据,自主精确定位,同时构建移动空间的地图。
3.根据获取移动空间环境的传感器不同,可以将slam分为激光雷达、声呐、毫米波雷达与视觉slam四种。现如今研究及应用较多的是激光雷达slam和视觉slam。激光雷达slam优点是可靠性高,建图精度高,地图可用于路径规划;缺点就是比较受雷达探测范围限制,安装有结构要求等。视觉slam优点是结构简单、安装方式多元化,无传感器探测距离限制、成本低;缺点是受环境光影响大。传统视觉slam在计算机器人位姿时,采用当前帧与前一帧之间的匹配位姿估计,当前一帧由于噪声影响出现位姿误差比较大时,导致以后的帧的位姿估计与实际帧的位姿相差较大,造成误差累计严重,最终导致定位失败。
4.为解决上述问题,现有技术中,通常基于回环检测方法消除误差,基于关键帧位姿的图优化slam在进行回环检测时常采用随机检测等算法,回环检测效率低,检测时间长,难以达到实时定位。


技术实现要素:

5.为了解决上述问题,本技术提供一种基于kinect的slam优化方法及装置,引入匹配图的方法解决视觉里程计中帧与帧匹配计算位姿精度差的问题,基于k叉树词典的丢帧回环检测算法解决回环检测时间长,鲁棒性低的问题,达到实时定位的要求。
6.本技术第一方面提供了一种基于kinect的slam优化方法,主要包括:
7.步骤s1、获取空间环境中的彩色图像与深度图像,每一帧彩色图像对应一帧深度图像;
8.步骤s2、提取图像的orb特征点,并且将提取到的orb特征点利用pnp算法进行机器人位姿计算,获得第一关键帧;
9.步骤s3、基于回环检测获得能够构成回环的第二关键帧,所述回环检测包括基于由k叉树聚类生成的字典对重要重检索帧进行回环检测,所述重要重检索帧是指连续丢失设定数量的帧后的当前帧,所述k叉树聚类生成的字典是指离线采用k叉树算法聚类并采用idf设置单词权重的字典;
10.步骤s4、基于所述第一关键帧及第二关键帧估计机器人移动轨迹。
11.优选的是,步骤s1之后,进一步包括:
12.步骤s11、根据当前帧与前一帧的旋转系数或者平移系数提取位姿变化帧,所述位姿变化帧用于进行机器人位姿计算或者回环检测。
13.优选的是,步骤s2中,进行机器人位姿计算时,进一步包括:
14.步骤s21、更新匹配图中的地图点,更新匹配图中的地图点包括当所述地图点在像素坐标系下不在当前帧内,或者所述地图点的匹配次数与观察次数的比值小于第一设定值时,或者所述地图点与当前帧的偏移角大于第二设定值时,淘汰所述地图点,同时加入当前帧的地图点以使所述匹配图中的地图点数量保持恒定值。
15.优选的是,步骤s3中,基于回环检测获得能够构成回环的第二关键帧包括:
16.步骤s31、若检测到连续丢失设定数量的帧时,将当前帧作为重要重检索帧;
17.步骤s32、对所述重要重检索帧进行回环检测;
18.步骤s33、若满足回环检测条件,则重新确定当前重要重检索帧的位姿;
19.步骤s34、若不满足回环检测条件,则将下一帧作为重要重检索帧,重复进行回环检测,直至回环检测成功或者循环次数达到设定值,当循环次数达到设定值时,重新计算当前重要重检索帧的位姿,将其设定为丢帧之前的相机位姿。
20.优选的是,步骤s32中,对所述重要重检索帧进行回环检测包括:
21.步骤s321、基于离线构造的k叉树词典计算当前关键帧与之前记录的关键帧的相似度,将相似度达到一定值的关键帧记录下来,形成相似帧;
22.步骤s322、对相似帧分别进行时间一致性检验和空间一致性检验,如果满足检验条件,则认为当前关键帧与之前记录的关键帧相似一致。
23.本技术第二方面提供了一种基于kinect的slam优化装置,主要包括:
24.图像获取模块,用于获取空间环境中的彩色图像与深度图像,每一帧彩色图像对应一帧深度图像;
25.位姿计算模块,用于提取图像的orb特征点,并且将提取到的orb特征点利用pnp算法进行机器人位姿计算,获得第一关键帧;
26.回环检测模块,用于基于回环检测获得能够构成回环的第二关键帧,所述回环检测包括基于由k叉树聚类生成的字典对重要重检索帧进行回环检测,所述重要重检索帧是指连续丢失设定数量的帧后的当前帧,所述k叉树聚类生成的字典是指离线采用k叉树算法聚类并采用idf设置单词权重的字典;
27.移动轨迹确定模块,用于基于所述第一关键帧及第二关键帧估计机器人移动轨迹。
28.优选的是,所述基于kinect的slam优化装置还包括:
29.图像筛选模块,用于根据当前帧与前一帧的旋转系数或者平移系数提取位姿变化帧,所述位姿变化帧用于进行机器人位姿计算或者回环检测。
30.优选的是,所述位姿计算模块进一步包括:
31.地图点更新单元,用于更新匹配图中的地图点,更新匹配图中的地图点包括当所述地图点在像素坐标系下不在当前帧内,或者所述地图点的匹配次数与观察次数的比值小于第一设定值时,或者所述地图点与当前帧的偏移角大于第二设定值时,淘汰所述地图点,同时加入当前帧的地图点以使所述匹配图中的地图点数量保持恒定值。
32.优选的是,所述回环检测模块包括第二关键帧获取单元,用于基于回环检测获得能够构成回环的第二关键帧,所述第二关键帧获取单元包括:
33.丢帧数量累计单元,用于累计丢帧数量,若检测到连续丢失设定数量的帧时,将当前帧作为重要重检索帧;
34.回环检测单元,用于对所述重要重检索帧进行回环检测;
35.位姿重计算单元,用于当满足回环检测条件时,重新确定当前重要重检索帧的位姿;
36.循环检测判断单元,用于当不满足回环检测条件时,将下一帧作为重要重检索帧,重复进行回环检测,直至回环检测成功或者循环次数达到设定值,当循环次数达到设定值时,重新计算当前重要重检索帧的位姿,将其设定为丢帧之前的相机位姿。
37.优选的是,所述回环检测单元包括:
38.相似度计算单元,用于基于离线构造的k叉树词典计算当前关键帧与之前记录的关键帧的相似度,将相似度达到一定值的关键帧记录下来,形成相似帧;
39.一致性检验单元,用于对相似帧分别进行时间一致性检验和空间一致性检验,如果满足检验条件,则认为当前关键帧与之前记录的关键帧相似一致。
40.本技术提高了检测效率,降低了机器人在实际运动过程中遇到突发问题导致丢帧从而引起定位失败的风险,提高了算法的鲁棒性。
附图说明
41.图1为本技术slam整体框架示意图。
42.图2为本技术匹配图更新示意图。
43.图3为未引入匹配图的slam的点云图。
44.图4为本技术引入匹配图的slam的点云图。
45.图5为本技术基于k叉树词典的回环检测示意图。
46.图6为本技术丢帧回环检测流程图。
47.图7为fr1/floor运动的真实轨迹与估计轨迹示意图。
具体实施方式
48.为使本技术实施的目的、技术方案和优点更加清楚,下面将结合本技术实施方式中的附图,对本技术实施方式中的技术方案进行更加详细的描述。在附图中,自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。所描述的实施方式是本技术一部分实施方式,而不是全部的实施方式。下面通过参考附图描述的实施方式是示例性的,旨在用于解释本技术,而不能理解为对本技术的限制。基于本技术中的实施方式,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施方式,都属于本技术保护的范围。下面结合附图对本技术的实施方式进行详细说明。
49.本技术的目的是为了解决移动机器人实时定位与地图创建时,机器人位姿估计精度低,定位时间长,实时性差,鲁棒性差等问题。提出了一种引入匹配图的方法,提高了位姿估计精度,同时提出基于k叉树词典的丢帧回环检测算法,提高了检测效率,降低了机器人在实际运动过程中遇到突发问题导致丢帧从而引起定位失败的风险,提高了算法的鲁棒性。
50.本技术第一方面提供了一种基于kinect的slam优化方法,主要包括:
51.步骤s1、获取空间环境中的彩色图像与深度图像,每一帧彩色图像对应一帧深度图像;
52.步骤s2、提取图像的orb特征点,并且将提取到的orb特征点利用pnp算法进行机器人位姿计算,获得第一关键帧;
53.步骤s3、基于回环检测获得能够构成回环的第二关键帧,所述回环检测包括基于由k叉树聚类生成的字典对重要重检索帧进行回环检测,所述重要重检索帧是指连续丢失设定数量的帧后的当前帧,所述k叉树聚类生成的字典是指离线采用k叉树算法聚类并采用idf设置单词权重的字典;
54.步骤s4、基于所述第一关键帧及第二关键帧估计机器人移动轨迹。
55.参考图1,本技术基于kinect的slam优化方法主要包括以下步骤:
56.(1)首先在传感器数据获取方面,调节好kinect相机的内参,并随着机器人的移动采集房间的彩色图像与深度图像,每一帧彩色图像都对应一帧深度图像,将获取到的图像数据存储到计算机中。
57.(2)在视觉里程计方面,提取图像的orb特征点,并且将提取到的orb特征点利用pnp算法进行机器人位姿计算。
58.(3)在回环检测方面,采用基于k叉树词典的丢帧回环检测。
59.(4)在后端非线性优化方面,将视觉里程计时得到的关键帧加入到位姿图中,并将回环检测中构成回环的关键帧加入到位姿图中,之后利用g2o进行整体的优化。最终估计机器人移动轨迹,实现对移动机器人实时定位的功能,并且通过点云拼接技术构建机器人移动过的房间三维地图。
60.基于上述步骤,本技术在能够精确估计机器人位姿情况下,能够达到实时定位与建图的效果。
61.在一些可选实施方式中,步骤s1之后,进一步包括:
62.步骤s11、根据当前帧与前一帧的旋转系数或者平移系数提取位姿变化帧,所述位姿变化帧用于进行机器人位姿计算或者回环检测。
63.该实施例中,匹配图由关键帧和地图点组成,匹配图如图2所示。由于slam算法在处理大规模的定位与建图时,不可能将所有机器人采集到的图像都用于后端优化与回环检测,这既降低了算法的处理效率,影响了机器人实时定位的要求,同时这也不利于提高定位的精确性。因此在算法运行时,提取出关键帧,用于算法后面的后端优化、回环检测及估计轨迹至关重要。提取关键帧主要依据两个方面:
64.a.当前帧与前一参考帧的旋转系数大于0.1;
65.b.当前帧与前一参考帧的平移系数大于0.1。
66.当满足上述两个条件中的一个时,即可认为相机位姿发生了重要改变,需添加关键帧,更新匹配图,计算当前帧位姿时,提取当前帧的特征点,让这些特征点与匹配图中地图点进行匹配并通过p3p算法进行位姿估计。
67.在一些可选实施方式中,步骤s2中,进行机器人位姿计算时,进一步包括:
68.步骤s21、更新匹配图中的地图点,更新匹配图中的地图点包括当所述地图点在像素坐标系下不在当前帧内,或者所述地图点的匹配次数与观察次数的比值小于第一设定值时,或者所述地图点与当前帧的偏移角大于第二设定值时,淘汰所述地图点,同时加入当前帧的地图点以使所述匹配图中的地图点数量保持恒定值。
69.该实施例中,在进行位姿估计计算之后,需要不断更新匹配图中的地图点。为了使
匹配图中的点不会过多,以及避免位姿图中存在一些过时无意义的地图点,提高计算效率,在增加地图点前,需要删除一些过时的地图点。删除地图点主要有以下三种情况:
70.将地图点从世界坐标系的坐标转换成像素坐标系下,如果该地图点在像素坐标系下不在当前帧中,则判断该地图点超出了当前帧活动的范围,对之后的位姿计算没有帮助,则删除该地图点,即认为该地图点失去了作为下一帧图像估计位姿的作用;
71.在更新地图点时,需更新地图点的匹配次数和该地图点的观察次数,匹配次数与观察次数的比值即为匹配率α。为了及时淘汰掉匹配率较差的地图点,通过多次最低匹配率测试,当最低匹配率为0.5时,加入匹配图的slam得到的点云拼接图效果最好。因此设定最低匹配率为0.5,当α<0.5时,即淘汰该地图点;
72.计算地图点与当前帧的偏移角β,当β>π/6时,淘汰该地图点。
73.而增加地图点只需要提前设定匹配图中地图点的数量阈值为100,当删除地图点之后的匹配图中地图点不足100时,除了匹配图中与当前帧中配对的点对,其他当前帧合格的点都加入到匹配图的地图点中,完成向匹配图中增加地图点的操作。
74.通过删除与增加匹配图中地图点的算法,保持了匹配图中地图点的实时性,又能够使匹配图不至于过大而影响计算效率。
75.为验证引入匹配图slam算法的优异性,设计了一组对比试验。该试验采用computer vision group提供的rgbd_dataset_freiburg1_xyz数据集。将该数据集采集到的彩色图像与深度图像分别代入到未引入匹配图的slam中以及引入匹配图改进的slam中,得到关键帧及其位姿,分别对其进行点云拼接,分别得到如图3所示的未引入匹配图的slam得到的房间三维点云拼接图以及如图4所示的引入匹配图的slam得到的房间三维点云拼接图。
76.从两幅图像对比可以清楚的看出图4较图3更加清晰明朗,基本能还原出原机器人运动房间的原貌。这是因为由于引入了匹配图,使得通过slam算法得到的机器人位姿更加精确,鲁棒性更强,因此在点云拼接时,拼接图像点更加契合,能够得到清晰精确的房间三维点云拼接图。通过以上对比试验,即可证明提出的引入匹配图方法的优异性,提高了位姿估计精度,增加了slam算法的鲁棒性。
77.在一些可选实施方式中,步骤s3中,基于回环检测获得能够构成回环的第二关键帧包括:
78.步骤s31、若检测到连续丢失设定数量的帧时,将当前帧作为重要重检索帧;
79.步骤s32、对所述重要重检索帧进行回环检测;
80.步骤s33、若满足回环检测条件,则重新确定当前重要重检索帧的位姿;
81.步骤s34、若不满足回环检测条件,则将下一帧作为重要重检索帧,重复进行回环检测,直至回环检测成功或者循环次数达到设定值,当循环次数达到设定值时,重新计算当前重要重检索帧的位姿,将其设定为丢帧之前的相机位姿。
82.在一些可选实施方式中,步骤s32中,对所述重要重检索帧进行回环检测包括:
83.步骤s321、基于离线构造的k叉树词典计算当前关键帧与之前记录的关键帧的相似度,将相似度达到一定值的关键帧记录下来,形成相似帧;
84.步骤s322、对相似帧分别进行时间一致性检验和空间一致性检验,如果满足检验条件,则认为当前关键帧与之前记录的关键帧相似一致。
85.以上两个实施例中,给出了基于k叉树词典的丢帧回环检测的具体步骤,随着机器人移动过程中,通过图像匹配与位姿估计来计算机器人的位姿,由于噪声和计算估计误差的存在,随着移动轨迹的增长,多次的累积误差使得位姿估计越来越不准确。因此在slam算法中加入回环检测部分,解决了随着时间的累积机器人移动轨迹的增加,导致误差越来越大,位姿估计越来越不准确的问题。回环检测就是随着机器人的不断运动,判断此时刻采集到的某一图像与之前某时刻采集到的图像是否回到了相同的位置,由于之前的图像位姿估计必然比现在的图像位姿估计准确,因此当判断两图像能够达到回环时,更正当前图像的位姿,消除累计误差,也为后面的图像位姿估计进行了校正。基于k叉树词典的回环检测框架如图5所示,主要分为以下三个步骤:
86.a、离线构造k叉树词典。离线采用k-means 算法聚类生成字典,并采用idf(inverse document frequency)设置单词权重;
87.b、在线计算当前关键帧与之前记录的关键帧的相似度,将相似度达到一定值的关键帧记录下来,定义为相似帧;
88.c、对相似帧进行一致性检验,分别进行时间一致性检验和空间一致性检验,如果满足检验条件,则认为两帧达到相似一致,即达成回环。
89.由于机器人在实际运动过程中会遇到很多意外情况,比如进入无纹理环境,地面凹凸不平导致镜头剧烈震动,这都会造成持续无法计算当前位姿的情况,如果对这种情况不及时处理,对之后机器人的定位和建图都造成不可避免的影响。
90.针对上述这种情况,充分利用设计的基于k叉数词典的回环检测算法检索高效的优点,创新性的设计了丢帧回环检测算法,通过引入该丢帧回环检测算法能够很好的提高机器人对实际运动过程中丢帧问题的抗性。该丢帧回环检测算法的程序流程图如图6所示,当检测到连续丢失了n帧之后,即启动丢帧回环检测,将现阶段最新获得到的图像设置为重要重检索帧,并将该重要重检索帧带入到之前设计的基于k叉树词典的回环检测中,如果满足回环检测条件,即重新确定当前关键帧的位姿,完成丢帧回环检测,实现了在丢帧时重新找回当前帧位姿的校正。如果当前帧没有完成回环检测,判断重新设定关键帧次数是否超过5次,如果没有超过5次,将该图像之后采集到的图像设定为重要重检索帧重新进行回环检测,直到完成丢帧回环检测。如果重新设定关键帧次数超过了5次,即认为丢帧恢复失败,重新计算当前帧位姿,即将当前帧位姿设定为丢帧之前的相机位姿,这样做的好处是尽可能的将当前帧的位姿校正为最接近正确位姿的位姿。
91.为了验证丢帧回环检测算法的优异性,设计了一个对比实验。本实验采用computer vision group提供的rgbd_dataset_freiburg1_floor数据集。为了设计丢帧的情况,将这组数据集中连续三张图片替换成空白图片,导致连续三次丢帧。将插入三张空白图片的fr1/floor数据集分别代入到带有丢帧回环检测算法slam算法中以及没有丢帧回环检测算法的slam算法中,分别得到的两种关键帧的位姿,用python分别画出fr1/floor数据集的真实运动轨迹,带有丢帧回环检测算法slam算法得到的关键帧的估计运动轨迹和没有丢帧回环检测算法slam算法得到的关键帧的估计运动轨迹,如图7所示。从图中可以看出在经历了连续丢帧时,没有丢帧回环检测算法slam算法得到的关键帧的估计运动轨迹与实际运动轨迹相差较大,而加入了丢帧回环检测算法的slam算法得到的关键帧的估计运动轨迹与实际运动轨迹基本一致,能够很好地克服丢帧带来的定位不准确的问题,实现移动机器
人的精确定位。
92.通过该丢帧回环检测算法,降低了机器人在实际运动过程中遇到突发问题导致丢帧从而引起定位失败的问题,提高了算法的鲁棒性,让机器人在运动过程中即便丢帧时也能很好的实现精确定位。
93.本技术优点包括:
94.(1)在视觉里程计中,本文创新性的引入了匹配图,克服了传统slam算法中帧与帧匹配时,当某一帧由于噪声影响出现位姿误差比较大时,导致以后的帧的位姿估计与实际帧的位姿相差较大,进而使之后对机器人姿态计算均不准确的缺点。并最终通过对比实验证明了引入匹配图方法的优异性,提高了位姿估计精度,提高了slam算法的鲁棒性。
95.(2)在回环检测方面采用基于k叉树词典的丢帧回环检测算法。首先离线条件下构造k叉数词典,之后进行相似度计算,最后通过了一致性检验的图片,即构成回环。同时充分利用本文设计的基于k叉数词典的回环检测算法检索高效的优点,创新性的设计了丢帧回环检测算法,降低了机器人在实际运动过程中遇到突发问题导致丢帧从而引起定位失败的风险,提高了slam算法对移动机器人自主定位与建图的稳定性。
96.本技术第二方面提供了一种与上述方法对应的基于kinect的slam优化装置,主要包括:
97.图像获取模块,用于获取空间环境中的彩色图像与深度图像,每一帧彩色图像对应一帧深度图像;
98.位姿计算模块,用于提取图像的orb特征点,并且将提取到的orb特征点利用pnp算法进行机器人位姿计算,获得第一关键帧;
99.回环检测模块,用于基于回环检测获得能够构成回环的第二关键帧,所述回环检测包括基于由k叉树聚类生成的字典对重要重检索帧进行回环检测,所述重要重检索帧是指连续丢失设定数量的帧后的当前帧,所述k叉树聚类生成的字典是指离线采用k叉树算法聚类并采用idf设置单词权重的字典;
100.移动轨迹确定模块,用于基于所述第一关键帧及第二关键帧估计机器人移动轨迹。
101.在一些可选实施方式中,所述基于kinect的slam优化装置还包括:
102.图像筛选模块,用于根据当前帧与前一帧的旋转系数或者平移系数提取位姿变化帧,所述位姿变化帧用于进行机器人位姿计算或者回环检测。
103.在一些可选实施方式中,所述位姿计算模块进一步包括:
104.地图点更新单元,用于更新匹配图中的地图点,更新匹配图中的地图点包括当所述地图点在像素坐标系下不在当前帧内,或者所述地图点的匹配次数与观察次数的比值小于第一设定值时,或者所述地图点与当前帧的偏移角大于第二设定值时,淘汰所述地图点,同时加入当前帧的地图点以使所述匹配图中的地图点数量保持恒定值。
105.在一些可选实施方式中,所述回环检测模块包括第二关键帧获取单元,用于基于回环检测获得能够构成回环的第二关键帧,所述第二关键帧获取单元包括:
106.丢帧数量累计单元,用于累计丢帧数量,若检测到连续丢失设定数量的帧时,将当前帧作为重要重检索帧;
107.回环检测单元,用于对所述重要重检索帧进行回环检测;
108.位姿重计算单元,用于当满足回环检测条件时,重新确定当前重要重检索帧的位姿;
109.循环检测判断单元,用于当不满足回环检测条件时,将下一帧作为重要重检索帧,重复进行回环检测,直至回环检测成功或者循环次数达到设定值,当循环次数达到设定值时,重新计算当前重要重检索帧的位姿,将其设定为丢帧之前的相机位姿。
110.在一些可选实施方式中,所述回环检测单元包括:
111.相似度计算单元,用于基于离线构造的k叉树词典计算当前关键帧与之前记录的关键帧的相似度,将相似度达到一定值的关键帧记录下来,形成相似帧;
112.一致性检验单元,用于对相似帧分别进行时间一致性检验和空间一致性检验,如果满足检验条件,则认为当前关键帧与之前记录的关键帧相似一致。
113.虽然,上文中已经用一般性说明及具体实施方案对本技术作了详尽的描述,但在本技术基础上,可以对之作一些修改或改进,这对本领域技术人员而言是显而易见的。因此,在不偏离本技术精神的基础上所做的这些修改或改进,均属于本技术要求保护的范围。
再多了解一些

本文用于企业家、创业者技术爱好者查询,结果仅供参考。

发表评论 共有条评论
用户名: 密码:
验证码: 匿名发表

相关文献