CN111044036B - 基于粒子滤波的远程定位方法 - Google Patents

Abstract

本发明公开了一种基于粒子滤波的远程定位方法，包括以下步骤：基于视觉位姿进行粒子群状态初始化；基于优化后轮式里程计进行粒子位姿预测；基于激光匹配进行粒子状态微调；基于激光观测信息进行粒子状态更新；基于视觉位姿和机器人系统当前定位信息判断机器人是否被绑架，如果被绑架，重新开始执行基于视觉位姿进行粒子群状态初始化；如果没有被绑架，判断视觉位姿是否可信，如果视觉位姿可信，结合视觉位姿、激光观测信息和二维激光似然域地图进行粒子权重更新；如果视觉位姿信息不可信，仅仅基于激光观测信息和二维激光似然域地图进行粒子权重更新；根据更新的粒子权重对粒子状态进行加权平均，计算机器人基于多传感器融合算法得出的最终位姿。

Description

基于粒子滤波的远程定位方法

技术领域

本发明属于移动机器人定位技术领域，具体涉及一种基于粒子滤波的远程定位方法。

背景技术

以视觉导航机器人不在少数，视觉导航是依靠在机器人上安装一个摄像头，将看到的信息收集，经过将这些信息算法处理，得出环境模型，再而选择一条最佳路线，实现机器人的移动。视觉导航技术的优势在于较为成熟、技术成本低、借助大数据就能够解决技术路线，因此视觉导航在机器人市场中占据一席之地。但是视觉导航机器人的问题也很明显，视觉测距的前提要测的准，而在实际操作中，视觉测距经常会出现位置漂移的情况，在机器人的摄像头获取环境信息后计算出距离，期间若是数据处理和算法跟不上，距离数据就会形成偏差。

激光导航的工作原理是通过激光获取环境信息，测量激光从发出到接收的时间计算出自身距离前方障碍物的距离，通过算法的处理，同样得出环境模型，在不断的扫描测距中获得定位和行走路线。激光雷达作为机器人技术中的后起之秀，激光雷达因为成本的高昂起步较慢，但随着低成本激光雷达的研发，激光雷达机器人的性能更加优异，激光雷达具有高精度、高分辨率的优势，在地图的精度和定位上更精准。

如上分析，激光的信息精确度高，并且对光照鲁棒，基于激光得到的里程计，置信度高；但是激光的信息量较少，特征少，导致在结构化重复性程度高的环境中容易出现定位失效的情况。视觉信息的特征丰富，容易通过闭环检测来减少累计误差，但是其精度不高，并且对光照变化剧烈的情况不鲁棒，容易受图像遮挡，运动物体干扰或者快速移动导致系统失效。激光和可见光的优缺点形成互补关系，因此考虑将激光信息和视觉信息进行融合。

发明内容

鉴于以上存在的技术问题，本发明用于提供一种基于粒子滤波的远程定位方法，针对室内环境中的机器人定位展开研究，采用多传感器融合的方式来解决在重复性结构多的环境中可能出现的定位失效问题。

为解决上述技术问题，本发明采用如下的技术方案：

一种基于粒子滤波的远程定位方法，包括以下步骤：

基于视觉位姿进行粒子群的状态初始化；

基于优化后轮式里程计进行粒子位姿预测；

基于激光匹配进行预测粒子状态的微调；

基于激光观测信息进行粒子状态更新；

基于视觉位姿和机器人系统当前定位信息判断机器人是否被绑架，如果判断得到机器人被绑架，重新开始执行基于视觉位姿进行粒子群的状态初始化；如果机器人没有被绑架，判断视觉位姿是否可信，如果视觉位姿可信，如果视觉位姿可信，结合视觉位姿、激光观测信息和二维激光似然域地图进行粒子权重更新；如果视觉位姿信息不可信，不结合视觉权重进行粒子权重更新，仅仅基于激光观测信息和二维激光似然域地图进行粒子权重更新；；

根据更新的粒子权重对粒子状态进行加权平均，计算机器人基于多传感器融合算法得出的最终位姿。

优选地，基于视觉位姿进行粒子群状态初始化具体为：

基于视觉SLAM获得环境的视觉地图，加载视觉地图，远程操控机器人移动，当机器人在运动过程中视觉回环成功后，返回当前相机位姿x_visual，以x_visual为均值，基于服从式

的高斯概率分布采样粒子{x⁽ⁱ⁾}，对粒子群进行初始化，粒子分布服从高斯分布，每个粒子都携带有机器人的位姿信息，粒子群的位姿均值为系统粒子滤波的最后输出机器人位姿x_PF。

优选地，基于优化后轮式里程计进行粒子位姿预测具体为：

基于实现的系统粒子群{x⁽ⁱ⁾}的初始化，移动机器人在运动的过程中粒子群也需实时移动，将优化后的轮式里程计作为机器人的控制输入信息u_t-1，假设当前时刻为t，机器人在t-1时刻的粒子群为

基于式

其中

代表基于里程计的运动更新，预测下一时刻

粒子群的位置。

优选地，基于激光匹配进行粒子状态微调具体为：

根据预测得到的粒子群在t时刻的状态

由于轮式里程计存在误差，需要对粒子群中的每一个粒子进行位置微调，在移动过程中基于激光似然域模型，基于当前激光观测信息和粒子所处的地图

进行匹配，计算得到当前粒子的置信度；以每个粒子以预测值为初值，在一定范围内对粒子进行移动，计算各个位置的粒子置信度，保留置信度最高的粒子，替换预测得到的粒子群，得到微调的粒子群

优选地，基于激光观测信息进行粒子状态更新具体为：

对于粒子群

中每个粒子

在其

周围一定范围的区域内有规则地选取K个粒子，对每个粒子点求取其置信度

其中

基于机器人的概率运动模型计算得到，

基于激光地图的似然域模型计算得到，基于这K个点的置信度计算近似的后验分布，最终计算得到粒子状态的后验分布

从该后验分布中重新采样得到t时刻最终的粒子群

优选地，判断机器人是否被绑架具体为：

对当前的激光传感器信息z_t地图m，计算基于多传感器融合算法得到的机器人位姿x_PF的置信度

和基于视觉SLAM得到的机器人位姿x_visual的置信度

若

则认为此时机器人已经被绑架，对粒子群基于视觉位姿重新初始化；

若

则认为此时视觉位姿信息失效，进行粒子权重更新；

若

则认为此时视觉位姿信息有效，进行粒子权重更新。

优选地，若

则认为此时视觉位姿信息失效，进行粒子权重更新具体为：

优选地，若

则认为此时视觉位姿信

息有效，进行粒子权重更新具体为：

其中

为视觉权重，x_visual为视觉SLAM系统输出的位姿，按照如下公计算：

采用本发明具有如下的有益效果：通过本发明实施例的基于粒子滤波的远程定位方法，通过基于优化后轮式里程计进行粒子位姿预测、基于激光匹配进行粒子状态微调以及基于激光观测信息进行粒子状态更新，并结合视觉信息与激光观测信息进行权重更新，实现机器人位姿信息的实时更新，提高定位精度。

附图说明

图1为本发明实施例的基于粒子滤波的远程定位方法的步骤流程图；

图2为本发明实施例的基于粒子滤波的远程定位方法中基于视觉位姿的粒子初始化；

图3为本发明实施例的基于粒子滤波的远程定位方法中基于轮式里程计的采样模型；

图4为本发明实施例的基于粒子滤波的远程定位方法中粒子重采样过程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

根据视觉和激光信息各自优缺点，激光的信息精确度高，并且对光照鲁棒，基于激光得到的里程计，置信度高；但是激光的信息量较少，特征少，导致在结构化重复性程度高的环境中容易出现定位失效的情况。视觉信息的特征丰富，容易通过闭环检测来减少累计误差，但是其精度不高，并且对光照变化剧烈的情况不鲁棒，容易受图像遮挡，运动物体干扰或者快速移动导致系统失效。激光和可见光的优缺点形成互补关系，故本发明实施例中将激光信息和视觉信息进行融合。

在基于滤波的方式中，同时定位与建图的核心思想是根据系统得到的传感器信息z_1:t来估计环境地图m和机器人位姿联合后验概率密度函数p(x_1:t,m|z_1:t,u_0:t)，被称作全SLAM后验；基于这种思想，环境地图和机器人的位姿需要被同时计算出来，这种方式较为复杂，可以将这种联合后验进行如下的因式分解：

p(x_1:t,m|z_1:t,u_1:t-1)＝p(m|x_1:t,z_1:t)·p(x_1:t|z_1:t,u_1:t-1)

上式表明，先对机器人的轨迹进行估计，得到p(x_1:t|z_1:t,u_1:t-1)，然后再基于轨迹和观测对地图进行估计，得到p(m|x_1:t,z_1:t)。

采用滤波的方式对机器人运动路径的后验进行计算，用p(x_1:t|z_1:t,u_1:t-1)表示。机器人运动路径和激光传感器观测模型存在着高度的非线性，基于这个特点，采用粒子滤波方法开展机器人的运动路径估计，粒子滤波是一种非参数的滤波实现，可以满足高度模型的非线性需求。

粒子滤波的主要思想是以有限个带噪音的状态来近似所研究对象的状态的后验分布。粒子滤波利用一系列从后验得到的随机状态采样表示后验bel(x_t)，其中后验分布中每个的样本状态被称作粒子(particles)，有：

其中每一个粒子

是所研究的对象的状态在t时刻的一个具体的实例，是根据真实世界状态在t时刻的一种可能假设。这里的M代表的是粒子群

的粒子数量。基于粒子群

来近似状态后验bel(x_t)，在理想情况下，状态假设x_t包含在粒子群

中的概率值与其贝叶斯的后验bel(x_t)成比例：

若粒子群

中粒子的数量越多，那么式

中粒子群近似的分布和状态的真实分布越接近。

针对以上融合需求，参照图1，所示为本发明实施例提供的一种基于粒子滤波的远程定位方法的步骤流程图，包括如下步骤：

基于视觉位姿进行粒子群的状态初始化；

基于优化后轮式里程计进行粒子位姿预测；

基于激光匹配进行预测粒子状态的微调；

基于激光观测信息进行粒子状态更新；

基于视觉位姿和机器人系统当前定位信息判断机器人是否被绑架，如果判断得到机器人被绑架，重新开始执行基于视觉位姿进行粒子群的状态初始化；如果机器人没有被绑架，判断视觉位姿是否可信，如果视觉位姿可信，如果视觉位姿可信，结合视觉位姿、激光观测信息和二维激光似然域地图进行粒子权重更新；如果视觉位姿信息不可信，不结合视觉权重进行粒子权重更新，仅仅基于激光观测信息和二维激光似然域地图进行粒子权重更新；；

根据更新的粒子权重对粒子状态进行加权平均，计算机器人基于多传感器融合算法得出的最终位姿。

下面对流程中的每一步骤的实现过程进行详细的说明。

1)基于视觉位姿进行粒子群状态初始化

多传感器融合移动机器人定位系统开启的时候，机器人不知道自己所在的位姿信息，可以人为指定初始位置进行初始化；基于视觉SLAM获得了环境的视觉地图，可以加载视觉地图，远程操控机器人移动，当机器人在运动过程中视觉回环成功后，返回当前相机位姿x_visual，以x_visual为均值，基于服从式

的高斯概率分布采样粒子{x⁽ⁱ⁾}。

如图2所示对粒子群进行初始化，粒子分布服从高斯分布，每个粒子都携带有机器人的位姿信息，粒子群的位姿均值为系统粒子滤波的最后输出位姿信息x_PF，进行基于优化后轮式里程计进行粒子位姿预测。

2)基于优化后轮式里程计进行粒子位姿预测

步骤1)中实现了系统粒子群{x⁽ⁱ⁾}的初始化，移动机器人在运动的过程中粒子群也需实时移动，将优化后的轮式里程计作为机器人的控制输入信息u_t-1，如图3所示，假设当前时刻为t，机器人在t-1时刻的粒子群为

基于式

其中

代表基于里程计的运动更新，预测下一时刻

粒子群的位置，进行基于激光匹配进行粒子状态微调。

3)基于激光匹配进行粒子状态微调

步骤2)预测得到的粒子群在t时刻的状态

由于轮式里程计存在误差，需要对粒子群中的每一个粒子进行位置微调。在移动过程中基于激光似然域模型，对当前激光观测信息和粒子所处的地图

进行匹配，可以计算得到当前粒子的置信度；以每个粒子的预测值为初值，在一定范围内对粒子进行移动，计算各个位置的粒子置信度，保留置信度最高的粒子，替换预测得到的粒子群，得到微调的粒子群

进行基于激光观测信息进行粒子状态更新。

4)基于激光观测信息进行粒子状态更新

经过上述步骤3)之后，得到了微调后的预测粒子群

在此基础上基于激光传感器获得的信息对预测得到的粒子群进行状态更新。其核心思想是从一个更接近目标分布的后验分布

中采样新的粒子。

对粒子基于目标分布进行重新采样，其采样的目标分布概率模型中考虑了机器人运动概率和激光观测模型。对每个粒子的重采样过程如下：对于粒子群

中每个粒子

在其

周围一定范围的区域内有规则地选取K个粒子，如图4所示。

对每个粒子点求取其置信度

其中

可以基于机器人的概率运动模型计算得到，见公式

其中P_i为基于激光传感器自身的不同噪音模型得到的概率值，k代表激光检测到的返回的k个激光信息点；

可以基于激光地图的似然域模型计算得到，见如下公式

p＝p₁×p₂×p₃。

基于这K个点的置信度计算近似的后验分布，计算如下：

最终计算得到粒子状态的后验分布

从该后验分布中重新采样得到t时刻最终的粒子群

进行机器人是否被绑架判断。

5)判断机器人是否被绑架

基于当前的激光传感器信息z_t地图m，计算基于多传感器融合算法得到的机器人位姿x_PF的置信度

和基于视觉SLAM得到的机器人位姿x_visual的置信度

1、若

则认为此时机器人已经被绑架，运行步骤1，对粒子群基于视觉位姿重新初始化，thershold₁依据实验环境选择，通常选择-0.2。

2、若

则认为此时视觉位姿信息失效，在进行粒子权重更新时候，不结合视觉权重，thershold₂依据实验环境选择，通常选择0.1。

3、若

则认为此时视觉位姿信息有效，结合视觉权重和基于激光传感器信息的似然域地图权重进行粒子权重更新，thershold3依据实验环境选择，通常选择0.1。

6)粒子权重更新

上述得到的粒子群中的每个粒子重要性程度是一样的，但是这不符合实际情况，由于每个粒子的置信度有不同，应该对每个粒子进行权重赋值，使得权重越大的粒子的重要性程度越高，在计算最终机器人位姿的时候更加合理。

若步骤5)中视觉信息此时有效，基于公式6a)对粒子权重进行更新，若视觉信息失效，基于公式6b)对粒子权重进行更新：

其中

为视觉权重，x_Visual为视觉SLAM系统输出的位姿，按照如下公式6c)计算：

7)计算多传感器融合位姿

基于粒子群

中每个粒子的状态和权重计算得到t时刻机器人的位姿x_PF：

应当理解，本文所述的示例性实施例是说明性的而非限制性的。尽管结合附图描述了本发明的一个或多个实施例，本领域普通技术人员应当理解，在不脱离通过所附权利要求所限定的本发明的精神和范围的情况下，可以做出各种形式和细节的改变。

Claims (6)

1.一种基于粒子滤波的远程定位方法，其特征在于，包括以下步骤：

基于视觉位姿进行粒子群的状态初始化；

基于优化后轮式里程计进行粒子位姿预测；

基于激光匹配进行预测粒子状态的微调；

基于激光观测信息进行粒子状态更新；

基于视觉位姿和机器人系统当前定位信息判断机器人是否被绑架，如果判断得到机器人被绑架，重新开始执行基于视觉位姿进行粒子群的状态初始化；如果机器人没有被绑架，判断视觉位姿是否可信，如果视觉位姿可信，结合视觉位姿、激光观测信息和二维激光似然域地图进行粒子权重更新；如果视觉位姿信息不可信，不结合视觉权重进行粒子权重更新，仅仅基于激光观测信息和二维激光似然域地图进行粒子权重更新；

根据更新的粒子权重对粒子状态进行加权平均，计算机器人基于多传感器融合算法得出的最终位姿；

基于视觉位姿进行粒子群状态初始化具体为：

基于视觉SLAM获得环境的视觉地图，加载视觉地图，远程操控机器人移动，当机器人在运动过程中视觉回环成功后，返回当前相机位姿x_visual，以x_visual为均值，基于服从式

的高斯概率分布来采样粒子{x⁽ⁱ⁾}，对粒子群进行初始化，粒子分布服从高斯分布，每个粒子都携带有机器人的位姿信息，粒子群的位姿均值为系统粒子滤波的最后输出机器人位姿x_PF；

基于优化后轮式里程计进行粒子位姿预测具体为：

基于实现的系统粒子群{x⁽ⁱ⁾}的初始化，移动机器人在运动的过程中粒子群也需实时移动，将优化后的轮式里程计作为机器人的控制输入信息u_t-1，假设当前时刻为t，机器人在t-1时刻的粒子群为

基于式

其中

代表基于里程计的运动更新，预测下一时刻

粒子群的位置。

2.如权利要求1所述的基于粒子滤波的远程定位方法，其特征在于，基于激光匹配进行粒子状态微调具体为：

根据预测得到的粒子群在t时刻的状态

由于轮式里程计存在误差，需要对粒子群中的每一个粒子进行位置微调，在移动过程中基于激光似然域模型，对当前激光观测信息和粒子所处的地图

进行匹配，计算得到当前粒子的置信度；以每个粒子以预测值为初值，在一定范围内对粒子进行移动，计算各个位置的粒子置信度，保留置信度最高的粒子，替换预测得到的粒子群，得到微调的粒子群

3.如权利要求2所述的基于粒子滤波的远程定位方法，其特征在于，基于激光观测信息进行粒子状态更新具体为：

对于粒子群

中每个粒子

在其

周围一定范围的区域内有规则地选取K个粒子，对每个粒子点求取其置信度

其中

基于机器人的概率运动模型计算得到，

基于激光地图的似然域模型计算得到，基于这K个点的置信度计算近似的后验分布，最终计算得到粒子状态的后验分布

从该后验分布中重新采样得到t时刻最终的粒子群

4.如权利要求1至3任一所述的基于粒子滤波的远程定位方法，其特征在于，判断机器人是否被绑架具体为：

基于当前的激光传感器信息z_t地图m，计算基于多传感器融合算法得到的机器人位姿x_PF的置信度

和基于视觉SLAM得到的机器人位姿x_visual的置信度

若

则认为此时机器人已经被绑架，对粒子群基于视觉位姿重新初始化；

若

则认为此时视觉位姿信息失效，进行粒子权重更新；

若

则认为此时视觉位姿信息有效，进行粒子权重更新。

5.如权利要求4所述的基于粒子滤波的远程定位方法，其特征在于，若

则认为此时视觉位姿信息失效，进行粒子权重更新具体为：

6.如权利要求5所述的基于粒子滤波的远程定位方法，其特征在于，若

则认为此时视觉位姿信息有效，进行粒子权重更新具体为

其中

为视觉权重，x_visual为视觉SLAM系统输出的位姿，按照如下公计算：

Images