CN110555225A - 一种基于分层粒子群优化算法的rbpf-slam计算方法 - Google Patents

一种基于分层粒子群优化算法的rbpf-slam计算方法 Download PDF

Info

Publication number
CN110555225A
CN110555225A CN201910243914.3A CN201910243914A CN110555225A CN 110555225 A CN110555225 A CN 110555225A CN 201910243914 A CN201910243914 A CN 201910243914A CN 110555225 A CN110555225 A CN 110555225A
Authority
CN
China
Prior art keywords
particle
weight
rbpf
time
particles
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
CN201910243914.3A
Other languages
English (en)
Other versions
CN110555225B (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.)
Xi'an Nanjia Network Technology Co.,Ltd.
Original Assignee
Shaanxi University of Technology
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 Shaanxi University of Technology filed Critical Shaanxi University of Technology
Priority to CN201910243914.3A priority Critical patent/CN110555225B/zh
Publication of CN110555225A publication Critical patent/CN110555225A/zh
Application granted granted Critical
Publication of CN110555225B publication Critical patent/CN110555225B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/006—Theoretical aspects
    • 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/88—Lidar systems specially adapted for specific applications
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/004—Artificial life, i.e. computing arrangements simulating life
    • G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]

Abstract

本发明公开了一种基于分层粒子群优化算法的RBPF‑SLAM计算方法，涉及基于激光雷达的室内移动机器人定位与地图构建的科学研究，将粒子群优化算法应用到RBPF‑SLAM算法中，在重采样过程中引入粒子群优化算法更新粒子位姿，根据权值划分粒子种类，对中等权值粒子保留，从而应用于室内移动机器人SLAM领域；发明借鉴粒子群寻优策略的新智能群体算法，提出一种新的重采样方法，即将RBPF中的粒子群通过粒子寻优策略调整采样粒子集，对重采样中权值较小和中等的粒子进行部分随机重采样，从而防止粒子的退化和保持粒子的多样性。

Description

一种基于分层粒子群优化算法的RBPF-SLAM计算方法
技术领域
本发明属于室内移动机器人定位方法技术领域，涉及一种基于分层粒子群优化算法的RBPF-SLAM计算方法。
背景技术
随着机器人技术的发展，基于激光雷达的室内移动机器人SLAM问题成为关注的热点，由于激光雷达的测距精准，不受光照条件影响，数据比较容易处理，是目前室内移动机器人SLAM领域主流趋势。目前最常用的是基于RBPF的激光SLAM算法。该算法将SLAM问题分解成机器人定位问题和基于位姿估计的环境特征位置估计问题，用粒子滤波算法做整个路径的位姿估计，用EKF估计环境特征的位置，每一个EKF对应一个环境特征。该方法融合EKF和概率方法的优点，既降低了计算的复杂度，又具有较好的鲁棒性。
传统的RBPF-SLAM算法在估计位姿和地图过程中，重采样算法为序列重要性重采样，采样过程中不断复制高权值的粒子，忽略大量的有用粒子，造成粒子退化和多样性丢失的问题。
发明内容
本发明的目的是提供一种基于分层粒子群优化算法的RBPF-SLAM计算方法，解决了传统的RBPF-SLAM算法在采样过程中不断重复高权值的粒子，造成有用粒子退化和多样性丢失的问题。
本发明所采用的技术方案是，一种基于分层粒子群优化算法的RBPF-SLAM计算方法，具体按照以下步骤实施：
步骤1、当t＝0时，选取N个粒子，加入最近一帧激光观测模型，求得代表机器人位置状态的粒子集S:{xt (i)}；
步骤2、利用PSO算法使得粒子快速地靠近与似然函数高的区域，优化调整机器人位置状态的粒子集S:{xt (i)}，得到更新后的新粒子集P:{xt (i)}*；
步骤3、计算新粒子集P:{xt (i)}*中每一个粒子对应的权值；
步骤4、设置一个粒子数目阈值ɑ，分别计算一个低权值阈值ωl、计算一个高权值阈值ωh，将步骤3计算得到的每个粒子的权值分别与所述低权值阈值ωl和高权值阈值ωh进行比较，将粒子集分为三个部分：低权值粒子集S1、中权值粒子集S2、高权值粒子集S3；
步骤5、判断是否需要重新采样，保留中权值粒子集S2，对低权值粒子集S1和高权值粒子集S3进行传统的分层重采样；
步骤6、对高权值粒子集S3计算取整份数为高权值粒子被复制的份数，抽取的高权值粒子总分数记为S3'，对低权值粒子集采用跳跃式的间隔M(M＝2，4，6)来进行随机抽取复制，抽取的总份数记为S1'，其中，其中，为粒子i在t时刻的权值，为粒子的平均权值；
步骤7、将S1'、S2、S3'组成新的粒子集，并返回步骤2；
步骤8、根据机器人轨迹和观测信息更新地图。
本发明的特征还在于：
步骤1中每个粒子对的位置状态表示如下：
式(1)中，是混合预测分布，为机器人观测模型，p(xt|ut-1,xt-1)为机器人运动模型，xt是机器人在t时刻的状态，mt-1是t-1时刻的地图，zt是t时刻激光雷达观测值，ut-1是机器人在t-1时刻的控制变量。
步骤2中粒子更新公式如下：
Vi(t+1)＝ωVi(t)+c1r1[xpbest-xi(t)]+c2r2[xgbest-xi(t)] (2)
xi(t+1)＝ωVi(t)+xi(t) (3)
式(2)中，Vi(t+1)是粒子i在t+1时刻的速度，vi(t)是粒子i在t时刻的速度，ω表示惯性因子，c1和c2是学习因子，r1和r2表示随机数；xpbest和xgbest分别表示机器人位姿的局部和全局最优解；
式(3)中，xi(t+1)表示粒子i在时刻t+1的位置，xi(t)表示粒子i在时刻t的位置。
已知t＝o时刻，每个粒子的权值为步骤3中权值计算的迭代公式如下：
式(4)中，为归一化因子，K表示常数，为粒子i在t-1时刻的权值。
步骤4中低权值阈值ωl和高权值阈值ωh的计算公式分别如下：
本发明的有益效果是：
本发明一种基于分层粒子群优化算法的RBPF-SLAM计算方法借鉴粒子群寻优策略的新智能群体算法，提出一种新的重采样方法，即将RBPF中的粒子群通过粒子寻优策略调整采样粒子集，对重采样中权值较小和中等的粒子进行分层重采样，从而防止粒子的退化和保持粒子的多样性。
附图说明
图1是本发明一种基于分层粒子群优化算法的RBPF-SLAM计算方法的流程图。
具体实施方式
下面结合附图和具体实施方式对本发明进行详细说明。
本发明一种基于分层粒子群优化算法的RBPF-SLAM计算方法，流程如如图1所示，具体按照以下步骤实施：
步骤1、当t＝0时，选取N个粒子，加入最近一帧激光观测模型，求得代表机器人位置状态的粒子集S:{xt (i)},其中，每个粒子对的位置状态表示如下：
式(1)中，是混合预测分布，为机器人观测模型，p(xt|ut-1,xt-1)为机器人运动模型，xt是机器人在t时刻的状态，mt-1是t-1时刻的地图，zt是t时刻激光雷达观测值，ut-1是机器人在t-1时刻的控制变量；
步骤2、利用PSO算法使得粒子快速地靠近与似然函数高的区域，优化调整机器人位置状态的粒子集S:{xt (i)}，得到更新后的新粒子集P:{xt (i)}*；
粒子更新公式如下：
Vi(t+1)＝ωVi(t)+c1r1[xpbest-xi(t)]+c2r2[xgbest-xi(t)] (2)
xi(t+1)＝ωVi(t)+xi(t) (3)
式(2)中，Vi(t+1)是粒子i在t+1时刻的速度，vi(t)是粒子i在t时刻的速度，ω表示惯性因子，c1和c2是学习因子，r1和r2表示随机数；xpbest和xgbest分别表示机器人位姿的局部和全局最优解；
式(3)中，xi(t+1)表示粒子i在时刻t+1的位置，xi(t)表示粒子i在时刻t的位置；
步骤3、计算新粒子集P:{xt (i)}*中每一个粒子对应的权值；
已知t＝o时刻，每个粒子的权值为步骤3中权值计算的迭代公式如下：
式(4)中，为归一化因子，K表示常数，为粒子i在t-1时刻的权值；
步骤4、设置一个粒子数目阈值ɑ，分别计算一个低权值阈值ωl、计算一个高权值阈值ωh，将步骤3计算得到的每个粒子的权值分别与所述低权值阈值ωl和高权值阈值ωh进行比较，将粒子集分为三个部分：低权值粒子集S1、中权值粒子集S2、高权值粒子集S3；
步骤5、判断是否需要重新采样，保留中权值粒子集S2，对低权值粒子集S1和高权值粒子集S3进行传统的分层重采样；
其中低权值阈值ωl和高权值阈值ωh的计算公式分别如下：
步骤6、对高权值粒子集S3计算取整份数为高权值粒子被复制的份数，抽取的高权值粒子总分数记为S3'，对低权值粒子集采用跳跃式的间隔M(M＝2，4，6)来进行随机抽取复制，抽取的总份数记为S1'，其中，其中，为粒子i在t时刻的权值，为粒子的平均权值；
步骤7、将S1'、S2、S3'组成新的粒子集，并返回步骤2；
步骤8、根据机器人轨迹和观测信息更新地图。
通过上述方式，本发明一种基于分层粒子群优化算法的RBPF-SLAM计算方法，借鉴粒子群寻优策略的新智能群体算法，提出一种新的重采样方法，即将RBPF中的粒子群通过粒子寻优策略调整采样粒子集，对重采样中权值较小和中等的粒子进行部分随机重采样，从而防止粒子的退化和保持粒子的多样性。

Claims (5)

1.一种基于分层粒子群优化算法的RBPF-SLAM计算方法，其特征在于，具体按照以下步骤实施：
步骤1、当t＝0时，选取N个粒子，加入最近一帧激光观测模型，求得代表机器人位置状态的粒子集S:{xt (i)}；
步骤2、利用PSO算法使得粒子快速地靠近与似然函数高的区域，优化调整机器人位置状态的粒子集S:{xt (i)}，得到更新后的新粒子集P:{xt (i)}*；
步骤3、计算新粒子集P:{xt (i)}*中每一个粒子对应的权值；
步骤4、设置一个粒子数目阈值ɑ，分别计算一个低权值阈值ωl、计算一个高权值阈值ωh，将步骤3计算得到的每个粒子的权值分别与所述低权值阈值ωl和高权值阈值ωh进行比较，将粒子集分为三个部分：低权值粒子集S1、中权值粒子集S2、高权值粒子集S3；
步骤5、判断是否需要重新采样，保留中权值粒子集S2，对低权值粒子集S1和高权值粒子集S3进行传统的分层重采样；
步骤6、对高权值粒子集S3计算取整份数为高权值粒子被复制的份数，抽取的高权值粒子总分数记为S3'，对低权值粒子集采用跳跃式的间隔M(M＝2，4，6)来进行随机抽取复制，抽取的总份数记为S1'，其中，其中，为粒子i在t时刻的权值，为粒子的平均权值；
步骤7、将S1'、S2、S3'组成新的粒子集，并返回步骤2；
步骤8、根据机器人轨迹和观测信息更新地图。
2.根据权利要求1所述一种基于分层粒子群优化算法的RBPF-SLAM计算方法，其特征在于，步骤1中每个粒子对的位置状态表示如下：
式(1)中，是混合预测分布，为机器人观测模型，p(xt|ut-1,xt-1)为机器人运动模型，xt是机器人在t时刻的状态，mt-1是t-1时刻的地图，zt是t时刻激光雷达观测值，ut-1是机器人在t-1时刻的控制变量。
3.根据权利要求1所述一种基于分层粒子群优化算法的RBPF-SLAM计算方法，其特征在于，步骤2中粒子更新公式如下：
Vi(t+1)＝ωVi(t)+c1r1[xpbest-xi(t)]+c2r2[xgbest-xi(t)] (2)
xi(t+1)＝ωVi(t)+xi(t) (3)
式(2)中，Vi(t+1)是粒子i在t+1时刻的速度，vi(t)是粒子i在t时刻的速度，ω表示惯性因子，c1和c2是学习因子，r1和r2表示随机数；xpbest和xgbest分别表示机器人位姿的局部和全局最优解；
式(3)中，xi(t+1)表示粒子i在时刻t+1的位置，xi(t)表示粒子i在时刻t的位置。
4.根据权利要求1所述一种基于分层粒子群优化算法的RBPF-SLAM计算方法，其特征在于，已知t＝o时刻，每个粒子的权值为步骤3中权值计算的迭代公式如下：
式(4)中，为归一化因子，K表示常数，为粒子i在t-1时刻的权值。
5.根据权利要求1所述一种基于分层粒子群优化算法的RBPF-SLAM计算方法，其特征在于，步骤4中低权值阈值ωl和高权值阈值ωh的计算公式分别如下：
CN201910243914.3A 2019-03-28 2019-03-28 一种基于分层粒子群优化算法的rbpf-slam计算方法 Active CN110555225B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910243914.3A CN110555225B (zh) 2019-03-28 2019-03-28 一种基于分层粒子群优化算法的rbpf-slam计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910243914.3A CN110555225B (zh) 2019-03-28 2019-03-28 一种基于分层粒子群优化算法的rbpf-slam计算方法

Publications (2)

Publication Number Publication Date
CN110555225A true CN110555225A (zh) 2019-12-10
CN110555225B CN110555225B (zh) 2022-10-18

Family

ID=68736311

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910243914.3A Active CN110555225B (zh) 2019-03-28 2019-03-28 一种基于分层粒子群优化算法的rbpf-slam计算方法

Country Status (1)

Country Link
CN (1) CN110555225B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112732854A (zh) * 2021-01-11 2021-04-30 哈尔滨工程大学 一种粒子滤波bslam方法
CN113703443A (zh) * 2021-08-12 2021-11-26 北京科技大学 一种不依赖gnss的无人车自主定位与环境探索方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104376581A (zh) * 2014-12-02 2015-02-25 北京航空航天大学 一种采用自适应重采样的高斯混合无迹粒子滤波算法
CN105333879A (zh) * 2015-12-14 2016-02-17 重庆邮电大学 同步定位与地图构建方法
CN108955689A (zh) * 2018-07-13 2018-12-07 北京工业大学 基于自适应细菌觅食优化算法的rbpf-slam方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104376581A (zh) * 2014-12-02 2015-02-25 北京航空航天大学 一种采用自适应重采样的高斯混合无迹粒子滤波算法
CN105333879A (zh) * 2015-12-14 2016-02-17 重庆邮电大学 同步定位与地图构建方法
CN108955689A (zh) * 2018-07-13 2018-12-07 北京工业大学 基于自适应细菌觅食优化算法的rbpf-slam方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
曾晓辉等: "改进的部分分层式粒子滤波重采样算法", 《计算机应用》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112732854A (zh) * 2021-01-11 2021-04-30 哈尔滨工程大学 一种粒子滤波bslam方法
CN113703443A (zh) * 2021-08-12 2021-11-26 北京科技大学 一种不依赖gnss的无人车自主定位与环境探索方法
CN113703443B (zh) * 2021-08-12 2023-10-13 北京科技大学 一种不依赖gnss的无人车自主定位与环境探索方法

Also Published As

Publication number Publication date
CN110555225B (zh) 2022-10-18

Similar Documents

Publication Publication Date Title
CN110473231B (zh) 一种具有预判式学习更新策略的孪生全卷积网络的目标跟踪方法
CN104834215B (zh) 一种变异粒子群优化的bp神经网络pid控制算法
CN111047085B (zh) 一种基于元学习的混合动力车辆工况预测方法
CN110555225B (zh) 一种基于分层粒子群优化算法的rbpf-slam计算方法
CN106408592A (zh) 一种基于目标模板更新的目标跟踪方法
CN108320293A (zh) 一种结合改进粒子群算法的快速点云边界提取技术
CN110687797B (zh) 一种基于位置和姿态的自适应mpc泊车横向控制方法
CN105333879A (zh) 同步定位与地图构建方法
CN113478489B (zh) 一种机械臂轨迹规划方法
CN110428093A (zh) 基于改进的布谷鸟算法的城市轨道列车运行优化方法
CN115562265B (zh) 一种基于改进a*算法的移动机器人路径规划方法
CN107403222A (zh) 一种基于辅助更新模型和有效性检验的运动跟踪方法
CN108897936A (zh) 一种基于pso-bp模型的污水源热泵机组故障诊断方法
CN113479187B (zh) 一种插电式混合动力汽车分层异步长能量管理方法
CN108734722B (zh) 一种基于psr的视觉跟踪错误修正方法
CN112564557B (zh) 一种永磁同步电机的控制方法、装置、设备及存储介质
WO2023019536A1 (zh) 基于深度强化学习的光伏组件智能追日方法
CN114200959A (zh) 一种融合高斯过程的飞行器轨迹控制方法
CN113050658B (zh) 一种基于狮群算法优化的slam算法
CN108010051A (zh) 基于AdaBoost算法的多源视频目标融合跟踪方法
Li et al. Multi-intersections traffic signal intelligent control using collaborative q-learning algorithm
CN113993152A (zh) 基于改进灰狼算法优化支持向量机回归机的通信基站流量预测方法
CN110649911B (zh) 一种基于α散度的分布式非线性卡尔曼滤波方法
Wang et al. Improved Particle Swarm Optimization Algorithm Based on Gaussian-Grid Search Method.
CN110058635A (zh) 基于改进粒子群算法与模糊算法相结合的mppt方法

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
TR01 Transfer of patent right

Effective date of registration: 20240103

Address after: 710000, Building 5-45, Jinghe Zhigu, Chongwen Town, Jinghe New City, Xixian New District, Xi'an City, Shaanxi Province, China

Patentee after: Xi'an Nanjia Network Technology Co.,Ltd.

Address before: No.1, East 1st ring road, Hantai District, Hanzhong City, Shaanxi Province

Patentee before: Shaanxi University of Technology

TR01 Transfer of patent right