CN108871341B - 一种全局优化的并发定位与建图方法 - Google Patents
一种全局优化的并发定位与建图方法 Download PDFInfo
- Publication number
- CN108871341B CN108871341B CN201810734733.6A CN201810734733A CN108871341B CN 108871341 B CN108871341 B CN 108871341B CN 201810734733 A CN201810734733 A CN 201810734733A CN 108871341 B CN108871341 B CN 108871341B
- Authority
- CN
- China
- Prior art keywords
- particle
- particles
- current moment
- calculating
- moment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000013507 mapping Methods 0.000 title claims abstract description 17
- 238000005457 optimization Methods 0.000 title claims abstract description 16
- 239000002245 particle Substances 0.000 claims abstract description 349
- 238000012952 Resampling Methods 0.000 claims abstract description 29
- 230000033001 locomotion Effects 0.000 claims abstract description 25
- 238000001914 filtration Methods 0.000 claims abstract description 13
- 230000008569 process Effects 0.000 claims abstract description 11
- 238000004364 calculation method Methods 0.000 claims description 18
- 230000035772 mutation Effects 0.000 claims description 10
- 238000012545 processing Methods 0.000 claims description 7
- 239000011159 matrix material Substances 0.000 claims description 5
- 238000010367 cloning Methods 0.000 claims description 4
- 238000010606 normalization Methods 0.000 claims description 4
- 239000000126 substance Substances 0.000 claims description 4
- 238000012216 screening Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 7
- 230000008859 change Effects 0.000 description 5
- 238000005070 sampling Methods 0.000 description 4
- 230000010076 replication Effects 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000015556 catabolic process Effects 0.000 description 2
- 238000006731 degradation reaction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 239000000427 antigen Substances 0.000 description 1
- 102000036639 antigens Human genes 0.000 description 1
- 108091007433 antigens Proteins 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 239000008187 granular material Substances 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- 239000002699 waste material Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4808—Evaluating distance, position or velocity data
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0272—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising means for registering the travel distance, e.g. revolutions of wheels
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种全局优化的并发定位与建图方法;首先根据机器人的初始坐标,在初始时刻随机生成n个粒子,得到初始时刻的粒子集;采用粒子滤波的SLAM算法,利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息;采用全局优化和重采样对当前时刻各粒子进行更新;利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点;重复以上过程,直至机器人完成所有时刻的运动,采用本方法,能够提高机器人的定位精度。
Description
技术领域
本发明涉及机器人导航领域,具体涉及一种全局优化的并发定位与建图方法。
背景技术
随着计算机和人工智能等学科的发展,移动机器人相关技术得到了突飞猛进的发展,是目前科学技术最前沿的一个领域。移动机器人应用场景非常广阔,主要包括卫生清洁,医疗辅助,安保消防等领域。而为了完成这些任务,就需要移动机器人具有感知周围环境信息和自身状态的能力。在已知环境下,机器人可以通过对路标点的感知,纠正自身的位姿信息。然而在周围环境信息未知时,移动机器人需要在没有先验信息的情况下,在自身运动的过程中一边建立周围环境的模型即建图,一边估计自身的运动状态即定位。在这个过程中机器人的建图和定位的精度是相互影响的,这就引出了机器人领域的热门研究同时定位与建图问题(SimultaneousLocalization And Mapping,SLAM)。
SLAM研究的是机器人在陌生环境下通过本体携带的传感器构建地图,同时确定自己所在地图的位置的问题。SLAM***主要有三个组成部分,并且是一个迭代的过程。首先通过当前机器人的位姿信息和运动方程估计下一时刻机器人的位姿信息。然后通过相机或者激光雷达等传感器获取路标信息,并且把路标信息和机器人的位姿信息相关联。等到下一时刻获取到了新的路标信息和机器人位姿信息之后,对于第一部分的信息进行更新。
通过贝叶斯滤波器解决SLAM问题的方法主要有两种。一种是基于扩展卡尔曼滤波(Extended Kalman Filter,EKF)的SLAM算法。一种是基于粒子滤波(Particle Filter,PF)的SLAM算法。EKF的方法解决了卡尔曼滤波(Kalman Filter,KF)仅限于处理线性***的问题,将之扩展到了非线性***问题的应用当中。EKF需要保证噪声服从高斯分布,而粒子滤波(Particle Filter,PF)是非线性非高斯的,适用性更好。粒子滤波的方法是在机器人的初始位置周围按照某种先验分布随机产生一定数目的粒子,然后根据状态转移方程更新粒子的位姿信息。通过观测模型获取的路标点评估当前粒子的优劣,计算权重根据权值进行重采样,然后进行下一时刻的估计。但是为了提高粒子滤波的效果不得不增加粒子的数目,从而造成计算量的增大。
发明内容
本发明的目的在于:提供一种全局优化的并发定位与建图方法,解决了目前采用粒子滤波的SLAM算法实现机器人的定位与建图时定位精度低的技术问题。
本发明采用的技术方案如下:
一种全局优化的并发定位与建图方法,包括以下步骤:
步骤1:根据机器人的初始坐标,在初始时刻随机生成n个粒子,得到初始时刻的粒子集;
步骤2:采用粒子滤波的SLAM算法,利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息;
步骤3:采用全局优化和重采样对当前时刻各粒子进行更新;
步骤4:利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点;
步骤5:重复步骤2-4,直至机器人完成所有时刻的运动。
进一步的,所述步骤2具体为:
步骤21:采用机器人运动模型,利用上一时刻各粒子的粒子信息中的位姿数据得到当前时刻各粒子的位姿数据,并记录当前时刻粒子观测到的路标点;
步骤22:采用提议分布对当前时刻的位姿数据和路标点位置进行更新,利用更新后的位姿数据和路标点位置计算各粒子的权重;
步骤23:对所述权重进行归一化处理,得到当前时刻各粒子的权重。
进一步的,所述步骤3具体步骤为:
步骤31:对当前时刻的各粒子进行克隆和变异操作,更新粒子集中的粒子;
步骤32:以更新后粒子集中的各个粒子为引领者随机产生N个跟随者,计算跟随者和引领者的可能性,利用可能性筛选所述引领者和跟随者,得到再次更新后的粒子集;
步骤33:利用再次更新后粒子集中粒子的权重计算有效粒子的数目Neff,当有效粒子数目Neff大于设定的阈值Φ,则不进行重采样,否则进行重采样,得到当前时刻最终的粒子集。
进一步的,所述步骤31具体为:
步骤312:利用所述亲和力faffinity计算当前时刻粒子需要克隆的数目Ncl,计算公式如下:
步骤313:将新获得的粒子添加至所述粒子集中,更新当前时刻的粒子集,利用所述亲和力faffinity对粒子进行变异操作,变异采用的公式为:
步骤314:利用变异后粒子间的位姿差计算两两粒子间的排斥力freject,计算公式为:
进一步的,所述步骤32具体为:
步骤321:对步骤21更新后粒子集中的每个粒子,以各个粒子为引领者随机产生对应的N个跟随者,分别计算引领者与跟随者的适应度ffitness,计算适应度采用的函数为:
步骤322:利用所述适应度ffitness计算各个粒子的可能性pi,采用的公式为:
步骤323:判断引领者和跟随者的可能性pi,当跟随者可能性大于引领者可能性,即利用可能性最大的跟随者替换引领者,否则不进行替换,得到再次更新后粒子集。
进一步的,所述步骤33具体为:
步骤331:利用更新后粒子集中粒子的权重计算有效粒子的数目Neff,计算公式为:
步骤332:当有效粒子数目Neff大于设定的阈值Φ,则不进行重采样,得到当前时刻最终的粒子集,否则进行重采样后跳转至步骤31。
进一步的,所述重采样为:复制权值大的粒子,去掉权值小的粒子进行,保持粒子集中粒子数目稳定。
进一步的,所述步骤4具体为:
步骤41:利用当前时刻最终粒子集中各粒子的粒子信息中的路标点位置,对不同时刻相同粒子观测到的同一路标点采用扩展卡尔曼滤波进行更新,得到当前时刻的路标点位置;
步骤43:利用当前时刻最终的路标点位置和路径中各点的位置,得到当前时刻机器人运动路径和路标点。
综上所述,由于采用了上述技术方案,本发明的有益效果是:
1.本发明能够极大的提升机器人的定位精度。
2.采用本发明公开的全局优化方法,粒子复制的数目随着亲和力的增加而减少,也就是对权值高的粒子进行多复制,权值小的粒子减少复制,通过这种方法可以有效的抑制亲和力高的抗体而促进亲和力低的抗体,加快收敛速度;通过变异操作使权值较大的粒子的位姿变化较小,而权值较小的粒子变化较大,从而使权值较小的粒子去寻找更好的位姿,增加粒子的多样性。
附图说明
本发明将通过例子并参照附图的方式说明,其中:
图1是本发明的整体流程图;
图2是本发明中全局优化的流程图;
图3是具体实施例2设定的地图和路标点;
图4是具体实施例2中采用EKFLAM算法运行的结果图;
图5是具体实施例2中采用FastSLAM算法运行的结果图;
图6是具体实施例2中采用本发明运行的结果图。
具体实施方式
本说明书中公开的所有特征,或公开的所有方法或过程中的步骤,除了互相排斥的特征和/或步骤以外,均可以以任何方式组合。
下面结合图1、图2对本发明作详细说明。
一种全局优化的并发定位与建图方法,包括以下步骤:
步骤1:根据机器人的初始坐标,在初始时刻随机生成n个粒子,得到初始时刻的粒子集;
步骤2:采用粒子滤波的SLAM算法,利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息;
步骤3:采用全局优化和重采样对当前时刻各粒子进行更新;
步骤4:利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点;
步骤5:重复步骤2-4,直至机器人完成所有时刻的运动。
进一步的,所述步骤2具体为:
步骤21:采用机器人运动模型,利用上一时刻各粒子的粒子信息中的位姿数据得到当前时刻各粒子的位姿数据,并记录当前时刻粒子观测到的路标点;
步骤22:采用提议分布对当前时刻的位姿数据和路标点位置进行更新,利用更新后的位姿数据和路标点位置计算各粒子的权重;
步骤23:对所述权重进行归一化处理,得到当前时刻各粒子的权重。
进一步的,所述步骤3具体步骤为:
步骤31:对当前时刻的各粒子进行克隆和变异操作,更新粒子集中的粒子;
步骤32:以更新后粒子集中的各个粒子为引领者随机产生N个跟随者,计算跟随者和引领者的可能性,利用可能性筛选所述引领者和跟随者,得到再次更新后的粒子集;
步骤33:利用再次更新后粒子集中粒子的权重计算有效粒子的数目Neff,当有效粒子数目Neff大于设定的阈值Φ,则不进行重采样,否则进行重采样,得到当前时刻最终的粒子集。
进一步的,所述步骤31具体为:
步骤312:利用所述亲和力faffinity计算当前时刻粒子需要克隆的数目Ncl,计算公式如下:
步骤313:将新获得的粒子添加至所述粒子集中,更新当前时刻的粒子集,利用所述亲和力faffinity对粒子进行变异操作,变异采用的公式为:
步骤314:利用变异后粒子间的位姿差计算两两粒子间的排斥力freject,计算公式为:
进一步的,所述步骤32具体为:
步骤321:对步骤21更新后粒子集中的每个粒子,以各个粒子为引领者随机产生对应的N个跟随者,分别计算引领者与跟随者的适应度ffitness,计算适应度采用的函数为:
步骤322:利用所述适应度ffitness计算各个粒子的可能性pi,采用的公式为:
步骤323:判断引领者和跟随者的可能性pi,当跟随者可能性大于引领者可能性,即利用可能性最大的跟随者替换引领者,否则不进行替换,得到再次更新后粒子集。
进一步的,所述步骤33具体为:
步骤331:利用更新后粒子集中粒子的权重计算有效粒子的数目Neff,计算公式为:
步骤332:当有效粒子数目Neff大于设定的阈值Φ,则不进行重采样,得到当前时刻最终的粒子集,否则进行重采样后跳转至步骤231。
进一步的,所述重采样为:复制权值大的粒子,去掉权值小的粒子进行,保持粒子集中粒子数目稳定。
进一步的,所述步骤4具体为:
步骤41:利用当前时刻最终粒子集中各粒子的粒子信息中的路标点位置,对不同时刻相同粒子观测到的同一路标点采用扩展卡尔曼滤波进行更新,得到当前时刻的路标点位置;
步骤43:利用当前时刻最终的路标点位置和路径中各点的位置,得到当前时刻机器人运动路径和路标点。
具体实施例1
一种全局优化的并发定位与建图方法,包括以下步骤:
步骤1:根据机器人的初始坐标,在初始时刻随机生成n个粒子,得到初始时刻的粒子集;
步骤2:采用粒子滤波的SLAM算法,利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息;
步骤21:采用机器人运动模型,利用上一时刻各粒子的粒子信息中的位姿数据得到当前时刻各粒子的位姿数据,并记录当前时刻粒子观测到的路标点;机器人的惯性传感器获取运动数据,所述运动信息中包括加速度数据和角度数据,所述加速度数据和角度数据即为位姿数据;机器人的视觉传感器观测所述路标点;
对于当前时刻观测到的路标,需要和数据库中已经观测过的路标进行关联,得到新的路标的关联表;
步骤22:采用提议分布对当前时刻的位姿数据和路标点位置进行更新,利用更新后的位姿数据和路标点位置计算各粒子的权重;
步骤23:对所述权重进行归一化处理,得到当前时刻各粒子的权重。
步骤3:由于多次迭代会造成粒子退化现象,造成很少一部分粒子权值较大,而大部分粒子权值较小,造成后续的计算很大一部分用在权值较小的粒子上面,而这些粒子又对最终机器人的位姿估计作用很小,造成资源的浪费。如果通过现有的重采样算法进行重采样,复制权值大的粒子,去除权值小的粒子,增加有效粒子数目,又会造成最后所有的粒子可能是由很少的一部分粒子重采样得来的,降低了粒子的多样性,造成粒子耗尽的问题;因此本发明采用全局优化和重采样对当前时刻各粒子进行更新;
步骤31:把粒子集中的粒子作为初始抗体种群存入记忆单元,将机器人的真实位姿作为抗原,粒子集合中的每一个粒子作为抗体。由于每一个粒子的权值可以表明当前粒子位姿估计的准确程度,所以认为每个粒子的亲和力和它自身的权值相关,如果权值大那么亲和力就小,权值小亲和力就大,通过亲和力可以控制粒子的克隆数目以及粒子的变异程度;因此对当前时刻的各粒子进行克隆和变异操作,更新粒子集中的粒子;
步骤312:利用所述亲和力faffinity计算当前时刻粒子需要克隆的数目Ncl,计算公式如下:
通过cos函数使得粒子复制的数目随着亲和力的增加而减少,也就是对权值高的粒子进行多复制,权值小的粒子减少复制,通过这种方法可以有效的抑制亲和力高的抗体而促进亲和力低的抗体,加快收敛速度。
步骤313:将新获得的粒子添加至所述粒子集中,更新当前时刻的粒子集,利用所述亲和力faffinity对粒子进行变异操作,通过变异操作使权值较大的粒子的位姿变化较小,而权值较小的粒子变化较大,从而使权值较小的粒子去寻找更好的位姿,增加粒子的多样性,变异采用的公式为:
步骤314:利用变异后粒子间的位姿差计算两两粒子间的排斥力freject,计算公式为:
步骤32:通过步骤31保留了权值较低的粒子的同时增加了粒子集的多样性,缓解了粒子退化问题,以更新后粒子集中的各个粒子为引领者随机产生N个跟随者,计算跟随者和引领者的可能性,利用可能性筛选所述引领者和跟随者,得到再次更新后的粒子集;
步骤321:以各个粒子为引领者在其领域内随机产生对应的N个跟随者(替代粒子),分别计算引领者与跟随者的适应度ffitness,计算适应度采用的函数为:
步骤322:利用所述适应度ffitness计算各个粒子的可能性pi,采用的公式为:
步骤323:判断引领者和跟随者的可能性pi,当跟随者可能性大于引领者可能性,即利用可能性最大的跟随者替换引领者,否则不进行替换,得到再次更新后粒子集。
步骤33:利用再次更新后粒子集中粒子的权重计算有效粒子的数目Neff,当有效粒子数目Neff大于设定的阈值Φ,则不进行重采样,否则进行重采样,得到当前时刻最终的粒子集。
步骤331:利用更新后粒子集中粒子的权重计算有效粒子的数目Neff,计算公式为:
步骤332:当有效粒子数目Neff大于设定的阈值Φ,则不进行重采样,得到当前时刻最终的粒子集,否则进行重采样后跳转至步骤31。
所述重采样为:复制权值大的粒子,去掉权值小的粒子进行,保持粒子集中粒子数目稳定。
步骤4:机器人在运动过程中,粒子集中每一个粒子对于观测的路标点都有自己的估计,所以每个粒子都有自己的地图,利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点;
步骤41:利用当前时刻最终粒子集中各粒子的粒子信息中的路标点位置,对不同时刻相同粒子观测到的同一路标点采用扩展卡尔曼滤波进行更新,得到当前时刻的路标点位置;
步骤43:利用当前时刻最终的路标点位置和路径中各点的位置,得到当前时刻机器人运动路径和路标点。
步骤5:完成当前时刻后,机器人继续运动进行下一时刻的运动,重复步骤2-4,直至机器人完成所有时刻的运动,生成地图信息和机器人的定位信息。
具体实施例2
本实施例基于具体实施例1,对本发明进行验证。
构建一个200m*200m的环形地图,如图3所示,其中路径点有21个,同时在路径点周围标定65个路标点用于模仿双目相机观察到的路标信息,在实验开始之前设定机器人从(0,0)开始运动,运动的速度是5m/s,运动的误差是0.5m/s,运动最大转向角是40度,最大转向角变化率25度/s,误差是4度。观测的距离误差是0.2m,角度误差是1度。
采用EKFSLAM算法进行处理,得到的位置估计均方根误差为2.1325,角度估计均方根误差为0.2379;
采用FastSLAM算法进行处理,得到的位置估计均方根误差为1.3826,角度估计均方根误差为0.1910;
而采用本发明进行处理,得到的位置估计均方根误差为1.0187,角度估计均方根误差为0.1473;
从以上数据可看出,采用本发明能提高机器人运动的定位精度。
Claims (6)
1.一种全局优化的并发定位与建图方法,其特征在于:包括以下步骤:
步骤1:根据机器人的初始坐标,在初始时刻随机生成n个粒子,得到初始时刻的粒子集;
步骤2:采用粒子滤波的SLAM算法,利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息;
步骤3:采用全局优化和重采样对当前时刻各粒子进行更新;
步骤4:利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点;
步骤5:重复步骤2-4,直至机器人完成所有时刻的运动,输出地图和机器人的定位信息;
所述步骤3具体步骤为:
步骤31:对当前时刻的各粒子进行克隆和变异操作,更新粒子集中的粒子;
步骤32:以更新后粒子集中的各个粒子为引领者随机产生N个跟随者,计算跟随者和引领者的可能性,利用可能性筛选所述引领者和跟随者,得到再次更新后的粒子集;
步骤33:利用再次更新后粒子集中粒子的权重计算有效粒子的数目Neff,当有效粒子数目Neff大于设定的阈值Φ,则不进行重采样,否则进行重采样,得到当前时刻最终的粒子集;
所述步骤31具体为:
步骤312:利用所述亲和力faffinity计算当前时刻粒子需要克隆的数目Ncl,计算公式如下:
步骤313:将新获得的粒子添加至所述粒子集中,更新当前时刻的粒子集,利用所述亲和力faffinity对粒子进行变异操作,变异采用的公式为:
步骤314:利用变异后粒子间的位姿差计算两两粒子间的排斥力freject,计算公式为:
所述步骤32具体为:
步骤321:对步骤21更新后粒子集中的每个粒子,以各个粒子为引领者随机产生对应的N个跟随者,分别计算引领者与跟随者的适应度ffitness,计算适应度采用的函数为:
步骤322:利用所述适应度ffitness计算各个粒子的可能性pi,采用的公式为:
步骤323:判断引领者和跟随者的可能性pi,当跟随者可能性大于引领者可能性,即利用可能性最大的跟随者替换引领者,否则不进行替换,得到再次更新后粒子集。
2.根据权利要求1所述的一种全局优化的并发定位与建图方法,其特征在于:所述步骤2具体为:
步骤21:采用机器人运动模型,利用上一时刻各粒子的粒子信息中的位姿数据得到当前时刻各粒子的位姿数据,并记录当前时刻粒子观测到的路标点;
步骤22:采用提议分布对当前时刻的位姿数据和路标点位置进行更新,利用更新后的位姿数据和路标点位置计算各粒子的权重;
步骤23:对所述权重进行归一化处理,得到当前时刻各粒子的权重。
3.根据权利要求1所述的一种全局优化的并发定位与建图方法,其特征在于:所述步骤32具体为:
步骤321:对步骤21更新后粒子集中的每个粒子,以各个粒子为引领者随机产生对应的N个跟随者,分别计算引领者与跟随者的适应度ffitness,计算适应度采用的函数为:
步骤322:利用所述适应度ffitness计算各个粒子的可能性pi,采用的公式为:
步骤323:判断引领者和跟随者的可能性pi,当跟随者可能性大于引领者可能性,即利用可能性最大的跟随者替换引领者,否则不进行替换,得到再次更新后粒子集。
5.根据权利要求4所述的一种全局优化的并发定位与建图方法,其特征在于:所述重采样为:复制权值大的粒子,去掉权值小的粒子进行,保持粒子集中粒子数目稳定。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810734733.6A CN108871341B (zh) | 2018-07-05 | 2018-07-05 | 一种全局优化的并发定位与建图方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810734733.6A CN108871341B (zh) | 2018-07-05 | 2018-07-05 | 一种全局优化的并发定位与建图方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108871341A CN108871341A (zh) | 2018-11-23 |
CN108871341B true CN108871341B (zh) | 2021-12-24 |
Family
ID=64299728
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810734733.6A Active CN108871341B (zh) | 2018-07-05 | 2018-07-05 | 一种全局优化的并发定位与建图方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108871341B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109798896B (zh) * | 2019-01-21 | 2023-01-03 | 东南大学 | 一种室内机器人定位与建图方法及装置 |
CN109917332B (zh) * | 2019-02-01 | 2022-12-16 | 广东工业大学 | 一种基于改进粒子滤波的室内机器人定位方法 |
CN110954113B (zh) * | 2019-05-30 | 2021-10-15 | 北京初速度科技有限公司 | 一种车辆位姿的修正方法和装置 |
CN111076724B (zh) * | 2019-12-06 | 2023-12-22 | 苏州艾吉威机器人有限公司 | 三维激光定位方法及*** |
CN111324116B (zh) * | 2020-02-14 | 2021-09-21 | 南京航空航天大学 | 基于粒子滤波的机器人定位方法 |
CN112698345B (zh) * | 2020-12-04 | 2024-01-30 | 江苏科技大学 | 一种激光雷达的机器人同时定位与建图优化方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR100816269B1 (ko) * | 2006-09-22 | 2008-03-25 | 학교법인 포항공과대학교 | 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법 |
CN103631264A (zh) * | 2013-12-04 | 2014-03-12 | 苏州大学张家港工业技术研究院 | 一种同时定位与地图创建方法及装置 |
CN104597900A (zh) * | 2014-12-02 | 2015-05-06 | 华东交通大学 | 一种基于类电磁机制优化的FastSLAM方法 |
CN105333879A (zh) * | 2015-12-14 | 2016-02-17 | 重庆邮电大学 | 同步定位与地图构建方法 |
CN106289257A (zh) * | 2016-07-27 | 2017-01-04 | 无锡知谷网络科技有限公司 | 室内定位方法及定位*** |
CN107103355A (zh) * | 2017-04-21 | 2017-08-29 | 北京工业大学 | 一种基于改进人工鱼群算法的智能车slam数据关联方法 |
-
2018
- 2018-07-05 CN CN201810734733.6A patent/CN108871341B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR100816269B1 (ko) * | 2006-09-22 | 2008-03-25 | 학교법인 포항공과대학교 | 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법 |
CN103631264A (zh) * | 2013-12-04 | 2014-03-12 | 苏州大学张家港工业技术研究院 | 一种同时定位与地图创建方法及装置 |
CN104597900A (zh) * | 2014-12-02 | 2015-05-06 | 华东交通大学 | 一种基于类电磁机制优化的FastSLAM方法 |
CN105333879A (zh) * | 2015-12-14 | 2016-02-17 | 重庆邮电大学 | 同步定位与地图构建方法 |
CN106289257A (zh) * | 2016-07-27 | 2017-01-04 | 无锡知谷网络科技有限公司 | 室内定位方法及定位*** |
CN107103355A (zh) * | 2017-04-21 | 2017-08-29 | 北京工业大学 | 一种基于改进人工鱼群算法的智能车slam数据关联方法 |
Non-Patent Citations (3)
Title |
---|
An improved FastSLAM algorithm using fitness sharing technique;Kwon O S 等;《Journal of Korean Institute of Intelligent Systems》;20121231;第22卷(第04期);第487-493页 * |
基于免疫克隆粒子群算法的无人机航迹规划;程泽等;《弹箭与制导学报》;20140630;第34卷(第03期);第179-182页 * |
移动机器人粒子滤波定位与地图创建方法研究;刘洞波;《中国博士学位论文全文数据库信息科技辑》;20140115;第I140-9页 * |
Also Published As
Publication number | Publication date |
---|---|
CN108871341A (zh) | 2018-11-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108871341B (zh) | 一种全局优化的并发定位与建图方法 | |
Zhang et al. | Navigation with IMU/GPS/digital compass with unscented Kalman filter | |
Ye et al. | EGP-CDKF for performance improvement of the SINS/GNSS integrated system | |
CN112639502A (zh) | 机器人位姿估计 | |
CN113050116B (zh) | 机器人定位方法、装置、机器人和可读存储介质 | |
Parikh et al. | Target tracking in the presence of intermittent measurements via motion model learning | |
CN111915675A (zh) | 基于粒子漂移的粒子滤波点云定位方法及其装置和*** | |
CN115639823A (zh) | 崎岖起伏地形下机器人地形感知与移动控制方法及*** | |
Assad et al. | Novel adaptive fuzzy extended Kalman filter for attitude estimation in GPS-denied environment | |
Mostafa et al. | A smart hybrid vision aided inertial navigation system approach for UAVs in a GNSS denied environment | |
Giremus et al. | An improved regularized particle filter for GPS/INS integration | |
Xia et al. | Low-cost MEMS-INS/GNSS integration using quaternion-based nonlinear filtering methods for USV | |
CN112577494A (zh) | 一种适用于月球车的slam方法、电子设备及存储介质 | |
Ryan | Information-theoretic tracking control based on particle filter estimate | |
Ma et al. | A contour-based path planning method for terrain-aided navigation systems with a single beam echo sounder | |
Pierce | Incorporation of a foot-mounted imu for multi-sensor pedestrian navigation | |
Hartmann et al. | Landmark initialization for unscented Kalman filter sensor fusion for monocular camera localization | |
Poddar et al. | Tuning of GPS aided attitude estimation using evolutionary algorithms | |
Marks et al. | Gamma‐SLAM: Visual SLAM in unstructured environments using variance grid maps | |
Aditya | Implementation of a 4D fast SLAM including volumetric sum of the UAV | |
Girrbach et al. | Adaptive compensation of measurement delays in multi-sensor fusion for inertial motion tracking using moving horizon estimation | |
Demim et al. | Simultaneous localization and mapping algorithm based on 3D laser for unmanned aerial vehicle | |
Fourmy | State estimation and localization of legged robots: a tightly-coupled approach based on a-posteriori maximization | |
Badalkhani et al. | Effects of Moving Landmark's Speed on Multi-Robot Simultaneous Localization and Mapping in Dynamic Environments. | |
Odry et al. | Adaptive Gradient-Descent Extended Kalman Filter for Pose Estimation of Mobile Robots with Sparse Reference Signals |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |