CN101625572B - 基于改进重采样方法和粒子选取的FastSLAM算法 - Google Patents

Abstract

本发明公开一种基于改进重采样方法和粒子选取的FastSLAM算法，包括：1)机器人根据输入控制数据，进行机器人位姿预测，根据机器人外部传感器的测量模型结合测量数据进行机器人位姿和路标位置更新；2)机器人位姿预测采用粒子滤波器进行计算，根据有效粒子数，粒子权重协方差和粒子测量残余一致性来进行粒子重采样判定标准的修正；3)使用指数等级方法和交叉算子进行新粒子的生成；4)依据生成的新粒子进行机器人的定位和地图创建。本发明通过改进FastSLAM算法中的粒子重采样判定标准和新粒子的生成方法，能够明显改进FastSLAM算法对机器人位姿估计的一致性，提高机器人同时定位与地图创建的精确度。

Description

基于改进重采样方法和粒子选取的FastSLAM算法

技术领域

本发明涉及机器人同时定位与地图创建领域，具体来说是一种对传统FastSLAM算法的改进算法，尤其是对FastSLAM算法中的重采样标准判定的修正和对粒子重采样方法的修正。

背景技术

移动机器人在自身位置和姿态都不确定的条件下，利用自身的内部设备和外部测量传感装置在完全未知的环境中创建地图，同时利用所创建地图进行自主定位被通称为同时定位与地图创建问题。自从Smith等人提出基于扩展卡尔曼滤波器的同时定位于地图创建算法之后，扩展卡尔曼算法便成为研究机器人同时定位与地图创建问题的主要方法。但是随着对机器人同时定位与地图创建问题的深入研究，人们发现扩展卡尔曼滤波器算法存在很明显的缺陷：一是其计算复杂度和环境中已检测路标数目(N)成平方比(O(N²))，并且在同一时刻即使只检测到一个路标，整个状态协方差矩阵都必须进行更新；二是扩展卡尔曼滤波器算法对数据关联问题处理不够健壮，它不能对数据关联的错误及时进行自恢复。

针对扩展卡尔曼滤波器所存在的问题，Montemelo等人提出了FastSLAM算法。在FastSLAM算法中机器人同时定位与地图创建问题被分解为机器人位姿估计和路标位置估计两个过程。对机器人位姿的估计使用粒子滤波器，对路标位置估计使用扩展卡尔曼滤波器算法进行；机器人位姿估计中每个粒子代表机器人一条可能的轨迹，同时使用粒子权重来表示此轨迹的好坏，每个粒子中的路标位置估计是相互独立的。和扩展卡尔曼滤波器算法的计算复杂度不同，FastSLAM算法的计算复杂度为O(MlogN)，其中M表示所使用的粒子数目，N表示已检测到的路标数目。对于数据关联而言，在FastSLAM算法中，每个粒子的数据关联是相互独立的，当其中部分出现问题时，在重采样阶段这部分粒子会被衰退掉而不影响机器人同时定位与地图创建的整体估计，从而可以提高数据关联的健壮性。尽管FastSLAM很好的解决了扩展卡尔曼滤波器算法所出现的复杂度和数据关联问题，但是随后Bailey等人发现使用FastSLAM算法对于机器人位姿估计一致性很差，而且粒子的多样性随着机器人的运动呈指数趋势下降，Bailey还发现通过增加粒子数目只能稍微提高机器人位姿估计的一致性，但是随之带来的问题是机器人同时定位与地图创建过程的实时性遭受到严重破坏。

发明内容

本发明提供了一种基于改进重采样方法和粒子选取的FastSLAM算法，主要是在FastSLAM的框架下，对机器人位姿估计的粒子滤波器的重采样判定标准进行了改进，并且对粒子重采样时新粒子的生成方法进行了改进。

一种基于改进重采样方法和粒子选取的FastSLAM算法，包括以下步骤：

1)当机器人有输入控制数据，依据机器人运动预测模型对每个粒子中的机器人位姿进行预测，当机器人外部传感器有路标的检测数据时，依据外部传感器的测量模型对每个粒子中的机器人位姿和对应路标位置进行更新；

2)对机器人的位姿使用粒子滤波器来进行估计，在每次使用机器人运动预测模型或者传感器的测量模型对机器人位姿进行更新后都需要进行粒子重采样判断，以确定是否需要进行粒子重采样。使用当前粒子有效数，粒子权重协方差，以及粒子测量残余一致性综合进行判断。当有效粒子数小于总粒子数的75％，粒子权重协方差大于粒子权重均值，并且粒子测量残余一致性在95％的一致性区域之外时，进行粒子重采样；

3)在步骤2中的粒子重采样条件满足时，使用指数等级方法和交叉算子进行新粒子的生成。首先对根据粒子权重对粒子排序，再使用指数方法对粒子权重进行调整，之后使用随机数生成方法选择两个父粒子，最后依据父粒子和交叉算子进行新粒子的生成；

4)依据生成的新粒子计算机器人的定位和地图创建。

所述的机器人运动预测模型、机器人外部传感器的测量模型均采用现有标准FastSLAM算法中常用的模型。

所述的粒子权重分布协方差的分布w_cov按照式(1)进行计算。

$w_{\mathrm{cov}}=\frac{1}{M-1}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}(w_i-\frac{1}{M}){(w_i-\frac{1}{M})}^T---\left(1\right)$

M表示粒子的总数，w_i表示粒子i的权重，T表示矩阵转置操作。

所述的粒子测量残余一致性按照式(2)进行计算：

${\mathrm{consistency}}_{t,m}=\frac{(z_{t,m}^i-z_t^i){(z_{t,m}^i-z_t^i)}^T}{{\mathrm{residule}\_\mathrm{cov}}_{t,m}}---\left(2\right)$

z_t，m ⁱ表示根据测量模型使用扩展卡尔曼滤波器所获得的路标i和粒子m中的机器人位姿之间的相对位置数据，z_t ⁱ为机器人外部传感器所检测到的路标i和机器人位姿之间的测量数据；residule_cov_t，m表示测量残余数据的协方差，其计算方法如式(3)所示：

residule_cov_t，m＝H_t，m，iP_m，i(H_t，m，i)^T+Q_t    (3)

H_t，m为测量模型对路标i位置向量的雅可比矩阵，P_m，i为粒子m中路标i在上一时刻被检测到时所获得的位置协方差矩阵，T表示矩阵转置操作，Q_t表示测量模型的噪声分布协方差。

所述的粒子重采样判定标准算法为：

在粒子重采样的判定标准中综合考虑三个因素：一是当前的有效粒子数；二是当前的粒子权重分布；三是各个粒子的测量残余一致性；粒子权重分布使用其权重协方差来表示，粒子测量残余一致性使用卡方分布标准来进行判断。当有效粒子数小于总粒子数的75％，粒子权重分布大于粒子权重均值，并且粒子测量残余一致性在95％的一致性区域之外时，表明满足粒子重采样判定标准，需要进行重采样。

所述的指数等级和交叉算子生成新粒子的算法为：

首先对待被复制粒子的权重进行等级划分，再使用指数方法重新调整每个粒子的权重，然后依据调整过的权重使用随机方法进行父粒子选择，最后使用选取的父粒子和交叉算子进行新粒子的生成。

本发明编码方法利用通过改进FastSLAM算法中的粒子重采样判定标准和新粒子的生成方法，能够明显改进FastSLAM算法对机器人位姿估计的一致性，提高机器人同时定位与地图创建的精确度。

附图说明

图1为本发明的FastSLAM算法的流程图；

图2为稀疏路标测试环境；

图3为密集路标测试环境；

图4为本发明算法和标准FastSLAM算法在稀疏路标环境下所得到机器人位姿一致性比较；

图5为本发明算法和标准FastSLAM算法在密集路标环境下所得到机器人位姿一致性比较；

图6为本发明算法和标准FastSLAM算法在稀疏路标环境下所得到的同一路标的多样性衰退比较；

图7为本发明算法和标准FastSLAM算法在密集路标环境下所得到的同一路标的多样性衰退比较；

图8为本发明算法和标准FastSLAM算法在稀疏路标环境下所得到的粒子重采样次数比较；

图9为本发明算法和标准FastSLAM算法在密集路标环境下所得到的粒子重采样次数比较。

具体实施方式

一种基于改进重采样方法和粒子选取的FastSLAM算法，算法流程如图1所示，当机器人接受到外部控制输入时，使用扩展卡尔曼滤波器，按照机器人运动预测模型对每个粒子中的机器人位姿进行预测估计；当接收到外部传感器对路标的检测数据时，同样使用扩展卡尔曼滤波器，按照传感器的测量模型对每个粒子中的机器人位姿和路标位置进行更新。在对机器人位姿使用运动预测模型预测或者使用测量模型更新后，开始判断是否需要进行粒子重采样。在粒子重采样判断中，当有效粒子数小于粒子总数的75％，粒子权重协方差大于粒子权重，并且粒子测量残余一致性数据不在一致性区域内时，确定需要进行重采样。在重采样过程中，首先对根据粒子权重对粒子排序，再使用指数方法对粒子权重进行调整，之后使用随机数生成方法选择两个父粒子，最后依据父粒子和交叉算子生成新粒子的粒子。

对标准FastSLAM算法从粒子重采样判定标准和新粒子生成方面进行改进，包括以下步骤：

首先需要根据粒子权重协方差的分布和粒子测量残余的一致性来进行粒子采样判定标准的修正。粒子权重协方差w_cov按照式(1)进行计算。

$w_{\mathrm{cov}}=\frac{1}{M-1}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}(w_i-\frac{1}{M}){(w_i-\frac{1}{M})}^T---\left(1\right)$

w_cov越大，则越可能满足重采样采条件。式(1)的粒子权重协方差计算中，M表示粒子的总数，w_i表示粒子i的权重，T表示矩阵转置操作。

粒子测量残余一致性按照式(2)进行计算。

${\mathrm{consistency}}_{t,m}=\frac{(z_{t,m}^i-z_t^i){(z_{t,m}^i-z_t^i)}^T}{{\mathrm{residule}\_\mathrm{cov}}_{t,m}}---\left(2\right)$

式(2)中，z_t，m ⁱ表示根据测量模型使用扩展卡尔曼滤波器所获得的路标i和粒子m中的机器人位姿之间的相对位置数据，z_t ⁱ为机器人外部传感器所检测到的路标i和机器人位姿之间的测量数据。residule_cov_t，m表示测量残余数据的协方差，其计算方法如式(3)所示，

residule_cov_t，m＝H_t，m，iP_m，i(H_t，m，i)^T+Q_t    (3)

式(3)中H_t，m为测量模型对路标i位置向量的雅可比矩阵，P_m，i为粒子m中路标i在上一时刻被检测到时所获得的位置协方差矩阵，T表示矩阵转置操作，Q_t表示测量模型的噪声分布协方差。

由式(3)的定义可知测量残余一致性符合自由度为测量数据维数的卡方分布(λ²distribution)。当机器人自动定位与地图创建环境只考虑二维情况，并且测量数据中只包括机器人和路标之间的距离和方位差时，可知等式(3)符合自由度为2的卡方分布。由卡方分布表可知自由度为2的双边一致区域为95％的范围是[0.057.38]。因此当有测量数据时由式(3)可以获得每个粒子针对此次测量数据的测量残余一致性，如果其值在双边一致区域[0.057.38]范围内则表示此粒子对此次测量数据是一致的，同样也认为此粒子中的机器人位姿估计是一致的，否则认为机器人此粒子中的机器人位姿估计是不一致的，需要进行重采样。

其次是使用指数等级方法和交叉算子进行粒子的重采样。在基于指数等级选择算法的重采样方法中，并不是使用粒子复制方式进行新粒子生成，而是先使用指数等级方法对待被复制的粒子进行选择，然后再使用所选粒子生成新粒子。指数等级选择方法是指：首先对待被复制粒子的权重进行等级划分，再使用指数方法重新调整每个粒子的权重，然后使用随机方法进行粒子选择，最后使用交叉算子进行新粒子的生成。

等式(4)表示对粒子权重使用指数进行调整后的结果，其中N表示粒子数目。表1描述了指数等级选择算法的过程，表2则描述了基于指数等级选择算法的粒子重采样算法的过程。

$p_i=\frac{c-1}{c^N-1}{c}^{N-i}$ i＝{1...N}(4)

表1指数等级选择算法

1.根据粒子权重进行粒子升序排序(假设粒子数目为n，排序后为W1，W2，...Wn-1，Wn，而且满足：W1＜W2＜...＜Wn-1＜Wn)2.随机选择c，使用等式(4)计算调整后的粒子权重pi，并且置s0＝03.根据粒子权重进行粒子升序排序(假设粒子数目为n，排序后为W1，W2，...Wn-1，Wn，而且满足：W1＜W2＜...＜Wn-1＜Wn)4.随机选择c，使用等式(4)计算调整后的粒子权重pi，并且置s0＝05.For i＝1 to N dosi＝si-1+piEnd for6.Randvalue＝rand()*sN7.For i＝1 to N-1 doIf Randvalue＞si & Randvalue＜si+1Selectindex＝iEnd if8.End for9.返回selectindex

表2基于指数等级选择算法的重采样算法

1.假设需要生成N1个新的粒子2.For i＝1 to N1Selectindex1＝指数等级选择算法；Selectindex2＝指数等级选择算法；Randvalue＝rand()；Newparticle(i)＝Randvalue*particle(Selectindex1)+(1-Randvalue)*particle(Selectindex2)3.Endfor4.返回新生成的N1个粒子

再次，根据本文算法，对稀疏路标环境和密集路标环境下进行机器人同步定位与地图创建。

如图2和图3所示，使用本文算法和标准FastSLAM算法进行40次蒙特卡洛实验，运用蒙特卡洛实验的结果对机器人位姿估计的一致性进行比较，如图4和图5所示。对同一路标粒子多样性的比较结果如图6和图7所示，重采样次数的比较结果如图8和图9所示。

在机器人位姿估计的一致性比较上，同样采样95％的一致性双边区域作为比较准则，由图4和图5可以看出，相对于标准FastSLAM算法，无论是在稀疏路标还是密集路标环境下，改进FastSLAM算法对机器人位姿估计明显优于标准FastSLAM算法。图6和图7反映了改进FastSLAM算法相对于标准FastSLAM算法在粒子多样性保持方面的优越性，由图6和图7可以看出，经过长时间的机器人同步定位与地图创建过程，改进FastSLAM算法中的粒子多样性保持的很好，而标准FastSLAM算法中的粒子多样性会严重衰退，失去了粒子多样性的特性。图8和图9反映了粒子重采样次数的对比，可以看出改进FastSLAM算法的粒子重采样次数明显小于标准FastSLAM算法。

Claims (1)

1.一种基于改进重采样方法和粒子选取的FastSLAM算法，包括以下步骤：

1)机器人根据输入控制数据，依据机器人运动预测模型进行机器人位姿预测，根据机器人外部传感器的测量模型结合测量数据进行机器人位姿和路标位置更新；

2)机器人位姿预测采用粒子滤波器进行计算，根据有效粒子数，粒子权重协方差和粒子测量残余一致性来进行粒子重采样判定标准的修正；

所述的粒子权重协方差的分布w_cov按照式(1)进行计算：

M表示粒子的总数，w_i表示粒子i的权重，T表示矩阵转置操作；

所述的粒子测量残余一致性按照式(2)进行计算：

表示根据测量模型使用扩展卡尔曼滤波器所获得的路标i和粒子m中的机器人位姿之间的相对位置数据， 为机器人外部传感器所检测到的路标i和机器人位姿之间的测量数据；residule_cov_t，m表示测量残余数据的协方差，其计算方法如式(3)所示：

H_t，m，i为测量模型对路标i位置向量的雅可比矩阵， 

为粒子m中路标i在上一时刻被检测到时所获得的位置协方差矩阵，T表示矩阵转置操作，Q_t表示测量模型的噪声分布协方差；

3)在重采样条件满足以下情况时，使用指数等级方法和交叉算子进行新粒子的生成：使用当前粒子有效数，粒子权重协方差，以及粒子测量残余一致性综合进行判断，当有效粒子数小于总粒子数的75％，粒子权重协方差大于粒子权重均值，并且粒子测量残余一致性在95％的一致性区域之外； 

所述的指数等级方法和交叉算子包括：首先根据粒子权重对粒子排序，再使用指数方法对粒子权重进行调整，之后使用随机数生成方法选择两个父粒子，最后依据父粒子和交叉算子进行新粒子的生成；

4)依据生成的新粒子进行机器人的定位和地图创建。 

Images