CN106767820A - 一种室内移动定位与制图方法 - Google Patents

一种室内移动定位与制图方法 Download PDF

Info

Publication number
CN106767820A
CN106767820A CN201611121736.XA CN201611121736A CN106767820A CN 106767820 A CN106767820 A CN 106767820A CN 201611121736 A CN201611121736 A CN 201611121736A CN 106767820 A CN106767820 A CN 106767820A
Authority
CN
China
Prior art keywords
map
sub
cloud
point cloud
global
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
Application number
CN201611121736.XA
Other languages
English (en)
Other versions
CN106767820B (zh
Inventor
李德仁
张桥
张文俊
孙凯
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
LEADOR SPATIAL INFORMATION TECHNOLOGY Co Ltd
Original Assignee
LEADOR SPATIAL INFORMATION TECHNOLOGY Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by LEADOR SPATIAL INFORMATION TECHNOLOGY Co Ltd filed Critical LEADOR SPATIAL INFORMATION TECHNOLOGY Co Ltd
Priority to CN201611121736.XA priority Critical patent/CN106767820B/zh
Publication of CN106767820A publication Critical patent/CN106767820A/zh
Application granted granted Critical
Publication of CN106767820B publication Critical patent/CN106767820B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data

Abstract

本发明公开了一种室内移动定位与制图方法，主要步骤包括：按要求输入采集的点云数据；点云初步处理，具体包含点云滤波、点云坐标变换及非线性插值和下采样；利用粒子滤波的方法进行轨迹的姿态预测；子地图生成、标靶添加以及约束计算，具体包含动态子地图的生成、虚拟标靶和全局标靶添加、局部捆集和全局约束计算；利用图优化进行全局优化，减小逐帧匹配造成的累积误差；更新结果，包含更新子地图数据、轨迹数据和根据轨迹生成的全局地图数据；保存结果数据。本发明不依赖GPS和IMU，数据处理速度快，易于并行处理；制图精度高，实现厘米级精度。

Description

一种室内移动定位与制图方法
技术领域
本发明涉及SLAM(Simultaneous Localization And Mapping)、移动机器人、摄影测量、人工智能技术领域，具体涉及一种室内移动定位与制图方法。
背景技术
室内移动测量技术的关键在于室内定位和制图。一般在室外条件下主要依靠全球定位系统(GPS)、惯性测量单元(IMU)等传感器定位实现高效的移动式测量方案。但是，室内环境没有GPS信号，传统的定位方式在室内环境下无用武之地，没有位置信息就无法实现移动测量，另外传统的定点室内测量方式效率低下，制图周期长，耗费大量的人力。
发明内容
为了解决上述技术问题，本发明提供了一种室内移动定位和制图方法，以解决传统移动测量效率低下，制图精度不高的问题。
本发明所采用的技术方案是：一种室内移动定位与制图方法，其特征在于，包括以下步骤：
步骤1：激光点云数据获取；
步骤2：点云数据预处理；
步骤3：将输入的极坐标系下的点云数据转换到笛卡尔坐标系下；
步骤4：点云插值及下采样；
步骤5：粒子滤波预测姿态；
步骤6：子地图生成；
步骤7：虚拟标靶及局部捆集约束计算；
步骤8：全局标靶添加及约束计算；
步骤9：利用图优化进行全局优化；
步骤10：根据步骤9得到的优化结果，更新轨迹和子地图数据；
步骤11：将每一帧点云数据按照轨迹姿态进行累加，生成全局地图；
步骤12：输出地图。
本发明的有益效果包括：
1)不依赖GPS和IMU；
2)数据处理速度快；
3)易于并行处理；
4)制图精度高，实现厘米级精度；
附图说明
图1是本发明实施例的流程图；
具体实施方式
为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。
请见图1，本发明提供的一种室内移动定位与制图方法，包括以下步骤：
步骤1：激光点云数据获取；
利用数据采集软件同步获取水平方向和竖直方向激光数据，且数据采集的前提是，激光需水平固定放置，不同激光的相对位置已标定完成；
步骤2：点云数据预处理；
由于激光扫描仪在一定的扫描范围内时，数据精度较高，超过规定范围，精度无法保证，因此设定合理阈值，剔除距离较远的点云数据，以保证输入点云数据的精度；
步骤3：坐标系变换；
将输入的极坐标系下的点云数据转换到笛卡尔坐标系下，便于后续处理；
步骤4：点云插值及下采样；
由于激光扫描仪在扫描过程中，对于距离较远的物体，得到的点云数据较稀；距离较近的物体，点云相对较密，这样近处物体的激光数据位于同一点的概率比远处物体高，在概率统计模型中，近处点的概率会多次叠加，影响统计结果。为了消除此影响，对点云数据根据距离进行非线性插值，这样将点云密度增加为基本均匀的状态，然后再对同一个点有多个值的进行抽样，这样就达到了每一个位置对应一个点云的效果。步骤如下：
步骤4.1：对于第i-1，i，i+1个点云数据d[i-1]，d[i]和d[i+1]，根据步骤3得到对应的位置坐标为(xi-1,yi-1)，(xi,yi)和(xi+1,yi+1)；
步骤4.2：判断i-1与i之间是否产生了突变；如果min(di-1,i,di,i+1)<di-1,i+1，则认为点云是连续的，未发生突变；则对这两点点云之间进行插值，插值个数为其中di-1,i表示i-1与i之间的距离，di,i+1表示i与i+1之间的距离，di-1,i+1表示i-1与i+1之间的距离；
步骤4.3：对插值后的点云，如果有某个(xi,yi)＝(xj,yj)，则丢掉其中一个点云，进而达到下采样的结果。
步骤5：粒子滤波预测状态；
室内定位最重要的部分就是预测每一帧扫描数据对应的轨迹，粒子滤波提供了一种简单快速的预测方法。首先给定目标的初始位置，在该位置附近撒粒子，得到一个候选位置，对点云做如下处理：
步骤5.1：将当前帧点云数据变换到候选位置所对应的姿态下；
步骤5.2：根据已生成的地图，统计每个点云数据是障碍物的概率，对概率求和；
步骤5.3：概率最大的位置则认为是当前的最优位置，也是下一个候选位置；
步骤5.4：重复步骤5.1-步骤5.3，直到最优位置不再变化或变化范围小于预设阀值，则结束。
步骤6：子地图生成；
即时定位和制图过程中需要不断探知新的地图和轨迹，不断扩展的地图将耗费大量的内存，且存在较大的冗余，另外我们发现在定位过程中与当前帧邻近的地图数据才起到定位的参考作用，较远的地图数据非但不能起到有效的定位参考作用甚至会导致定位错误。通过以上分析，设计了一个可扩展的地图集，地图集内可存放多个互为网格关系的子地图，地图集中的子地图数量可任意扩展，这样不仅有效地减少了不必要的内存冗余，还可根据采集环境的不同动态扩展。对于动态地图数据结构，可以采取分段的形式保存子地图。具体思路如下：
步骤6.1：设第一个落入地图中的点云位置为地图的起始位置start；
步骤6.2：当有一个新的点云newPoint落入地图中时，由于点云的连续性，其位置一定在start的8-邻域中，则在该位置申请一小块内存，用于存储newPoint的8-邻域的点，n小块内存组成的地图即为子地图miniMap，且任意两个miniMap需要有重叠；
步骤6.3：重复步骤6.2，生成最终地图finalMap，且finalMap是不规则地图的外接矩形，占用内存最小。
步骤7：虚拟标靶及局部捆集约束计算；
相邻两个子地图之间通过虚拟标靶和局部捆集约束联系。首先计算两个子地图相交部分概率最大的几个特征点，将这些特征点设置为虚拟标靶，其次对每一个虚拟标靶分别与这两个子地图包含的点云的轨迹计算约束，也就是在某个轨迹坐标系中计算标靶的姿态，即为局部捆集约束；
步骤8：全局标靶添加及约束计算；
根据轨迹之间的距离可以初步计算出全局闭环，再利用它们所在的子地图之间的匹配关系得到闭环约束。同样地选取几个显著的特征点作为全局标靶，并计算标靶与对应子地图包含点云的轨迹之间的约束，具体方法为：
步骤8.1：设点A为子地图miniMap_i的一个特征点，点B为子地图miniMap_j的对应于A的特征点，则(A,B)记为子地图i和j的一对全局标靶；
步骤8.2：分别计算A与子地图miniMap_j，B与子地图miniMap_i中所有轨迹的对应关系，形成全局约束。
步骤9：利用图优化进行全局优化；
在点云逐帧匹配的过程中，误差被逐渐累积，根据步骤7和步骤8中计算的约束，利用图优化算法解决大规模地图生成过程中的累积误差问题；
步骤10：更新轨迹和子地图；
根据步骤9得到的优化结果，更新轨迹和子地图数据，得到较精确的结果；
步骤11：根据轨迹生成全局地图；
将每一帧点云数据按照轨迹姿态进行累加，得到最终的地图；
步骤12：输出地图和轨迹；
根据需要的格式，保存地图和轨迹数据。
本发明提供了一种不依赖GPS和IMU的室内移动定位与制图方法，该方法可以高效、快捷地获取高精确、多细节的室内建筑的二维和三维地图，大大节约了时间成本和人为误差，为室内导航、VR体验、展馆虚拟浏览提供基础数据服务，还可用于室内基础设施管理，快速全面采集室内消防设施和室内安防设施信息，实现室内智能安防地理信息数据库建设。
应当理解的是，本说明书未详细阐述的部分均属于现有技术。
应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。

Claims (9)

1.一种室内移动定位与制图方法，其特征在于，包括以下步骤：
步骤1：激光点云数据获取；
步骤2：点云数据预处理；
步骤3：将输入的极坐标系下的点云数据转换到笛卡尔坐标系下；
步骤4：点云插值及下采样；
步骤5：粒子滤波预测姿态；
步骤6：子地图生成；
步骤7：虚拟标靶及局部捆集约束计算；
步骤8：全局标靶添加及约束计算；
步骤9：利用图优化进行全局优化；
步骤10：根据步骤9得到的优化结果，更新轨迹和子地图数据；
步骤11：将每一帧点云数据按照轨迹姿态进行累加，生成全局地图；
步骤12：输出地图。
2.根据权利要求1所述的室内移动定位与制图方法，其特征在于：步骤1中，首先激光水平固定放置，标定不同激光的相对位置，然后同步获取水平方向和竖直方向激光数据。
3.根据权利要求1所述的室内移动定位与制图方法，其特征在于：步骤2中，设定阈值，剔除距离大于阀值的点云数据，以保证输入点云数据的精度。
4.根据权利要求1所述的室内移动定位与制图方法，其特征在于：步骤4的具体实现包括以下子步骤：
步骤4.1：对于第i-1，i，i+1个点云数据d[i-1]，d[i]和d[i+1]，根据步骤3得到对应的位置坐标为(xi-1,yi-1)，(xi,yi)和(xi+1,yi+1)；
步骤4.2：判断i-1与i之间是否产生了突变；如果min(di-1,i,di,i+1)<di-1,i+1，则认为点云是连续的，未发生突变；则对这两点点云之间进行插值，插值个数为其中di-1,i表示i-1与i之间的距离，di,i+1表示i与i+1之间的距离，di-1,i+1表示i-1与i+1之间的距离；
步骤4.3：对插值后的点云，如果有某个(xi,yi)＝(xj,yj)，则丢掉其中一个点云，进而达到下采样的结果。
5.根据权利要求1所述的室内移动定位与制图方法，其特征在于：步骤5的具体实现包括以下子步骤：
步骤5.1：将当前帧点云数据变换到候选位置所对应的姿态下；
步骤5.2：根据已生成的地图，统计每个点云数据是障碍物的概率，对概率求和；
步骤5.3：概率最大的位置则认为是当前的最优位置，也是下一个候选位置；
步骤5.4：重复步骤5.1-步骤5.3，直到最优位置不再变化或变化范围小于预设阀值，则结束。
6.根据权利要求1所述的室内移动定位与制图方法，其特征在于：步骤6的具体实现包括以下子步骤：
步骤6.1：设第一个落入地图中的点云位置为地图的起始位置start；
步骤6.2：当有一个新的点云newPoint落入地图中时，由于点云的连续性，其位置一定在start的8-邻域中，则在该位置申请一小块内存，用于存储newPoint的8-邻域的点，n小块内存组成的地图即为子地图miniMap，且任意两个miniMap需要有重叠；
步骤6.3：重复步骤6.2，生成最终地图finalMap，且finalMap是不规则地图的外接矩形，占用内存最小。
7.根据权利要求1所述的室内移动定位与制图方法，其特征在于：步骤7中，首先计算两个子地图相交部分概率最大的几个特征点，将这些特征点设置为虚拟标靶，其次对每一个虚拟标靶分别与这两个子地图包含的点云的轨迹计算约束，也就是在某个轨迹坐标系中计算标靶的姿态，即为局部捆集约束。
8.根据权利要求1所述的室内移动定位与制图方法，其特征在于，步骤8的具体实现包括以下子步骤：
步骤8.1：设点A为子地图miniMap_i的一个特征点，点B为子地图miniMap_j的对应于A的特征点，则(A,B)记为子地图i和j的一对全局标靶；
步骤8.2：分别计算A与子地图miniMap_j，B与子地图miniMap_i中所有轨迹的对应关系，形成全局约束。
9.根据权利要求1所述的室内移动定位与制图方法，其特征在于：步骤9中，在点云逐帧匹配的过程中，误差被逐渐累积，根据步骤7和步骤8中计算的约束，利用图优化算法解决大规模地图生成过程中的累积误差问题。
CN201611121736.XA 2016-12-08 2016-12-08 一种室内移动定位与制图方法 Active CN106767820B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201611121736.XA CN106767820B (zh) 2016-12-08 2016-12-08 一种室内移动定位与制图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201611121736.XA CN106767820B (zh) 2016-12-08 2016-12-08 一种室内移动定位与制图方法

Publications (2)

Publication Number Publication Date
CN106767820A true CN106767820A (zh) 2017-05-31
CN106767820B CN106767820B (zh) 2017-11-14

Family

ID=58877317

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201611121736.XA Active CN106767820B (zh) 2016-12-08 2016-12-08 一种室内移动定位与制图方法

Country Status (1)

Country Link
CN (1) CN106767820B (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107402014A (zh) * 2017-07-27 2017-11-28 惠州市格农科技有限公司 分布式地图数据处理方法
CN107588775A (zh) * 2017-09-21 2018-01-16 哈尔滨理工大学 一种新型室内定位方法
CN107747941A (zh) * 2017-09-29 2018-03-02 歌尔股份有限公司 一种双目视觉定位方法、装置及系统
CN108334080A (zh) * 2018-01-18 2018-07-27 大连理工大学 一种针对机器人导航的虚拟墙自动生成方法
CN108550318A (zh) * 2018-03-12 2018-09-18 浙江大华技术股份有限公司 一种构建地图的方法及装置
CN109085605A (zh) * 2018-08-29 2018-12-25 长春博立电子科技有限公司 全自动探索未知空间并建立地图的方法及系统
CN109116867A (zh) * 2018-09-28 2019-01-01 拓攻（南京）机器人有限公司 一种无人机飞行避障方法、装置、电子设备及存储介质
CN109213154A (zh) * 2018-08-10 2019-01-15 远形时空科技（北京）有限公司 一种基于Slam定位方法、装置、电子设备及计算机存储介质
CN109933056A (zh) * 2017-12-18 2019-06-25 九阳股份有限公司 一种基于slam的机器人导航方法以及机器人
CN110377776A (zh) * 2018-07-23 2019-10-25 北京京东尚科信息技术有限公司 生成点云数据的方法和装置
CN113190019A (zh) * 2021-05-26 2021-07-30 立得空间信息技术股份有限公司 一种基于虚拟仿真的巡检机器人任务点布置方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2009014332A2 (en) * 2007-07-23 2009-01-29 Electronics And Telecommunications Research Institute Method and system for creating indoor environment map
CN102043155A (zh) * 2009-10-10 2011-05-04 北京理工大学 机载凝视成像三维选通成像控制数据拼接方法和系统
CN103017739A (zh) * 2012-11-20 2013-04-03 武汉大学 基于激光雷达点云与航空影像的真正射影像的制作方法
CN104897161A (zh) * 2015-06-02 2015-09-09 武汉大学 基于激光测距的室内平面地图制图方法
CN105973145A (zh) * 2016-05-19 2016-09-28 深圳市速腾聚创科技有限公司 移动式三维激光扫描系统及移动式三维激光扫描方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2009014332A2 (en) * 2007-07-23 2009-01-29 Electronics And Telecommunications Research Institute Method and system for creating indoor environment map
CN102043155A (zh) * 2009-10-10 2011-05-04 北京理工大学 机载凝视成像三维选通成像控制数据拼接方法和系统
CN103017739A (zh) * 2012-11-20 2013-04-03 武汉大学 基于激光雷达点云与航空影像的真正射影像的制作方法
CN104897161A (zh) * 2015-06-02 2015-09-09 武汉大学 基于激光测距的室内平面地图制图方法
CN105973145A (zh) * 2016-05-19 2016-09-28 深圳市速腾聚创科技有限公司 移动式三维激光扫描系统及移动式三维激光扫描方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
徐贺等: "三维激光扫描技术在地下建筑三维建模中的应用研究", 《城市勘测》 *
李明磊等: "激光扫描点云准确快速去噪方法", 《测绘通报》 *
王研等: "地面三维扫描仪标靶拼接误差分析", 《测绘技术装备》 *

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107402014A (zh) * 2017-07-27 2017-11-28 惠州市格农科技有限公司 分布式地图数据处理方法
CN107588775A (zh) * 2017-09-21 2018-01-16 哈尔滨理工大学 一种新型室内定位方法
CN107747941A (zh) * 2017-09-29 2018-03-02 歌尔股份有限公司 一种双目视觉定位方法、装置及系统
CN109933056A (zh) * 2017-12-18 2019-06-25 九阳股份有限公司 一种基于slam的机器人导航方法以及机器人
CN108334080A (zh) * 2018-01-18 2018-07-27 大连理工大学 一种针对机器人导航的虚拟墙自动生成方法
CN108550318A (zh) * 2018-03-12 2018-09-18 浙江大华技术股份有限公司 一种构建地图的方法及装置
CN110377776A (zh) * 2018-07-23 2019-10-25 北京京东尚科信息技术有限公司 生成点云数据的方法和装置
CN109213154A (zh) * 2018-08-10 2019-01-15 远形时空科技（北京）有限公司 一种基于Slam定位方法、装置、电子设备及计算机存储介质
CN109085605A (zh) * 2018-08-29 2018-12-25 长春博立电子科技有限公司 全自动探索未知空间并建立地图的方法及系统
CN109116867A (zh) * 2018-09-28 2019-01-01 拓攻（南京）机器人有限公司 一种无人机飞行避障方法、装置、电子设备及存储介质
CN113190019A (zh) * 2021-05-26 2021-07-30 立得空间信息技术股份有限公司 一种基于虚拟仿真的巡检机器人任务点布置方法及系统
CN113190019B (zh) * 2021-05-26 2023-05-16 立得空间信息技术股份有限公司 一种基于虚拟仿真的巡检机器人任务点布置方法及系统

Also Published As

Publication number Publication date
CN106767820B (zh) 2017-11-14

Similar Documents

Publication Publication Date Title
CN106767820B (zh) 一种室内移动定位与制图方法
CN104764457B (zh) 一种用于无人车的城市环境构图方法
CN107504972A (zh) 一种基于鸽群算法的飞行器航迹规划方法及装置
CN106949895A (zh) 一种适用于变电站环境下的巡检机器人定位方法
CN105865449A (zh) 基于激光和视觉的移动机器人的混合定位方法
CN109255302A (zh) 目标物识别方法及终端、移动装置控制方法及终端
CN108805327A (zh) 基于虚拟现实的机器人路径规划与环境重建的方法和系统
CN107015240A (zh) 一种基于无人机激光雷达的电力网络管理系统及方法
CN107608364A (zh) 一种用于数据中心物理设备上下架的智能机器人
CN113776534B (zh) 一种基于立体剖分网格的无人机三维时变空域导航方法
CN108267121A (zh) 一种可变场景下多设备的视觉导航方法及系统
CN111381245A (zh) 激光slam自适应分辨率栅格地图构建方法
CN109885046B (zh) 一种基于粒子滤波的移动机器人定位加速方法
CN114493052B (zh) 多模型融合自适应新能源功率预测方法和系统
CN112527010B (zh) 基于人工势场与粒子优化的室内变电站无人机多机协同巡检方法
CN114581619A (zh) 一种基于三维定位和二维建图的煤仓建模方法
CN107273466B (zh) 球面三角形离散格网编码向地理经纬度坐标的快速转换方法
CN110411454B (zh) 一种改进随机路径图法的移动机器人路径规划方法
CN111142116A (zh) 一种基于三维激光的道路检测与建模方法
Wang et al. Coverage path planning design of mapping UAVs based on particle swarm optimization algorithm
Li et al. Path planning of mobile robot based on improved td3 algorithm
CN117389305A (zh) 一种无人机巡检路径规划方法、系统、设备及介质
CN112486182A (zh) 一种实现未知环境地图构建与路径规划的扫地机器人及其使用方法
Zhang et al. Real time obstacle detection method based on lidar and wireless sensor
CN110618700A (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