CN111077495B - 一种基于三维激光的定位恢复方法 - Google Patents
一种基于三维激光的定位恢复方法 Download PDFInfo
- Publication number
- CN111077495B CN111077495B CN201911257085.0A CN201911257085A CN111077495B CN 111077495 B CN111077495 B CN 111077495B CN 201911257085 A CN201911257085 A CN 201911257085A CN 111077495 B CN111077495 B CN 111077495B
- Authority
- CN
- China
- Prior art keywords
- positioning
- dimensional laser
- robot
- point cloud
- 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.)
- Active
Links
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
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/16—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using electromagnetic waves other than radio waves
Landscapes
- Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于三维激光的定位恢复方法,包括:步骤1、机器人按照原始Global地图进行定位导航;步骤2、机器人通过激光传感器接收新一帧三维激光点云数据;步骤3、新一帧三维激光点云数据与原始Global地图进行实时匹配,计算机器人的位姿、原始Global地图坐标系下的三维激光点云数据和匹配置信度作为匹配结果;步骤4、将实时匹配得到的机器人的位姿信息进行发布;实时定量保存匹配结果;步骤5、对定位导航中的置信度进行判断,若连续帧三维激光点云数据的置信度小于设定的置信度阈值,则判定定位丢失,触发机器人原地待命机制,启动定位恢复操作;否则返回步骤2,即机器人接收新一帧三维激光点云数据,继续进行定位导航。本发明能有效防止机器人因定位丢失而导致损坏,提高了机器人的自主定位导航的可靠性和安全性,增强了机器人对环境变化的适应性。
Description
技术领域
本发明涉及巡检机器人领域,具体涉及一种基于三维激光的定位恢复方法。
背景技术
近年来,随着科技的发展,巡检机器人已经成为诸如变电站等特殊工程场所设备巡检的重要辅助,巡检机器人可以利用传感器设备感知周边环境实现对高精度环境地图的构建并根据该地图实现机器人可重复的自主导航。随着技术的不断发展进步,3D激光较2D激光,由于具有点云信息丰富、定位更加稳定的优势,在机器人构建地图与自主定位导航方面得到更加广泛的应用。目前,国内外已有的主流3D激光SLAM(即时定位与建图)技术,已经实现了机器人在理想场景环境中基于激光传感器采集数据构建地图以及根据地图自主定位的功能。然而,上述技术构建的地图是静态的,但实际的环境可能会发生动态的改变,例如环境中随季节变化的植物生长与凋零、人为的场景改造等因素都会造成地图与实际环境存在较大的不吻合。图2是现有主流SLAM自主定位导航的技术流程,依据之前的地图在发生变化的场景区域中行进,机器人定位精度会受到影响,不稳定且不可控,乃至发生定位丢失的情况,造成机器人的损坏或耗费较多人力辅助机器人,费时费力。
发明内容
为了解决上述问题,本发明提出了一种基于三维激光的定位恢复方法,包括以下步骤:步骤1、机器人按照原始Global地图进行定位导航;
步骤2、机器人通过激光传感器接收新一帧三维激光点云数据;
步骤3、新一帧三维激光点云数据与原始Global地图进行实时匹配,计算机器人的位姿、原始Global地图坐标系下的三维激光点云数据和匹配置信度作为匹配结果;
步骤4、将实时匹配得到的机器人的位姿信息进行发布;实时定量保存匹配结果;
步骤5、对定位导航中的置信度进行判断,若连续帧三维激光点云数据的置信度小于设定的置信度阈值,则判定定位丢失,触发机器人原地待命机制,启动定位恢复操作;否则返回步骤2,即机器人接收新一帧三维激光点云数据,继续进行定位导航。
进一步地,所述步骤5中的定位恢复操作具体包括:
将匹配结果中的三维激光点云数据进行Ceres优化匹配,得到Ceres优化匹配结果;
将Ceres优化匹配结果保存并形成局部子图,生成具有位置信息的局部子图文件、局部子图描述文件和导航点文件;
终止当前定位导航过程,以局部子图文件作为导航指引地图进行定位导航,机器人回退至可信区域;
机器人终止以局部子图文件作为导航指引地图进行的定位导航过程,再次以原始Global地图作为导航指引地图继续进行定位导航。
进一步地,所述步骤4中的实时定量保存匹配结果具体包括:
建立一个缓存池,用于数量可配置地保存匹配结果,以待定位丢失后启用。
进一步地,所述Ceres优化匹配具体为:
保存下来的若干帧的点云及其相关数据,通过原始Global地图坐标系和局部坐标系及两个坐标系之间的转换关系在保存下来的三维激光点云数据之间建立位姿残差模型,运用最小二乘的方法,对每一帧三维激光点云数据的位姿进行优化处理,得到准确的点云匹配结果。
进一步地,所述置信度在0到1之间,置信度阈值为0.4。
进一步地,所述实时定量保存的匹配结果中的原始Global地图坐标系下的三维激光点云数据为连续300帧原始Global地图坐标系下的三维激光点云数据;步骤5中所述的连续帧三维激光点云数据为连续100帧三维激光点云数据。
与现有技术相比,本发明具有以下有益效果:
本发明提出的基于三维激光的定位恢复方法能够在机器人自主导航过程中,在定位丢失时,机器人会停止运动,并生成准确实时的局部子图,并依据局部子图自动恢复到可信区域,实现定位恢复,从而有效防止机器人因定位丢失而导致损坏,提高了机器人的自主定位导航的可靠性和安全性,增强了机器人对环境变化的适应性。
附图说明
图1为本发明的流程图。
图2为现有主流SLAM定位导航的技术流程图。
图3为机器人仿真激光导航定位示意图。
图4为机器人激光导航定位丢失后生成的局部子图示意图。
具体实施方式
下面结合附图,对本发明提出的基于三维激光的定位恢复方法进行详细描述,具体实施步骤如图1所示:
步骤1、机器人按照原始Global地图进行定位导航:图3为原始Global地图的示意图,是前期机器人对整个作业区域采集数据并构建的概率栅格地图。其中空心圆圈表示机器人,箭头表示机器人的前进方向,小的虚线框表示现实环境发生较大变化的区域。正常情况下,机器人将该原始Global地图作为指引地图,用实时的三维激光点云数据与原始Global地图进行匹配,实现机器人定位导航。
步骤2、机器人通过激光传感器接收新一帧三维激光点云数据:机器人在行进过程中,通过激光传感器不断获取实时的三维激光点云数据,并采用逐帧或跳帧的方式对三维激光点云数据进行处理,然后将处理过的三维激光点云数据加入到定位导航系统中。
步骤3、实时匹配:新一帧点云数据通过基于多传感器(里程计、IMU)融合的三维激光点击中概率栅格地图获取最高置信度的方法与原始Global地图中的概率栅格地图进行匹配,得到当前帧点云数据对应的机器人的位姿和原始Global地图坐标系下的三维激光点云数据和匹配置信度。
步骤4.匹配后位姿信息的发布与反馈:对实时匹配得到机器人的位姿信息进行发布,作为后续用UKF(Unscented Kalman Filter,无迹卡尔曼滤波)方法进行预测定位的位置先验值。
实时定量保存匹配结果:建立一个缓存池,用于数量可配置地保存实时匹配后的每一帧三维点云的位姿信息和全局坐标系下的点云数据以及匹配置信度结果数据,以待定位丢失后启用。例如,本实施例中保存的数据数量是连续300帧三维激光点云数据,当连续100帧三维激光点云数据匹配置信度小于阈值时,则判定定位丢失,即在连续300帧三维激光点云数据中还有连续200帧点云数据是定位可信的。
步骤5.定位置信度判断:实时匹配后会得到匹配的置信度(在0和1之间),置信度越高,说明新一帧的三维激光点云数据定位的越准确。若导航定位过程中实时匹配的置信度一直大于人工设定的经验阈值,则定位一直比较准确,机器人返回步骤2持续进行定位导航;若连续若干帧(本实施例为100帧)三维激光点云数据的置信度低于人工设定的置信度阈值(本实施例将其设定为0.4),则判定定位丢失,则会触发机器人原地待命机制,如图4所示,在A点机器人判定已经定位丢失,然后停在A点处,并开始进行自动定位恢复的计算。
步骤6.Ceres优化匹配:当机器人在步骤5中被判定为定位丢失后,就会启用保存在缓存池中的三维激光点云数据,进行Ceres(Ceres solver是谷歌开发的一款用于非线性优化的库)优化匹配,即通过原始Global地图坐标系和局部坐标系及两个坐标系之间的转换关系在保存下来的三维激光点云数据之间建立位姿残差模型,运用最小二乘的方法,对每一帧三维激光点云数据的位姿进行优化处理,得到Ceres优化匹配结果。
步骤7.最新局部子图的生成与转换:将步骤6中Ceres优化匹配后的统一坐标系的点云和位姿数据先存储并生成一个实时的局部子图的中间文件中,然后将该中间文件进行转换具有位置信息的局部子图文件、局部子图描述文件和导航点文件。
步骤8.最新局部子图的复用:当步骤7中的局部子图文件生成后将终止当前定位导航过程,并以刚生成的局部子图文件作为导航指引地图进行定位导航。
步骤9.可信区域定位的恢复:如图4所示,机器人在最新局部子图的指引下,将局部子图的终点A作为机器人导航回退的起点,读取导航点文件中的点坐标(图4中空心圆)作为指引,逐步回退到可信区域(局部子图起点B)。
步骤10.原始Global地图的重新启用:当机器人恢复到可信区域后,终止以局部子图作为导航指引地图进行的定位导航过程,再以原始Global地图作为导航指引地图继续进行定位导航,从而完成自动定位恢复执行后续任务。
本发明增强了机器人自动定位导航的稳定性、安全性,大大降低了机器人跌落到道路之外的风险,有助于实现机器人自救并可持续执行任务。
以上所述仅为本发明的较佳实施例,并不用以限制本发明,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (5)
1.一种基于三维激光的定位恢复方法,其特征在于,包括以下步骤:
步骤1、机器人按照原始Global地图进行定位导航;
步骤2、机器人通过激光传感器接收新一帧三维激光点云数据;
步骤3、新一帧三维激光点云数据与原始Global地图进行实时匹配,计算机器人的位姿、原始Global地图坐标系下的三维激光点云数据和匹配置信度作为匹配结果;
步骤4、将实时匹配得到的机器人的位姿信息进行发布;实时定量保存匹配结果;
步骤5、对定位导航中的置信度进行判断,若连续帧三维激光点云数据的置信度小于设定的置信度阈值,则判定定位丢失,触发机器人原地待命机制,启动定位恢复操作;否则返回步骤2,即机器人接收新一帧三维激光点云数据,继续进行定位导航;
所述步骤5中的定位恢复操作具体包括:
将匹配结果中的三维激光点云数据进行Ceres优化匹配,得到Ceres优化匹配结果;
将Ceres优化匹配结果保存并形成局部子图,生成具有位置信息的局部子图文件、局部子图描述文件和导航点文件;
终止当前定位导航过程,以局部子图文件作为导航指引地图进行定位导航,机器人回退至可信区域;
机器人终止以局部子图文件作为导航指引地图进行的定位导航过程,再次以原始Global地图作为导航指引地图继续进行定位导航。
2.根据权利要求1所述的基于三维激光的定位恢复方法,其特征在于,所述步骤4中的实时定量保存匹配结果具体包括:
建立一个缓存池,用于数量可配置地保存匹配结果,以待定位丢失后启用。
3.根据权利要求2所述的基于三维激光的定位恢复方法,其特征在于,所述Ceres优化匹配具体为:
保存下来的若干帧的点云及其相关数据,通过原始Global地图坐标系和局部坐标系及两个坐标系之间的转换关系在保存下来的三维激光点云数据之间建立位姿残差模型,运用最小二乘的方法,对每一帧三维激光点云数据的位姿进行优化处理,得到准确的点云匹配结果。
4.根据权利要求3所述的基于三维激光的定位恢复方法,其特征在于,所述置信度在0到1之间,置信度阈值为0.4。
5.根据权利要求4所述的基于三维激光的定位恢复方法,其特征在于,所述实时定量保存的匹配结果中的原始Global地图坐标系下的三维激光点云数据为连续300帧原始Global地图坐标系下的三维激光点云数据;步骤5中所述的连续帧三维激光点云数据为连续100帧三维激光点云数据。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911257085.0A CN111077495B (zh) | 2019-12-10 | 2019-12-10 | 一种基于三维激光的定位恢复方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911257085.0A CN111077495B (zh) | 2019-12-10 | 2019-12-10 | 一种基于三维激光的定位恢复方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111077495A CN111077495A (zh) | 2020-04-28 |
CN111077495B true CN111077495B (zh) | 2022-02-22 |
Family
ID=70313688
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911257085.0A Active CN111077495B (zh) | 2019-12-10 | 2019-12-10 | 一种基于三维激光的定位恢复方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111077495B (zh) |
Families Citing this family (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111522043B (zh) * | 2020-04-30 | 2023-07-25 | 北京联合大学 | 一种无人车激光雷达快速重新匹配定位方法 |
CN111895989A (zh) * | 2020-06-24 | 2020-11-06 | 浙江大华技术股份有限公司 | 一种机器人的定位方法、装置和电子设备 |
CN111947666B (zh) * | 2020-08-21 | 2022-10-28 | 广州高新兴机器人有限公司 | 一种室外激光导航位置丢失自动找回方法 |
CN112034855A (zh) * | 2020-09-07 | 2020-12-04 | 中国南方电网有限责任公司超高压输电公司天生桥局 | 一种提高巡检机器人定位速度的方法及装置 |
CN112700495A (zh) * | 2020-11-25 | 2021-04-23 | 北京旷视机器人技术有限公司 | 位姿确定方法、装置、机器人、电子设备及存储介质 |
CN113432533B (zh) * | 2021-06-18 | 2023-08-15 | 北京盈迪曼德科技有限公司 | 一种机器人定位方法、装置、机器人及存储介质 |
CN114413882B (zh) * | 2022-03-29 | 2022-08-05 | 之江实验室 | 一种基于多假设跟踪的全局初定位方法和装置 |
CN116840820B (zh) * | 2023-08-29 | 2023-11-24 | 上海仙工智能科技有限公司 | 一种检测2d激光定位丢失的方法及系统、存储介质 |
Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103292789A (zh) * | 2012-02-27 | 2013-09-11 | 联想(北京)有限公司 | 一种定位方法及电子设备 |
CN107917710A (zh) * | 2017-11-08 | 2018-04-17 | 武汉大学 | 一种基于单线激光的室内实时定位与三维地图构建方法 |
CN107990899A (zh) * | 2017-11-22 | 2018-05-04 | 驭势科技(北京)有限公司 | 一种基于slam的定位方法和系统 |
CN108151760A (zh) * | 2017-12-28 | 2018-06-12 | 亿嘉和科技股份有限公司 | 一种基于里程计的机器人定位恢复方法 |
CN108375976A (zh) * | 2018-01-22 | 2018-08-07 | 中国民用航空飞行学院 | 一种服务机器人导航方法及系统 |
CN108873001A (zh) * | 2018-09-17 | 2018-11-23 | 江苏金智科技股份有限公司 | 一种精准评判机器人定位精度的方法 |
CN108931245A (zh) * | 2018-08-02 | 2018-12-04 | 上海思岚科技有限公司 | 移动机器人的局部自定位方法及设备 |
CN110006432A (zh) * | 2019-04-15 | 2019-07-12 | 广州高新兴机器人有限公司 | 一种基于几何先验信息下的室内机器人快速重定位的方法 |
KR20190094303A (ko) * | 2019-04-25 | 2019-08-13 | 엘지전자 주식회사 | 인공지능을 이용하여 로봇의 위치를 재정의하는 방법 및 로봇 |
CN110146098A (zh) * | 2019-05-06 | 2019-08-20 | 北京猎户星空科技有限公司 | 一种机器人地图扩建方法、装置、控制设备和存储介质 |
CN110220524A (zh) * | 2019-04-23 | 2019-09-10 | 炬星科技(深圳)有限公司 | 路径规划方法、电子设备、机器人及计算机可读存储介质 |
CN110533722A (zh) * | 2019-08-30 | 2019-12-03 | 的卢技术有限公司 | 一种基于视觉词典的机器人快速重定位方法及系统 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160188977A1 (en) * | 2014-12-24 | 2016-06-30 | Irobot Corporation | Mobile Security Robot |
CN109828280A (zh) * | 2018-11-29 | 2019-05-31 | 亿嘉和科技股份有限公司 | 一种基于三维激光栅格的定位方法以及机器人自主充电方法 |
-
2019
- 2019-12-10 CN CN201911257085.0A patent/CN111077495B/zh active Active
Patent Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103292789A (zh) * | 2012-02-27 | 2013-09-11 | 联想(北京)有限公司 | 一种定位方法及电子设备 |
CN107917710A (zh) * | 2017-11-08 | 2018-04-17 | 武汉大学 | 一种基于单线激光的室内实时定位与三维地图构建方法 |
CN107990899A (zh) * | 2017-11-22 | 2018-05-04 | 驭势科技(北京)有限公司 | 一种基于slam的定位方法和系统 |
CN108151760A (zh) * | 2017-12-28 | 2018-06-12 | 亿嘉和科技股份有限公司 | 一种基于里程计的机器人定位恢复方法 |
CN108375976A (zh) * | 2018-01-22 | 2018-08-07 | 中国民用航空飞行学院 | 一种服务机器人导航方法及系统 |
CN108931245A (zh) * | 2018-08-02 | 2018-12-04 | 上海思岚科技有限公司 | 移动机器人的局部自定位方法及设备 |
CN108873001A (zh) * | 2018-09-17 | 2018-11-23 | 江苏金智科技股份有限公司 | 一种精准评判机器人定位精度的方法 |
CN110006432A (zh) * | 2019-04-15 | 2019-07-12 | 广州高新兴机器人有限公司 | 一种基于几何先验信息下的室内机器人快速重定位的方法 |
CN110220524A (zh) * | 2019-04-23 | 2019-09-10 | 炬星科技(深圳)有限公司 | 路径规划方法、电子设备、机器人及计算机可读存储介质 |
KR20190094303A (ko) * | 2019-04-25 | 2019-08-13 | 엘지전자 주식회사 | 인공지능을 이용하여 로봇의 위치를 재정의하는 방법 및 로봇 |
CN110146098A (zh) * | 2019-05-06 | 2019-08-20 | 北京猎户星空科技有限公司 | 一种机器人地图扩建方法、装置、控制设备和存储介质 |
CN110533722A (zh) * | 2019-08-30 | 2019-12-03 | 的卢技术有限公司 | 一种基于视觉词典的机器人快速重定位方法及系统 |
Non-Patent Citations (3)
Title |
---|
Real-Time 6D Lidar SLAM in Large Scale Natural Terrains for UGV;Zhongze Liu,et al;《2018 IEEE Intelligent Vehicles Symposium (IV)》;20181231;p662-667 * |
共融移动服务机器人导航与交互关键技术研究;江国来;《中国博士学位论文全文数据库 信息科技辑》;20190615;全文 * |
基于3D激光雷达的变电站巡检机器人定位与建图方法研究;张伟;《中国优秀硕士学位论文全文数据库 工程科技II辑》;20190615;全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN111077495A (zh) | 2020-04-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111077495B (zh) | 一种基于三维激光的定位恢复方法 | |
CN108253958B (zh) | 一种稀疏环境下的机器人实时定位方法 | |
CN111060135B (zh) | 一种基于局部地图的地图修正方法及系统 | |
CN111693050B (zh) | 基于建筑信息模型的室内中大型机器人导航方法 | |
KR20190082068A (ko) | 포인트 클라우드 데이터를 융합하기 위한 방법 및 장치 | |
CN106949895A (zh) | 一种适用于变电站环境下的巡检机器人定位方法 | |
CN107063264A (zh) | 一种适用于大规模变电站环境的机器人地图创建方法 | |
CN103458501A (zh) | 一种定位方法和装置 | |
JP2012089174A (ja) | ロボット及び情報処理装置のプログラム | |
CN111784835A (zh) | 制图方法、装置、电子设备及可读存储介质 | |
CN113238554A (zh) | 一种基于激光与视觉融合slam技术的室内导航方法及系统 | |
CN111862200B (zh) | 一种煤棚内无人机定位方法 | |
Liu | A robust and efficient lidar-inertial-visual fused simultaneous localization and mapping system with loop closure | |
CN112904901B (zh) | 一种基于双目视觉slam与融合算法的路径规划方法 | |
CN109945877B (zh) | 一种巡检轨迹生成方法及装置 | |
CN113932796A (zh) | 高精地图车道线生成方法、装置和电子设备 | |
CN111951341A (zh) | 一种基于rgb-d slam的闭环检测改进方法 | |
US10718616B2 (en) | Patrol route setting apparatus, patrol route setting method, and non-transitory storage medium | |
CN116151628A (zh) | 隧道施工中地面沉降的监测与预警系统 | |
CN110794434A (zh) | 一种位姿的确定方法、装置、设备及存储介质 | |
CN116124144A (zh) | 融合注意力的目标检测与几何约束的视觉惯性室内动态环境定位系统 | |
CN115619954A (zh) | 稀疏语义地图的构建方法、装置、设备及存储介质 | |
CN115790621A (zh) | 高精地图更新方法、装置及电子设备 | |
CN116012446A (zh) | 一种移动机器人可靠定位方法及装置 | |
CN109901589B (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 |