CN109885046A - 一种基于粒子滤波的移动机器人定位加速方法 - Google Patents

Abstract

本发明公开了一种基于粒子滤波的移动机器人定位加速方法，在预处理阶段，对地图的每个坐标，按照设定的分辨率，对其每个角度计算并存储最近障碍物的距离，得到结果查找表；在粒子滤波阶段，使用GPU并行维护粒子，并直接从查找表中查找粒子周围障碍物距离信息，用于计算粒子权重。由于采用GPU并行处理及改进的查找表相结合的方式，通过GPU并行处理能降低地图预处理的时间及粒子滤波定位过程中的时间，另外通过建立改进的查找表能有效降低粒子滤波定位过程中对内存的占用量；最终保证移动机器人定位的及时性。

Description

一种基于粒子滤波的移动机器人定位加速方法

技术领域

本发明涉及一种机器人定位加速方法，具体是一种基于粒子滤波的移动机器人定位加速方法。

背景技术

定位与导航技术是室内自主移动机器人的关键技术之一。室内定位与导航技术根据环境地图等先验信息，可分为三类：第一类是环境地图事先已知；第二类是同时定位与地图构建(Simultaneous Localization And Mapping,SLAM)；第三类是不依赖环境地图。第一类和第二类是常见的室内机器人自主导航方法，而定位是这两种方法的关键步骤。定位是指机器人通过读取传感器信息，与当前地图进行匹配，获取自身在地图中的位置。在SLAM问题中，应用传感器感知的信息实现可靠的定位是自主移动机器人最基本、最重要的一项功能，也是移动机器人研究中备受关注、富有挑战性的一个重要研究主题。

移动机器人搭载里程计和激光雷达，在栅格地图中使用粒子滤波来定位，是当前一种常见的机器人定位方法。粒子滤波属于贝叶斯滤波，在此框架下它又使用了蒙特卡洛方法和重要性采样方法。在用于定位场景时，粒子滤波使用大量的粒子来模拟机器人的位姿，并使用激光雷达数据和地图数据来计算粒子权重。它首先将粒子在地图中坐标周围的最近障碍物距离计算出来，然后结合激光雷达数据计算粒子的权重。但由于蒙特卡洛方法的自身特点，提高定位精度就需要使用更多的粒子；并且在计算和统计地图中粒子周围最近障碍物距离时，需要使用光线投射或其他距离计算方法。对如此众多的粒子进行相同的计算，会占用大量的计算资源，导致粒子滤波速度严重受限。由于机器人对移动速度要求越来越高，因此定位速度的加速优化一直受到很大关注。

目前，基于粒子滤波和栅格地图的定位中，地图查找表法是一种较为成熟的加速优化方法。查找表法是指在滤波过程中，放弃最近障碍物距离的实时计算，而是从预先计算好的查找表中查找对应结果。为得到查找表，首先按照一定的坐标分辨率，遍历地图所有坐标点。对每个坐标点，按照一定的角度分辨率计算该点一周的最近障碍物距离并确定障碍物与该点之间的连线相对起始坐标轴的旋转角度。然后以坐标和角度为索引，将计算结果存储到查找表中。查找表法的优点是在几乎不影响滤波精度的前提下，能大幅度提高滤波速度。但这种方法的缺点是在进行查找表过程时会占用大量的内存，尤其是地图较大时，对内存资源有限的移动机器人的移动计算平台是一种很大的压力，极易造成数据处理崩溃。

发明内容

针对上述现有技术存在的问题，本发明提供一种基于粒子滤波的移动机器人定位加速方法，通过GPU并行处理及改进的查找表，不仅有效提高粒子滤波定位速度，而且有效降低内存的占用情况，保证移动机器人定位的及时性。

为了实现上述目的，本发明采用的技术方案是：一种基于粒子滤波的移动机器人定位加速方法，具体步骤为：

A、采用GPU并行处理建立改进的查找表：首先获取移动机器人所需行驶的环境栅格地图，并将该地图处于XY轴坐标系下，然后GPU开辟多个线程，同时开辟多个内存块；将环境栅格地图复制到各个内存块中，GPU对每个内存块中环境栅格地图并行处理，每个内存块内按照角度分辨率θ将获取地图以地图中心点(即地图对应两条对角线的交点)为原点分别顺时针旋转角度α₁，α₂，…α_n，以坐标分辨率σ对x轴上的坐标进行遍历，并记录X轴上各个坐标水平高度上的所有障碍物的y坐标，分别记为bin[x₁]，bin[x₂]…bin[x_n]；最后将各个内存块不同旋转角度下各自所有bin[x]的集合分别为container[α₁]，container[α₂]，…container[α_n]；完成一轮处理后，若有剩余旋转角度需要处理，则将剩余的旋转角度分配给各个内存块，然后GPU重复上述并行处理过程，直至完成所有旋转角度的处理，最后存储各个数据；

B、采用GPU对移动机器人进行定位：设定并行加速阈值及位姿方差阈值，对移动机器人进行定位时采用粒子滤波原理在栅格地图上生成粒子，GPU开辟多个线程，生成的粒子分配给各个线程，GPU并行维护粒子；然后判断当前粒子数是否超过并行加速阈值，

若未超过并行加速阈值，则GPU进行串行处理过程，通过粒子滤波根据粒子所处位置读取改进查找表中该粒子的周围环境，进而计算粒子权重及重采样粒子，最终得出的粒子位姿，若粒子位姿方差小于设定的位姿方差阈值，则输出该数据得到滤波结果，从而对移动机器人进行定位，若粒子位姿方差大于等于阈值，则重复计算出当前粒子观测值的概率，直至得出的粒子位姿方差小于阈值；

若超过并行加速阈值，则GPU并行处理，将各个粒子分配给多个线程，每个线程按照上述串行处理过程并行计算得出当前粒子位姿。

进一步，所述计算粒子权重的过程为：

计算过程分为预测和更新两步(或先验和后验)；当系统符合马尔科夫假设时，预测和更新的公式分别如下：

p(x_k|y_1：k-1)＝∫p(x_k|x_k-1)p(x_k-1|y_1：k-1)dx_k-1 (1)

预测是指根据上一时刻的后验估计p(x_k|y_1：k)，对本时刻系统的状态x_k做出预估；更新是指通过最新得到的观测数据y_k，和第一步预测结果综合，得到最优估计；

当遇到难以解析的系统模型(如非线性非高斯的系统)时，由于无法积分，所以无法直接使用上述公式，此时引入蒙特卡洛采样；蒙特卡洛采样是指使用大量采样的平均值来代替积分：首先按照系统状态的后验概率从系统中进行采样得到N个样本(即粒子)，然后统计采样结果，得到系统状态的期望值；

由于后验概率p(x_k|y_1：k)无法直接得到，故使用重要性采样；重要性采样是指用一个能解析的概率分布函数q(x_k|y_1∶k)进行采样；并用表示此采样和用后验概率采样的相关性，即粒子的权重：

使用重要性采样方法后，则计算系统状态的期望有：

其中是归一化后的权重：

在粒子权重的计算中，每次采样时后验概率均要重新计算，使用序贯重要性采样通过迭代的方式减少运算量，最终得出：

当滤波迭代计算数次以后，会出现粒子退化现象，即大部分的粒子权重都很小，它们对估计系统状态几乎不起作用。继续维持权重小的粒子会造成计算资源的浪费。因此，需要进行重要性重采样。按照当前粒子的权重给予粒子被重采样的概率。

进一步，所述重要性采样函数选择为p(x_k|x_k-1)，即先验概率函数；在计算粒子权重时，将代入式(7)中，得

上式中表示粒子在该位置获得传感器观测值的概率。

与现有技术相比，本发明采用GPU并行处理及改进的查找表相结合的方式，通过GPU并行处理能降低地图预处理的时间及粒子滤波定位过程中的时间，另外通过建立改进的查找表能有效降低粒子滤波定位过程中对内存的占用量；因此本发明不仅有效提高粒子滤波定位速度，而且有效降低内存的占用情况，保证移动机器人定位的及时性。

附图说明

图1是本发明对栅格地图旋转角度为α＝0°的预处理示意图；

图2是本发明对栅格地图旋转角度为α＝90°的预处理示意图；

图3是本发明中GPU并行处理建立改进的查找表的流程图；

图4是本发明的整体流程图；

图5是实验证明中采用的地图。

具体实施方式

下面将对本发明做进一步说明。

如图所示，一种基于粒子滤波的移动机器人定位加速方法，具体步骤为：

A、采用GPU并行处理建立改进的查找表：首先获取移动机器人所需行驶的环境栅格地图，并将该地图处于XY轴坐标系下，然后GPU开辟多个线程，同时开辟多个内存块；将环境栅格地图复制到各个内存块中，GPU对每个内存块中环境栅格地图并行处理，每个内存块内按照角度分辨率θ将获取地图以地图中心点(即地图对应两条对角线的交点)为原点分别顺时针旋转角度α1，α2，…αn，以坐标分辨率σ对x轴上的坐标进行遍历，并记录X轴上各个坐标水平高度上的所有障碍物的y坐标，分别记为bin[x1]，bin[x2]…bin[xn]；最后将各个内存块不同旋转角度下各自所有bin[x]的集合分别为container[α1]，container[α2]，…container[αn]；完成一轮处理后，若有剩余旋转角度需要处理，则将剩余的旋转角度分配给各个内存块，然后GPU重复上述并行处理过程，直至完成所有旋转角度的处理，最后存储各个数据；当α＝0°时，预处理地图如图1所示。按照坐标分辨率σ对x轴进行遍历，并记录x＝0+σ·n(n＝1,2,...)的水平高度上的所有障碍物的y坐标,记为bin[x]。α＝0°时所有的bin的集合记为container[0]。例如图2中点A(50.0,100.0)对应的bin[50.0]：{100.0,200.0,300.0}。

当计算图1中任一点90°方向上的障碍物距离时，首先将地图旋转-90°，如图2所示。然后按照坐标分辨率σ对x轴进行遍历，并记录x＝0+σ·n(n＝1,2,...)水平高度上的所有障碍物的y坐标值。α＝90°时所有bin的集合记为container[90]。图2中点A是图2中旋转90°后得到的，坐标为(130.0,150.0)，对应的bin[130.0]：{150.0,270.0}。

B、采用GPU对移动机器人进行定位：设定并行加速阈值及位姿方差阈值，对移动机器人进行定位时采用粒子滤波原理在栅格地图上生成粒子，GPU开辟多个线程，生成的粒子分配给各个线程，GPU并行维护粒子；然后判断当前粒子数是否超过并行加速阈值，

若未超过并行加速阈值，则GPU进行串行处理过程，通过粒子滤波根据粒子所处位置读取改进查找表中该粒子的周围环境，进而计算粒子权重及重采样粒子，最终得出的粒子位姿，若粒子位姿方差小于设定的位姿方差阈值，则输出该数据得到滤波结果，从而对移动机器人进行定位，若粒子位姿方差大于等于阈值，则重复计算出当前粒子观测值的概率，直至得出的粒子位姿方差小于阈值；

若超过并行加速阈值，则GPU并行处理，将各个粒子分配给多个线程，每个线程按照上述串行处理过程并行计算得出当前粒子位姿。

进一步，所述计算粒子权重的过程为：

计算过程分为预测和更新两步(或先验和后验)；当系统符合马尔科夫假设时，预测和更新的公式分别如下：

p(x_k|y_1：k-1)＝∫p(x_k|x_k-1)p(x_k-1|y_1：k-1)dx_k-1 (1)

预测是指根据上一时刻的后验估计p(x_k|y_1：k)，对本时刻系统的状态x_k做出预估；更新是指通过最新得到的观测数据y_k，和第一步预测结果综合，得到最优估计；

当遇到难以解析的系统模型(如非线性非高斯的系统)时，由于无法积分，所以无法直接使用上述公式，此时引入蒙特卡洛采样；蒙特卡洛采样是指使用大量采样的平均值来代替积分：首先按照系统状态的后验概率从系统中进行采样得到N个样本(即粒子)，然后统计采样结果，得到系统状态的期望值；

由于后验概率p(x_k|y_1：k)无法直接得到，故使用重要性采样；重要性采样是指用一个能解析的概率分布函数q(x_k|y_1：k)进行采样；并用表示此采样和用后验概率采样的相关性，即粒子的权重：

使用重要性采样方法后，则计算系统状态的期望有：

其中是归一化后的权重：

在粒子权重的计算中，每次采样时后验概率均要重新计算，使用序贯重要性采样通过迭代的方式减少运算量，最终得出：

当滤波迭代计算数次以后，会出现粒子退化现象，即大部分的粒子权重都很小，它们对估计系统状态几乎不起作用。继续维持权重小的粒子会造成计算资源的浪费。因此，需要进行重要性重采样。按照当前粒子的权重给予粒子被重采样的概率。

进一步，所述重要性采样函数选择为p(x_k|x_k-1)，即先验概率函数；在计算粒子权重时，将代入式(7)中，得

上式中表示粒子在该位置获得传感器观测值的概率。

常规的查找表和本发明改进查找表之间的比对：

计算得到查找表时，假设地图长度(y轴方向)为l，宽度为d，坐标分辨率为σ，角度分辨率为θ，记常规所需存储的数据为n，改进的查找表所需存储的数据为n’，则有：

m_ij为地图旋转θ*i后的地图中，x＝j*σ高度上障碍物的y坐标个数。假设所有m_ij的平均值为m，则有：

显然，除非障碍物极其稠密的地图，否则m是远小于的，因此改进的查找表法所需存储的数据远小于常规的查找表。在使用改进的查找表过程中，相比常规查找表法只需要多一步对坐标的旋转，因此改进的查找表法和常规查找表法相比，速度略有降低，但几乎一致。

实验证明：

实验条件如下：

地图：尺寸为26m×26m、分辨率为0.05m/cell的地图；

粒子数：1000个；

粒子滤波使用环境：在粒子收敛的前提下，机器人从A点走到B点(如图5所示)；

粒子收敛阈值：x：0.09(m*m)；y：0.09(m*m)；角度：0.25(rad*rad)；

测试次数：10；

实验结果对比：取10次测试结果的平均值。

表1不同查找表法的实验结果

表1列出了使用查找表法、无查找表以及使用改进查找表三种方法的实验结果。由表1可见，与不使用查找表的情况相比，使用常规的查找表需要较长的初始化时间，即地图预计算时间，而且内存占用也相当大。但在保持粒子收敛的前提下，最大行驶速度可以提升4倍多，运算速度的提高也较为明显。而使用改进的查找表法，不但可以保证机器人有几乎相同的最大的行驶速度，而且初始化时间与使用查找表法相比降低了近20倍，内存占用降低了近30倍。

对以上三种情况各自使用CUDA调用GPU，测试粒子保持收敛的前提下最大行驶速度，结果如表2所示。从表2中可以看出，对比不使用GPU，使用GPU并行运算后，三种情况下的粒子滤波速度都有较大的提升。

表2使用和不使用GPU时最大行驶速度(m/s)的对比

	GPU	无GPU
			使用查找表	6.13	4.80
无查找表	3.03	1.03
			改进查找表	5.89	4.77

综上可得：

粒子滤波定位是一种常见而有效的一种定位方法。但粒子滤波需要维持大量的粒子来模拟机器人的位置，导致计算量和内存占用较大。常规的加速方法是使用三维的查找表，但是这种方法占用了较大的内存。使用改进的查找表法，由于存储方式变为只存储障碍物的单一坐标值，所以占用内存很小，并且经过实验验证，和查找表法有几乎相等的加速性能。另外，粒子的维护是非常消耗计算资源的，本发明将对粒子串行的计算改为GPU并行计算，经实验验证可以有效提高定位速度。

Claims (3)

1.一种基于粒子滤波的移动机器人定位加速方法，其特征在于，具体步骤为：

A、采用GPU并行处理建立改进的查找表：首先获取移动机器人所需行驶的环境栅格地图，并将该地图处于XY轴坐标系下，然后GPU开辟多个线程，同时开辟多个内存块；将环境栅格地图复制到各个内存块中，GPU对每个内存块中环境栅格地图并行处理，每个内存块内按照角度分辨率θ将获取地图以地图中心点为原点分别顺时针旋转角度α₁，α₂，…α_n，以坐标分辨率σ对x轴上的坐标进行遍历，并记录X轴上各个坐标水平高度上的所有障碍物的y坐标，分别记为bin[x₁]，bin[x₂]…bin[x_n]；最后将各个内存块不同旋转角度下各自所有bin[x]的集合分别为container[α₁]，container[α₂]，…container[α_n]；完成一轮处理后，若有剩余旋转角度需要处理，则将剩余的旋转角度分配给各个内存块，然后GPU重复上述并行处理过程，直至完成所有旋转角度的处理，最后存储各个数据；

B、采用GPU对移动机器人进行定位：设定并行加速阈值及位姿方差阈值，对移动机器人进行定位时采用粒子滤波原理在栅格地图上生成粒子，GPU开辟多个线程，生成的粒子分配给各个线程，GPU并行维护粒子；然后判断当前粒子数是否超过并行加速阈值，

若未超过并行加速阈值，则GPU进行串行处理过程，通过粒子滤波根据粒子所处位置读取改进查找表中该粒子的周围环境，进而计算粒子权重及重采样粒子，最终得出的粒子位姿，若粒子位姿方差小于设定的位姿方差阈值，则输出该数据得到滤波结果，从而对移动机器人进行定位，若粒子位姿方差大于等于阈值，则重复计算出当前粒子观测值的概率，直至得出的粒子位姿方差小于阈值；

若超过并行加速阈值，则GPU并行处理，将各个粒子分配给多个线程，每个线程按照上述串行处理过程并行计算得出当前粒子位姿。

2.根据权利要求1所述的一种基于粒子滤波的移动机器人定位加速方法，其特征在于，所述计算粒子权重的过程为：

计算过程分为预测和更新两步；当系统符合马尔科夫假设时，预测和更新的公式分别如下：

p(x_k|y_1：k-1)＝∫p(x_k|x_k-1)p(x_k-1|y_1：k-1)dx_k-1 (1)

预测是指根据上一时刻的后验估计p(x_k|y_1：k)，对本时刻系统的状态x_k做出预估；更新是指通过最新得到的观测数据y_k，和第一步预测结果综合，得到最优估计；

当遇到难以解析的系统模型时，由于无法积分，所以无法直接使用上述公式，此时引入蒙特卡洛采样；蒙特卡洛采样是指使用大量采样的平均值来代替积分：首先按照系统状态的后验概率从系统中进行采样得到N个样本(即粒子)，然后统计采样结果，得到系统状态的期望值；

由于后验概率p(x_k|y_1：k)无法直接得到，故使用重要性采样；重要性采样是指用一个能解析的概率分布函数q(x_k|y_1：k)进行采样；并用表示此采样和用后验概率采样的相关性，即粒子的权重：

使用重要性采样方法后，则计算系统状态的期望有：

其中是归一化后的权重：

在粒子权重的计算中，每次采样时后验概率均要重新计算，使用序贯重要性采样通过迭代的方式减少运算量，最终得出：

3.根据权利要求2所述的一种基于粒子滤波的移动机器人定位加速方法，其特征在于，所述重要性采样函数选择为p(x_k|x_k-1)，即先验概率函数；在计算粒子权重时，将代入式(7)中，得

上式中表示粒子在该位置获得传感器观测值的概率。