专利名称：基于改进重采样方法和粒子选取的FastSLAM算法的制作方法
技术领域：
本发明涉及才凡器人同时定位与地图创建领域，具体来说是一种对传统 FastSLAM算法的改进算法,尤其是对FastSLAM算法中的重采样标准判 定的#~正和对粒子重采样方法的#^正。
背景技术：
移动机器人在自身位置和姿态都不确定的条件下，利用自身的内部设 备和外部测量传感装置在完全未知的环境中创建地图，同时利用所创建地 图进行自主定位被通称为同时定位与地图创建问题。自从Sm他等人提出 基于扩展卡尔曼滤波器的同时定位于地图创建算法之后，扩展卡尔曼算法 便成为研究机器人同时定位与地图创建问题的主要方法。但是随着对机器 人同时定位与地图创建问题的深入研究，人们发现扩展卡尔曼滤波器算法 存在很明显的缺陷 一是其计算复杂度和环境中已检测路标数目(7V)成平 方比(CY2V2))，并且在同一时刻即使只检测到一个^4示，整个状态协方差矩 阵都必须进行更新；二是扩展卡尔曼滤波器算法对数据关联问题处理不够 健壮，它不能对数据关联的错误及时进行自恢复。
针对扩展卡尔曼滤波器所存在的问题，Montemelo等人提出了 FastSLAM算法。在FastSLAM算法中机器人同时定位与地图创建问题被 分解为机器人位姿估计和路标位置估计两个过程。对机器人位姿的估计使 用粒子滤波器，对路标位置估计使用扩展卡尔曼滤波器算法进行；机器人 位姿估计中每个粒子代表机器人一条可能的轨迹，同时使用粒子权重来表 示此轨迹的好坏，每个粒子中的路标位置估计是相互独立的。和扩展卡尔 曼滤波器算法的计算复杂度不同，FastSLAM算法的计算复杂度为 C^MogA9，其中M表示所使用的粒子数目，W表示已检测到的路标数目。 对于数据关联而言，在FastSLAM算法中，每个粒子的数据关联是相互独 立的，当其中部分出现问题时，在重采样阶段这部分粒子会被衰退掉而不 影响机器人同时定位与地图创建的整体估计，从而可以提高数据关联的健壮性。尽管FastSLAM很好的解决了扩展卡尔曼滤波器算法所出现的复杂 度和数据关联问题，但是随后Bailey等人发现使用FastSLAM算法对于机 器人位姿估计一致性很差，而且粒子的多样性随着机器人的运动呈指数趋 势下降，Baiiey还发现通过增加粒子数目只能稍微提高机器人位姿估计的 一致性，但是随之带来的问题是机器人同时定位与地图创建过程的实时性 遭受到严重破坏。
发明内容
本发明提供了一种基于改进重采样方法和粒子选取的FastSLAM算 法，主要是在FastSLAM的框架下，对机器人位姿估计的粒子滤波器的重 采样判定标准进行了改进，并且对粒子重采样时新粒子的生成方法进行了 改进。
一种基于改进重采样方法和粒子选取的FastSLAM算法，包括以下步
骤
1) 当机器人有输入控制数据,依据机器人运动预测模型对每个粒子 中的机器人位姿进行预测，当机器人外部传感器有路标的检测数据时，依 据外部传感器的测量模型对每个粒子中的机器人位姿和对应路标位置进
行更新；
2) 对机器人的位姿使用粒子滤波器来进行估计，在每次使用机器人 运动预测模型或者传感器的测量模型对机器人位姿进行更新后都需要进 行粒子重采样判断，以确定是否需要进行粒子重采样。使用当前粒子有效 数，粒子权重协方差，以及粒子测量残余一致性综合进行判断。当有效粒
子数小于总粒子数的75%,粒子权重协方差大于粒子权重均值，并且粒子 测量残余一致性在95%的一致性区域之外时,进行粒子重采样;
3) 在步骤2中的粒子重采样条件满足时，使用指数等级方法和交叉 算子进行新粒子的生成。首先对根据粒子权重对粒子排序，再使用指数方 法对粒子权重进行调整，之后使用随机数生成方法选择两个父粒子，最后 依据父粒子和交叉算子进行新粒子的生成;
4) 依据生成的新粒子计算机器人的定位和地图创建。 所述的机器人运动预测模型、机器人外部传感器的测量模型均采用现
有标准FastSLAM算法中常用的模型。所述的粒子权重分布协方差的分布W^按照式(1)进行计算。
Af表示粒子的总数，"表示粒子z'的权重，r表示矩阵转置操作。
所述的粒子测量残余一致性按照式(2)进行计算
C表示根据测量模型使用扩展卡尔曼滤波器所获得的路标z'和粒子m 中的机器人位姿之间的相对位置数据，《为机器人外部传感器所检测到的 路标/和机器人位姿之间的测量数据；m油/e—cov^表示测量残余数据的协 方差，其计算方法如式(3)所示
//w为测量模型对路标/位置向量的雅可比矩阵，^为粒子m中路标
/在上一时刻被检测到时所获得的位置协方差矩阵，r表示矩阵转置操作，
G表示测量模型的噪声分布协方差。 所述的粒子重采样判定标准算法为
在粒子重采样的判定标准中综合考虑三个因素 一是当前的有效粒子 数；二是当前的粒子权重分布；三是各个粒子的测量残余一致性；粒子权 重分布使用其权重协方差来表示，粒子测量残余一致性使用卡方分布标准 来进行判断。当有效粒子数小于总粒子数的75%,粒子权重分布大于粒子 权重均值，并且粒子测量残余一致性在95°/。的一致性区域之外时，表明满 足粒子重采样判定标准，需要进行重采样。
所述的指数等级和交叉算子生成新粒子的算法为
首先对待被复制粒子的权重进行等级划分，再使用指数方法重新调整 每个粒子的权重,然后依据调整过的权重使用随机方法进行父粒子选择， 最后使用选取的父粒子和交叉算子进行新粒子的生成。
本发明编码方法利用通过改进FastSL AM算法中的粒子重采样判定标 准和新粒子的生成方法,能够明显改进FastSLAM算法对机器人位姿估计 的一致性，提高机器人同时定位与地图创建的精确度。
图1为本发明的FastSLAM算法的流程图； 图2为稀疏路标测试环境; 图3为密集路标测试环境;
图4为本发明算法和标准FastSLAM算法在稀疏S各标环境下所得到才几 器人位姿一致性比较；
图5为本发明算法和标准FastSLAM算法在密集路标环境下所得到机 器人位姿一致性比较；
图6为本发明算法和标准FastSLAM算法在稀疏路标环境下所得到的 同 一路标的多样性衰退比较;
图7为本发明算法和标准FastSLAM算法在密集路标环境下所得到的 同 一路标的多样性衰退比较;
图8为本发明算法和标准FastSLAM算法在稀疏路标环境下所得到的 粒子重采样次数比较；
图9为本发明算法和标准FastSLAM算法在密集路标环境下所得到的 粒子重采样次数比较。
具体实施例方式
一种基于改进重采样方法和粒子选取的FastSLAM算法，算法流程如 图l所示，当机器人接受到外部控制输入时，使用扩展卡尔曼滤波器，按 照机器人运动预测模型对每个粒子中的机器人位姿进行预测估计；当接收 到外部传感器对路标的检测数据时，同样使用扩展卡尔曼滤波器，按照传 感器的测量模型对每个粒子中的机器人位姿和路标位置进行更新。在对机 器人位姿使用运动预测模型预测或者使用测量模型更新后，开始判断是否 需要进行粒子重采样。在粒子重采样判断中，当有效粒子数小于粒子总数 的75%，粒子权重协方差大于粒子权重，并且粒子测量残余一致性数据不 在一致性区域内时,确定需要进行重采样。在重采样过程中，首先对根据 粒子权重对粒子排序,再使用指数方法对粒子权重进4亍调整，之后使用随 机数生成方法选择两个父粒子,最后依据父粒子和交叉算子生成新粒子的 粒子。对标准FastSLAM算法从粒子重采样判定标准和新粒子生成方面进行 改进，包4舌以下步骤
首先需要根据粒子权重协方差的分布和粒子测量残余的一致性来进 行粒子采样判定标准的修正。粒子权重协方差w^按照式(1 )进行计算。
m^越大，则越可能满足重采样采条件。式(1 )的粒子权重协方差计
算中，M表示粒子的总数，^表示粒子/的权重，r表示矩阵转置操作。
粒子测量残余一致性按照式(2)进行计算。
式(2)中，么表示根据测量模型使用扩展卡尔曼滤波器所获得的路标/ 和粒子m中的机器人位姿之间的相对位置数据，z;为机器人外部传感器所 检测到的路标/和机器人位姿之间的测量数据。ms她/ejm^表示测量残余
数据的协方差，其计算方法如式(3)所示，
式(3)中为测量模型对路标/位置向量的雅可比矩阵，g,为粒子w
中路标/在上一时刻被检测到时所获得的位置协方差矩阵，r表示矩阵转
置操作，Q,表示测量模型的噪声分布协方差。
由式(3)的定义可知测量残余一致性符合自由度为测量数据维数的卡 方分布("distribution),当机器人自动定位与地图创建环境只考虑二维情 况，并且测量数据中只包括机器人和路标之间的距离和方位差时，可知等 式(3)符合自由度为2的卡方分布。由卡方分布表可知自由度为2的双边一 致区域为95%的范围是
。因此当有测量数据时由式(3)可以获得 每个粒子针对此次测量数据的测量残余一致性，如果其值在双边一致区域
范围内则表示此粒子对此次测量数据是一致的,同样也认为此 粒子中的机器人位姿估计是一致的，否则认为机器人此粒子中的机器人位 姿估计是不一致的，需要进行重采样。
其次是使用指数等级方法和交叉算子进行粒子的重采样。在基于指数 等级选择算法的重采样方法中，并不是使用粒子复制方式进行新粒子生成，而是先使用指数等级方法对待被复制的粒子进行选择，然后再使用所
选粒子生成新粒子。指数等级选择方法是指首先对待被复制粒子的权重 进行等级划分，再^f吏用指数方法重新调整每个粒子的权重，然后使用随机 方法进行粒子选择，最后使用交叉算子进行新粒子的生成。
等式(4)表示对粒子权重使用指数进行调整后的结果，其中iV表示粒 子数目。表i描述了指数等级选择算法的过程，表2则描述了基于指数等 级选择算法的粒子重采样算法的过程。
a;^T"'"(1…M (4) 表1指数等级选择算法
入根据粒子杈重进行粒子升序排序(假设粒子数目为 ,排序后为附，…
附",『"，而且满足:『/ < 2 <…<:
随机选择c，使用等式(4)计算调整后的粒子权重a并且置&=0j.根据粒子权重进行粒子升序排序(假设粒子数目为 ,排序后为『入W么…
亂而且满足:伊J < JT2 < ... < W"-W")4.随机选择c，使用等式(4)计算调整后的粒子权重a并且置5.For!' = / to Wo o
6.i 朋dv"/"g = r朋必* , w7.尸or / = / /o 'V-/ d"
y/tew/va/Me > &在 及a"o va/we <
￡W //"
&9.返回化/e"/"c/e;c
表2基于指数等级选择算法的重采样算法
假设需要生成W/个新的粒子2.For i. = 〃o A7
fefe"!'wfec/ -指数等级逸择算法；
&fec//mtoc2 -指数等級选择算法;
3.
4返回新生成的jW个粒子
再次，根据本文算法，对稀疏路标环境和密集路标环境下进行机器人 同步定位与地图创建。如图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算法。
权利要求
1.一种基于改进重采样方法和粒子选取的FastSLAM算法，包括以下步骤1)机器人根据输入控制数据，依据机器人运动预测模型进行机器人位姿预测，根据机器人外部传感器的测量模型结合测量数据进行机器人位姿和路标位置更新；2)机器人位姿预测采用粒子滤波器进行计算，根据有效粒子数，粒子权重协方差和粒子测量残余一致性来进行粒子重采样判定标准的修正；3)在重采样条件满足情况下，使用指数等级方法和交叉算子进行新粒子的生成；4)依据生成的新粒子进行机器人的定位和地图创建。
2.根据权利要求1所述的FastSLAM算法，其特征在于所述的粒子权 重分布协方差的分布M^按照式(1)进行计算<formula>formula see original document page 2</formula>M表示粒子的总数，沐,表示粒子/的权重，r表示矩阵转置操作。
3.根据权利要求i所述的FastSLAM算法，其特征在于所述的粒子测 量残余一致性按照式(2)进行计算<formula>formula see original document page 2</formula>《表示根据测量模型使用扩展卡尔曼滤波器所获得的路标/和粒子m中的机器人位姿之间的相对位置数据，z;为机器人外部传感器所检测到的路标z'和机器人位姿之间的测量数据；^油/e一on^表示测量残余数据的协 方差，其计算方法如式(3)所示簡恭一on", - //,,m,/m., (L,)' + Q (3) i^为测量模型对路标/位置向量的雅可比矩阵，^为粒子m中路标/在上一时刻被检测到时所获得的位置协方差矩阵，r表示矩阵转置操作， g/表示测量模型的噪声分布协方差。
4.根据权利要求1所述的FastSLAM算法，其特征在于所述的指数等 级方法和交叉算子包括首先对才艮椐粒子权重对粒子排序，再^f吏用指数方 法对粒子权重进行调整，之后使用随机数生成方法选择两个父粒子，最后 依据父粒子和交叉算子进行新粒子的生成。
全文摘要
本发明公开一种基于改进重采样方法和粒子选取的FastSLAM算法，包括1)机器人根据输入控制数据，进行机器人位姿预测，根据机器人外部传感器的测量模型结合测量数据进行机器人位姿和路标位置更新；2)机器人位姿预测采用粒子滤波器进行计算，根据有效粒子数，粒子权重协方差和粒子测量残余一致性来进行粒子重采样判定标准的修正；3)使用指数等级方法和交叉算子进行新粒子的生成；4)依据生成的新粒子进行机器人的定位和地图创建。本发明通过改进FastSLAM算法中的粒子重采样判定标准和新粒子的生成方法，能够明显改进FastSLAM算法对机器人位姿估计的一致性，提高机器人同时定位与地图创建的精确度。
文档编号G05D1/00GK101625572SQ20091010096
公开日2010年1月13日 申请日期2009年8月10日 优先权日2009年8月10日
发明者亮 张, 蒋荣欣, 陈耀武 申请人:浙江大学