CN110082776B - 一种基于2d激光数据的机器人实时定位方法 - Google Patents

一种基于2d激光数据的机器人实时定位方法 Download PDF

Info

Publication number
CN110082776B
CN110082776B CN201910176931.XA CN201910176931A CN110082776B CN 110082776 B CN110082776 B CN 110082776B CN 201910176931 A CN201910176931 A CN 201910176931A CN 110082776 B CN110082776 B CN 110082776B
Authority
CN
China
Prior art keywords
robot
laser data
particle
particles
real
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
Application number
CN201910176931.XA
Other languages
English (en)
Other versions
CN110082776A (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.)
Guizhou Power Grid Co Ltd
Original Assignee
Guizhou Power Grid 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 Guizhou Power Grid Co Ltd filed Critical Guizhou Power Grid Co Ltd
Priority to CN201910176931.XA priority Critical patent/CN110082776B/zh
Publication of CN110082776A publication Critical patent/CN110082776A/zh
Application granted granted Critical
Publication of CN110082776B publication Critical patent/CN110082776B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G01S17/46—Indirect determination of position data
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4808—Evaluating distance, position or velocity data
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于2D激光数据的机器人实时定位方法，主要用于解决当前机器人定位算法实时性不高且定位精度过低的问题，本发明方法基于2D激光数据，以自动蒙特卡洛算法为基础，根据机器人的速度使用激光数据实时更新机器人位姿，并以估计位姿的置信度过滤不合理位姿。采用本发明方法用于机器人定位，不仅具有较高的实时性，而且定位精度能够控制在1cm范围内。

Description

一种基于2D激光数据的机器人实时定位方法
技术领域
本发明涉及巡检机器人技术领域，特别涉及一种基于2D激光数据的机器人实时定位方法。
背景技术
随着经济的不断发展，对机器人的无人化控制的需求越来越强烈，而机器人无人化控制最重要的是机器人能够实时感知自己的位姿，且定位精度要控制在一定范围内。借助于SLAM(即时定位与地图构建)技术的不断发展，机器人在定位导航时的实时性与精度都得到了有效改善。刘浩敏等在《基于单目视觉的同时定位与地图构建方法综述》中采用视觉方案估计机器人的位姿，该方案依次根据二维图像、三维点云估计机器人位姿，结合滤波算法与闭环检测，有效提高了定位精度，但为保证定位实时性，需要具有较高计算能力的控制单元，且视觉传感器容易受到光照的影响，产生的图像、三维点云数据不够精确，导致定位异常。季宇寒等在《基于激光雷达的巡检机器人导航系统研究》中，根据2D激光雷达数据，采用自适应蒙特卡洛定位算法定位机器人，有效解决了全局定位失效及机器人绑架的问题，但由于其根据固定的距离、角度阈值更新使用激光数据，当阈值过小时，控制单元的内存消耗过大，而阈值过大时，定位实时性不佳，且粒子散布时可能散布在无效区域，浪费计算资源。
发明内容
有鉴于此，本发明的目的是提供一种基于2D激光数据的机器人实时定位方法，用于解决当前定位算法的实时性不高且定位精度过低的问题。
本发明的目的是通过以下技术方案实现的：
本发明的一种基于2D激光数据的机器人实时定位方法，包括以下步骤：
步骤1：创建粒子滤波器，并生成采样粒子集
Figure BDA0001989887560000011
设k＝0，其中x表示单个粒子，k为某一时刻，i为粒子的索引号，N为粒子总数；
步骤2：初始化粒子，将粒子限制在机器人可行走区域；
步骤3：若机器人当前更新值大于阈值时更新里程计模型、激光数据模型，分线程处理激光数据模型；
步骤4：计算每个粒子的权值
Figure BDA0001989887560000012
及置信度
Figure BDA0001989887560000013
并作归一化处理；
步骤5：根据粒子权重获取最优的机器人估计位姿
Figure BDA0001989887560000025
及对应的位姿置信度
Figure BDA0001989887560000026
步骤6：若置信度小于阈值则放弃该位姿，返回步骤3)强制更新里程计模型、激光数据模型，否则接受该估计位姿；
步骤7：计算所有粒子权重的加权值；
步骤8：若加权值小于阈值，则进行粒子重采样，否则，k＝k+1，返回步骤3)。
特别地，所述步骤(3)中，机器人当前更新值的具体计算公式为：
Figure BDA0001989887560000021
其中INT(x)表示取x的整数部分，v为速度，k、a均为调整系数。
特别地，所述步骤4)中，置信度
Figure BDA0001989887560000027
计算公式为
Figure BDA0001989887560000022
其中z为激光数据到地图特征物的最近距离，σ为阈值，M为激光数据的总数；
特别地，所述步骤4)中，归一化公式为：
Figure BDA0001989887560000023
其中
Figure BDA0001989887560000029
为每个粒子的权重值。
特别地，所述步骤(7)中计算所有粒子的权重加权值，加权值计算公式为
Figure BDA0001989887560000024
本发明的有益效果是：
(1)该方法根据速度动态更新里程计模型、激光数据模型，且分线程处理激光数据模型，优化了内存使用率，提高了机器人定位的实时性；
(2)该方法将粒子散布在机器人可行走区域，增加了粒子有效使用率，从而提高了定位算法的稳定性；
(3)该方法通过计算估计位姿的置信度，能够过滤不合理位姿，有效提高了机器人的定位精度。
本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书和权利要求书来实现和获得。
附图说明
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步的详细描述，其中：
图1是本发明一种基于2D激光数据的高精度实时定位方法的流程图；
图2是在室外创建的定位地图；
图3、图4分别是采用自适应蒙特卡洛算法与本发明方法的初始粒子散布示意图。
具体实施方式
以下将参照附图，对本发明的优选实施例进行详细的描述。应当理解，优选实施例仅为了说明本发明，而不是为了限制本发明的保护范围。
如图1所示，本发明的一种基于2D激光数据的机器人实时定位方法，包括以下步骤：
步骤1：创建粒子滤波器，并生成采样粒子集
Figure BDA0001989887560000034
设k＝0，其中x表示单个粒子，k为某一时刻，i为粒子的索引号，N为粒子总数；
步骤2：初始化粒子，将粒子限制在机器人可行走区域；
步骤3：若机器人当前更新值大于阈值时更新里程计模型、激光数据模型，分线程处理激光数据模型；机器人当前更新值的具体计算公式为：
Figure BDA0001989887560000031
其中INT(x)表示取x的整数部分，v为速度，k、a均为调整系数。
步骤4：计算每个粒子的权值
Figure BDA0001989887560000035
及置信度
Figure BDA0001989887560000036
并作归一化处理；
本实施例中，置信度
Figure BDA0001989887560000037
计算公式为
Figure BDA0001989887560000032
其中z为激光数据到地图特征物的最近距离，σ为阈值，M为激光数据的总数。
归一化公式为：
Figure BDA0001989887560000033
其中
Figure BDA0001989887560000042
为每个粒子的权重值。
步骤5：根据粒子权重获取最优的机器人估计位姿
Figure BDA0001989887560000043
及对应的位姿置信度
Figure BDA0001989887560000044
步骤6：若置信度小于阈值则放弃该位姿，返回步骤3)强制更新里程计模型、激光数据模型，否则接受该估计位姿；
步骤7：计算所有粒子权重的加权值；本实施例中，加权值计算公式为
Figure BDA0001989887560000041
步骤8：若加权值小于阈值，则进行粒子重采样，否则，k＝k+1，返回步骤3)。
图2是在室外创建的定位地图；图3、图4分别是采用自适应蒙特卡洛算法与本发明方法的初始粒子散布示意图。由图中可以看出，本发明的方法将粒子散布在机器人可行走区域，增加了粒子有效使用率，从而提高了定位算法的稳定性，同时通过计算估计位姿的置信度，能够过滤不合理位姿，有效提高了机器人的定位精度。
本发明方法基于2D激光数据，以自动蒙特卡洛算法为基础，根据速度使用激光数据实时更新机器人位姿，并以估计位姿的置信度过滤不合理位姿。采用本发明方法用于机器人定位，不仅具有较高的实时性，而且定位精度能够控制在1cm范围内。
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (3)

1.一种基于2D激光数据的机器人实时定位方法，其特征在于：所述方法包括以下步骤：
步骤1：创建粒子滤波器，并生成采样粒子集
Figure FDA0004030278090000011
设k＝0，其中x表示单个粒子，k为某一时刻，i为粒子的索引号，N为粒子总数；
步骤2：初始化粒子，将粒子限制在机器人可行走区域；
步骤3：若机器人当前更新值大于阈值时更新里程计模型、激光数据模型，分线程处理激光数据模型；所述步骤3中，机器人当前更新值的具体计算公式为：
Figure FDA0004030278090000012
其中INT(x)表示取x的整数部分，v为机器人的速度，k、a均为调整系数；
步骤4：计算每个粒子的权值
Figure FDA0004030278090000013
及置信度
Figure FDA0004030278090000014
并作归一化处理；所述步骤4中，置信度
Figure FDA0004030278090000015
计算公式为
Figure FDA0004030278090000016
其中z为激光数据到地图特征物的最近距离，σ为阈值，M为激光数据的总数；
步骤5：根据粒子权重获取最优的机器人估计位姿
Figure FDA0004030278090000017
及对应的位姿置信度
Figure FDA0004030278090000018
步骤6：若置信度小于阈值则放弃该位姿，返回步骤3强制更新里程计模型、激光数据模型，否则接受该估计位姿；
步骤7：计算所有粒子权重的加权值；
步骤8：若加权值小于阈值，则进行粒子重采样，否则，k＝k+1，返回步骤3。
2.根据权利要求1所述的一种基于2D激光数据的机器人实时定位方法，其特征在于：所述步骤4中，归一化公式为：
Figure FDA0004030278090000019
其中
Figure FDA00040302780900000110
为每个粒子的权重值。
3.根据权利要求1所述的一种基于2D激光数据的机器人实时定位方法，其特征在于：所述步骤7中计算所有粒子的权重加权值，加权值计算公式为
Figure FDA0004030278090000021
CN201910176931.XA 2019-03-08 2019-03-08 一种基于2d激光数据的机器人实时定位方法 Active CN110082776B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910176931.XA CN110082776B (zh) 2019-03-08 2019-03-08 一种基于2d激光数据的机器人实时定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910176931.XA CN110082776B (zh) 2019-03-08 2019-03-08 一种基于2d激光数据的机器人实时定位方法

Publications (2)

Publication Number Publication Date
CN110082776A CN110082776A (zh) 2019-08-02
CN110082776B true CN110082776B (zh) 2023-04-07

Family

ID=67412334

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910176931.XA Active CN110082776B (zh) 2019-03-08 2019-03-08 一种基于2d激光数据的机器人实时定位方法

Country Status (1)

Country Link
CN (1) CN110082776B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111044036B (zh) * 2019-12-12 2021-10-15 浙江大学 基于粒子滤波的远程定位方法
CN111174782B (zh) * 2019-12-31 2021-09-17 智车优行科技（上海）有限公司 位姿估计方法、装置、电子设备及计算机可读存储介质
CN111580530B (zh) * 2020-06-16 2021-10-26 福勤智能科技（昆山）有限公司 一种定位方法、装置、自主移动设备及介质
CN111948673A (zh) * 2020-06-21 2020-11-17 珠海市一微半导体有限公司 基于imu数据更新激光数据的方法和机器人

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100809352B1 (ko) * 2006-11-16 2008-03-05 삼성전자주식회사 파티클 필터 기반의 이동 로봇의 자세 추정 방법 및 장치
CN104200226A (zh) * 2014-09-01 2014-12-10 西安电子科技大学 基于机器学习的粒子滤波目标跟踪方法
CN107063264A (zh) * 2017-04-13 2017-08-18 杭州申昊科技股份有限公司 一种适用于大规模变电站环境的机器人地图创建方法
CN107741745A (zh) * 2017-09-19 2018-02-27 浙江大学 一种实现移动机器人自主定位与地图构建的方法
CN108692701A (zh) * 2018-05-28 2018-10-23 佛山市南海区广工大数控装备协同创新研究院 基于粒子滤波器的移动机器人多传感器融合定位方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9927814B2 (en) * 2016-03-28 2018-03-27 Fetch Robotics, Inc. System and method for localization of robots

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100809352B1 (ko) * 2006-11-16 2008-03-05 삼성전자주식회사 파티클 필터 기반의 이동 로봇의 자세 추정 방법 및 장치
CN104200226A (zh) * 2014-09-01 2014-12-10 西安电子科技大学 基于机器学习的粒子滤波目标跟踪方法
CN107063264A (zh) * 2017-04-13 2017-08-18 杭州申昊科技股份有限公司 一种适用于大规模变电站环境的机器人地图创建方法
CN107741745A (zh) * 2017-09-19 2018-02-27 浙江大学 一种实现移动机器人自主定位与地图构建的方法
CN108692701A (zh) * 2018-05-28 2018-10-23 佛山市南海区广工大数控装备协同创新研究院 基于粒子滤波器的移动机器人多传感器融合定位方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
A New Particle Filter and Its Application in Mobile Robot Localization;Yi-min Xia 等;《2008 Fifth International Conference on Fuzzy Systems and Knowledge Discovery》;20081105;全文 *
Analysis of Filtering Methods for the SINS/CNS Integrated Navigation System of Missile Motion;Dan Zhang 等;《Proceeding of the 11th World Congress on Intelligent Control and Automation》;20150305;全文 *
Double-resampling Based Monte Carlo Localization for Mobile Robot;Li TianCheng 等;《Acta Automatica Sinica》;20100930;第36卷(第09期);全文 *
Multiresolutional Quasi-Monte Carlo-based particle filters;Lingling Zhao 等;《2009 IEEE International Conference on Intelligent Computing and Intelligent Systems》;20091228;全文 *
基于激光雷达的机器人定位与地图构建;肖洒 等;《农业装备与车辆工程》;20190228;第57卷(第02期);全文 *
移动机器人SLAM与路径规划在ROS框架下的实现;陈卓 等;《医疗卫生装备》;20170215;第38卷(第02期);全文 *

Also Published As

Publication number Publication date
CN110082776A (zh) 2019-08-02

Similar Documents

Publication Publication Date Title
CN110082776B (zh) 一种基于2d激光数据的机器人实时定位方法
CN107687850B (zh) 一种基于视觉和惯性测量单元的无人飞行器位姿估计方法
CN109798896B (zh) 一种室内机器人定位与建图方法及装置
CN109828588A (zh) 一种基于多传感器融合的机器人室内路径规划方法
CN109579824B (zh) 一种融入二维码信息的自适应蒙特卡诺定位方法
CN111427370A (zh) 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法
CN108490939B (zh) 在局部感知能力下的势流法的避障方法
CN111521195A (zh) 一种智能机器人
CN114387319B (zh) 点云配准方法、装置、设备以及存储介质
CN112967345B (zh) 鱼眼相机的外参标定方法、装置以及系统
CN113192140B (zh) 一种基于点线特征的双目视觉惯性定位方法和系统
CN109976189A (zh) 一种智能舰艇自动巡航模拟仿真方法
WO2022078342A1 (zh) 动态占据栅格估计方法及装置
CN108226887A (zh) 一种观测量短暂丢失情况下的水面目标救援状态估计方法
CN112505718B (zh) 用于自动驾驶车辆的定位方法、系统及计算机可读介质
WO2024041447A1 (zh) 位姿的确定方法、装置、电子设备和存储介质
Fan et al. A fire protection robot system based on SLAM localization and fire source identification
CN116883460A (zh) 一种视觉感知定位方法、装置、电子设备及存储介质
CN108694725A (zh) 一种基于视觉显著性的机器人动态跟踪方法及系统
US20230036294A1 (en) Method for processing image, electronic device and storage medium
CN116772858A (zh) 车辆定位方法、装置、定位设备及存储介质
Rud et al. Development of GPU-accelerated localization system for autonomous mobile robot
CN112857379A (zh) 一种基于改进的Gmapping-SLAM地图更新方法及系统
CN113483769B (zh) 基于粒子滤波器的车辆自定位方法、系统、设备和介质
CN113917917A (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