法律状态公告日
法律状态信息
法律状态
2020-07-24
专利权人的姓名或者名称、地址的变更 IPC(主分类):G05D1/02 变更前: 变更后: 申请日:20170120
专利权人的姓名或者名称、地址的变更
2020-05-19
专利权的转移 IPC(主分类):G05D1/02 登记生效日:20200428 变更前: 变更后: 申请日:20170120
专利申请权、专利权的转移
2018-02-06
授权
授权
2017-07-04
实质审查的生效 IPC(主分类):G05D1/02 申请日:20170120
实质审查的生效
2017-06-09
公开
公开
技术领域
本发明涉及仿生学和机器视觉领域,尤其涉及一种基于啮齿类动物模型和RTAB-Map闭环检测算法的SLAM方法。
背景技术
传统的概率算法能够处理传感器与环境的模糊性,有很好的同步定位与地图构建性能并建立高准确度、高精度的笛卡尔地图,但这些方法很少能够完全解决整个地图构建和导航问题。如何通过现有其他技术解决动态复杂环境下整个地图构建和导航成为移动机器人同步定位和地图构建的关键问题之一。
视觉里程计仅用相邻帧图像估计运动,存在累积误差,闭环检测通过场景的重定位消除累积误差,保证了轨迹与地图的全局一致性。一些闭环检测算法,如FAB-MAP和IAB-Map等在每一时刻将当前场景与所有历史场景实时匹配,效率较低,不能满足闭环检测的实时性要求。
发明内容
许多生物虽然不具备高精度传感器和高分辨率的地图,但仍具有强大的导航能力,并且能够解决整个SLAM问题。RTAB-Map闭环检测算法基于关键点的检测策略在满足系统稳定的前提下具有良好的实时性能。
本发明针对现有技术的不足,提供一种基于啮齿类动物模型和RTAB-Map闭环检测算法的SLAM方法,以解决现有技术导致的上述多项缺陷。
本发明提出的一种基于啮齿类动物模型和RTAB-Map闭环检测算法的SLAM方法,包括以下步骤:
步骤1)通过吸引子竞争网络的动力学模型控制位姿网内部的活动,构建位姿细胞内部动态过程;
步骤2)局部场景细胞进行视觉场景学习形成局部场景;
步骤3)通过位姿感知细胞和局部场景细胞在特定时间获得的瞬时图像进行经历地图的构建;
所述步骤1)中,通过吸引子竞争网络的动力学模型控制位姿网内部的活动,其动态过程经历三个阶段:兴奋度更新,对所有细胞的全局抑制和对位姿感知细胞活动的归一化。
所述步骤2)中,局部场景细胞进行视觉场景学习形成局部场景,视觉细胞激活程度ai,位姿细胞网络最大活性位置Pi和产生视觉模板Vi之间关系为:
Vi={ai,Pi}
注入位姿细胞网络的局部场景细胞活性可由下式表示:
其中,δ为视觉校准加强常数。
所述步骤3)中,经验ei编码经历能级Ei,位姿细胞位置Pi,视觉模板Vi和视觉细胞位置pi
ei={Ei,Pi,Vi,pi}
第i个经历的总能级可表示为:
其中,x'pc,y'pc,θ'pc为最大活性姿态细胞坐标;xi',yi',θi'为与该经历相关的位姿感知细胞坐标;ra为(x',y')平面的区域常数;θa为θ'维上区域常数;Vcurr为当前视景;Vi为与经历i相关场景。
经历位姿改变
其中,α为认知速度常数,Nf为经历i到其他经历的连接数;Nt为从其他经历到经历i的连接数。实验表明认知速度α=0.5可以在地图上迅速收敛到一稳定状态。
所述步骤3)中,经历地图可通过RTAB-MAP闭环检测算法进行实时调整。
所述步骤3)中,RTAB-MAP闭环检测算法包括创建定位点、权重更新、贝叶斯过滤器更新、选择闭环假设和该算法所用的存储器间的相互转化。
所述经验为步骤(3)中特定时间特定位置采集到瞬时图像的经历地图构建的信息集合。
与现有技术相比,本发明的优点在于:
1.复杂度与导航范围:以测距和图像匹配SLAM法为例,移动机器人本身所携带的传感器有测距、畸变等问题等。这种现象在机器人行驶于崎岖不平的路面上情况会更严重。更进一步分析,实际环境中测量值可能是错误的,例如激光可以穿透玻璃墙,激光光束在反射回探测器前可能经过了多次反射等。即使是图像传感器也有其应用的局限性,例如,照相机在区分图像标准色方面表现很差。若采用灰度的方式表示图像,光强的改变又很难表现出来,另外,若采用全景相机或广角镜头采集图像,采集到的图像会产生严重畸变。借鉴鼠类等哺乳动物通过位置细胞、头方向细胞、视觉细胞等细胞依赖鲁棒性处理策略而不是对世界的精确描述,利用这种仿生学原理可以在无需高精度传感器和高分辨率地图的状况下解决整个SLAM问题。
2.实时性:通过RTAB-Map闭环检测策略融合原有扩展海马模型RatSLAM,既修正了移动机器人长时间导航下里程计的漂移现象,与其他的闭环检测算法相比又具有良好的实时性能。对闭环检测算法的优化实时性能具体如下:常用的如FAB-MAP等闭环检测算法通过逐个比较当前帧数据与各历史帧数据的相似性,复杂环境下导航的实时性能较差,选取的RTAB-Map闭环检测算法通过引入工作存储器(WM,Working Memory)、长期存储器(LTM,Long-Term Memory)和短期存储器(STM,Short-Term Memory)这三种存储器,将当前位置时刻至过去某时刻内的信息储存在短期存储器中,并选取所有过去时刻出现频次最高的信息储存在工作存储器中。通过当前位置与短期存储器存储位置比较进行新位置的判别和权重更新,通过当前位置与工作存储器存储位置比较进行闭环检测,同时,实时更新各存储器。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分,示出了符合本发明的实施例,并与说明书一起用于解释本发明的原理。
图1为本发明一种基于啮齿类动物模型和RTAB-Map闭环检测算法的SLAM方法系统模型示意图;
图2为本发明一种基于啮齿类动物模型和RTAB-Map闭环检测算法的SLAM方法流程图;
图3为本发明一种基于啮齿类动物模型和RTAB-Map闭环检测算法的SLAM方法算法流程图。
具体实施方式
如图1所示,局部场景细胞在环境中学习独特场景,由头方向细胞和位置细胞合并形成的位姿细胞表征当前位置,拓扑化的经验图用节点和链路编码局部场景细胞和位姿细胞。RTAB-Map闭环检测算法利用RatSLAM系统局部场景细胞活跃度进行场景重定位。
如图2所示,在RTAB-Map图像闭环检测方法中,先采集陌生场景信息,由于拍摄的连续图像有很大部分相似内容,故通过设定阈值对连续图像信息进行收集就可知道当前场景下所包含的信息,将当前位置时刻至过去某时刻内的词储存在短期存储器STM中并选取过去时刻出现频次最高的词储存在工作存储器WM中。通过当前位置与STM存储位置比较进行新位置的判别和权重更新,通过当前位置与WM存储位置比较进行闭环检测,同时,各存储器自身也在实时更新。在RatSLAM系统结构中,传感器的实时监测影响局部场景细胞和视觉里程计,进而间接影响位姿感知细胞和经验图的构建。局部场景细胞在环境中学习独特场景,由头方向细胞和位置细胞合并形成的位姿细胞表征当前位置,拓扑化的经验图用节点和链路编码局部场景细胞和位姿细胞。两者通过局部场景细胞联系在一起,通过局部场景细胞的活跃度进行场景重定位。
如图3所示,RTAB-Map图像闭环检测方法具体过程如下:
第一步:定位点创建。RTAB-Map使用词袋模型创建图像签名,并由在现增量式的视觉词典中的词的一个集合来表示,这种词典的优势在于针对特定环境无需预训练过程。
利用图像签名和时间索引建立定位点,并与上一时刻在图中建立一个时间上的双向链接。
第二步:权重更新。将当前位置Lt与STM储存的最后一个定位点比较,通过更新STM权重进行训练,具体过程通过下式表示:
其中,s为相似度,Npair表示定位点签名匹配上的单词对的数量,
第三步:贝叶斯估计更新
离散贝叶斯滤波器的作用是通过估计现有位置Lt和储存在WM中已走过的位置进行闭环假设判断,令St为t时刻所有闭环假设的随机变量。St=i(i<t)表示位置Lt与过去i时刻所在位置Li能构成一闭环,St=-1表示Lt处于一个新的位置。计算方式如下:
其中,η是标准化系数,p(Lt|St)是观测模型,p(St|St-1=i)为运动模型,
似然函数L(St|Lt)评估观测模型p(Lt|St),通过相似度标准偏差σ和相似度平均数μ将每个值标准化,公式如下:
特别地,当St=-1时,有
似然函数估计值L(St=-1|Lt)越大,Lt就越有可能在一个新的定位点。运动模型p(St|St-1=i)可以在已知St-1分布条件下来预测St分布,构成下列四种概率可能性。
1)p(St=-1|St-1=-1)=0.9当t-1时刻不出现闭环时在t时刻不出现闭环的概率较大。
2)
3)p(St=-1|St-1=j)=0.1j∈[0:t-ρ]当t-1时刻出现闭环在t时刻不出现闭环的概率较小。
4)p(St=i|St-1=j)i,j∈[0;t-ρ]当t-1时刻出现闭环时在t时刻在临近位置出现闭环的概率,其概率为中心点在j处的离散高斯曲线,其值是确定的八个非空邻元素(i=j-4,...,j+4)并且它们和的概率值为0.9。
第四步:闭环假设选择
提出一种无需融合定位点的闭环假设选择,其优势在于能够保存同一定位点的不同签名,这对在高度动态变化环境中有着更好的闭环假设估计。
第五步:恢复
当p(St=i|Lt)取到最大概率时将LTM存储位置恢复至WM中。视觉词典对匹配到的新词进行实时更新,若词典中有这个词则用新词取代它,若词典中没有这个词则把这个词加到词典中。
第六步:转换
当处理时间大于Ttime时,将最初位置和最低权重从WM转换到LTM。因为Ttime高意味更多的位置可以被保存到WM中,可以更好的代表整体环境,因此根据机器人CPU能够处理的计算复杂度和操作环境尽可能大的设置Ttime。根据经验,采集速度为1Hz的图像,Ttime一般设置在600-800ms以确保在图像采集率下所有图像都已被处理。RatSLAM模型算法具体过程如下:
第一步:位姿细胞内部动态过程
1)兴奋度更新
创建的兴奋度权重矩阵εabc和由于兴奋性使位姿感知细胞活动产生变化ΔPx'y'θ'表示如下:
其中,kx'y'和kθ'分别为位姿感知细胞平面(x',y')和θ'维中对应着兴奋性一维分布的的方差常数;a、b、c为x'、y'、θ'的分布系数;Nx',Ny',Nθ'为(x',y',θ')空间中的位姿感知细胞三维矩阵元素。
2)对所有细胞的全局抑制
由于多位姿假设(由多个活动包表示)的完成需要时间并且会进一步被视觉输入加强,故相互竞争的行动包会在相当长的一段时间内同时存在,模型如下:
其中,抑制常数
3)归一化
为保持姿态细胞在获得视觉输入和积分路径输入后的总活性采取归一化处理,归一化后的位姿感知细胞的活动水平
第二步:视觉细胞场景学习
视觉细胞激活程度ai,位姿细胞网络最大活性位置Pi产生视觉模板Vi
Vi={ai,Pi}
注入位姿细胞网络的局部场景细胞活性如下表示:
其中,δ为视觉校准加强常数。
第三步:经历制图
经验ei编码经历能级Ei,位姿细胞位置Pi,视觉模板Vi和视觉细胞位置pi
ei={Ei,Pi,Vi,pi}
第i个经历的总能级
其中,x'pc,y'pc,θ'pc为最大活性姿态细胞坐标;xi',yi',θi'为与该经历相关的位姿感知细胞坐标;ra为(x',y')平面的区域常数;θa为θ'维上区域常数;Vcurr为当前视景;Vi为与经历i相关场景。
经历位姿改变
其中,α为认知速度常数,Nf为经历i到其他经历的连接数;Nt为从其他经历到经历i的连接数。实验表明认知速度α=0.5可以在地图上迅速收敛到一稳定状态。
以上显示和描述了本发明的基本原理、主要特征和优点。本行业的技术人员应该了解,上述实施例不以任何形式限制本发明,凡采用等同替换或等效变换的方式所获得的技术方案,均落在本发明的保护范围内。
机译: 基于共享Slam MAP的高精度定位方法和系统
机译: 保存和加载SLAM MAP的SLAM方法
机译: 保存和加载SLAM MAP的SLAM方法