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

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

Info

Publication number
CN110555225B
CN110555225B CN201910243914.3A CN201910243914A CN110555225B CN 110555225 B CN110555225 B CN 110555225B CN 201910243914 A CN201910243914 A CN 201910243914A CN 110555225 B CN110555225 B CN 110555225B
Authority
CN
China
Prior art keywords
particle
weight
particles
time
robot
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
CN201910243914.3A
Other languages
English (en)
Other versions
CN110555225A (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

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
    • 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]

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Theoretical Computer Science (AREA)
  • Computational Linguistics (AREA)
  • General Health & Medical Sciences (AREA)
  • Biomedical Technology (AREA)
  • Biophysics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Data Mining & Analysis (AREA)
  • Evolutionary Computation (AREA)
  • Artificial Intelligence (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Health & Medical Sciences (AREA)
  • Feedback Control In General (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

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计算
Figure BDA0002010509670000021
取整份数为高权值粒子被复制的份数，抽取的高权值粒子总分数记为S3'，对低权值粒子集采用跳跃式的间隔M(M＝2，4，6)来进行随机抽取复制，抽取的总份数记为S1'，其中，其中，
Figure BDA0002010509670000022
为粒子i在t时刻的权值，
Figure BDA0002010509670000023
为粒子的平均权值；
步骤7、将S1'、S2、S3'组成新的粒子集，并返回步骤2；
步骤8、根据机器人轨迹和观测信息更新地图。
本发明的特征还在于：
步骤1中每个粒子对的位置状态表示如下：
Figure BDA0002010509670000024
式(1)中，
Figure BDA0002010509670000031
是混合预测分布，
Figure BDA0002010509670000032
为机器人观测模型，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时刻，每个粒子的权值为
Figure BDA0002010509670000038
步骤3中权值计算的迭代公式如下：
Figure BDA0002010509670000033
式(4)中，
Figure BDA0002010509670000034
为归一化因子，K表示常数，
Figure BDA0002010509670000035
为粒子i在t-1时刻的权值。
步骤4中低权值阈值ωl和高权值阈值ωh的计算公式分别如下：
Figure BDA0002010509670000036
Figure BDA0002010509670000037
本发明的有益效果是：
本发明一种基于分层粒子群优化算法的RBPF-SLAM计算方法借鉴粒子群寻优策略的新智能群体算法，提出一种新的重采样方法，即将RBPF中的粒子群通过粒子寻优策略调整采样粒子集，对重采样中权值较小和中等的粒子进行分层重采样，从而防止粒子的退化和保持粒子的多样性。
附图说明
图1是本发明一种基于分层粒子群优化算法的RBPF-SLAM计算方法的流程图。
具体实施方式
下面结合附图和具体实施方式对本发明进行详细说明。
本发明一种基于分层粒子群优化算法的RBPF-SLAM计算方法，流程如如图1所示，具体按照以下步骤实施：
步骤1、当t＝0时，选取N个粒子，加入最近一帧激光观测模型，求得代表机器人位置状态的粒子集S:{xt (i)},其中，每个粒子对的位置状态表示如下：
Figure BDA0002010509670000041
式(1)中，
Figure BDA0002010509670000042
是混合预测分布，
Figure BDA0002010509670000043
为机器人观测模型，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时刻，每个粒子的权值为
Figure BDA0002010509670000055
步骤3中权值计算的迭代公式如下：
Figure BDA0002010509670000051
式(4)中，
Figure BDA0002010509670000052
为归一化因子，K表示常数，
Figure BDA0002010509670000053
为粒子i在t-1时刻的权值；
步骤4、设置一个粒子数目阈值ɑ，分别计算一个低权值阈值ωl、计算一个高权值阈值ωh，将步骤3计算得到的每个粒子的权值分别与所述低权值阈值ωl和高权值阈值ωh进行比较，将粒子集分为三个部分：低权值粒子集S1、中权值粒子集S2、高权值粒子集S3；
步骤5、判断是否需要重新采样，保留中权值粒子集S2，对低权值粒子集S1和高权值粒子集S3进行传统的分层重采样；
其中低权值阈值ωl和高权值阈值ωh的计算公式分别如下：
Figure BDA0002010509670000054
Figure BDA0002010509670000061
步骤6、对高权值粒子集S3计算
Figure BDA0002010509670000062
取整份数为高权值粒子被复制的份数，抽取的高权值粒子总分数记为S3'，对低权值粒子集采用跳跃式的间隔M(M＝2，4，6)来进行随机抽取复制，抽取的总份数记为S1'，其中，其中，
Figure BDA0002010509670000063
为粒子i在t时刻的权值，
Figure BDA0002010509670000064
为粒子的平均权值；
步骤7、将S1'、S2、S3'组成新的粒子集，并返回步骤2；
步骤8、根据机器人轨迹和观测信息更新地图。
通过上述方式，本发明一种基于分层粒子群优化算法的RBPF-SLAM计算方法，借鉴粒子群寻优策略的新智能群体算法，提出一种新的重采样方法，即将RBPF中的粒子群通过粒子寻优策略调整采样粒子集，对重采样中权值较小和中等的粒子进行部分随机重采样，从而防止粒子的退化和保持粒子的多样性。

Claims (1)

1.一种基于分层粒子群优化算法的RBPF-SLAM计算方法，其特征在于，具体按照以下步骤实施：
步骤1、当t＝0时，选取N个粒子，加入最近一帧激光观测模型，求得代表机器人位置状态的粒子集S:{xt (i)}；
每个粒子对的位置状态表示如下：
Figure FDA0003794783950000011
式(1)中，
Figure FDA0003794783950000012
是混合预测分布，
Figure FDA0003794783950000013
为机器人观测模型，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)}*；
步骤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的位置；
步骤3、计算新粒子集P:{xt (i)}*中每一个粒子对应的权值；
已知t＝o时刻，每个粒子的权值为
Figure FDA0003794783950000021
步骤3中权值计算的迭代公式如下：
Figure FDA0003794783950000022
式(4)中，
Figure FDA0003794783950000023
为归一化因子，K表示常数，
Figure FDA0003794783950000024
为粒子i在t-1时刻的权值；
步骤4、设置一个粒子数目阈值ɑ，分别计算一个低权值阈值ωl、计算一个高权值阈值ωh，将步骤3计算得到的每个粒子的权值分别与所述低权值阈值ωl和高权值阈值ωh进行比较，将粒子集分为三个部分：低权值粒子集S1、中权值粒子集S2、高权值粒子集S3；
步骤4中低权值阈值ωl和高权值阈值ωh的计算公式分别如下：
Figure FDA0003794783950000025
Figure FDA0003794783950000026
步骤5、判断是否需要重新采样，保留中权值粒子集S2，对低权值粒子集S1和高权值粒子集S3进行传统的分层重采样；
步骤6、对高权值粒子集S3计算
Figure FDA0003794783950000027
取整份数为高权值粒子被复制的份数，抽取的高权值粒子总分数记为S3'，对低权值粒子集采用跳跃式的间隔M(M＝2，4，6)来进行随机抽取复制，抽取的总份数记为S1'，其中，其中，
Figure FDA0003794783950000028
为粒子i在t时刻的权值，
Figure FDA0003794783950000029
为粒子的平均权值；
步骤7、将S1'、S2、S3'组成新的粒子集，并返回步骤2；
步骤8、根据机器人轨迹和观测信息更新地图。
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 CN110555225A (zh) 2019-12-10
CN110555225B true 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)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112732854B (zh) * 2021-01-11 2023-03-31 哈尔滨工程大学 一种粒子滤波bslam方法
CN113703443B (zh) * 2021-08-12 2023-10-13 北京科技大学 一种不依赖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
改进的部分分层式粒子滤波重采样算法;曾晓辉等;《计算机应用》;20141210;第34卷(第12期);第3656-3659页 *

Also Published As

Publication number Publication date
CN110555225A (zh) 2019-12-10

Similar Documents

Publication Publication Date Title
CN108648457B (zh) 一种速度预测的方法、装置和计算机可读存储介质
CN111267831A (zh) 一种混合动力车辆智能变时域模型预测能量管理方法
CN104834215B (zh) 一种变异粒子群优化的bp神经网络pid控制算法
CN110555225B (zh) 一种基于分层粒子群优化算法的rbpf-slam计算方法
CN111563188A (zh) 一种移动多智能体协同目标搜索方法
CN111561732B (zh) 基于人工智能的换热站供热调节方法及系统
CN105333879A (zh) 同步定位与地图构建方法
CN110687797B (zh) 一种基于位置和姿态的自适应mpc泊车横向控制方法
CN108897936A (zh) 一种基于pso-bp模型的污水源热泵机组故障诊断方法
CN111681258A (zh) 基于混合灰狼优化svm的混合增强智能轨迹预测方法和装置
WO2023010828A1 (zh) 一种无人驾驶设备的横向控制
CN113050658B (zh) 一种基于狮群算法优化的slam算法
CN116560227A (zh) 基于广义扩张状态观测器的鲁棒弦稳定车队纵向控制方法
CN110288634A (zh) 一种基于改进粒子群优化算法的目标跟踪方法
CN113993152A (zh) 基于改进灰狼算法优化支持向量机回归机的通信基站流量预测方法
CN114906128A (zh) 一种基于mcts算法的自动泊车运动规划方法
CN117826590A (zh) 基于前置跟随拓扑结构的无人车编队控制方法及系统
CN113479187A (zh) 一种插电式混合动力汽车分层异步长能量管理方法
CN116758767A (zh) 基于多策略强化学习的交通信号灯控制方法
CN114200959A (zh) 一种融合高斯过程的飞行器轨迹控制方法
CN115423149A (zh) 一种能源互联网负荷预测和噪声等级估计的增量迭代聚类方法
CN115426149A (zh) 基于雅各比显著图的单交叉口信号灯控制的交通状态对抗扰动生成方法
CN110649911B (zh) 一种基于α散度的分布式非线性卡尔曼滤波方法
Wang et al. Improved Particle Swarm Optimization Algorithm Based on Gaussian-Grid Search Method.
CN112733423A (zh) 基于pso-rbfnn的工业机器人逆运动学求解方法

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
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