CN110333513B - 一种融合最小二乘法的粒子滤波slam方法 - Google Patents
一种融合最小二乘法的粒子滤波slam方法 Download PDFInfo
- Publication number
- CN110333513B CN110333513B CN201910621086.2A CN201910621086A CN110333513B CN 110333513 B CN110333513 B CN 110333513B CN 201910621086 A CN201910621086 A CN 201910621086A CN 110333513 B CN110333513 B CN 110333513B
- Authority
- CN
- China
- Prior art keywords
- thread
- robot
- map
- particle
- particles
- 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
- 239000002245 particle Substances 0.000 title claims abstract description 141
- 238000000034 method Methods 0.000 title claims abstract description 63
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 35
- 238000004364 calculation method Methods 0.000 claims abstract description 19
- 238000005070 sampling Methods 0.000 claims abstract description 12
- 238000012952 Resampling Methods 0.000 claims description 22
- 238000001914 filtration Methods 0.000 claims description 11
- 238000005259 measurement Methods 0.000 claims description 10
- 238000012163 sequencing technique Methods 0.000 claims description 4
- 230000003044 adaptive effect Effects 0.000 claims description 3
- 238000004088 simulation Methods 0.000 claims description 3
- 238000009826 distribution Methods 0.000 abstract description 17
- 230000008569 process Effects 0.000 abstract description 15
- 238000010276 construction Methods 0.000 abstract description 4
- 238000010586 diagram Methods 0.000 description 11
- 238000004519 manufacturing process Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- XKLMZUWKNUAPSZ-UHFFFAOYSA-N N-(2,6-dimethylphenyl)-2-{4-[2-hydroxy-3-(2-methoxyphenoxy)propyl]piperazin-1-yl}acetamide Chemical compound COC1=CC=CC=C1OCC(O)CN1CCN(CC(=O)NC=2C(=CC=CC=2C)C)CC1 XKLMZUWKNUAPSZ-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000011521 glass Substances 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000001483 mobilizing effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001737 promoting effect Effects 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
- 238000005728 strengthening Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000009827 uniform distribution Methods 0.000 description 1
Images
Classifications
-
- 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
Landscapes
- Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种融合最小二乘法的粒子滤波SLAM方法,涉及机器人即时定位与地图构建,解决了机器人在地图中的位置数据采样严重依赖里程计数据的问题。本发明包括线程Ⅰ和线程Ⅱ双线程执行以获取机器人在地图中的位置,由于线程Ⅱ所使用的粒子滤波算法耗时长,故在t‑1~t时间间隔内线程Ⅱ已经进行多次更新。线程Ⅱ内的最小二乘匹配算法的目的就是计算t‑1~t时间内的机器人相对移动位置,同时本申请使用了局部地图的方式,当线程Ⅰ内粒子滤波***更新完成后局部地图重置。其核心在于每次迭代过程中,使用最小二乘匹配计算得到的机器人位置对粒子集进行扩充,当一次更新间隔内里程计误差较大时可以有效将部分粒子限制在真实分布附近。
Description
技术领域
本发明涉及机器人即时定位与地图构建,具体涉及一种融合最小二乘法的粒子滤波SLAM方法。
背景技术
我国为推进实施“科技兴安”战略,调动社会力量参与安全生产科技攻关,进一步强化防范遏制重特大事故科技保障能力,在重点行业领域开展“机械化换人、自动化减人”科技强安专项行动,重点是以机械化生产替换人工作业、以自动化控制减少人为操作,大力提高企业安全生产科技保障能力。
在机器人的定位与地图构建中,通常情况下使用粒子滤波的SLAM算法可以得到一个较好的估计结果,但由于采样过程对里程计数据的严重依赖,导致当机器人里程计数据误差较大时,基于里程计的提议分布与目标分布严重不符,后续进行的扫描匹配,权值计算等步骤毫无意义。且由于粒子滤波***本身的复杂度较高,当机器人移动速度过快,t-1至t时刻的时间段内机器人位置已经有了较大变化,而此时算法正在执行t-1时刻的估计过程,这种现象会使未知环境下的扫描匹配过程精度下降。目前广泛应用研究的SLAM***大多专注于轮式移动机器人,且要求机器人自身携带定位传感器,如编码器,陀螺仪等,并由此计算得到机器人运动里程。但此类算法严重依赖里程计的精度,当地面有坑洼或轮子发生漂移时,里程计等传感器误差较大,极限情况下机器人实际位置并没有发生改变,但里程计数据已经显示机器人移动一段距离,这种情况不仅会使SLAM算法无法得到机器人的准确位置,创建的栅格地图也会发生撕裂、扭曲等现象。
发明内容
本发明目的在于提供一种融合最小二乘法的粒子滤波SLAM方法,该方法不需要过度依赖里程计数据作为估计初值,本申请解决的技术问题为:机器人在地图中的位置数据采样严重依赖里程计数据。
本发明通过下述技术方案实现:
一种融合最小二乘法的粒子滤波SLAM方法,包括线程Ⅰ和线程Ⅱ,所述线程Ⅰ为采用粒子滤波获取机器人在地图中的位置,线程Ⅱ为采用最小二乘匹配获取机器人在地图中的位置,具体步骤包括:
步骤1,t=0时,线程Ⅰ和线程Ⅱ同时获取雷达数据得到机器人在地图中的初始位置,此后,机器人开始移动;
步骤2,当机器人移动距离超过设定阈值时,进入t=1时刻,线程Ⅰ中粒子滤波算法使用里程计运动模型对粒子进行采样得到的样本代替地图内机器人位置,并根据权值对粒子进行排序;
步骤3,在t=0~t-1时间段内,线程Ⅱ同步获取雷达数据得到机器人位置x1′,则可得到粒子集内权值最大的N个粒子,使用最小二乘法对x1′采样得到的样本代替t=0~t-1时间段内机器人位置即对局部地图重置,得到扩充后的粒子集,粒子集内粒子的总数变为M+N;
步骤4,线程Ⅱ对粒子集进行扫描匹配、权值计算、重采样完成更新,其中权值最大的粒子代表的机器人在地图中的位置,即代表机器人此时的位置与环境地图数据;
步骤5,线程Ⅱ对粒子集进行排序,舍弃权值较小的N个粒子,粒子总数变为M,由于线程Ⅰ在t=1时刻地图内机器人位置更新已经完成,线程Ⅱ内对局部地图重置,待下一个t=2时刻到来时继续执行步骤2~5,则线程Ⅰ的一个更新时刻内线程Ⅱ会进行多次更新。
通过长期的研究和实践,本申请的发明人发现,在机器人的定位与地图构建中,由于数据机器人移动位置采样过程对里程计数据的严重依赖,当地面有坑洼或轮子发生漂移时,里程计等传感器误差较大,极限情况下机器人实际位置并没有发生改变,但里程计数据已经显示机器人移动一段距离,这种情况不仅会使SLAM算法无法得到机器人的准确位置,创建的栅格地图也会发生撕裂、扭曲等现象。本申请提供一种融合最小二乘法的粒子滤波SLAM方法,包括线程Ⅰ和线程Ⅱ双线程执行以获取机器人在地图中的位置,由于线程Ⅱ所使用的粒子滤波算法耗时长,故在t-1~t时间间隔内线程Ⅱ已经进行多次更新。线程Ⅱ内的最小二乘匹配算法的目的就是计算t-1~t时间内的机器人相对移动位置,同时本申请使用了局部地图的方式,当线程Ⅰ内粒子滤波***更新完成后局部地图重置。其核心在于每次迭代过程中,使用最小二乘匹配计算得到的机器人位置对粒子集进行扩充,当一次更新间隔内里程计误差较大时可以有效将部分粒子限制在真实分布附近。
进一步的,所述线程Ⅰ对机器人位置进行初始化的方式包括:默认初始机器人位姿向量为(0,0,0)T,采用ROS、Gazebo和RVIZ的三维仿真环境,并使用Gazebo中的激光雷达模拟获取虚拟激光数据,根据第一帧激光数据初始化环境地图。
进一步的,所述扫描匹配采用基于可能性区域模型的扫描匹配算法。
进一步的,所述权值计算与重采样包括第m个粒子的权值计算方式如下式3-22所示:
其中Neff又被称为有效粒子数,当Neff值小于某一给定阈值时进行重采样步骤,否则在权值计算完成后直接进行下一次算法迭代。
进一步的,所述建立观测模型包括采用激光雷达的可能性区域模型对观测信息进行建模。
进一步的,为了获取所述观测概率值,观测模型引入了测量噪声和随机测量噪声。
进一步的,所述随机测量噪声中,当激光传感器击中半透明玻璃,或者声纳传感器接收到同波段声波时都会产生不固定的随机噪声,此随机噪声用均匀分布表示,如下式3-10:
进一步的,观测概率值p(zt|xt,m)的计算公式3-10如下:
其中q表示每个激光数据的观测概率,zhit表示测量噪声出现的概率阈值,zrand表示随机噪声出现的概率阈值,zhit远大于zrand且两者和为1。
本发明具有如下的优点和有益效果:
每次迭代过程中,使用最小二乘匹配计算得到的机器人位置对粒子集进行扩充,当一次更新间隔内里程计误差较大时可以有效将部分粒子限制在真实分布附近,避免过度依赖里程计数据作为估计初值。
附图说明
此处所说明的附图用来提供对本发明实施例的进一步理解,构成本申请的一部分,并不构成对本发明实施例的限定。在附图中:
图1为本发明的流程图。
图2为本发明中线程Ⅰ的粒子滤波初始化地图示意图。
图3为本发明中待匹配激光数据示意图。
图4为本发明中粒子数为30的采样示意图。
图5a为本发明中粒子采样分布图。
图5b为本发明中扫描匹配粒子分布图。
图6为本发明中机器人运动轨迹示意图。
图7a为本发明中扫描匹配的距离误差对比示意图。
图7b为本发明中扫描匹配的角度误差对比示意图。
图8为本发明中线程Ⅰ和线程Ⅱ更新地图的时间轴示意图。
图9a为本发明中里程计漂移示意图。
图9b为本发明中里程计漂移结果图。
图10a为本发明中局部地图位置更新误差示意图。
图10b为本发明中局部地图角度更新误差示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白,下面结合实施例和附图,对本发明作进一步的详细说明,本发明的示意性实施方式及其说明仅用于解释本发明,并不作为对本发明的限定。
在以下描述中,为了提供对本发明的透彻理解阐述了大量特定细节。然而,对于本领域普通技术人员显而易见的是:不必采用这些特定细节来实行本发明。在其他实例中,为了避免混淆本发明,未具体描述公知的结构、电路、材料或方法。
在整个说明书中,对“一个实施例”、“实施例”、“一个示例”或“示例”的提及意味着:结合该实施例或示例描述的特定特征、结构或特性被包含在本发明至少一个实施例中。因此,在整个说明书的各个地方出现的短语“一个实施例”、“实施例”、“一个示例”或“示例”不一定都指同一实施例或示例。此外,可以以任何适当的组合和、或子组合将特定的特征、结构或特性组合在一个或多个实施例或示例中。此外,本领域普通技术人员应当理解,在此提供的示图都是为了说明的目的,并且示图不一定是按比例绘制的。这里使用的术语“和/或”包括一个或多个相关列出的项目的任何和所有组合。
在本发明的描述中,需要理解的是,术语“前”、“后”、“左”、“右”、“上”、“下”、“竖直”、“水平”、“高”、“低”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明保护范围的限制。
实施例
一种融合最小二乘法的粒子滤波SLAM方法,包括线程Ⅰ和线程Ⅱ,所述线程Ⅰ为采用粒子滤波获取机器人在地图中的位置,线程Ⅱ为采用最小二乘匹配获取机器人在地图中的位置,具体步骤包括:
步骤1,t=0时,线程Ⅰ和线程Ⅱ同时获取雷达数据得到机器人在地图中的初始位置,此后,机器人开始移动;
步骤2,当机器人移动距离超过设定阈值时,进入t=1时刻,线程Ⅰ中粒子滤波算法使用里程计运动模型对粒子进行采样得到的样本代替地图内机器人位置,并根据权值对粒子进行排序;
步骤3,在t=0~t-1时间段内,线程Ⅱ同步获取雷达数据得到机器人位置x1′,则可得到粒子集内权值最大的N个粒子,使用最小二乘法对x1′采样得到的样本代替t=0~t-1时间段内机器人位置即对局部地图重置,得到扩充后的粒子集,粒子集内粒子的总数变为M+N;
步骤4,线程Ⅱ对粒子集进行扫描匹配、权值计算、重采样完成更新,其中权值最大的粒子代表的机器人在地图中的位置,即代表机器人此时的位置与环境地图数据;
步骤5,线程Ⅱ对粒子集进行排序,舍弃权值较小的N个粒子,粒子总数变为M,由于线程Ⅰ在t=1时刻地图内机器人位置更新已经完成,线程Ⅱ内对局部地图重置,待下一个t=2时刻到来时继续执行步骤2~5,则线程Ⅰ的一个更新时刻内线程Ⅱ会进行多次更新。
通过长期的研究和实践,本申请的发明人发现,在机器人的定位与地图构建中,由于数据机器人移动位置采样过程对里程计数据的严重依赖,当地面有坑洼或轮子发生漂移时,里程计等传感器误差较大,极限情况下机器人实际位置并没有发生改变,但里程计数据已经显示机器人移动一段距离,这种情况不仅会使SLAM算法无法得到机器人的准确位置,创建的栅格地图也会发生撕裂、扭曲等现象。本申请提供一种融合最小二乘法的粒子滤波SLAM方法,包括线程Ⅰ和线程Ⅱ双线程执行以获取机器人在地图中的位置,由于线程Ⅱ所使用的粒子滤波算法耗时长,故在t-1~t时间间隔内线程Ⅱ已经进行多次更新。线程Ⅱ内的最小二乘匹配算法的目的就是计算t-1~t时间内的机器人相对移动位置,同时本申请使用了局部地图的方式,当线程Ⅰ内粒子滤波***更新完成后局部地图重置。其核心在于每次迭代过程中,使用最小二乘匹配计算得到的机器人位置对粒子集进行扩充,当一次更新间隔内里程计误差较大时可以有效将部分粒子限制在真实分布附近。
针对粒子滤波严重依赖里程计数据,本申请提出了一种融合最小二乘法的粒子滤波SLAM方法。其核心在于每次迭代过程中,使用最小二乘匹配计算得到的机器人位置对粒子集进行扩充,当一次更新间隔内里程计误差较大时可以有效将部分粒子限制在真实分布附近。算法整体流程如图1所示,包括线程Ⅰ和线程Ⅱ,所述线程Ⅰ为采用粒子滤波获取机器人在地图中的位置,线程Ⅱ为采用最小二乘匹配获取机器人在地图中的位置,由于线程Ⅱ所使用的粒子滤波算法耗时长,故在t-1~t时间间隔内线程Ⅱ已经进行多次更新。线程Ⅱ内的最小二乘匹配算法的目的就是计算t-1~t时间内的机器人相对移动位置,同时本申请使用了局部地图的方式,当线程Ⅰ内粒子滤波***更新完成后局部地图重置。具体的执行步骤如下:
1)初始化,开始运行时两线程同时执行初始化步骤,此时时间记作t=0。粒子滤波初始化方式为,SLAM算法起始即t=0时刻开始执行初始化步骤,由于此时未接收到任何传感器数据,因此本申请默认初始机器人位姿向量为(0,0,0)T,初始化步骤的目的有两个,一是根据粒子总数初始化粒子集,二是根据第一帧激光数据初始化环境地图,此地图将作为后续迭代估计算法的输入,初始化步骤得到的地图表示为m0。初始化完成后的粒子内容为式3-17,其中每个粒子的权值相同,均为1/M。
公式3-10中,q表示每个激光数据的观测概率,zhit表示测量噪声出现的概率阈值,zrand表示随机噪声出现的概率阈值,一般情况下zhit远大于zrand且两者和为1,具体的,设置地图分辨率为5cm,使用RVIZ观察初始化步骤得到的地图如图2所示,其中黑色区域表示机器人周围障碍物,白色区域表示空闲无障碍。最小二乘匹配线程则使用机器人初始位置更新地图。
2)当机器人移动距离超过设定阈值时,进入t=1时刻,线程Ⅰ中粒子滤波算法根据里程计信息对粒子进行采样,并根据权值对粒子进行排序;
3)由于在t=0~t-1时间段内,线程Ⅱ同步计算机器人位置,则可以得到线程中的机器人位置x1′,获取粒子集内权值最大的N个粒子,对x1′采样得到的样本代替其内机器人位置,得到扩充后的粒子集,粒子集总数变为M+N。
4)对粒子集进行扫描匹配、权值计算、重采样完成更新,其中权值最大的粒子所代表的机器人位置以及地图即表示了机器人此时的位置与环境地图数据。
扫描匹配。粒子滤波***的整体估计精度与粒子数目的选取密切相关,但对于SLAM算法,每个粒子内不仅包括机器人位置数据,还维护一份包含有多个个网格的栅格地图。粒子数目过大会使整个***运算时间增加,算法可用性降低。为了减少粒子数目,优化粒子采样结果,本申请使用了基于可能性区域模型的扫描匹配算法。其核心思想在于将激光数据与已有地图进行匹配,进一步改善机器人位置,特别的,图3即表示了一组待匹配地图与激光数据。
粒子滤波算法中,在权值计算步骤前根据激光雷达数据与当前栅格地图对每个粒子内的机器人位置数据进行优化。如公式3-20所示:
对于图4中所示的粒子分布,扫描匹配后的结果如图5a和图5b所示,可以看到,扫描匹配步骤后粒子总体开始朝机器人真实位置处变化。
更进一步,由于t时刻的粒子是由t-1时刻的粒子通过采样计算得到,粒子误差会随时间累积。为了更好的描述扫描匹配的作用,此处假设不进行重采样,即粒子集内的样本不会发生删除,取代等操作。定义误差和E,表示t时刻每个粒子内机器人位置与真实位置的偏差值。如下式:
采用图6中的仿真方式,记录有无扫描匹配过程的总误差和可以得到图7a和图7b,其中B线条表示无扫描匹配过程发生,A线条表示扫描匹配过程后的误差总和,可以看到扫描匹配算法极大的改善了粒子集中机器人位置分布情况,使其更接近机器人真实位置。但随着时间戳的增加,机器人不断移动,无论是否进行扫描匹配,粒子集代表的总体机器人位置误差都在不断增加,为了解决此问题,需要重采样步骤改善粒子内容,将误差较大的粒子舍弃,保留精准粒子。
权值计算与重采样。与标准粒子滤波算法中权值的计算略有不同,第m个粒子的权值计算方式如下式3-22所示:
其中Neff又被称为有效粒子数,当Neff值小于某一给定阈值时进行重采样步骤,否则在权值计算完成后直接进行下一次算法迭代。与标准粒子滤波中的重采样步骤类似,重采样发生时,每个粒子根据其权值的大小确定此粒子被保留的概率,小权值粒子会在重采样过程中由权值较大的粒子代替,保证粒子总数不发生变化。
5)对粒子集进行排序,舍弃权值较小的N个粒子,粒子总数变为M。由于线程Ⅰ在t=1时刻的更新已经完成,线程Ⅱ内局部地图重置,待下一个t=2时刻到来时继续执行2~5步骤。具体的,两线程的更新时间轴图8所示,线程Ⅰ的一个更新时刻内线程Ⅱ会进行多次更新。
为了描述融合后算法的效果,使用图9a所示的特殊情况来代替轮子打滑过程,使用键盘控制机器人向墙面移动,由于轮子持续转动导致里程计数据不断累积而机器人真实位置不发生变化。表1即描述了此情况下t时刻粒子的分布情况,表1表示里程计漂移时标准粒子分布,暂时不考虑角度数据,t-1时刻的机器人位置为(2.37,1.97),t时刻的机器人位置为(2.71.1.97)取粒子数目为30。
表1
从表1可以看出,由于t-1时刻进行的扫描匹配步骤,导致粒子集内机器人位置分布较为集中,而在t-1~t时刻内由于漂移现象里程计误差较大,t时刻采样得到的粒子中很难找到与真实位置相近的粒子,这种现象会直接导致定位精度下降,地图发生撕裂,如图9b所示。针对上述情况,使用融合最小二乘匹配的SLAM算法,粒子总数设置为25,每次采样时扩充的粒子数为5,t时刻的粒子分布如下表2所示,表2表示里程计漂移时改进粒子滤波算法粒子分布,可以看到,由于线程Ⅱ中最小二乘匹配的作用,扩充后序号为26~30的5个粒子并没有根据里程计数据进行采样,当其他粒子无法表示真实机器人位置时,仍能得到一个较好的估计结果。
表2
同样控制机器人沿着图6中描述的轨迹移动,记录每次局部地图更新结果并与机器人真实移动量进行对比,x方向、y方向、角度的误差图像如图10a和图10b所示,可以看到,随着机器人移动,局部地图更新位置误差基本可以保持在10cm内,角度误差基本保持在0.1弧度内,精度较高。
以上所述的具体实施方式,对本发明的目的、技术方案和有益效果进行了进一步详细说明,所应理解的是,以上所述仅为本发明的具体实施方式而已,并不用于限定本发明的保护范围,凡在本发明的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (7)
1.一种融合最小二乘法的粒子滤波SLAM方法,其特征在于,包括线程Ⅰ和线程Ⅱ双线程执行以获取机器人在地图中的位置,所述线程Ⅰ为采用粒子滤波获取机器人在地图中的位置,线程Ⅱ为采用最小二乘匹配获取机器人在地图中的位置,具体步骤包括:
步骤1,t=0时,线程Ⅰ和线程Ⅱ同时获取雷达数据得到机器人在地图中的初始位置,此后,机器人开始移动;
步骤2,当机器人移动距离超过设定阈值时,进入t=1时刻,线程Ⅰ中粒子滤波算法使用里程计运动模型对粒子进行采样得到的样本代替地图内机器人位置,并根据权值对粒子进行排序;
步骤3,在t=0~t-1时间段内,线程Ⅱ同步获取雷达数据得到机器人位置x1′,则可得到粒子集内权值最大的N个粒子,使用最小二乘法对x1′采样得到的样本代替t=0~t-1时间段内机器人位置即对局部地图重置,得到扩充后的粒子集,粒子集内粒子的总数变为M+N;
步骤4,线程Ⅱ对粒子集进行扫描匹配、权值计算、重采样完成更新,其中权值最大的粒子代表的机器人在地图中的位置,即代表机器人此时的位置与环境地图数据;
步骤5,线程Ⅱ对粒子集进行排序,舍弃权值较小的N个粒子,粒子总数变为M,由于线程Ⅰ在t=1时刻地图内机器人位置更新已经完成,线程Ⅱ内对局部地图重置,待下一个t=2时刻到来时继续执行步骤2~5,则线程Ⅰ的一个更新时刻内线程Ⅱ会进行多次更新。
2.根据权利要求1所述的一种融合最小二乘法的粒子滤波SLAM方法,其特征在于,所述线程Ⅰ对机器人位置进行初始化的方式包括:默认初始机器人位姿向量为(0,0,0)T,采用ROS、Gazebo和RVIZ的三维仿真环境,并使用Gazebo中的激光雷达模拟获取虚拟激光数据,根据第一帧激光数据初始化环境地图。
3.根据权利要求1所述的一种融合最小二乘法的粒子滤波SLAM方法,其特征在于,所述扫描匹配采用基于可能性区域模型的扫描匹配算法。
6.根据权利要求5所述的一种融合最小二乘法的粒子滤波SLAM方法,其特征在于,所述建立观测模型包括采用激光雷达的可能性区域模型对观测信息进行建模。
7.根据权利要求6所述的一种融合最小二乘法的粒子滤波SLAM方法,其特征在于,为了获取所述观测概率值,观测模型引入了测量噪声和随机测量噪声。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910621086.2A CN110333513B (zh) | 2019-07-10 | 2019-07-10 | 一种融合最小二乘法的粒子滤波slam方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910621086.2A CN110333513B (zh) | 2019-07-10 | 2019-07-10 | 一种融合最小二乘法的粒子滤波slam方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110333513A CN110333513A (zh) | 2019-10-15 |
CN110333513B true CN110333513B (zh) | 2023-01-10 |
Family
ID=68146230
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910621086.2A Active CN110333513B (zh) | 2019-07-10 | 2019-07-10 | 一种融合最小二乘法的粒子滤波slam方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110333513B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110887489A (zh) * | 2019-11-22 | 2020-03-17 | 深圳晨芯时代科技有限公司 | 一种基于ar机器人的slam算法的实验方法 |
CN113032411A (zh) * | 2021-03-02 | 2021-06-25 | 上海景吾智能科技有限公司 | 在已有地图的基础上更新局部地图的方法、***及介质 |
Citations (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR100809352B1 (ko) * | 2006-11-16 | 2008-03-05 | 삼성전자주식회사 | 파티클 필터 기반의 이동 로봇의 자세 추정 방법 및 장치 |
CN104197958A (zh) * | 2014-08-27 | 2014-12-10 | 北京航空航天大学 | 一种基于激光测速仪航位推算***的里程计标定方法 |
CN104503339A (zh) * | 2015-01-05 | 2015-04-08 | 黑龙江工程学院 | 基于激光雷达和四轴飞行器的多分辨室内三维场景重构装置及方法 |
CN105631017A (zh) * | 2015-12-29 | 2016-06-01 | 福州华鹰重工机械有限公司 | 离线坐标校准和地图创建的方法及装置 |
CN105865449A (zh) * | 2016-04-01 | 2016-08-17 | 深圳杉川科技有限公司 | 基于激光和视觉的移动机器人的混合定位方法 |
WO2016162568A1 (en) * | 2015-04-10 | 2016-10-13 | The European Atomic Energy Community (Euratom), Represented By The European Commission | Method and device for real-time mapping and localization |
CN106211072A (zh) * | 2016-07-14 | 2016-12-07 | 广东工业大学 | 一种小型移动主用户的定位方法及装置 |
CN106595659A (zh) * | 2016-11-03 | 2017-04-26 | 南京航空航天大学 | 城市复杂环境下多无人机视觉slam的地图融合方法 |
CN106969768A (zh) * | 2017-04-22 | 2017-07-21 | 深圳力子机器人有限公司 | 一种无轨导航agv的精确定位及停车方法 |
CN107240133A (zh) * | 2017-04-24 | 2017-10-10 | 西华大学 | 一种立体视觉映射模型建立方法 |
CN107356252A (zh) * | 2017-06-02 | 2017-11-17 | 青岛克路德机器人有限公司 | 一种融合视觉里程计与物理里程计的室内机器人定位方法 |
CN107607107A (zh) * | 2017-09-14 | 2018-01-19 | 斯坦德机器人(深圳)有限公司 | 一种基于先验信息的Slam方法和装置 |
CN108917759A (zh) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | 基于多层次地图匹配的移动机器人位姿纠正算法 |
CN109579824A (zh) * | 2018-10-31 | 2019-04-05 | 重庆邮电大学 | 一种融入二维码信息的自适应蒙特卡诺定位方法 |
CN109709801A (zh) * | 2018-12-11 | 2019-05-03 | 智灵飞(北京)科技有限公司 | 一种基于激光雷达的室内无人机定位***及方法 |
CN109752725A (zh) * | 2019-01-14 | 2019-05-14 | 天合光能股份有限公司 | 一种低速商用机器人、定位导航方法及定位导航*** |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10317421B2 (en) * | 2014-03-31 | 2019-06-11 | Stmicroelectronics S.R.L | Positioning apparatus comprising an inertial sensor and inertial sensor temperature compensation method |
-
2019
- 2019-07-10 CN CN201910621086.2A patent/CN110333513B/zh active Active
Patent Citations (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR100809352B1 (ko) * | 2006-11-16 | 2008-03-05 | 삼성전자주식회사 | 파티클 필터 기반의 이동 로봇의 자세 추정 방법 및 장치 |
CN104197958A (zh) * | 2014-08-27 | 2014-12-10 | 北京航空航天大学 | 一种基于激光测速仪航位推算***的里程计标定方法 |
CN104503339A (zh) * | 2015-01-05 | 2015-04-08 | 黑龙江工程学院 | 基于激光雷达和四轴飞行器的多分辨室内三维场景重构装置及方法 |
WO2016162568A1 (en) * | 2015-04-10 | 2016-10-13 | The European Atomic Energy Community (Euratom), Represented By The European Commission | Method and device for real-time mapping and localization |
CN105631017A (zh) * | 2015-12-29 | 2016-06-01 | 福州华鹰重工机械有限公司 | 离线坐标校准和地图创建的方法及装置 |
CN105865449A (zh) * | 2016-04-01 | 2016-08-17 | 深圳杉川科技有限公司 | 基于激光和视觉的移动机器人的混合定位方法 |
CN106211072A (zh) * | 2016-07-14 | 2016-12-07 | 广东工业大学 | 一种小型移动主用户的定位方法及装置 |
CN106595659A (zh) * | 2016-11-03 | 2017-04-26 | 南京航空航天大学 | 城市复杂环境下多无人机视觉slam的地图融合方法 |
CN106969768A (zh) * | 2017-04-22 | 2017-07-21 | 深圳力子机器人有限公司 | 一种无轨导航agv的精确定位及停车方法 |
CN107240133A (zh) * | 2017-04-24 | 2017-10-10 | 西华大学 | 一种立体视觉映射模型建立方法 |
CN107356252A (zh) * | 2017-06-02 | 2017-11-17 | 青岛克路德机器人有限公司 | 一种融合视觉里程计与物理里程计的室内机器人定位方法 |
CN107607107A (zh) * | 2017-09-14 | 2018-01-19 | 斯坦德机器人(深圳)有限公司 | 一种基于先验信息的Slam方法和装置 |
CN108917759A (zh) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | 基于多层次地图匹配的移动机器人位姿纠正算法 |
CN109579824A (zh) * | 2018-10-31 | 2019-04-05 | 重庆邮电大学 | 一种融入二维码信息的自适应蒙特卡诺定位方法 |
CN109709801A (zh) * | 2018-12-11 | 2019-05-03 | 智灵飞(北京)科技有限公司 | 一种基于激光雷达的室内无人机定位***及方法 |
CN109752725A (zh) * | 2019-01-14 | 2019-05-14 | 天合光能股份有限公司 | 一种低速商用机器人、定位导航方法及定位导航*** |
Non-Patent Citations (3)
Title |
---|
"Simultaneous localization and mapping embedded with particle filter algorithm";Wei Wang 等;《2016 10th European Conference on Antennas and Propagation (EuCAP)》;20161231;1-4 * |
"室内环境下同步定位与地图创建改进算法";赵立军;《机器人》;20090930;438-444 * |
"移动机器人同步定位与地图构建关键技术的研究";曲丽萍;《中国博士论文全文数据库》;20141231;全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN110333513A (zh) | 2019-10-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110007670B (zh) | 移动机器人定位建图方法 | |
CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
CN110866927B (zh) | 一种基于垂足点线特征结合的ekf-slam算法的机器人定位与构图方法 | |
CN109798896B (zh) | 一种室内机器人定位与建图方法及装置 | |
CN111324848B (zh) | 移动激光雷达测量***车载轨迹数据优化方法 | |
CN112882056B (zh) | 基于激光雷达的移动机器人同步定位与地图构建方法 | |
CN111060099B (zh) | 一种无人驾驶汽车实时定位方法 | |
CN111521195B (zh) | 一种智能机器人 | |
CN112612862B (zh) | 一种基于点云配准的栅格地图定位方法 | |
CN110333513B (zh) | 一种融合最小二乘法的粒子滤波slam方法 | |
CN109579824A (zh) | 一种融入二维码信息的自适应蒙特卡诺定位方法 | |
CN114237235B (zh) | 一种基于深度强化学习的移动机器人避障方法 | |
CN110736456A (zh) | 稀疏环境下基于特征提取的二维激光实时定位方法 | |
CN112880674A (zh) | 一种行驶设备的定位方法、装置、设备及存储介质 | |
CN108152812B (zh) | 一种调整网格间距的改进agimm跟踪方法 | |
CN114387462A (zh) | 一种基于双目相机的动态环境感知方法 | |
CN114608568B (zh) | 一种基于多传感器信息即时融合定位方法 | |
CN116385493A (zh) | 野外环境下多运动目标检测与轨迹预测方法 | |
Zhao et al. | Adaptive non-linear joint probabilistic data association for vehicle target tracking | |
CN116774247A (zh) | 基于ekf的多源信息融合的slam前端策略 | |
CN115103299B (zh) | 一种基于rfid的多传感器融合定位方法 | |
CN116380039A (zh) | 一种基于固态激光雷达和点云地图的移动机器人导航*** | |
CN114115263B (zh) | 用于agv的自主建图方法、装置、移动机器人及介质 | |
CN115950414A (zh) | 一种不同传感器数据的自适应多重融合slam方法 | |
CN113554705B (zh) | 一种变化场景下的激光雷达鲁棒定位方法 |
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 |