CN110702093A - 基于粒子滤波的定位方法、装置、存储介质及机器人 - Google Patents
基于粒子滤波的定位方法、装置、存储介质及机器人 Download PDFInfo
- Publication number
- CN110702093A CN110702093A CN201910924423.5A CN201910924423A CN110702093A CN 110702093 A CN110702093 A CN 110702093A CN 201910924423 A CN201910924423 A CN 201910924423A CN 110702093 A CN110702093 A CN 110702093A
- Authority
- CN
- China
- Prior art keywords
- particle
- particles
- filter
- vector
- weight
- 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
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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Theoretical Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Data Mining & Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Algebra (AREA)
- Automation & Control Theory (AREA)
- Mathematical Analysis (AREA)
- Computational Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了基于粒子滤波的定位方法、装置、存储介质及机器人,其中定位方法包括粒子初始化步骤、初采样步骤、权值计算步骤、判定步骤、重采样步骤和状态估算步骤,通过改善重采样步骤,具体通过划分区间并产生随机样本,按照权值判断粒子区间,并对第i个粒子复制ni次得到重采样粒子集;获得更精确的重采样粒子集有利于状态估算步骤的执行,从而提升了定位精度。
Description
技术领域
本发明涉及导航领域,特别是基于粒子滤波的定位方法、装置、存储介质及机器人。
背景技术
智能移动机器人定位是确定其在未知环境中所处位置的过程,是实现智能移动机器人自主导航能力的关键。根据粒子滤波原理的定位方法作为概率定位方法中的一种,因其计算效率较高,广泛应用于智能移动机器人。但常用的根据粒子滤波原理的定位方法,遇到传感器感知效果不准确的情况,容易导致定位效果差,也存在计算复杂度增加的问题。
发明内容
本发明的目的在于至少解决现有技术中存在的技术问题之一,提供基于粒子滤波的定位方法、装置、存储介质及机器人。
本发明解决其问题所采用的技术方案是:
本发明的第一方面,基于粒子滤波的定位方法,包括以下步骤:
粒子初始化步骤:根据运动模型的先验概率选取区域内均匀分布的M个粒子构成粒子集合,并使每个粒子的权值均为1/M;
初采样步骤:根据粒子分布规律从粒子集合采集初始样本,并产生下一状态的粒子集合;
判定步骤:计算有效粒子数Neff,若有效粒子数Neff小于阈值,则执行重采样步骤;
根据本发明的第一方面,所述运动模型的先验概率由状态向量和运动控制输入向量得到。
根据本发明的第一方面,所述状态向量包括在世界坐标系中的x轴位置坐标、y轴位置坐标和朝向角;所述运动控制输入向量为输入至移动机构的向量。
本发明的第二方面,基于粒子滤波的定位装置,包括:
粒子初始化单元,用于根据运动模型的先验概率选取区域内均匀分布的M个粒子构成粒子集合,并使每个粒子的权值均为1/M;
初采样单元,用于根据粒子分布规律从粒子集合采集初始样本,并产生下一状态的粒子集合;
权值计算单元,用于计算下一状态的粒子集合的粒子的权值
判定单元,用于根据有效粒子数Neff判定是否执行重采样单元,若有效粒子数Neff小于阈值,则执行重采样单元;
根据本发明的第二方面,所述运动模型的先验概率由状态向量和运动控制输入向量得到。
根据本发明的第二方面,所述状态向量包括在世界坐标系中的x轴位置坐标、y轴位置坐标和朝向角;所述运动控制输入向量为输入至动力机构的向量。
根据本发明的第二方面,所述状态向量包括在世界坐标系中的x轴位置坐标、y轴位置坐标和朝向角;所述运动控制输入向量为输入至动力机构的向量。
本发明的第三方面,一种存储介质,所述存储介质存储有可执行指令,所述可执行指令用于使计算机执行如本发明第一方面所述的基于粒子滤波的定位方法。
本发明的第四方面,一种机器人,包括处理器和移动机构,所述处理器执行如本发明第一方面所述的基于粒子滤波的定位方法来定位。
上述技术方案至少具有以下的有益效果:通过改善重采样步骤,具体通过划分区间并产生随机样本,按照权值判断粒子区间,并对第i个粒子复制ni次得到重采样粒子集;获得更精确的重采样粒子集有利于状态估算步骤的执行,从而提升了定位精度。
本发明的附加方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本发明的实践了解到。
附图说明
下面结合附图和实例对本发明作进一步说明。
图1是本发明实施例基于粒子滤波的定位方法的流程图;
图2是传统定位方法的滤波定位轨迹图;
图3是基于粒子滤波的定位方法的滤波定位轨迹图;
图4是传统定位方法的滤波定位误差图;
图5是基于粒子滤波的定位方法的滤波定位误差图;
图6是重采样步骤的具体步骤图。
具体实施方式
本部分将详细描述本发明的具体实施例,本发明之较佳实施例在附图中示出,附图的作用在于用图形补充说明书文字部分的描述,使人能够直观地、形象地理解本发明的每个技术特征和整体技术方案,但其不能理解为对本发明保护范围的限制。
本发明的描述中,除非另有明确的限定,设置、安装、连接等词语应做广义理解,所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。
参照图1和图6,本发明的实施例,提供了基于粒子滤波的定位方法,包括以下步骤:
步骤S1、粒子初始化步骤:根据运动模型的先验概率选取区域内均匀分布的M个粒子构成粒子集合,并使每个粒子的权值均为1/M;
步骤S2、初采样步骤:根据粒子分布规律从粒子集合采集初始样本,并产生下一状态的粒子集合,此时的概率密度函数为p(xk|xk-1,uk-1);其中xk为目标在k时刻的状态向量,uk为目标在k时刻的运动控制输入向量;
步骤S4、判定步骤:计算有效粒子数Neff,若有效粒子数Neff小于阈值,则执行重采样步骤;
步骤S5、重采样步骤:
步骤S51、将(0,1]的区间划分为N个相互独立的区间;
在该实施例中,在传统的根据粒子滤波的定位方法的基础上,对重采样步骤进行改善。具体地,基于分层统计通过划分区间并产生随机样本,按照权值判断随机数的区间;并对第i个粒子复制ni次得到重采样粒子集。根据随机数落在某个区间的区间数来得到粒子复制次数,目标是进行粒子的优胜略汰,得到最优粒子来估计系统状态,从而获得更精确的重采样粒子集有利于状态估算步骤的执行,从而提升了定位精度。
参照图2至图5,从图2和图3的对比可以看出,传统定位方法的自定位轨迹与真实路径相差较大,而本发明采用的基于粒子滤波的定位方法的自定位轨迹与真实路径比较接近。从图4和图5的对比可以看出,从初始时刻至目标运动2s时,两种方法的定位误差均急剧下降,在目标运动2s之后,两种方法的误差都趋于稳定状态,但本发明采用的基于粒子滤波的定位方法的误差维持在1m-2m之间,波动更小;传统定位方法的定位误差波动较大,且仍在增大。结果表明本发明采用的基于粒子滤波的定位方法的定位精度和稳定性均优于传统方法,且到达最佳位姿估计时间更短。
进一步,所述运动模型的先验概率由状态向量和运动控制输入向量得到。
进一步,所述状态向量包括在世界坐标系中的x轴位置坐标、y轴位置坐标和朝向角;所述运动控制输入向量为输入至移动机构的向量。
本发明的另一个实施例,提供了基于粒子滤波的定位装置,能执行上述的基于粒子滤波的定位方法;定位装置包括:
粒子初始化单元,用于根据运动模型的先验概率选取区域内均匀分布的M个粒子构成粒子集合,并使每个粒子的权值均为1/M;
初采样单元,用于根据粒子分布规律从粒子集合采集初始样本,并产生下一状态的粒子集合;
判定单元,用于根据有效粒子数Neff判定是否执行重采样单元,若有效粒子数Neff小于阈值,则执行重采样单元;
进一步,所述运动模型的先验概率由状态向量和运动控制输入向量得到。
进一步,所述状态向量包括在世界坐标系中的x轴位置坐标、y轴位置坐标和朝向角;所述运动控制输入向量为输入至动力机构的向量。
进一步,所述状态向量包括在世界坐标系中的x轴位置坐标、y轴位置坐标和朝向角;所述运动控制输入向量为输入至动力机构的向量。
本发明的另一个实施例,提供了一种存储介质,所述存储介质存储有可执行指令,所述可执行指令用于使计算机执行上述的基于粒子滤波的定位方法。
本发明的另一个实施例,提供了一种机器人,包括处理器和移动机构,所述处理器执行上述的基于粒子滤波的定位方法来定位。
以上所述,只是本发明的较佳实施例而已,本发明并不局限于上述实施方式,只要其以相同的手段达到本发明的技术效果,都应属于本发明的保护范围。
Claims (10)
1.基于粒子滤波的定位方法,其特征在于,包括以下步骤:
粒子初始化步骤:根据运动模型的先验概率选取区域内均匀分布的M个粒子构成粒子集合,并使每个粒子的权值均为1/M;
初采样步骤:根据粒子分布规律从粒子集合采集初始样本,并产生下一状态的粒子集合;
权值计算步骤:计算下一状态的粒子集合的粒子的权值
判定步骤:计算有效粒子数Neff,若有效粒子数Neff小于阈值,则执行重采样步骤;
状态估算步骤:根据系统似然函数计算重采样粒子集中粒子的权值并进行归一化,更新状态估计值得到定位信息。
2.根据权利要求1所述的基于粒子滤波的定位方法,其特征在于,所述运动模型的先验概率由状态向量和运动控制输入向量得到。
3.根据权利要求2所述的基于粒子滤波的定位方法,其特征在于,所述状态向量包括在世界坐标系中的x轴位置坐标、y轴位置坐标和朝向角;所述运动控制输入向量为输入至移动机构的向量。
5.基于粒子滤波的定位装置,其特征在于,包括:
粒子初始化单元,用于根据运动模型的先验概率选取区域内均匀分布的M个粒子构成粒子集合,并使每个粒子的权值均为1/M;
初采样单元,用于根据粒子分布规律从粒子集合采集初始样本,并产生下一状态的粒子集合;
权值计算单元,用于计算下一状态的粒子集合的粒子的权值
判定单元,用于根据有效粒子数Neff判定是否执行重采样单元,若有效粒子数Neff小于阈值,则执行重采样单元;
状态估算单元,用于根据系统似然函数计算重采样粒子集中粒子的权值并进行归一化,更新状态估计值得到定位信息。
6.根据权利要求5所述的基于粒子滤波的定位装置,其特征在于,所述运动模型的先验概率由状态向量和运动控制输入向量得到。
7.根据权利要求6所述的基于粒子滤波的定位装置,其特征在于,所述状态向量包括在世界坐标系中的x轴位置坐标、y轴位置坐标和朝向角;所述运动控制输入向量为输入至动力机构的向量。
8.根据权利要求5所述的基于粒子滤波的定位装置,其特征在于,所述状态向量包括在世界坐标系中的x轴位置坐标、y轴位置坐标和朝向角;所述运动控制输入向量为输入至动力机构的向量。
9.一种存储介质,其特征在于,所述存储介质存储有可执行指令,所述可执行指令用于使计算机执行如权利要求1-4任一项所述的基于粒子滤波的定位方法。
10.一种机器人,其特征在于,包括处理器和移动机构,所述处理器执行如权利要求1-4任一项所述的基于粒子滤波的定位方法来定位。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910924423.5A CN110702093B (zh) | 2019-09-27 | 2019-09-27 | 基于粒子滤波的定位方法、装置、存储介质及机器人 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910924423.5A CN110702093B (zh) | 2019-09-27 | 2019-09-27 | 基于粒子滤波的定位方法、装置、存储介质及机器人 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110702093A true CN110702093A (zh) | 2020-01-17 |
CN110702093B CN110702093B (zh) | 2022-10-25 |
Family
ID=69196809
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910924423.5A Active CN110702093B (zh) | 2019-09-27 | 2019-09-27 | 基于粒子滤波的定位方法、装置、存储介质及机器人 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110702093B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111352066A (zh) * | 2020-03-27 | 2020-06-30 | 西安震有信通科技有限公司 | 基于粒子滤波的定位方法、装置、计算机设备和存储介质 |
CN112465878A (zh) * | 2021-02-02 | 2021-03-09 | 北京邮电大学 | 一种基于粒子滤波的位置预测方法及设备 |
WO2021184320A1 (zh) * | 2020-03-19 | 2021-09-23 | 华为技术有限公司 | 车辆定位方法和装置 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105333879A (zh) * | 2015-12-14 | 2016-02-17 | 重庆邮电大学 | 同步定位与地图构建方法 |
CN106296727A (zh) * | 2016-07-26 | 2017-01-04 | 华北电力大学 | 一种基于高斯扰动的重采样粒子滤波算法 |
CN107246873A (zh) * | 2017-07-03 | 2017-10-13 | 哈尔滨工程大学 | 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法 |
CN109460539A (zh) * | 2018-10-15 | 2019-03-12 | 中国科学院声学研究所 | 一种基于简化容积粒子滤波的目标定位方法 |
CN109798896A (zh) * | 2019-01-21 | 2019-05-24 | 东南大学 | 一种室内机器人定位与建图方法及装置 |
CN109870167A (zh) * | 2018-12-25 | 2019-06-11 | 四川嘉垭汽车科技有限公司 | 基于视觉的无人驾驶汽车的同时定位与地图创建方法 |
CN110113030A (zh) * | 2019-04-18 | 2019-08-09 | 东南大学 | 一种二次采样的粒子滤波算法 |
-
2019
- 2019-09-27 CN CN201910924423.5A patent/CN110702093B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105333879A (zh) * | 2015-12-14 | 2016-02-17 | 重庆邮电大学 | 同步定位与地图构建方法 |
CN106296727A (zh) * | 2016-07-26 | 2017-01-04 | 华北电力大学 | 一种基于高斯扰动的重采样粒子滤波算法 |
CN107246873A (zh) * | 2017-07-03 | 2017-10-13 | 哈尔滨工程大学 | 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法 |
CN109460539A (zh) * | 2018-10-15 | 2019-03-12 | 中国科学院声学研究所 | 一种基于简化容积粒子滤波的目标定位方法 |
CN109870167A (zh) * | 2018-12-25 | 2019-06-11 | 四川嘉垭汽车科技有限公司 | 基于视觉的无人驾驶汽车的同时定位与地图创建方法 |
CN109798896A (zh) * | 2019-01-21 | 2019-05-24 | 东南大学 | 一种室内机器人定位与建图方法及装置 |
CN110113030A (zh) * | 2019-04-18 | 2019-08-09 | 东南大学 | 一种二次采样的粒子滤波算法 |
Non-Patent Citations (2)
Title |
---|
FEIBIN LI等: "A robust tracking algorithm based on sparse histogram", 《SIXTH INTERNATIONAL CONFERENCE ON INFORMATION SCIENCE AND TECHNOLOGY》 * |
黄慧敏等: "粒子滤波及其在机载雷达距离跟踪问题中的应用", 《IEEE》 * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2021184320A1 (zh) * | 2020-03-19 | 2021-09-23 | 华为技术有限公司 | 车辆定位方法和装置 |
CN111352066A (zh) * | 2020-03-27 | 2020-06-30 | 西安震有信通科技有限公司 | 基于粒子滤波的定位方法、装置、计算机设备和存储介质 |
CN111352066B (zh) * | 2020-03-27 | 2022-02-22 | 西安震有信通科技有限公司 | 基于粒子滤波的定位方法、装置、计算机设备和存储介质 |
CN112465878A (zh) * | 2021-02-02 | 2021-03-09 | 北京邮电大学 | 一种基于粒子滤波的位置预测方法及设备 |
CN112465878B (zh) * | 2021-02-02 | 2021-05-11 | 北京邮电大学 | 一种基于粒子滤波的位置预测方法及设备 |
Also Published As
Publication number | Publication date |
---|---|
CN110702093B (zh) | 2022-10-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110702093B (zh) | 基于粒子滤波的定位方法、装置、存储介质及机器人 | |
Grisetti et al. | Improved techniques for grid mapping with rao-blackwellized particle filters | |
Fox et al. | Monte carlo localization: Efficient position estimation for mobile robots | |
CN103471589B (zh) | 一种室内行人行走模式识别和轨迹追踪的方法 | |
CN109798896B (zh) | 一种室内机器人定位与建图方法及装置 | |
Pfaff et al. | Robust monte-carlo localization using adaptive likelihood models | |
CN111985093A (zh) | 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法 | |
JP2008126401A (ja) | パーティクルフィルター基盤の移動ロボットの姿勢推定方法、装置及び媒体 | |
KR20170008486A (ko) | 빠르고 강건한 궤적 설계를 통한 산업용 로봇의 파라미터 추정 방법 | |
CN111711432B (zh) | 一种基于ukf和pf混合滤波的目标跟踪算法 | |
CN110209180A (zh) | 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法 | |
CN110174907A (zh) | 一种基于自适应卡尔曼滤波的人体目标跟随方法 | |
CN113591020A (zh) | 一种基于轴对称盒空间滤波的非线性系统状态估计方法 | |
CN114608585A (zh) | 一种移动机器人同步定位与建图方法及装置 | |
CN112986977B (zh) | 一种克服雷达扩展卡尔曼航迹滤波发散的方法 | |
CN105424043A (zh) | 一种基于判断机动的运动状态估计方法 | |
Taghirad et al. | Robust solution to three-dimensional pose estimation using composite extended Kalman observer and Kalman filter | |
CN108680162B (zh) | 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法 | |
CN104091352A (zh) | 基于结构相似度的视觉跟踪方法 | |
CN116400318B (zh) | 基于在线粒子群优化的多观测目标位置估计方法及装置 | |
Wang et al. | Design of mean shift tracking algorithm based on target position prediction | |
CN111895990A (zh) | 一种基于多指标绑架检测及移动机器人重定位的方法 | |
CN111274529A (zh) | 一种鲁棒的高斯逆威沙特phd多扩展目标跟踪算法 | |
Chen et al. | Research on adaptive Monte Carlo location method based on fusion posture estimation | |
CN115114985A (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 |