CN111895990A - 一种基于多指标绑架检测及移动机器人重定位的方法 - Google Patents

Abstract

本发明提供一种基于多指标绑架检测及移动机器人重定位的方法，其特征在于：通过采用概率的方法估计机器人会出现的位置，通过虚拟的粒子模拟每个位姿，控制机器人移动时，同时将控制信息传递给每个粒子，通过激光雷达、里程计传感器的观测信息与转移后的粒子进行对比，筛选概率较大的粒子进行复制，不断迭代，最后粒子聚集的地方就代表机器人的位姿信息，若中途机器人被绑架，则根据粒子均权重、粒子协方差和机器人位姿变换进行判断，判断绑架发生之后，机器人通过在地图中产生随机粒子来进行重定位。本发明改进方法中将随机粒子与机器人的观测相关联，并时刻检测粒子均权重的变化，以及控制随机粒子的数量，减少运算负担，增强重新定位效率。

Description

一种基于多指标绑架检测及移动机器人重定位的方法

技术领域

本发明属于移动机器人自主导航及定位领域，具体涉及一种基于多指标绑架检测及移动机器人重定位的方法。

背景技术

自主移动机器人技术主要包括定位、地图构建与路径规划，而定位是机器人自主移动的主要问题，也是实现地图构建与路径规划的关键。

机器人的定位问题被划分为三个子问题，最为简单的是位姿跟踪问题，机器人的初始位姿已知，需要通过传感器数据来补偿比较小的里程计误差即可。较为复杂的是全局定位问题，此时机器人的初始位姿未知，机器人必须通过感知外界环境来确定自己的位姿。最具挑战性的难题是机器人绑架问题，它是指机器人经过一个未知运动移动到其他位置后的重定位问题。

当机器人在完成全局定位之后，在不告知被移动的情况下，将其转移到其他地方属于第一类绑架问题；第二类绑架问题是在动态环境中，机器人进行定位的过程中，粒子收敛的早期，机器人要根据上一时刻的位置信息得到下一时刻的位置信息，此时若失去前一帧的位姿信息，例如与外界发生碰撞导致位移，机器人就会遭到绑架而处于迷失状态。

国内外很多研究者对机器人绑架检测方法进行了大量的研究，但它们都基于单一指标且存在各种缺陷。例如，不能在机器人发生绑架的时候给出其明确的绑架时间点，而且，这些检测方法大多适用于机器人在全局地图当中定位完成之后发生的第一类绑架问题，本发明基于现有技术存在的问题，针对目前绑架检测算法，提出一种多指标多指标绑架检测及移动机器人重定位的方法，在蒙特卡洛算法上，基于粒子均权重单个指标引入粒子协方差和机器人位姿变化量，形成多个判断指标，进行机器人绑架检测，在检测到绑架发生之后，利用拓扑地图进行重定位。

发明内容

本发明提供一种基于多指标绑架检测及移动机器人重定位的方法，在蒙特卡洛算法的基础之上进行改进，在检测粒子均权重时，加入额外的两个检测指标：粒子协方差和机器人位置变化量，在判断机器人被绑架之后，利用改进的蒙特卡洛算法进行定位恢复。

本发明为解决现有技术中存在的问题采用的技术方案如下：

一种基于多指标绑架检测及移动机器人重定位的方法，其特征在于：通过采用概率的方法估计机器人可能出现的位置，通过虚拟的粒子去模拟每个位姿，控制机器人移动时，同时将控制信息传递给每个粒子，通过激光雷达、里程计等一系列传感器的观测信息与转移后的粒子进行对比，筛选概率较大的粒子进行复制，不断迭代，最后粒子所聚集的地方就可代表机器人的位姿信息，若中途机器人被绑架，则根据粒子均权重、粒子协方差和机器人位姿变换进行判断，判断绑架发生之后，机器人会通过在地图中产生随机粒子来进行重定位，本文改进方法中将随机粒子与机器人的观测相关联，并时刻检测粒子均权重的变化，以及控制随机粒子的数量，减少运算负担，增强重新定位效率。

所述一种基于多指标绑架检测及移动机器人重定位的方法，其特征在于，包括如下具体步骤：

第一步，粒子初始化，用大量粒子模拟移动机器人的运动状态，在整个空间中播撒N个粒子进行均匀分布，在k＝0时刻，对于i＝1,2,...,N，由已知的先验概率p(X₀)产生粒子群

且所有粒子的权值为

第二步，状态预测，根据系统的预测过程预测各个粒子的状态，机器人在k时刻，利用k-1时刻的状态估计X_k-1＝[x_k-1,y_k-1,θ_k-1]^T以及k-1时刻至k时刻执行的控制量U_k，其移动的相对位移为(Δx，Δy)，方向变化为Δθ，通过运动模型得到状态转移分布p(X_k|U_k,X_k-1)，并从状态转移分布中随机采样获取k时刻的假定状态

其中

U_k＝[Δs+Δθ]^T；

第三步，权值更新，根据机器人携带的传感器，如：激光雷达、里程计等，得到k时刻的观测值Z_K，可以得到粒子重要性权重的估计值：

第四步，权重归一化，

第五步，重采样，由第三步得到粒子的权重估计值，根据每个粒子的权重估计值大小进行复制和删除，将粒子的位姿与传感器观测的环境信息进行对比，若粒子权重估计值(可信度的概率)大于等于70％，则将该粒子进行复制，若权重估计值(可信度的概率)小于等于30％，则删除该粒子，得到新的粒子集

此时令所有的粒子权值为

k＝k+1，转到第二步，进行反复迭代，直到满足收敛条件，此时粒子聚集的位姿就代表机器人的位姿。

由于基于粒子均权重指标的检测方法在粒子未完成收敛时不能够准确检测出绑架事件的发生，多指标检测方法又加入了额外的两个指标：粒子协方差和机器人位姿变化量，具体判断如下：

每一次粒子收敛循环的过程都会进行状态预测，t时刻时，假设粒子群

的重要性权重的估计值为

将协方差的概念引入绑架判断，若已知集合{Q₁,Q₂,Q₃,....,Q_n}，则该集合的协方差定义为：Q＝cov{Q₁,Q₂,Q₃,....,Q_n}；

令

为t时刻粒子集合的平均权重，即粒子均权重，

为t时刻粒子集合的协方差，p_t＝(Δx_t,Δy_t,Δθ_t)为t时刻机器人位姿的变化量，设置粒子均权重、粒子协方差、机器人位姿变化量的阈值分别为ξ、α、β；阈值需根据机器人的硬件参数合理调试设置，不同机器人阈值大小不一样，阈值的设置对于本领域技术人员来说属于公知常识。

绑架的判定条件为：

(被绑架)；ELSE——Notkidnappede(未被绑架)，其中：

Δp_t＝p_t-p_t-1＝(Δx_t,Δy_t,Δθ_t)-(Δx_t-1,Δy_t-1,Δθ_t-1)，(t＞1)

机器人处于粒子收敛的早期，粒子的均权重不会有很明显的改变，但是Δw_t和Δp_t均会有不同程度的上升，若其中一个参数上升到超过阈值α或者β时，检测机制就会判断出此时机器人受到了“绑架”，将此时对应的迭代次数和时间记录下来，绑架检测过程结束。

检测到绑架后，为了使机器人进行重定位，则需改变重采样前粒子的状态信息，即改变原有粒子的状态分布，再进行重采样，进而完成解绑架定位恢复的过程。

原始蒙特卡罗定位在进行发散式采样时，会随机的在地图中生成随机粒子，但随机粒子的增加会增加计算机的运算负担，这样会使重新定位变得很低效，且由于随机粒子并不能很好代表机器人真实位姿，在重采样过程中很容易被迭代掉，导致定位失败，改进方法中将随机粒子与机器人的观测相关联，并时刻检测粒子均权重的变化，以及控制随机粒子的数量。

假设机器人在t时刻发生运动(Δx_t,Δy_t,Δθ_t)，原始方法中重新产生的粒子样本集为：

而当机器人被绑架过程中，粒子均权重会发生变化，粒子均权重是根据机器人激光测距传感器返回的点云数据和真实环境匹配得到的概率值，能准确反映机器人当前状态。

改进方法中粒子的样本集变为：

引入上文第三步-第五步其中D代表机器人定位失效的状态，根据失效的不同情形，D值采取自适应的变化，自适应的D值取值对于本领域技术人员来说属于公知常识，为保证其可行性和效率，在对绑架的情形判定时，采用了多组相关性参数来确保机器人不会陷入局部最优解，同时对不同情况的绑架进行分类处理，以加快粒子寻优的效率。

本发明具有如下优点：

在原有绑架检测方法的基础之上新加入了粒子协方差和机器人位姿变化两个指标，弥补了机器人定位早期若发生绑架但无法准确检测判断的缺陷，多指标绑架检测方法能够及时判断机器人绑架情况，并对粒子的迭代次数和时间记录下来，方便重定位；在重定位上也对粒子的随机生成进行了改进，将粒子的生成与粒子均权重的变化相关联，重新生成的粒子能够更准确反应机器人当前状态，加快了粒子寻优的效率。

附图说明

图1是本发明的整体流程图。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明，如图1所示，一种基于多指标绑架检测及移动机器人重定位的方法，包括如下步骤：

在整个环境空间播撒N个粒子，在k＝0时刻，由已知的先验概率p(X₀)产生粒子群

其中N的大小根据环境范围合适选择，此时，应保证所有粒子的权值为

根据机器人的运动状态模型以及对机器人的控制量去预测粒子的状态信息，假设机器人k-1时刻的状态估计X_k-1＝[x_k-1,y_k-1,θ_k-1]^T以及k-1时刻至k时刻执行的控制量U_k，其移动的相对位移为(Δx，Δy)，方向变化为Δθ，通过运动模型得到状态转移分布p(X_k|U_k,X_k-1)，并从状态转移分布中随机采样获取k时刻的假定状态

其中

U_k＝[Δs+Δθ]^T。

得到了k时刻粒子状态估计的同时，根据机器人携带的传感器，如：激光雷达、里程计等，也可以得到k时刻的观测值Z_K，即可计算出粒子的重要性权重估计值：

根据粒子的重要性权重估计可以依次计算出k时刻的粒子均权重

粒子协方差

p_k＝(Δx_k,Δy_k,Δθ_k)为k时刻机器人位姿的变化量，设置粒子均权重、粒子协方差、机器人位姿变化量的阈值分别为ξ、α、β。

进行基于多指标的绑架检测，判定条件为：

Kidnappede(被绑架)

ELSE

Not Kidnappede(未被绑架)

其中：

Δp_t＝p_t-p_t-1＝(Δx_t,Δy_t,Δθ_t)-(Δx_t-1,Δy_t-1,Δθ_t-1)，(t＞1)

若判定未被绑架，则对粒子权重进行归一化，使

若此时粒子已经满足收敛条件，则定位过程结束，若还不满足收敛条件，则根据粒子权重的估值进行重采样，剔除权重较小的粒子，复制权重大的粒子，得到新的粒子集

此时令所有的粒子权值为

令k＝k+1，转到根据机器人的运动状态模型以及对机器人的控制量去预测粒子的状态信息，即附图1中的状态预测，进行循环。

在机器人运动定位过程中，t时刻，机器人若与外界环境发生碰撞导致位移或被人为移动，则会被多指标检测机制判定为绑架发生，此时会根据本文改进方法重新生成粒子，将生成粒子的信息与机器人的观测关联起来，若观测得到该时刻的运动为(Δx_t,Δy_t,Δθ_t)，则粒子的样本集变为

其中D代表机器人定位失效的状态，根据失效的不同情形，D值采取自适应的变化，为保证其可行性和效率，在对绑架的情形判定时，采用了多组相关性参数来确保机器人不会陷入局部最优解，同时对不同情况的绑架进行分类处理，以加快粒子寻优的效率。

重新生成粒子后转到附图1状态预测步骤进行循环，直至粒子满足收敛条件，即重定位过程结束。

根据以上具体实施步骤，能够解决基于单一指标绑架检测存在的缺陷，特别是在机器人进行定位的过程中，粒子收敛的早期，原始检测方法不能及时准备判断，对绑架后粒子的重新生成也进行了改进，使其与机器人的观测相结合，加快了重定位的速度。

本发明的保护范围并不限于上述的实施例，显然，本领域的技术人员可以对本发明进行各种改动和变形而不脱离本发明的范围和精神。倘若这些改动和变形属于本发明权利要求及其等同技术的范围内，则本发明的意图也包含这些改动和变形在内。

Claims (4)

1.一种基于多指标绑架检测及移动机器人重定位的方法，其特征在于：通过采用概率的方法估计机器人会出现的位置，通过虚拟的粒子模拟每个位姿，控制机器人移动时，同时将控制信息传递给每个粒子，通过激光雷达、里程计传感器的观测信息与转移后的粒子进行对比，筛选概率较大的粒子进行复制，不断迭代，最后粒子聚集的地方就代表机器人的位姿信息，若中途机器人被绑架，则根据粒子均权重、粒子协方差和机器人位姿变换进行判断，判断绑架发生之后，机器人通过在地图中产生随机粒子来进行重定位。

2.如权利要求1所述的一种基于多指标绑架检测及移动机器人重定位的方法，其特征在于，包括如下具体步骤：

第一步，粒子初始化，用大量粒子模拟移动机器人的运动状态，在整个空间中播撒N个粒子进行均匀分布，在k＝0时刻，对于i＝1,2,...,N，由已知的先验概率p(X₀)产生粒子群

且所有粒子的权值为

第二步，状态预测，根据系统的预测过程预测各个粒子的状态，机器人在k时刻，利用k-1时刻的状态估计X_k-1＝[x_k-1,y_k-1,θ_k-1]^T以及k-1时刻至k时刻执行的控制量U_k，其移动的相对位移为(Δx，Δy)，方向变化为Δθ，通过运动模型得到状态转移分布p(X_k|U_k,X_k-1)，并从状态转移分布中随机采样获取k时刻的假定状态

其中

U_k＝[Δs+Δθ]^T；

第三步，权值更新，根据机器人携带的传感器，得到k时刻的观测值Z_K，进而得到粒子重要性权重的估计值：

i＝1,2,...,N；

第四步，权重归一化，

第五步，重采样，由第三步得到粒子的权重估计值，根据每个粒子的权重估计值大小进行复制和删除，将粒子的位姿与传感器观测的环境信息进行对比，若粒子权重估计值大于等于70％，则将该粒子进行复制，若权重估计值小于等于30％，则删除该粒子，得到新的粒子集

此时令所有的粒子权值为

k＝k+1，转到第二步，进行反复迭代，直到满足收敛条件，此时粒子聚集的位姿就代表机器人的位姿。

3.如权利要求2所述的一种基于多指标绑架检测及移动机器人重定位的方法，其特征在于：在所述第三步中引入粒子协方差和机器人位姿变化量两个指标进行检测，具体判断如下：每一次粒子收敛循环的过程都会进行状态预测，t时刻时，假设粒子群

的重要性权重的估计值为

将协方差的概念引入绑架判断，若已知集合{Q₁,Q₂,Q₃,....,Q_n}，则该集合的协方差定义为：Q＝cov{Q₁,Q₂,Q₃,....,Q_n}；

令

为t时刻粒子集合的平均权重，即粒子均权重，

为t时刻粒子集合的协方差，p_t＝(Δx_t,Δy_t,Δθ_t)为t时刻机器人位姿的变化量，设置粒子均权重、粒子协方差、机器人位姿变化量的阈值分别为ξ、α、β；

绑架的判定条件为：IF

——Kidnappede(被绑架)；ELSE——Not kidnappede(未被绑架)，其中：

Δp_t＝p_t-p_t-1＝(Δx_t,Δy_t,Δθ_t)-(Δx_t-1,Δy_t-1,Δθ_t-1)，(t＞1)

机器人处于粒子收敛的早期，粒子的均权重不会有很明显的改变，但是Δw_t和Δp_t均会有不同程度的上升，若其中一个参数上升到超过阈值α或者β时，检测机制就会判断出此时机器人受到了“绑架”，将此时对应的迭代次数和时间记录下来，绑架检测过程结束。

4.如权利要求3所述的一种基于多指标绑架检测及移动机器人重定位的方法，其特征在于：

检测到机器人受到了“绑架”后，需将机器人进行重定位，改变重采样前粒子的状态信息，即改变原有粒子的状态分布，再进行重采样，进而完成解绑架定位恢复的过程，重采样时假设机器人在t时刻发生运动(Δx_t,Δy_t,Δθ_t)，将粒子的样本集定义为

引入所述步骤中的第三步到第五步，其中D代表机器人定位失效的状态，根据失效的不同情形，D值采取自适应的变化。

Images