CN109917332B - 一种基于改进粒子滤波的室内机器人定位方法 - Google Patents

Abstract

一种基于改进粒子滤波的室内机器人定位方法，包括：S1：提取参数；包括：上一时刻的粒子集合、里程信息、测量信息、地图、上一时刻的粒子数目、上上一时刻的粒子数目，上一时刻状态粒子位置的更新趋向；S2：对上一时刻的粒子集合中的每个粒子进行更新，并融合进上一时刻的更新趋向，得到新的粒子集合；S3：对更新后的粒子集进行判断，判断|n_x，t‑n_x，t‑1|>3|n_x，t‑1‑n_x,t‑2|是否成立，若成立，则执行S4；若不成立，则x_d＝y_d＝0；并执行S5；S4：计算该时刻状态粒子位置的更新趋向；S5：计算当前时刻的粒子数目；S6：输出结果；本发明所述方法在粒子更新的同时优化了计算，可以使机器人定位修复快速收敛至正确位置，可满足机器人室内定位需求。

Description

一种基于改进粒子滤波的室内机器人定位方法

技术领域

本发明涉及机器人定位领域，更具体地，涉及一种基于改进粒子滤波的室内机器人定位方法。

背景技术

定位在机器人执行任务时起着重要作用；实时建图、路径规划和自动避障都需要机器人在世界坐标中准确的地理坐标，其基于概率的定位算法主要有扩展卡尔曼滤波算法，无迹卡尔曼滤波算法、直方图滤波算法及粒子滤波算法，前两者需要基于特征的地标辅助，不适于全局定位，而直方图滤波的好坏严重依赖于栅格地图的粗糙度，对于硬件低廉的普通机器人，一个精细的地图意味着需要更长的计算时间。

粒子滤波采用蒙特卡洛机制实现了递推贝叶斯滤波算法；它不受非高斯噪声和非线性模型的限制，并以维护粒子集合X_t来近似后验置信度bel(x_t)；目前针对粒子滤波算法的优化主要集中在滤波过程中对粒子数目的减少、粒子收敛速度的提高、粒子的抗干扰能力等方面；Liu等实施残差策略只对部分关键粒子进行更新，而其余粒子保持不变，减少了计算方面的冗余，但只更新部分粒子降低了算法的抗干扰能力，Li T等考虑了粒子的空间信息，使用分组策略对粒子进行合并，保持了粒子的多样性，但计算合并过程占用了相当大的开销；有的优化方法使用并行分布式计算，利用GPU为粒子滤波提高计算速度，但该改进对硬件要求过高；还有的则通过对真实后验的拟合，尽可能调整粒子滤波中每次迭代所需的粒子数目；但由于多数室内机器人体积小成本低廉，上述改进研究并很难符合室内定位需求。

发明内容

本发明为克服上述现有技术所述的基于粒子滤波的机器人定位很难符合室内定位需求缺陷，提供一种基于改进粒子滤波的室内机器人定位方法。

所述方法包括以下步骤：

S1：提取参数；提取的参数包括：上一时刻的粒子集合X_t-1、里程信息u_t、测量信息z_t、地图m、上一时刻的粒子数目n_x，t-1、上上一时刻的粒子数目n_x，t-2，上一时刻状态间x轴和y轴上的粒子位置的更新趋向x_d，t-1、y_d，t-1；

其中，t-1表示上一时刻，t-2表示上上一时刻；

S2：结合上一时刻状态间x轴和y轴上的粒子位置的更新趋向x_d，t-1、y_d，t-1，对上一时刻的粒子集合X_t-1中的每个粒子的位置信息和重要性权重进行更新，得到新的粒子集合X_t；

S3：对新的粒子集合X_t进行判断，判断|n_x，t-n_x，t-1|＞3|n_x，t-1-n_x，t-2|是否成立，若成立，则进行步骤S4；若不成立，则x轴和y轴上粒子位置的更新趋向x_d和y_d为：x_d＝y_d＝0；从而执行步骤S5；

在粒子滤波更新过程中的每次迭代过程中，当n_x数目急剧变化，表明当前分布与后验分布相差很大，即机器人定位误差变大，需要更大的粒子数目来适应新变化的分布，此时，可添加进测量信息，使粒子快速收敛回正常水平。设n_x，t、n_x，t-1、n_x，t-2分别代表t、t-1、t-2时刻的粒子数目，若|n_x，t-n_x，t-1|＞3|n_x，t-1-n_x，t-2|成立，则说明粒子变化程度明显，需采用带测量信息的粒子采样更新；

S4：计算x轴和y轴上的粒子位置的更新趋向x_d和y_d；x轴和y轴为基于地图坐标系上的坐标轴。

S5：计算当前时刻的粒子数目n_x，t；

S6：输出结果；输出新的粒子集合X_t、x轴和y轴上粒子位置的更新趋向x_d和y_d、当前时刻的粒子数目n_x，t。

本发明以KLD(Kullback-Leibler Distance)采样为基础，通过引入基于KLD的实时粒子数目作为判断机器人误差大小的依据，在误差较大阶段，结合t-1和t-2两个时刻雷达激光测量信息，选取权值较大的粒子的方位融合进粒子滤波的采样更新阶段，使粒子更新能以更快速度收敛至高似然区域，实现对粒子滤波的改进。

优选地，S2中采样更新后的新粒子位置信息的计算公式为：

rot1′＝rot1-g(ε₁)

rot2′＝rot2一g(ε₂)

tran′＝tran-g(ε₃)

x_q，t＝x_q，t-1+x_d，t-1+tran′*cos(θ+rot1′)

y_q，t＝y_q，t-1+y_d，t-1+tran′*Cos(θ+rot1′)

θ′＝θ+rot1′+rot2′

其中，rotl，rot2为一个周期内运动的旋转分量，tran为线性分量，rotl，rot2，tran均由里程信息u_t得到，g为标准正态分布，ε为自定义方差，x_q，t，y_q，t即为采样更新后的新粒子位置，x_q，t-1，y_q，t-1即为上一时刻采样更新后的新粒子位置，θ为更新后的角度信息，θ为更新前的角度信息，

为第i个粒子上一时刻的权值，

为状态转移概率，

为新采样的粒子。

优选地，步骤S4中x_d和y_d的计算公式为：

xd＝x′_t-1-x′_t-2

y_d＝y′_t-1-y′_t-2

x_q，t-1，y_q，t-1为第q个粒子上一时刻的真实坐标；x_q，t-2，y_q，t-2为第q个粒子上上一时刻的真实坐标，w_q，t-1为第q个粒子上一时刻的权重，Q代表最优粒子子集里粒子的个数。

优选地，步骤S5中当前时刻的粒子数目的计算公式为：

其中k为粒子分布直方图条数，ε为自定义方差，z_1-δ为正态分布上标准正常分位数，该公式是的算法kullback-leibler divergence RESAMPLE(KLD RESAMPLE)中的一个计算粒子数目的式子，用于在每个周期合理得到所需的粒子数目。

优选地，步骤S6中新的粒子集合X_t的集合为

其均值代表机器人最新位置。

与现有技术相比，本发明技术方案的有益效果是：本发明基于KLD-粒子滤波算法实时跟踪每次迭代所需粒子数，提出了一个KLD粒子滤波定位改进算法；结合里程信息及部分测量信息，将前时刻激光雷达的测量信息融合进粒子定位算法中；本算法在粒子更新的同时优化了计算，可以使机器人定位修复快速收敛至正确位置，该算法比普通KLD采样有更好的收敛效果，本发明所述的基于粒子滤波的机器人定位方法能够满足机器人室内定位需求。

附图说明

图1为本发明所述的基于改进粒子滤波的室内机器人定位方法的流程图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

本实施例提供一种基于改进粒子滤波的室内机器人定位方法。

提供一种基于改进粒子滤波的室内机器人定位方法。

所述方法包括以下步骤：

S1：提取参数；提取的参数包括：上一时刻的粒子集合X_t-1、里程信息u_t、测量信息z_t、地图m、上一时刻的粒子数目n_x，t-1、上上一时刻的粒子数目n_x，t-2，上一时刻状态间x轴和y轴上的粒子位置的更新趋向x_d，t-1、y_d，t-1；

其中，t-1表示上一时刻，t-2表示上上一时刻；

S2：结合上一时刻状态间x轴和y轴上的粒子位置的更新趋向x_d，t-1、y_d，t-1，对上一时刻的粒子集合X_t-1中的每个粒子的位置信息和重要性权重进行更新，得到新的粒子集合X_t；

采样更新后的新粒子位置信息的计算公式为：

rot1′＝rot1-g(ε₁)

rot2′＝rot2-g(ε₂)

tran′＝tran-g(ε₃)

x_q，t＝x_q，t-1+x_d，t-₁+tran′*cos(θ+rot1′)

y_q，t＝y_q，t-1+y_d，t-1+tran′*cos(θ+rot1′)

θ′＝θ+rot1′+rot2′

其中，rotl，rot2为一个周期内运动的旋转分量，tran为线性分量，rotl，rot2，tran均由里程信息u_t得到，g为标准正态分布，ε为自定义方差，x_q，t，y_q，t为采样更新后的新粒子位置，x_q，t-1，y_q，t-1为上一时刻采样更新后的新粒子位置，θ为更新后的角度信息，θ为更新前的角度信息，

为第i个粒子上一时刻的权值，

为状态转移概率，

为新采样的粒子。

S3：对新的粒子集合X_t进行判断，判断|n_x，t-n_x，t-1|＞3|n_x，t-1-n_x，t-2|是否成立，若成立，则进行步骤S4；若不成立，则x轴和y轴上粒子位置的更新趋向x_d和y_d为：x_d＝y_d＝0；从而执行步骤S5；

在粒子滤波更新过程中的每次迭代过程中，当n_x数目急剧变化，表明当前分布与后验分布相差很大，即机器人定位误差变大，需要更大的粒子数目来适应新变化的分布，此时，可添加进测量信息，使粒子快速收敛回正常水平。设n_x，t、n_x，t-1、n_x，t-2分别代表t、t-1、t-2时刻的粒子数目，若|n_x，t-n_x，t-1|＞3|n_x，t-1-n_x，t-2|成立，则说明粒子变化程度明显，需采用带测量信息的粒子采样更新；

S4：计算x轴和y轴上的粒子位置的更新趋向x_d和y_d；x轴和y轴为基于地图上的世界坐标系上的坐标轴。

x_d和y_d的计算公式为：

x_d＝x′_t-1-x′_t-2

y_d＝y′_t-1-y′_t-2

x_q，t-1，y_q，t-1为第q个粒子上一时刻的真实坐标；x_q，t-2，y_q，t-2为第q个粒子上上一时刻的真实坐标，w_q，t-1为第q个粒子上一时刻的权重，Q代表最优粒子子集里粒子的个数。

S5：计算当前时刻的粒子数目n_x，t；当前时刻的粒子数目的计算公式为：

其中k为粒子分布直方图条数，ε为自定义方差，z_1-δ为正态分布上标准正常分位数，z_1-δ可由标准正态分布表查阅得到，ε＝0.1，z_1-δ＝0.98，k为自动更新的，该公式是的算法Kullback-Leibler Divergence Resample(KLD RESAMPLE)中的一个计算粒子数目的式子，用于在每个周期合理得到所需的粒子数目。

上式用于决定在以概率为1-δ、由粒子所代表的分布与真实后验分布的相近度小于ε的条件下，粒子滤波所需要的粒子数目。

S6：输出结果；输出X_t、x_d、y_d、n_x，t。

其中，步骤S6中新的粒子集合X_t的集合为

其均值代表机器人最新位置。

本发明以KLD(Kullback-Leibler Distance)采样为基础，通过引入基于KLD的实时粒子数目作为判断机器人误差大小的依据，在误差较大阶段，结合t-1和t-2两个时刻雷达激光测量信息，选取权值较大的粒子的方位融合进粒子滤波的采样更新阶段，使粒子更新能以更快速度收敛至高似然区域，实现对粒子滤波的改进。

附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制；

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (4)

1.一种基于改进粒子滤波的室内机器人定位方法，其特征在于，所述方法包括以下步骤：

S1：提取参数；

S2：结合S1所提取的参数，对上一时刻的粒子集合X_t-1中的每个粒子的位置信息和重要性权重进行更新，得到新的粒子集合；

S3：对新的粒子集合进行判断，判断|n_x，t-n_x，t-1|＞3|n_x，t-1-n_x，t-2|是否成立，若成立，则进行步骤S4；若不成立，则x轴和y轴上粒子位置的更新趋向都为零；从而执行步骤S5；

S4：分别计算x轴和y轴上粒子位置的更新趋向；

S5：计算当前时刻的粒子数目；

S6：输出结果；输出新的粒子集合、x轴和y轴上粒子位置的更新趋向、当前时刻的粒子数目；

S1中提取的参数包括：上一时刻的粒子集合X_t-1、里程信息u_t、测量信息z_t、地图m、上一时刻的粒子数目n_x，t-1、上上一时刻的粒子数目n_x，t-2，上一时刻状态间x轴和y轴上的粒子位置的更新趋向x_d，t-1、y_d，t-1，其中，t-1表示上一时刻，t-2表示上上一时刻；

S2中采样更新后的新粒子位置信息的计算公式为：

rot1′＝rot1-g(ε₁)

rot2′＝rot2-g(ε₂)

tran′＝tran-g(ε₃)

x_q，t＝x_q，t-1+x_d，t-1+tran′*cos(θ+rot1′)

y_q，t＝y_q，t-1+y_d，t-1+tran′*cos(θ+rot1′)

θ′＝θ+rot1′+rot2′

其中，rotl，rot2为一个周期内运动的旋转分量，tran为线性分量，rotl，rot2，tran均由里程信息u_t得到，g为标准正态分布，ε₁、ε₂、ε₃分别为自定义方差，x_q，t，y_q，t为采样更新后的新粒子位置，x_q，t-1为第q个粒子上一时刻的x轴上的真实坐标，y_q，t-1为第q个粒子上一时刻y轴上的真实坐标，θ′为更新后的角度信息，θ为更新前的角度信息；

步骤S4中计算x轴上粒子位置的更新趋向x_d的计算公式为：

x_d＝x′_t-1-x′_t-2

x_q，t-1为第q个粒子上一时刻的x轴上的真实坐标；x_q，t-2为第q个粒子上上一时刻x轴上的真实坐标，w_q，t-1为第q个粒子上一时刻的权重，w_q，t-2为第q个粒子上上一时刻的权重，Q代表最优粒子子集里粒子的个数；

步骤S4中计算y轴上粒子位置的更新趋向yd的计算公式为：

y_d＝y′_t-1-y′_t-2

y_q，t-1为第q个粒子上一时刻y轴上的真实坐标；y_q，t-2为第q个粒子上上一时刻的y轴上的真实坐标，w_q，t-1为第q个粒子上一时刻的权重，Q代表最优粒子子集里粒子的个数。

2.根据权利要求1所述的基于改进粒子滤波的室内机器人定位方法，其特征在于，S2中重要性权重的计算公式为：

其中，

为第i个粒子上一时刻的权值，

为状态转移概率，

为新采样的粒子。

3.根据权利要求1所述的基于改进粒子滤波的室内机器人定位方法，其特征在于，步骤S5中当前时刻的粒子数目的计算公式为：

其中k为粒子分布直方图条数，ε为自定义方差，z_1-δ为正态分布上标准正常分位数。

4.根据权利要求1所述的基于改进粒子滤波的室内机器人定位方法，其特征在于，步骤S6中新的粒子集合X_t的集合为

其均值代表机器人最新位置。

Images