CN113111081A - 一种弱特征环境下的移动机器人建图方法 - Google Patents
一种弱特征环境下的移动机器人建图方法 Download PDFInfo
- Publication number
- CN113111081A CN113111081A CN202110408720.1A CN202110408720A CN113111081A CN 113111081 A CN113111081 A CN 113111081A CN 202110408720 A CN202110408720 A CN 202110408720A CN 113111081 A CN113111081 A CN 113111081A
- Authority
- CN
- China
- Prior art keywords
- matrix
- robot
- map
- mapping
- cloud data
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 31
- 238000013507 mapping Methods 0.000 title claims abstract description 25
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 40
- 239000011159 matrix material Substances 0.000 claims description 53
- 238000013519 translation Methods 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 8
- 238000000354 decomposition reaction Methods 0.000 claims description 6
- 238000005457 optimization Methods 0.000 abstract description 8
- 230000000694 effects Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000010276 construction Methods 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000005299 abrasion Methods 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000036544 posture Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/24—Querying
- G06F16/245—Query processing
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/23—Updating
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/29—Geographical information databases
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Databases & Information Systems (AREA)
- Data Mining & Analysis (AREA)
- Physics & Mathematics (AREA)
- General Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Computational Linguistics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开一种弱特征环境下的移动机器人建图方法,应用于移动机器人领域,针对现有技术在弱特征环境下的建图精度过低的问题,本发明基于图优化SLAM算法对扫描匹配进行改进,能有效的提高移动机器人的建图质量,具体的:在图优化SLAM中,机器人的位姿作为顶点,位姿之间的关系构成边(edge),一旦图构成了,通过改进的扫描匹配算法调整机器人的位姿去尽量满足这些边构成约束,从而得到最终的建图;采用本发明的方法能明显改善弱特征环境建图精度。
Description
技术领域
本发明属于移动机器人领域,特别涉及一种移动机器人建图技术。
背景技术
目前室内自主移动机器人大多数采用码盘,陀螺仪,IMU,激光雷达等传感器采集相应信息通过融合算法来估算机器人实时的姿态。码盘类似于汽车的里程计,通过计算轮毂转动的圈数来推算机器人运动的路程,但是机器人在运动过程中难免受到打滑,轮胎磨损等因素的影响导致码盘检测误差,并且会随着机器人运动逐渐累加,无法有效消除误差积累。陀螺仪和IMU(Inertial measurement unit惯性测量单元)由于电子器件的电气特性会出现零漂(零漂是输入为0的时候的输出)的现象,并且会随着时间误差逐渐累积,同样无法有效消除误差积累。
在类似监所等环境中,由于环境相似度大,即呈现弱特征环境,对机器人有效建图提出了很大的挑战,弱特征环境即环境中存在少量相似的直线特征,但是角点特征较少,特征点的相似会直接导致出现建图漂移或错误。
发明内容
为解决上述技术问题,本发明提出一种弱特征环境下的移动机器人建图方法,通过将器人位姿当做顶点,位姿关系当做边,使得顶点满足边构成的约束,得到最终的建图。
本发明采用的技术方案为:一种弱特征环境下的移动机器人建图方法,将机器人位姿作为顶点,位姿关系作为边,通过优化顶点满足边构成的约束,完成建图。
具体包括以下步骤:
S1、通过传感器采集机器人的位姿数据;
S2、机器人运动至下一位置,使用体素网格滤波器对点云数据进行采集;
S3、采用改进的扫描匹配算法对采集到的点云数据进行计算,若扫描匹配结果为已知子地图,则对已知子地图进行优化,然后执行步骤S4;否则,***新的位姿点,更新子地图,执行步骤S4;
S4、判断建图是否完成,若完成,则结束;否则返回步骤S2。
步骤S3具体包括以下分步骤:
S31、取两帧点云数据,将两帧点云数据各自均分为k组,并计算两帧点云数据各自的质心,若质心在所划分组内,则将这一组移除;
S32、从经步骤S31处理后的两帧点云数据各取一组构成矩阵W,计算矩阵W的旋转矩阵与平移矩阵;
S33、根据旋转矩阵与平移矩阵计算两帧点云的距离,若距离小于设定的阈值,则说明两组匹配,否则不匹配;
S34、重复步骤S32-S33,直至完成所有组的匹配,判断匹配组数是否满足要求,若是则匹配成功,则对已知子地图进行优化,然后执行步骤S4;否则,***新的位姿点,更新子地图,执行步骤S4。
步骤S32具体为:若矩阵W满秩,则由奇异值分解计算算矩阵W的旋转矩阵与平移矩阵;否则,通过使得E(R,t)最小,迭代求解算矩阵W的旋转矩阵与平移矩阵,其中,R表示矩阵W的旋转矩阵,t表示矩阵W的平移矩阵,pi表示第p组第i个点云数据,qi表示第q组第j个点云数据,n表示每组的点云个数。
步骤S34所述***新的位姿点,更新子地图,具体为:当匹配失败时,则判断机器人目前所处的位置为新的位姿点,不在已知的子地图上,则***新的位姿点,更新子地图。
本发明的有益效果:本发明的方法通过将机器人的位姿作为一个节点或顶点(vetex),位姿之间的关系构成边(edge),然后采用改进的扫描匹配算,调整机器人的位姿去尽量满足这些边构成的约束,从而得到最终的建图;本发明的方法具备以下优点:
1、针对弱特征环境建图精度相对于一些开源的SLAM算法有明显改善;
2、由于无须迭代,算法时间复杂度较低,针对建图实时性较好。
附图说明
图1为图优化SLAM算法信息矩阵获取过程示意图;
其中,图1(a)为观测地标m1的示意图,图1(b)为机器人从x1运动到x2的示意图,图1(c)为运动几步之后的示意图;
图2为本发明的方法流程图;
图3为本发明实施例提供的仿真环境;
图4为本发明实施例提供的Gmapping算法建图结果;
图5为本发明实施例提供的Hector-SLAM算法建图结果;
图6为本发明实施例提供的Cartographer-SLAM算法建图结果;
图7为采用本发明方法的建图结果;
图8为本发明方法与现有技术的位姿误差对比;
图9为本发明方法与现有技术的角度误差对比。
具体实施方式
为便于本领域技术人员理解本发明的技术内容,下面结合附图对本发明内容进一步阐释。
本发明提供一种弱特征环境下移动机器人建图方法。主要是基于图优化SLAM(Simultaneous Localization and Mapping)算法对扫描匹配进行改进,能有效的提高移动机器人的建图质量。在图优化SLAM中,机器人的位姿是一个节点或顶点(vetex),位姿之间的关系构成边(edge),比如t’+1时刻和t’时刻之间的odometry(里程计)关系构成边,或者由视觉计算出来的位姿转换矩阵也可以构成边。一旦图构成了,就要调整机器人的位姿去尽量满足这些边构成的约束。如图1所示为图优化SLAM算法信息矩阵获取过程示意图,其中图1(a)为观测地标m1的示意图,图1(b)为机器人从x1运动到x2的示意图,图1(c)为运动几步之后的示意图;图优化SLAM能分解成两个任务:
1、构建图。机器人位姿当做顶点,位姿关系当做边,称为前端。
2、优化图。调整机器人位姿顶点尽量满足边的约束,称为后端。
图优化过程中:先堆积数据,机器人位姿为构建的顶点。边是位姿之间的关系,可以是编码器数据计算的位姿,也可以是通过ICP匹配计算出来的位姿,还可以是闭环检测的位姿关系。
由于室内环境下的基于ICP匹配的算法比较复杂,并且ICP算法高度依赖初始估计,限制了其实际应用范围,在此提出一种基于激光雷达的改进扫描匹配算法。由于构建图方法不是算法消耗和精度提升的主要部分,本算法只针对优化图中扫描匹配进行优化,算法流程图如图2所示;包括如下步骤:
S1:激光雷达、里程计等传感器采集数据输入。本领域技术人员应知本步骤中激光雷达采集点云数据,里程计采集运动距离数据等。
S2:使用图优化SLAM算法进行图形构建(前端),机器人运动至下一位置,使用体素网格滤波器对点云数据进行采集。
S3:改进的扫描匹配算法:
其中,pi表示第p组第i个点云数据,qi表示第q组第j个点云数据;
由奇异值分解法求解R和t推导如下,设任意两组点云为:
使下式最小即可求得R和t:
其中,μq和μp分别是Q点云集与P点云集的质心。展开上式并化简得:
q′i、p′i分别为原始点云减去对应质心的点。上式只与R有关,因此寻求使得上式最小的R值。R是正交矩阵,因此RTR=1,上式的最小值等价于求下式的最大值:
用矩阵的迹表示:
其中tr(·)表示矩阵的迹。对W进行奇异值分解得到W=UΛVT,W=UΛVT为W的奇值分解,其中U和V均为单位正交阵,T表示矩阵的转置。U称为左奇异矩阵;V称为右奇异矩阵;Λ仅在主对角线上有值,称为奇异值,其它元素均为0。
假设有一变量K=VUT,因此KW=VUTUΛVT=VΛVT,故KW是一个正定对称的矩阵,根据定理可知,对正定对称矩阵A,对任意正交矩阵B,都有tr(A)≥tr(BA),因此,RT=VUT。故有:
S3.3:求取距离若d小于阈值,则说明两组匹配,则匹配组数增加;否则,该组不匹配。这里的阈值的取值依据为:例如对同一物体在不同位置或角度进行扫描,两组点云匹配度大于95%以上,则说明此时的阈值较为合理,选取不同物体重复上述过程,选择匹配度最合适的阈值。
S3.4:判断是否所有组均完成匹配,若是,判断匹配的成功率是否满足要求,若是,代表成功,否则,失败;若并非所有组均完成了匹配,则继续匹配。
S4:根据S3.4判断扫描匹配结果是否成功。若是,则对已知匹配的子地图进行优化;否则,***新的位姿点,更新子地图。
步骤S4中判是否所有组均完成匹配,比如匹配率设置为95%,若所有组的匹配率达到95%,则表示匹配成功,否则匹配失败。
当匹配失败时,则判断机器人目前所处的位置为新的位姿点,不在已知的子地图上,由于建图过程是连续的,本发明通过已经建立好的已知子地图上的位姿点可以推断出新得位姿点所处的位置。
S5:判断建图是否完成。若是,则结束算法;否则,转S2。本步骤中依据实际环境特征是否均扫描完成作为判断建图是否完成的依据。
本实施例在操作***是Ubuntu16.04,仿真软件是Gazebo7的条件下进行,建立如图3所示仿真环境,黑色圆表示带有RPLIDAR A1激光雷达的机器人模型,***表示障碍物墙体,主要是模拟一个特征点少,特征相似且直线段较明显的弱特征环境(例如监所环境的长通道)。
参数设置:提取机器人先后两帧经步骤S1、S2处理过的点云数据,设置机器人的扫描角度为200°,该型号的激光雷达每隔1°扫描一个特征点,因此根据步骤S3.1将组数分为10组,每个组20个相邻的点云数据,根据计算质心坐标,若计算的质心正好在这20个点中某一个,则除去该质心,根据步骤S3.2计算W矩阵,判断W矩阵是否非奇异的,若是,则旋转矩阵R=VUT且平移矩阵t=upi-Ruqj;否则,根据公式E(R,t)迭代计算求解R和t。最后根据步骤S3.3计算d,若d在阈值0.012cm内,则表示两组点云匹配成功,否则不成功。后续步骤遍历先后两帧点云组数据。最后根据步骤S4和S5判断即可。
分别使用开源的算法Gmapping、Hector-SLAM、Cartographer-SLAM和改进的图优化SLAM算法进行建图分析,结果如下:
如图4至图7所示,Gmapping算法在建图后期会出现漂移现象,而Hector算法在建图中期出现断层,直接导致建图失败,Cartographer-SLAM算法相对前面两种算法来说效果较好,改进的算法效果最好。
统计位姿误差和角度误差分别如图8和图9所示。Gmapping算法的误差随时间的增大而增大,是由于后期出现漂移所致;Hector-SLAM算法的误差从一开始就很大,无法应对弱特征的环境;Cartographer-SLAM算法是开源算法中效果最好的,但是相对改进的算法来说误差的稳定性以及建图效果均相对差一些,因此,本算法提出的改进扫描匹配的算法能够在弱特征环境下建图起到较好的效果。
图8和图9中,标t表示时间,单位:秒;纵坐标表示位姿误差,即x方向和y方向的平方和的绝对平方根,单位:厘米;Improved Proposal表示改进的Graph SLAM算法。
本发明主要是对类似监所长走廊环境下,角点特征较少,环境相似度大的情况,针对基本的Graph SLAM的ICP算法复杂度大,实际应用不理想的情况,提出一种改进的扫描匹配算法,通过扫描点分组,奇异值分解计算旋转矩阵以及平移矩阵,通过仿真验证,建图效果得到改善,说明扫描匹配算法是可行的。
本领域的普通技术人员将会意识到,这里所述的实施例是为了帮助读者理解本发明的原理,应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的权利要求范围之内。
Claims (5)
1.一种弱特征环境下的移动机器人建图方法,其特征在于,将机器人位姿作为顶点,位姿关系作为边,通过优化顶点满足边构成的约束,完成建图。
2.根据权利要求1所述的一种弱特征环境下的移动机器人建图方法,其特征在于,具体包括以下步骤:
S1、通过传感器采集机器人的位姿数据;
S2、机器人运动至下一位置,使用体素网格滤波器对点云数据进行采集;
S3、采用改进的扫描匹配算法对采集到的点云数据进行计算,若扫描匹配结果为已知子地图,则对已知子地图进行优化,然后执行步骤S4;否则,***新的位姿点,更新子地图,执行步骤S4;
S4、判断建图是否完成,若完成,则结束;否则返回步骤S2。
3.根据权利要求2所述的一种弱特征环境下的移动机器人建图方法,其特征在于,步骤S3具体包括以下分步骤:
S31、取两帧点云数据,将两帧点云数据各自均分为k组,并计算两帧点云数据各自的质心,若质心在所划分组内,则将这一组移除;
S32、从经步骤S31处理后的两帧点云数据各取一组构成矩阵W,计算矩阵W的旋转矩阵与平移矩阵;
S33、根据旋转矩阵与平移矩阵计算两帧点云的距离,若距离小于设定的阈值,则说明两组匹配,否则不匹配;
S34、重复步骤S32-S33,直至完成所有组的匹配,判断匹配组数是否满足要求,若是则匹配成功,则对已知子地图进行优化,然后执行步骤S4;否则,***新的位姿点,更新子地图,执行步骤S4。
5.根据权利要求4所述的一种弱特征环境下的移动机器人建图方法,其特征在于,步骤S34所述***新的位姿点,更新子地图,具体为:当匹配失败时,则判断机器人目前所处的位置为新的位姿点,不在已知的子地图上,则***新的位姿点,更新子地图。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110408720.1A CN113111081A (zh) | 2021-04-16 | 2021-04-16 | 一种弱特征环境下的移动机器人建图方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110408720.1A CN113111081A (zh) | 2021-04-16 | 2021-04-16 | 一种弱特征环境下的移动机器人建图方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN113111081A true CN113111081A (zh) | 2021-07-13 |
Family
ID=76717697
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110408720.1A Pending CN113111081A (zh) | 2021-04-16 | 2021-04-16 | 一种弱特征环境下的移动机器人建图方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113111081A (zh) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109100731A (zh) * | 2018-07-17 | 2018-12-28 | 重庆大学 | 一种基于激光雷达扫描匹配算法的移动机器人定位方法 |
CN109974712A (zh) * | 2019-04-22 | 2019-07-05 | 广东亿嘉和科技有限公司 | 一种基于图优化的变电站巡检机器人建图方法 |
CN110333720A (zh) * | 2019-07-10 | 2019-10-15 | 国网四川省电力公司电力科学研究院 | 一种基于粒子滤波的slam优化方法 |
CN110645974A (zh) * | 2019-09-26 | 2020-01-03 | 西南科技大学 | 一种融合多传感器的移动机器人室内地图构建方法 |
CN112230243A (zh) * | 2020-10-28 | 2021-01-15 | 西南科技大学 | 一种移动机器人室内地图构建方法 |
US20210078174A1 (en) * | 2019-09-17 | 2021-03-18 | Wuyi University | Intelligent medical material supply robot based on internet of things and slam technology |
-
2021
- 2021-04-16 CN CN202110408720.1A patent/CN113111081A/zh active Pending
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109100731A (zh) * | 2018-07-17 | 2018-12-28 | 重庆大学 | 一种基于激光雷达扫描匹配算法的移动机器人定位方法 |
CN109974712A (zh) * | 2019-04-22 | 2019-07-05 | 广东亿嘉和科技有限公司 | 一种基于图优化的变电站巡检机器人建图方法 |
CN110333720A (zh) * | 2019-07-10 | 2019-10-15 | 国网四川省电力公司电力科学研究院 | 一种基于粒子滤波的slam优化方法 |
US20210078174A1 (en) * | 2019-09-17 | 2021-03-18 | Wuyi University | Intelligent medical material supply robot based on internet of things and slam technology |
CN110645974A (zh) * | 2019-09-26 | 2020-01-03 | 西南科技大学 | 一种融合多传感器的移动机器人室内地图构建方法 |
CN112230243A (zh) * | 2020-10-28 | 2021-01-15 | 西南科技大学 | 一种移动机器人室内地图构建方法 |
Non-Patent Citations (1)
Title |
---|
翁潇文等: "基于图优化的二维激光SLAM研究", 《自动化与仪表》 * |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111076733B (zh) | 一种基于视觉与激光slam的机器人室内建图方法及*** | |
CN103413352A (zh) | 基于rgbd多传感器融合的场景三维重建方法 | |
CN112183171A (zh) | 一种基于视觉信标建立信标地图方法、装置 | |
CN114018248B (zh) | 一种融合码盘和激光雷达的里程计方法与建图方法 | |
CN110749895B (zh) | 一种基于激光雷达点云数据的定位方法 | |
CN113587933A (zh) | 一种基于分支定界算法的室内移动机器人定位方法 | |
CN111368759A (zh) | 基于单目视觉的移动机器人语义地图构建*** | |
CN111739066B (zh) | 一种基于高斯过程的视觉定位方法、***及存储介质 | |
CN113701739B (zh) | 一种地图更新方法以及装置 | |
CN111123953B (zh) | 人工智能大数据下粒子化移动机器人组及其控制方法 | |
CN110688440B (zh) | 一种适用于子地图重叠部分较少的地图融合方法 | |
CN109443355B (zh) | 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法 | |
Luperto et al. | Predicting performance of SLAM algorithms | |
CN117392268A (zh) | 一种基于自适应结合cpd和icp算法的激光扫描建图方法及*** | |
CN116907477A (zh) | 一种基于视觉路标辅助的高精度激光slam方法及装置 | |
CN113111081A (zh) | 一种弱特征环境下的移动机器人建图方法 | |
CN115950414A (zh) | 一种不同传感器数据的自适应多重融合slam方法 | |
CN113554705B (zh) | 一种变化场景下的激光雷达鲁棒定位方法 | |
CN115060268A (zh) | 一种机房融合定位方法、***、设备及存储介质 | |
Zhao et al. | 3D indoor map building with monte carlo localization in 2D map | |
Danping et al. | Simultaneous localization and mapping based on Lidar | |
Watanabe et al. | Robust localization with architectural floor plans and depth camera | |
Guo et al. | 3D Lidar SLAM Based on Ground Segmentation and Scan Context Loop Detection | |
Jiang et al. | An indoor co-positioning algorithm using WLAN and monocular vision | |
CN113009917B (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 | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20210713 |
|
RJ01 | Rejection of invention patent application after publication |