CN111427370A - 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法 - Google Patents
一种基于稀疏位姿调整的移动机器人的Gmapping建图方法 Download PDFInfo
- Publication number
- CN111427370A CN111427370A CN202010515565.9A CN202010515565A CN111427370A CN 111427370 A CN111427370 A CN 111427370A CN 202010515565 A CN202010515565 A CN 202010515565A CN 111427370 A CN111427370 A CN 111427370A
- Authority
- CN
- China
- Prior art keywords
- pose
- robot
- particle
- map
- updating
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 92
- 238000013507 mapping Methods 0.000 title claims abstract description 27
- 239000002245 particle Substances 0.000 claims abstract description 131
- 238000010276 construction Methods 0.000 claims abstract description 25
- 238000009826 distribution Methods 0.000 claims abstract description 23
- 238000005070 sampling Methods 0.000 claims abstract description 13
- 238000004364 calculation method Methods 0.000 claims description 46
- 238000005457 optimization Methods 0.000 claims description 31
- 230000008569 process Effects 0.000 claims description 27
- 238000012952 Resampling Methods 0.000 claims description 22
- 239000011159 matrix material Substances 0.000 claims description 21
- 238000007476 Maximum Likelihood Methods 0.000 claims description 10
- 238000012937 correction Methods 0.000 claims description 6
- 238000010606 normalization Methods 0.000 claims description 6
- 230000015572 biosynthetic process Effects 0.000 claims description 4
- 238000003786 synthesis reaction Methods 0.000 claims description 4
- 230000002093 peripheral effect Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 abstract description 37
- 230000007812 deficiency Effects 0.000 abstract description 2
- 230000006870 function Effects 0.000 description 14
- 238000002474 experimental method Methods 0.000 description 10
- 238000010586 diagram Methods 0.000 description 9
- 238000011160 research Methods 0.000 description 7
- 230000036544 posture Effects 0.000 description 5
- 238000004891 communication Methods 0.000 description 4
- 230000004807 localization Effects 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000006243 chemical reaction Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 239000003638 chemical reducing agent Substances 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000002955 isolation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 229910052705 radium Inorganic materials 0.000 description 1
- HCWPIIXVSYCSAN-UHFFFAOYSA-N radium atom Chemical compound [Ra] HCWPIIXVSYCSAN-UHFFFAOYSA-N 0.000 description 1
- 238000011897 real-time detection Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000000638 solvent extraction Methods 0.000 description 1
- 238000003860 storage Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0253—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting relative motion information from a plurality of images taken successively, e.g. visual odometry, optical flow
-
- 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/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- 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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
Description
技术领域
本发明涉及机器人技术领域,尤其涉及一种基于稀疏位姿调整的移动机器人的Gmapping建图方法。
背景技术
近年来,随着“工业4.0”、“智能制造”及“中国制造2025”等概念的提出,机器人领域取得了长足的进步与蓬勃的发展。在服务机器人领域,室内移动机器人的研究成为了一个热点问题。当前,对室内移动机器人的研究主要围绕地图构建、定位、导航等方面展开,即解决移动机器人的“我是谁”、“我在哪儿”及“我要去哪儿”的问题。但以上问题并不是孤立存在的,在未知环境下精确的地图构建依赖于准确的定位信息,而精准的定位依赖于准确的建图,移动机器人的自主导航与路径规划则依赖于准确的环境地图与定位,所以移动机器人的研究的首要问题就是建图与定位问题。
由于Gmapping-SLAM算法在环境复杂、里程计收到噪声干扰且噪声不全为高斯噪声时,可能会产生滤波器的过度过滤问题。基于此,本发明针对Gmapping-SLAM进行了改进,使用图优化理论和闭环检测对Gmapping的扫描匹配过程进行了全局非线性优化,使其可以在保持原有构建地图的速度的基础上,增加了构建栅格地图的质量,从而实现对Gmapping-SLAM的优化。
发明内容
为解决原有Gmapping算法在较少粒子时存在的边界模糊、缺失、及滑移的技术问题,本发明提供一种基于稀疏位姿调整的移动机器人的Gmapping建图方法,包括:
S5:更新第i个粒子的权重,所述第i个粒子的权重的更新公式为:
S6:更新机器人位姿与粒子地图,所述更新机器人位姿的方法为:通过所述第i个粒子的更新位姿与位姿修正量更新机器人位姿,所述更新粒子地图的方法为:通过所述直接稀疏特征矩阵H、第i个粒子的更新位姿、观测量,及更新第i个粒子的地图及采样粒子集更新粒子地图及粒子信息;
S7:判断地图创建是否完成,若地图创建完成,则结束流程;
若地图创建未完成,则判断是否进行粒子重采样;
判断是否进行粒子重采样的方法为:
S3’:构建位姿图;所述位姿图构建包括,计算机器人极大似然估计值,然后进入S4’;
优选的,所述提议分布的计算公式为:
优选的,所述误差雅克比矩阵J的计算方法为:
附图说明
图1为实施例一提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法算法流程图
图2为实施例二提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法的移动机器人SLAM过程示意图
图3为实施例二提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法的移动机器人坐标系图
图4为实施例二提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法的激光雷达坐标表示图
图5为实施例二提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法的相邻时刻机器人位姿估计图
图6(a)为实施例二提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法的无障碍添加时建图实验使用15个粒子的Gmapping算法建图
图6(b)为实施例二提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法的无障碍添加时建图实验使用15个粒子的本发明算法建图。
具体实施方式
下面结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护范围。
实施例一
本实施例提供了一种基于稀疏位姿调整的移动机器人的Gmapping建图方法,如图1所示,步骤包括:
S5:更新第i个粒子的权重,所述第i个粒子的权重的更新公式为:
S6:更新机器人位姿与粒子地图,所述更新机器人位姿的方法为:通过所述第i个粒子的更新位姿与位姿修正量更新机器人位姿,所述更新粒子地图的方法为:通过所述直接稀疏特征矩阵H、第i个粒子的更新位姿、观测量,及更新第i个粒子的地图及采样粒子集更新粒子地图及粒子信息;
S7:判断地图创建是否完成,若地图创建完成,则结束流程;
若地图创建未完成,则判断是否进行粒子重采样;
判断是否进行粒子重采样的方法为:
优选的,所述提议分布的计算公式为:
所述第i个粒子的机器人估计位姿的计算公式为:
优选的,所述误差雅克比矩阵J的计算方法为:
本实施例主要针对移动机器人在大型室内复杂环境下的2D激光雷达建图方法进行了研究。对其中的具有代表性、应用广泛的ROS Gmapping-SLAM进行了深入研究,针对其在大型复杂非结构化室内场景下的地图构建过程中的边界模糊与边界畸变等问题,提出了一种基于SPA优化的改进Gmapping算法。实现了***建模与搭建基于ROS与Stage的实验平台,在此基础上进行了两组实验并对实验结果进行了分析。最后得出了相同粒子数下,本发明提出的基于SPA优化的改进Gmapping算法具有更高的建图精度、鲁棒性与稳定性的结论。并通过研究发现,使用本发明提出的基于SPA优化的改进Gmapping算法相比ROS Gmapping-SLAM可以降低Gmapping-SLAM建图所需的粒子数,节省了计算资源,降低了对硬件的要求,具有更广泛的应用范围和实用价值。
实施例二
本实施例在实施例一的基础上,进一步提供了一种基于稀疏位姿调整的移动机器人的Gmapping建图方法,如图2-图6(b)所示。
当前,所有的激光雷达2D SLAM算法在地图构建过程中均依赖于概率模型,其本质是移动机器人的状态估计问题。应用概率模型的优点即是可以增强***对观测噪声的鲁棒性与***在测量估计过程中正式表示其不确定性的能力。基于概率模型的激光雷达2DSLAM(2维平面同时定位与建图方法)主要依靠贝叶斯定理求解。基于滤波器的SLAM主要可分为基于卡尔曼滤波器KF(Kalman filters)的SLAM和基于粒子滤波PF(Particlefilters)的SLAM方法。
KF滤波器是贝叶斯滤波器的成功实现之一,主要包含状态空间预测与状态更新两个阶段。为解决移动机器人模型的动态非线性问题,诞生了扩展卡尔曼滤波器EKF(Extended Kalman Filter),采用1阶泰勒公式,将非线性问题线性化。由于基于卡尔曼滤波器的SLAM计算量大,***易受环境噪声影响鲁棒性较差且长时间运行存在线性化误差积累等缺点,催生出了基于PF的SLAM方法。
基于粒子滤波器PF的SLAM方法是贝叶斯滤波器的另一个应用,后验概率由一组加权的粒子表示,赋予每个粒子重要性,并假设当前状态只与上一状态有关。基于PF的SLAM代表性方法是Montemarlo(蒙特卡罗)等提出的FastSLAM1.0(实时同时定位与建图框架)及FastSLAM2.0(实时同时定位与建图框架)算法。其核心思想是每个粒子具有K个KF(卡尔曼滤波器),以此来计算地图K个地标的位置,从而实现对地图的估计,通过PF(粒子滤波器)对机器人轨迹进行估计。该算法的时间复杂度要低于EKF-SLAM(基于扩展卡尔曼滤波器的同时定位与建图方法)。算法主要分为采样、分配权重、重采样和地图估计四个过程。
由于FastSLAM(实时同时定位与建图框架)的提议分布中没有考虑观测量,所以导致其提议分布具有盲目性。同时,每当有观察到新的观测量,***就要重新计算所有粒子的权重,并进行重采样过程,而这就导致了算法所需粒子数目过多及粒子退化问题。在大规模长时间建图过程中,会导致算法陷入局部最优,从而导致算法失效的问题。针对此问题,Grisettiyz等人提出了基于RBPF(改进的粒子滤波器)的改进SLAM方法,也就是ROS***中集成的Gmapping算法。该算法在采样过程中的提议分布中加入了观测量,使得粒子仅分布于机器人位姿的局部范围;更新了权重的计算公式,使其具有递归性;加入了自适应重采样过程,仅当粒子数小于有效粒子数时,开始重采样过程,大大缩减了重采样的次数与所需粒子数。移动机器人SLAM过程示意图如图1所示。从t-1时刻到t+2时刻的移动机器人的位姿表示为,里程计的运动控制量表示为,地标表示为,激光雷达的扫描观测量表示为。
由于Gmapping-SLAM(基于Gmapping算法的同时定位与建图方法)在提议分布中加入了观测量,更新了权重计算公式,并加入了自适应重采样过程,使得其在初始化时具有较为准确的估计值。采用概率估计的方法,当前时刻的估计量仅与上一时刻的状态有关,减少了内存消耗,保证其可以使用较少粒子的情况下,高效建出较为精确的地图。但在大规模复杂室内环境中,特别是里程计信息受到噪声干扰时,Gmapping-SLAM的滤波器会产生过度过滤的问题;而在观察期间,由于观测不良或者重叠过小会导致扫描匹配的问题,从而影响了建图质量。基于Gmapping-SLAM建图过程中出现的一些问题,国内外的一些学者提出了针对Gmapping-SLAM的改进方法。
罗元等提出了一种针对RBPF-SLAM(基于改进粒子滤波器的同时定位与建图方法)的改进算法,在特定状态下的粒子集中选取一个代表粒子,进行KF(卡尔曼滤波器)状态估计与更新;同时结合了Gmapping-SLAM(基于Gmapping算法的同时定位与建图方法)的提议分布与自适应重采样技术,降低了算法复杂度,提高了***的实时性,但降低了***的鲁棒性。王田橙等针对粒子退化与粒子衰竭等问题,提出了一种改进的SLAM方法。该方法使用区域粒子群优化算法调整粒子的提议分布,使用部分高斯分布重采样方法对权值过高过低的粒子进行重采样,提高了建图精度,提升了效率。Hui Xiong等提出了一种基于粒子滤波的扫描匹配SLAM方法,该方法可以在线构建复杂环境的二维栅格地图,可以减少内存消耗和循环闭合的问题,并构建较为准确的地图。Liu Fuchun等提出了一种改进的RBPF-SLAM(基于改进粒子滤波器的同时定位与建图方法),在激光扫描分区时使用动态阈值,使用几何匹配和定位的高精度特征改进了RBPF的重要性密度函数,提升了***的鲁棒性与环境构建的一致性。Taizhi Lv等提出了FC&ASD-PSO(分数演算和阿尔法稳定分布)的改进FastSLAM2.0(实时同时定位与建图框架2.0)算法,该算法引入了调整噪声协方差矩阵Qt和Rt的步骤,使噪声协方差矩阵的值与实际值相匹配,减少了累积误差的影响,提升了建图精度。
以上学者均对基于RBPF(改进粒子滤波器)的SLAM做出了一些研究与改动,但是对于在机器人领域应用广泛的Gmapping-SLAM(基于Gmapping算法的同时定位与建图方法)缺乏改进。因此本发明针对大范围复杂环境下Gmapping-SLAM进行了优化改进,在保证Gmapping算法使用较少粒子、快速建图的优势上,加入后端闭环检测与基于位姿图优化的方法,使其构建更加准确的环境地图。
稀疏位姿调整SPA(Sparse Pose Adjustment)位姿图优化方法是一种可以从约束图中在线计算稀疏矩阵并使用直接稀疏cholesky分解法求解线性***的方法,是一种非线性优化的位姿图优化方法。非线性优化是一种全局优化的方法,与EKF(扩展卡尔曼滤波器)等基于滤波器的方法相比,非线性优化可以有效地消除***的线性误差与累积误差,从而得到比较好的优化效果与地图。基于图的优化最早被认为是一种消耗计算资源的方法,近年来随着机器人***计算能力的快速发展与一系列更为高效的优化方法出现,基于位姿图的优化方法逐渐成为了研究热点之一。在移动机器人位姿图构建的过程中,将机器人的每个时刻下的运动状态(位姿和观测量)看成一个节点,运动状态之间的约束(坐标转换)看成一条边,将一个个节点通过边连接起来,就构成了位姿图。因此,地图构建过程就可以转化为找到满足当前所有约束下的最优节点,使用图的均值表示地图。其目标是共同优化节点的位姿,从而将边带来的误差最小化,基于图优化的SLAM框架图如图2所示。
非线性优化经典方法有捆绑调整Bundle Adjustment、Levenberg-Marquardt非线性优化器及Graph-SLAM(基于位姿图的同时定位与建图)等方法。但传统的非线性优化随着环境的增大,内存会呈现快速增长的趋势,计算量会大幅增加,时间复杂度较高。SPA(基于稀疏位姿调整的位姿图优化方法)是基于直接稀疏分解cholesky方法,采用构造稀疏矩阵的方式进行后端图优化,节省了内存,降低了算法复杂度,适用于大规模的场景建图过程中。同经典非线性优化的方法相比,具有以下几点优势。
1)SPA考虑了约束中的协方差信息,更加准确。
2)SPA具有很强的鲁棒性,在增量处理地图与批量处理下均高效,且具有较低的故障率和较快的收敛速度。
3)与EKF等滤波器相比,SPA是完全非线性方法。每次迭代时,SPA会针对当前位姿的所有约束进行线性化优化。
4)SPA在建图过程中可以连续优化位姿图,从而增量的构建地图。在大型环境且闭环较大时也无需借助子图存储或复杂分区的方案,可以占用很少的计算资源实现所有节点的最佳全局估计。
本实施例提出了一种基于SPA优化算法改进的Gmapping地图构建方法,将图优化过程作为与前端交错运行的单独线程实现对地图构建过程中的优化。算法步骤如下:
Step2:进行扫描匹配。基于地图信息、机器人估计位姿、观测量从周围有限区域进行扫描匹配。若扫描匹配成功,则求解机器人位姿的极大似然估计值,计算公式如(3)所示;并运行图优化线程,进行位姿图构建、闭环检测与SPA优化,公式如(4-8)所示。式中H为直接稀疏特征矩阵,构造线性***函数模型如(4)所示。为梯度下降法到Newton-Euler(牛顿-欧拉)法的转换因子,为线性方程待求量。J为误差雅克比矩阵,定义为,如公式(5)所示。为精度矩阵,其值为协方差的倒数。第i时刻机器人位姿定义为,两个位姿、之间的观测偏移量定义为,为关于的矩阵。观测量测量定义为,计算见(6)所示,为误差函数,计算见公式(7)、(8)所示。
若扫描匹配失败,则跳过Step3、Step4,重新计算提议分布、更新粒子权值与位姿估计,计算公式如(9)、(10)所示。
Step5:更新权重,见公式(15);通过图优化过程更新粒子的位姿,如公式(4)所示。
Step6:通过新位姿和观测量更新粒子i的地图并更新采样粒子集。
Step7:进行粒子重采样过程。计算有效样本量是否小于阈值T,计算公式如(16)所示,若小于阈值,则进行重采样,否则不需要进行重采样。若重采样成功,返回Step5;若无需重采样,返回Step1。
针对两轮差动机器人模型的进行坐标系转换、激光雷达建模与里程计建模。
基于激光雷达的移动机器人坐标系主要包含世界坐标系、机器人坐标系及激光雷达坐标系。在机器人坐标系的表示中,采用笛卡尔坐标系是最广泛、最直观的表示方式。世界坐标系是固定在环境中的绝对坐标系,采用直角坐标的形式描述。机器人坐标系及激光雷达坐标系是相对坐标系,需要将其转换至世界坐标系中。由于激光雷达的观测坐标采用极坐标系的表示方式,因此需要将其以直角坐标形式转换至世界坐标系中,具体转换方法在后面给出。设两轮移动机器人的世界坐标系为,机器人坐标系为,表示方式如图4所示。机器人坐标系的原点在机器人的中心,其坐标在世界坐标系中的表示为,代表机器人在世界坐标系的位姿。设机器人坐标系中的某点坐标为,与夹角为,则其在世界坐标系中的坐标如(17)所示:
本发明研究基于单线激光雷达的室内两轮移动机器人的地图构建,故将激光雷达的观测数据作为观测量。如上文所述,由于激光雷达观测数据采用极坐标表示,故需将其转换成直角坐标形式表示,并将激光雷达的观测点坐标转换至世界坐标系中。如图4所示,激光雷达测量点与激光雷达的距离为,激光测量点与机器人坐标系的夹角为,k表示激光测量点的编号。设t时刻机器人位姿为,激光雷达在机器人坐标系中的位置为,则激光测量点在中的坐标由公式(18)计算得到。式中、及为测量噪声,一般假定为零均值高斯白噪声。
里程计是通过获得单位时间内的编码器数据进行移动机器人的位姿估计的装置。通过转换装置将电机转动量转换成脉冲信号,计算单位时间内接收到的脉冲信号数量,从而得到当前时刻的电机转速。设为单位时间内机器人车轮位移量,则通过公式(19)即可求得,从而获得机器人航迹模型。最终通过积分即可求得给定时刻机器人位姿,实现对相邻时刻下的机器人位姿估计。
式中R为车轮半径,P为光电编码器的分辨率,为电机转速的减速比,为单位脉冲过程中车轮的位移,为单位时间内发射的脉冲数量。机器人相邻时刻位姿估计图如图5所示。为相邻时刻下机器人转动的圆弧角度,为相邻时刻下机器人的偏航角,为机器人转动半径,为机器人两轮之间的距离,为右轮比左轮多运动的距离,、分别为左右轮的线速度。机器人的直线运动速度极为左右轮线速度的均值,如公式(20)所示。
在本发明提出的基于SPA非线性优化改进Gmapping 2D地图构建方法及建立的两轮差动机器人模型的基础上,设计并搭建了移动机器人硬件平台、基于ROS的机器人通信架构及Stage实验仿真平台。
硬件平台主要由镭神智能的LSLIDAR-N301型激光雷达、英特尔NUC7i5BNH型主机、STM32控制板、移动机器人底盘、显示器、24V电池电源等组成。
LSLIDAR-N301激光雷达放置在机器人正前方,实时测量数据并将其应用于移动机器人的激光SLAM过程中。最大测量距离可达30m,扫描角度可达(在实际使用中将其限制为),角度分辨率为,扫描精度可达cm,发布数据的频率10Hz。
移动机器人的主机采用英特尔公司的NUC7i5BNHW微型PC,用以实时处理数据并实现建图、定位与运动规划等功能。该微型PC采用酷睿i5-7260U处理器,电源电压为12V-19V,包含1个232串口、1个HDMI接口及6个USB接口,支持蓝牙及WIFI功能,具有强大的计算能力及通信能力,支持机器人通电自启动功能、远程操控功能、网络唤醒等功能。采用英特尔IrisTM Plus Graphics 640核芯显卡及双通道DDR4-2133 SODIMMs内存,具有较强的图像处理能力。该微型PC具有功能强大、接口丰富、易于携带、便于安装、运行功耗小且支持Windows和Linux双***运行等特点,可以满足实验的要求。
移动机器人的底盘由两个主动轮与两个万向从动轮组成;两个主动轮在机器人底盘横向对称布置,由包含减速器与光电编码器(与主动轮同轴安装)的直流电机控制***直接驱动,通过左右轮的差速运动实现了移动机器人的全向运动,构成了双轮差分机器人运动***;两个从动轮在机器人底盘纵向对称布置,用于辅助转向。
通信架构采用ROS分布式机器人操作平台,可以实现多线程实时通信,将功能以一个功能包(Package)及元功能包(MetaPackage)的形式封装运行,具有松耦合特性。仿真平台采用Stage仿真器,Stage具有方便准确的使用特点,由于本实验主要研究单线激光雷达的2D地图构建,采用Stage仿真器可以满足要求。仿真过程中采用戴尔笔记本电脑,使用ubuntu 16.04***,采用英特尔CoreTM i7-7700HQ处理器,安装内存为16g,ROS平台采用kinetic的版本,可以满足仿真和实验要求。
本实施例对采用键盘控制机器人进行地图构建,并在Rviz可视化界面与Stage仿真器中进行实时显示,地图的尺寸为4733m,构建地图的分辨率采用0.025m;在实验过程中将实验分成两组进行测试,首先,比较无障碍添加时相同粒子数的Gmapping算法与本发明提出的基于SPA的改进Gmapping算法的建图效果,然后,比较添加障碍物时相同粒子数的两种算法的建图效果。
第一组实验中采用控制变量法分析比较无障碍添加时的Gmapping算法与本发明提出的基于SPA优化的改进Gmapping算法地图构建效果。影响Gmapping算法地图构建的主要参数为粒子数量,对于大型规模的复杂环境地图构建过程中的最优粒子数为30个。基于此,将实验一分为三个具有不同粒子数的对照组进行分析比较。
图6(a)、图6(b)为使用15个粒子时的两种算法地图构建的对比
Gmapping-SLAM使用30个粒子所得到的地图整体精度较高,但在局部范围出现了边界模糊的问题,导致对障碍物、地图的显示不完整,影响了建图的精度;而本发明提出的基于SPA优化的改进Gmapping算法则对当前构建地图的特征进行了实时检测优化,在上述区域构建出了更加清晰准确的边界与障碍物轮廓,建立了更加准确的地图。
如图6(a)、图6(b)所示,当采用15个粒子构建地图时Gmapping-SLAM的建图精度有了一定程度的降低,出现了不同程度的边界缺失问题,影响地图构建的精度。本发明提出的基于SPA优化的改进Gmapping算法的精度同使用30个粒子时的改进算法相比略有下降,但对构建地图的精度影响很小。同图6(a)相比,改进算法的得到的地图图6(b)在相关区域的得到了更为准确的地图边界与特征,所构建的地图更加准确。
当采用5个粒子构建地图时,Gmapping-SLAM出现了较为严重的边界模糊与障碍物模糊现象,而本发明所使用的改进算法则呈现出了相对较好的建图精度,得到的障碍物特征更加清晰,地图更加规整。
综上所述,在结构化的大型室内场景中,使用相同粒子数时,本发明提出的基于SPA优化的Gmapping算法可以构建出精度优于Gmapping算法的地图,尤其是在粒子数很少的时候,可以实现与粒子数较多时接近的地图精度,具有很高的稳定性和适应性。
综上所述,从上述实验结果可知,本发明的有益效果在于:
(1)使用相同分辨率与最小匹配得分时,Gmapping算法构建地图的精度受到粒子数很大的影响,而本发明提出的基于SPA优化的改进Gmapping算法受粒子数影响较少,具有很高的鲁棒性与适应性。由于使用较多的粒子数会占用过多的计算资源,对硬件要求较高。随着粒子数的减少,Gmapping-SLAM的建图精度会随之下降,在大型复杂室内场景中,使用较少的粒子难以满足建图要求,精度较差,而本发明提出的算法可以满足使用较少粒子构建地图的要求,构建精度优于Gmapping-SLAM算法的地图,具有很高的实用价值。
(2)使用相同粒子数进行比较时,本发明提出的基于SPA优化的改进Gmapping算法建图精度明显优于Gmapping-SLAM算法。由以上实验可知,Gmapping-SLAM在地图构建过程中会出现局部边界模糊与边界畸变的问题,在使用较少粒子建图时会出现边界偏移的问题。以上问题的出现会影响构建地图的精度及使用地图进行导航的精度。而本发明所提出的基于SPA的改进Gmapping算法的地图精度明显优于Gmapping算法,得到的障碍物及边界更加清晰完整,且具有很高的稳定性。
以上所述的具体实施例,对本发明的目的,技术方案和有益效果进行了进一步详细说明,所应理解的是,以上所述仅为本发明的具体实施例,并不用于限定本发明的保护范围,凡在本发明的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (5)
1.一种基于稀疏位姿调整的移动机器人的Gmapping建图方法,其特征在于,包括:
S5:更新第i个粒子的权重,所述第i个粒子的权重的更新公式为:
S6:更新机器人位姿与粒子地图,所述更新机器人位姿的方法为:通过所述第i个粒子的更新位姿与位姿修正量更新机器人位姿,所述更新粒子地图的方法为:通过直接稀疏特征矩阵H、第i个粒子的更新位姿、观测量,及更新第i个粒子的地图及采样粒子集更新粒子地图及粒子信息;
S7:判断地图创建是否完成,若地图创建完成,则结束流程;
若地图创建未完成,则判断是否进行粒子重采样;
判断是否进行粒子重采样的方法为:
S3’:构建位姿图;所述位姿图构建包括,计算机器人极大似然估计值,然后进入S4’;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010515565.9A CN111427370B (zh) | 2020-06-09 | 2020-06-09 | 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010515565.9A CN111427370B (zh) | 2020-06-09 | 2020-06-09 | 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111427370A true CN111427370A (zh) | 2020-07-17 |
CN111427370B CN111427370B (zh) | 2020-09-04 |
Family
ID=71559080
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010515565.9A Active CN111427370B (zh) | 2020-06-09 | 2020-06-09 | 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111427370B (zh) |
Cited By (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111856499A (zh) * | 2020-07-30 | 2020-10-30 | 浙江大华技术股份有限公司 | 基于激光雷达的地图构建方法和装置 |
CN111929699A (zh) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及*** |
CN112070201A (zh) * | 2020-08-26 | 2020-12-11 | 成都睿芯行科技有限公司 | 一种基于Gmapping提升建图速度的方法 |
CN112241002A (zh) * | 2020-10-11 | 2021-01-19 | 西北工业大学 | 一种基于Karto SLAM的鲁棒闭环检测新方法 |
CN112268561A (zh) * | 2020-10-12 | 2021-01-26 | 西北工业大学 | 一种融合磁场信息的机器人蒙特卡罗定位方法 |
CN112857379A (zh) * | 2021-01-22 | 2021-05-28 | 南京邮电大学 | 一种基于改进的Gmapping-SLAM地图更新方法及*** |
CN113218408A (zh) * | 2021-03-26 | 2021-08-06 | 合肥中科智驰科技有限公司 | 适用于多地形的多传感器融合的2Dslam方法及其*** |
CN113375658A (zh) * | 2021-06-15 | 2021-09-10 | 电子科技大学中山学院 | 移动机器人故障下同时fdd和slam的方法及*** |
CN113703443A (zh) * | 2021-08-12 | 2021-11-26 | 北京科技大学 | 一种不依赖gnss的无人车自主定位与环境探索方法 |
CN114253511A (zh) * | 2020-09-21 | 2022-03-29 | 成都睿芯行科技有限公司 | 基于激光雷达的slam硬件加速器及其实现方法 |
CN114577199A (zh) * | 2022-02-17 | 2022-06-03 | 广州大学 | 一种基于Gmapping算法的垃圾分类机器人二维栅格地图构建*** |
CN114993285A (zh) * | 2022-04-27 | 2022-09-02 | 大连理工大学 | 一种基于四轮全向全驱移动机器人的二维激光雷达建图方法 |
CN116203586A (zh) * | 2023-05-06 | 2023-06-02 | 南京农业大学 | 一种基于改进Gmapping的果园二维环境地图精准构建方法及*** |
CN116309907A (zh) * | 2023-03-06 | 2023-06-23 | 中国人民解放军海军工程大学 | 一种基于改进的Gmapping算法的移动机器人建图方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20120029698A1 (en) * | 2006-11-16 | 2012-02-02 | Samsung Electronics Co., Ltd | Method, apparatus, and medium for estimating pose of mobile robot using particle filter |
CN105509755A (zh) * | 2015-11-27 | 2016-04-20 | 重庆邮电大学 | 一种基于高斯分布的移动机器人同步定位与地图构建方法 |
CN106842230A (zh) * | 2017-01-13 | 2017-06-13 | 深圳前海勇艺达机器人有限公司 | 移动机器人导航方法与*** |
CN107063264A (zh) * | 2017-04-13 | 2017-08-18 | 杭州申昊科技股份有限公司 | 一种适用于大规模变电站环境的机器人地图创建方法 |
-
2020
- 2020-06-09 CN CN202010515565.9A patent/CN111427370B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20120029698A1 (en) * | 2006-11-16 | 2012-02-02 | Samsung Electronics Co., Ltd | Method, apparatus, and medium for estimating pose of mobile robot using particle filter |
CN105509755A (zh) * | 2015-11-27 | 2016-04-20 | 重庆邮电大学 | 一种基于高斯分布的移动机器人同步定位与地图构建方法 |
CN106842230A (zh) * | 2017-01-13 | 2017-06-13 | 深圳前海勇艺达机器人有限公司 | 移动机器人导航方法与*** |
CN107063264A (zh) * | 2017-04-13 | 2017-08-18 | 杭州申昊科技股份有限公司 | 一种适用于大规模变电站环境的机器人地图创建方法 |
Cited By (22)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111929699A (zh) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及*** |
CN111929699B (zh) * | 2020-07-21 | 2023-05-09 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及*** |
CN111856499A (zh) * | 2020-07-30 | 2020-10-30 | 浙江大华技术股份有限公司 | 基于激光雷达的地图构建方法和装置 |
CN112070201A (zh) * | 2020-08-26 | 2020-12-11 | 成都睿芯行科技有限公司 | 一种基于Gmapping提升建图速度的方法 |
CN114253511A (zh) * | 2020-09-21 | 2022-03-29 | 成都睿芯行科技有限公司 | 基于激光雷达的slam硬件加速器及其实现方法 |
CN112241002A (zh) * | 2020-10-11 | 2021-01-19 | 西北工业大学 | 一种基于Karto SLAM的鲁棒闭环检测新方法 |
CN112241002B (zh) * | 2020-10-11 | 2022-10-18 | 西北工业大学 | 一种基于Karto SLAM的鲁棒闭环检测新方法 |
CN112268561A (zh) * | 2020-10-12 | 2021-01-26 | 西北工业大学 | 一种融合磁场信息的机器人蒙特卡罗定位方法 |
CN112857379A (zh) * | 2021-01-22 | 2021-05-28 | 南京邮电大学 | 一种基于改进的Gmapping-SLAM地图更新方法及*** |
CN112857379B (zh) * | 2021-01-22 | 2023-12-12 | 南京邮电大学 | 一种基于改进的Gmapping-SLAM地图更新方法及*** |
CN113218408A (zh) * | 2021-03-26 | 2021-08-06 | 合肥中科智驰科技有限公司 | 适用于多地形的多传感器融合的2Dslam方法及其*** |
CN113218408B (zh) * | 2021-03-26 | 2024-06-11 | 合肥中科智驰科技有限公司 | 适用于多地形的多传感器融合的2Dslam方法及其*** |
CN113375658A (zh) * | 2021-06-15 | 2021-09-10 | 电子科技大学中山学院 | 移动机器人故障下同时fdd和slam的方法及*** |
CN113375658B (zh) * | 2021-06-15 | 2023-05-09 | 电子科技大学中山学院 | 移动机器人故障下同时fdd和slam的方法及*** |
CN113703443A (zh) * | 2021-08-12 | 2021-11-26 | 北京科技大学 | 一种不依赖gnss的无人车自主定位与环境探索方法 |
CN113703443B (zh) * | 2021-08-12 | 2023-10-13 | 北京科技大学 | 一种不依赖gnss的无人车自主定位与环境探索方法 |
CN114577199A (zh) * | 2022-02-17 | 2022-06-03 | 广州大学 | 一种基于Gmapping算法的垃圾分类机器人二维栅格地图构建*** |
CN114993285B (zh) * | 2022-04-27 | 2024-04-05 | 大连理工大学 | 一种基于四轮全向全驱移动机器人的二维激光雷达建图方法 |
CN114993285A (zh) * | 2022-04-27 | 2022-09-02 | 大连理工大学 | 一种基于四轮全向全驱移动机器人的二维激光雷达建图方法 |
CN116309907A (zh) * | 2023-03-06 | 2023-06-23 | 中国人民解放军海军工程大学 | 一种基于改进的Gmapping算法的移动机器人建图方法 |
CN116309907B (zh) * | 2023-03-06 | 2024-06-04 | 中国人民解放军海军工程大学 | 一种基于改进的Gmapping算法的移动机器人建图方法 |
CN116203586A (zh) * | 2023-05-06 | 2023-06-02 | 南京农业大学 | 一种基于改进Gmapping的果园二维环境地图精准构建方法及*** |
Also Published As
Publication number | Publication date |
---|---|
CN111427370B (zh) | 2020-09-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111427370B (zh) | 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法 | |
KR102292277B1 (ko) | 자율 주행 차량에서 3d cnn 네트워크를 사용하여 솔루션을 추론하는 lidar 위치 추정 | |
CN109709801B (zh) | 一种基于激光雷达的室内无人机定位***及方法 | |
KR102335389B1 (ko) | 자율 주행 차량의 lidar 위치 추정을 위한 심층 학습 기반 특징 추출 | |
KR102350181B1 (ko) | 자율 주행 차량에서 rnn 및 lstm을 사용하여 시간적 평활화를 수행하는 lidar 위치 추정 | |
CN107239076B (zh) | 基于虚拟扫描与测距匹配的agv激光slam方法 | |
CN111308490B (zh) | 基于单线激光雷达的平衡车室内定位与导航*** | |
CN108089572A (zh) | 用于稳健且有效的车辆定位的算法和基础设施 | |
Li et al. | Localization and navigation for indoor mobile robot based on ROS | |
CN113238251B (zh) | 一种基于车载激光雷达的目标级语义定位方法 | |
CN113189613B (zh) | 一种基于粒子滤波的机器人定位方法 | |
CN113310488A (zh) | 一种基于slam的果园机器人导航方法 | |
CN115388892A (zh) | 一种基于改进rbpf-slam算法的多传感器融合slam方法 | |
Hu et al. | A small and lightweight autonomous laser mapping system without GPS | |
CN115993089B (zh) | 基于pl-icp的在线四舵轮agv内外参标定方法 | |
CN115655311A (zh) | 一种基于扫描匹配的阿克曼型机器人里程计标定方法 | |
Zhang et al. | An Improved Localization Algorithm for Intelligent Robot | |
Han et al. | Novel cartographer using an oak-d smart camera for indoor robots location and navigation | |
Sun et al. | VMC-LIO: Incorporating Vehicle Motion Characteristics in LiDAR Inertial Odometry | |
CN117288195A (zh) | 一种基于深度相机的机器人导航避障方法 | |
Pan et al. | Outdoor SLAM Using Monocular Vision-Based Localization with LIDAR-Aided Mapping for service robot in Highway | |
Zhang et al. | Multi-Sensor Fusion Localization | |
CN116242348A (zh) | 一种室内移动机器人自动导航***及其导航方法 | |
CN115388893A (zh) | 基于遗传的滤波slam算法的移动机器人建图方法 | |
CN118097051A (zh) | 基于关键帧模板匹配的脑启发slam方法 |
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 |