一种基于分层粒子群优化算法的RBPF-SLAM计算方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN110555225A(43)申请公布日2019.12.10(21)申请号201910243914.3(22)申请日2019.03.28(71)申请人陕西理工大学地址723000陕西省汉中市汉台区东一环路1号(72)发明人张鹏超王彦(74)专利代理机构西安弘理专利事务所61214代理人涂秀清(51)Int.Cl.G06F17/50(2006.01)G01S17/00(2006.01)G01S17/88(2006.01)G06N3/00(2006.01)权利要求书2页说明书4页附图1页(54)发明名称一种基于分层粒子群优化算法的RBPF-SLAM计算方法(57)摘要本发明公开了一种基于分层粒子群优化算法的RBPF-SLAM计算方法，涉及基于激光雷达的室内移动机器人定位与地图构建的科学研究，将粒子群优化算法应用到RBPF-SLAM算法中，在重采样过程中引入粒子群优化算法更新粒子位姿，根据权值划分粒子种类，对中等权值粒子保留，从而应用于室内移动机器人SLAM领域；发明借鉴粒子群寻优策略的新智能群体算法，提出一种新的重采样方法，即将RBPF中的粒子群通过粒子寻优策略调整采样粒子集，对重采样中权值较小和中等的粒子进行部分随机重采样，从而防止粒子的退化和保持粒子的多样性。CN110555225ACN110555225A权利要求书1/2页1.一种基于分层粒子群优化算法的RBPF-SLAM计算方法，其特征在于，具体按照以下步骤实施：步骤1、当t＝0时，选取N个粒子，加入最近一帧激光观测模型，求得代表机器人位置状(i)态的粒子集S:{xt}；步骤2、利用PSO算法使得粒子快速地靠近与似然函数高的区域，优化调整机器人位置(i)(i)*状态的粒子集S:{xt}，得到更新后的新粒子集P:{xt}；(i)*步骤3、计算新粒子集P:{xt}中每一个粒子对应的权值；步骤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中权值计算的迭代公式如下：2CN110555225A权利要求书2/2页式(4)中，为归一化因子，K表示常数，为粒子i在t-1时刻的权值。5.根据权利要求1所述一种基于分层粒子群优化算法的RBPF-SLAM计算方法，其特征在于，步骤4中低权值阈值ωl和高权值阈值ωh的计算公式分别如下：3CN110555225A说明书1/4页一种基于分层粒子群优化算法的RBPF-SLAM计算方法技术领域[0001]本发明属于室内移动机器人定位方法技术领域，涉及一种基于分层粒子群优化算法的RBPF-SLAM计算方法。背景技术[0002]随着机器人技术的发展，基于激光雷达的室内移动机器人SLAM问题成为关注的热点，由于激光雷达的