CN106599368B - 基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法 - Google Patents

Abstract

本发明公开了一种基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，包括：(1)利用平方根转换无味卡尔曼滤波器对最优粒子提议分布进行估计，并对机器人位姿状态进行采样；(2)利用平方根容积卡尔曼滤波器更新每个粒子对应的特征地图信息；(3)利用基于相对熵的自适应粒子重采样方法确定当前时刻所需的粒子个数；(4)根据重采样后的粒子集确定机器人的位姿状态和路标特征地图信息。本发明同时从采样粒子的质量和数量两个方面对传统的FastSLAM算法进行改进：增强算法在估计过程中的数值稳定性和精度，提高采样粒子的质量；在粒子重采样过程中根据估计不确定度动态确定最少所需粒子个数，提高算法的计算效率。

Description

基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法

技术领域

本发明属于移动机器人自主导航领域，具体涉及一种基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法。

背景技术

移动机器人作为智能机器人的重要分支，在诸如制造工业、国防军事、航天航空、卫生医疗、家庭服务等领域得到了广泛的应用。为了实现移动机器人在复杂未知环境下，不经过人工干预而顺利完成特定作业任务，移动机器人必须具备自主导航的能力。作为自主导航的关键前提，移动机器人需要通过携带的内部、外部传感器对自身姿态进行估计，并且同时对未知环境进行地图描述。移动机器人同时定位与地图创建(SimultaneousLocalization and Mapping，SLAM)方法可以充分利用定位与地图创建两者之间的相关性，在没有先验环境地图信息及GPS等辅助定位设备的情形下，实现未知环境的增量式地图创建以及自身姿态的在线估计。

Montemerlo等人基于Rao-Blackwellized粒子滤波器思想提出了FastSLAM算法，有效地克服了基于高斯滤波器的SLAM算法计算复杂度高、数据关联鲁棒性差等缺点，实现了任意非线性非高斯随机系统的状态估计。在FastSLAM算法中，粒子提议分布的计算是影响算法性能的关键因素之一。

传统的FastSLAM算法利用一阶泰勒级数展开对机器人的非线性运动模型和测量模型进行线性化近似，忽略了二阶以上统计项的信息，因而在处理非线性程度较高的模型时容易造成较大的累积误差，最终导致算法的估计一致性较差。

Kim等人和Song等人分别将无味卡尔曼滤波和容积卡尔曼滤波器应用于FastSLAM的粒子提议分布的计算中，通过一系列特定选取的Sigma点来直接近似状态的后验概率密度函数，从而避免了对非线性函数的线性化操作。但是当状态向量的维度较高时，无味卡尔曼滤波器的中心Sigma点的权值可能为负数，因而不能保证后验协方差矩阵的正定性；容积卡尔曼滤波器用来限定容积点的球体半径与状态向量的维度成正比，因而出现所谓的非局部采样问题。

另一方面，粒子重要性重采样过程也是影响FastSLAM算法性能的重要因素之一。通常而言，采用较多的粒子数目能够更加精确地逼近机器人位姿状态的后验概率分布，而较少的粒子数目能够大幅度减少算法对计算资源的需求。因而，在FastSLAM的粒子重要性重采样过程中，在保证算法估计精度的同时尽量提高算法的计算效率，应考虑如何选择尽可能少的符合实际状态后验概率分布的粒子。

发明内容

本发明提供了一种基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，旨在同时从采样粒子的质量和数量两个方面对传统的FastSLAM算法进行改进，增强算法在估计过程中的数值稳定性和精度，提高采样粒子的质量，同时在粒子重采样过程中根据估计不确定度动态确定最少所需粒子个数，提高算法的计算效率。

一种基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，包括以下步骤：

(1)将运动噪声和测量噪声同时增广至机器人位姿状态向量中，利用平方根转换无味卡尔曼滤波器对最优粒子提议分布进行估计，并根据该粒子提议分布对机器人位姿状态进行采样；

(2)利用平方根容积卡尔曼滤波器更新每个粒子对应的路标特征地图信息；

(3)在粒子重采样阶段，利用基于相对熵的自适应粒子重采样方法确定当前时刻所需的粒子个数；

(4)根据重采样后的粒子集确定机器人的位姿状态和路标特征地图信息。

在步骤(1)中，所述的转换无味卡尔曼滤波器的转换无味Sigma点按照以下方式计算：

对于n_a维的机器人状态向量x及给定的非线性运动(或观测)函数y＝g(x)，首先将对应的2n_a个转换无味Sigma点表示为n_a维向量其元素由下式确定：

其中，j表示Sigma点的序号，r表示向量中的元素序号，表示取下整运算符，当n_a为奇数时，最后一个元素为

然后，结合机器人状态向量的估计均值和协方差矩阵平方根因子S，根据下式计算新的转换无味Sigma点：

最后，代入非线性函数得到新的转换无味变换Sigma点y的相关统计量可估计为：

其中，权值系数α＝1/2n_a。

步骤(1)具体包括如下步骤：

(1-1)将k时刻的机器人位姿和路标特征地图的联合后验概率表示为如下N_k个粒子组成的粒子集：

其中，i表示粒子序号，表示k时刻第i个粒子的权重，和分别表示k时刻第i个机器人位姿状态的估计值和相应的协方差平方根因子；和分别表示k时刻第i个粒子中关于第m个路标特征路标的位置均值和相应的协方差平方根因子；

(1-2)将运动噪声和测量噪声同时增广至机器人位姿状态向量和协方差矩阵平方根因子中，保证协方差的半正定性；

(1-3)根据机器人运动函数、增广后的机器人位姿状态向量均值及其协方差矩阵的平方根因子，计算机器人位姿状态的预测均值及其协方差平方根因子；

(1-4)当第l个路标特征被机器人再次成功观测到时，根据路标特征测量值z_k，计算机器人位姿状态后验估计均值及其协方差矩阵平方根因子；

(1-5)计算粒子的提议分布，并根据此分布采样生成新一代粒子。

在步骤(1-1)中，初始粒子个数N₀由用户给定，k(k≥1)时刻需要采样的粒子个数N_k由k-1时刻的粒子重采样过程决定。

在步骤(1-2)中，假设运动噪声和测量噪声均服从零均值高斯分布，其协方差矩阵分别为Q_k和R_k，k-1时刻第i个粒子的机器人位姿状态向量均值及其协方差矩阵平方根因子分别增广为：

其中，S_Q，k和S_R，k分别表示Q_k和R_k的平方根因子，且满足：

其中，为S_Q，k的转置，为S_R，k的转置。

步骤(1-3)的具体步骤为：

(1-3-1)根据增广后的机器人位姿状态向量均值和协方差矩阵的平方根因子，计算转换无味Sigma点集：

其中，和分别为增广后的机器人位姿状态向量均值和协方差矩阵的平方根因子，表示增广状态向量的维度，表示第j个转换Sigma点，每个转换无味Sigma点包含了机器人位姿状态分量运动噪声分量和测量噪声分量 为n_a维向量，其元素由下式确定：

其中，r表示向量中的元素序号，表示取下整运算符，当n_a为奇数时，最后一个元素为

(1-3-2)根据机器人运动函数生成预测转换无味Sigma点：

其中，f(·)表示给定的机器人运动函数，u_k表示当前时刻的控制输入；

(1-3-3)根据预测的转换无味Sigma点计算机器人位姿状态的预测均值及其协方差平方根因子：

其中，qr(·)表示矩阵的QR分解运算，∏_v，k|k-1定义为以下加权中心偏差矩阵：

步骤(1-4)的具体步骤为：

(1-4-1)当第l个路标特征被机器人再次成功观测到时，根据测量函数h(·)生成测量值Sigma点：

其中，表示第l个路标特征在k-1时刻的位置状态估计值；h(·)表示给定的机器人观测函数；

(1-4-2)根据测量值Sigma点计算预测测量值均值：

(1-4-3)利用QR分解，计算预测测量值均值的协方差矩阵的平方根因子：

其中，∏_z，k|k-1定义为以下加权中心偏差矩阵：

(1-4-4)计算机器人位姿状态的交叉协方差矩阵和卡尔曼增益：

(1-4-5)根据路标特征的测量值z_k，计算机器人位姿状态后验估计均值及其协方差矩阵平方根因子：

其中，cholupdate(·)函数表示矩阵的Cholesky更新分解运算，‘-’表示对参数先进行矩阵减法运算；

在步骤(1-5)中，新一代粒子表示为：

步骤(2)包括对新发现路标特征进行初始化操作和对已知路标特征进行位置状态更新操作，具体包括如下步骤：

(2-1)利用平方根容积卡尔曼滤波器对新路标特征位置状态进行初始化，新路标特征测量值在传入非线性函数h^-1(·)之前的容积点为：

其中表示第j个容积点；S_R，k表示测量噪声Q_k的平方根因子；

新路标特征测量值在传入非线性函数h^-1(·)之后的容积点为：

其中，n_z＝dim(z_n)为测量值的维度，为经过粒子采样后得到的机器人位姿状态，h^-1(·)表示测量函数的逆函数，用来将测量值转换为2维空间的坐标值；

(2-2)计算新路标特征对应的位置状态估计均值及其协方差矩阵平方根因子：

其中，∏_n，k定义为以下加权中心偏差矩阵：

当同一时刻有多个新路标特征被观测到时，对每一个新路标特征均按照步骤(2-1)和步骤(2-2)计算其位置状态均值及其协方差矩阵平方根因子；

(2-3)利用平方根容积卡尔曼滤波器对已知路标特征的位置状态进行更新，对于每一个已知路标特征，根据其先验状态均值及其协方差矩阵平方根因子生成如下容积点：

(2-4)计算已知路标特征的预测测量均值及其协方差矩阵平方根因子：

其中，∏_z，k定义为以下加权中心偏差矩阵：

(2-5)计算已知路标特征的交叉协方差矩阵及卡尔曼增益

在步骤(3)中，所述的基于相对熵散度的自适应粒子重采样方法中，机器人位姿状态的真实后验概率分布p(x_k)与其基于粒子的近似分布q(x_k)之间的相对熵散度定义为：

步骤(3)具体包括如下步骤：

(3-1)处理所有当前时刻被观测到的已知特征对应的测量值，计算所有粒子的重要性权值，将其表示为多个高斯概率密度函数的乘积形式：

其中，det(·)表示矩阵行列式运算，L表示被观测到的已知特征的个数，为第l个已知特征对应的测量预测协方差，z_l，k和分别为第l个已知特征对应的实际观测值和预测观测值；

(3-2)根据粒子重要性权值从当前粒子集中随机选择一个粒子，并更新已被重采样粒子个数；

(3-3)将被选粒子对应的机器人位姿状态进行子空间划分，判断它对应的状态值是否落入到某个空子空间上，如果被选粒子对应的机器人位姿状态落入某空子空间，则更新当前非空子空间总数，并将该子空间的标志置为非空，同时计算当前所需粒子个数：

其中，B表示非空子空间的个数，e表示给定的误差上限阈值，σ表示给定的显著性水平值，Z_1-σ表示标准正态分布的1-σ上分位点。

(3-4)当被重采样的粒子总数同时小于所需个数N_k和预设的最大值N_max时，执行步骤(3-2)至步骤(3-4)，当被重采样的粒子总数超过所需个数N_k或者预设的最大值N_max时，此次粒子重采样过程结束，执行步骤(3-5)；

(3-5)将所有被重采样的粒子对应的权值做归一化处理，转入k+1时刻的粒子滤波过程。

在步骤(4)中，从自适应重采样后的粒子集中选取粒子重要性权值最大的粒子，并将其对应的机器人位姿状态和路标特征地图作为当前时刻的估计值。

本发明基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，采用平方根转换无味卡尔曼滤波器对最优粒子提议分布进行近似，增强了算法的数值稳定性和精度，从而提高了采样粒子的质量。另一方面，在粒子重采样过程中，根据机器人位姿状态后验分布与其粒子近似分布之间的相对熵动态确定最少所需粒子个数，从而提高了算法的计算效率。

附图说明

图1为本发明方法的具体流程图；

图2为Matlab下仿真测试环境；

图3为本发明算法和FastSLAM2.0算法、UFastSLAM算法及CFastSLAM算法关于机器人位置估计均方根误差对比；

图4为为本发明算法和FastSLAM2.0算法、UFastSLAM算法及CFastSLAM算法关于机器人航向角估计均方根误差对比；

图5为为本发明算法和FastSLAM2.0算法、UFastSLAM算法及CFastSLAM算法关于粒子重采样步骤后粒子个数对比。

具体实施方式

为了更为具体地描述本发明，下面结合附图及具体实施方式对本发明的技术方案进行详细说明。

本实施例中，假设机器人采用二维线速度运动模型和距离-方位角测量模型，因此机器人及路标的空间坐标维度都为2。

如图1所示，本发明基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，包括以下步骤：

步骤1，将k时刻的机器人位姿和路标特征地图的联合后验概率表示为如下N_k个粒子组成的粒子集：

其中，(i)表示粒子序号，表示k时刻第i个粒子的权重，和分别表示k时刻第i个机器人位姿状态的估计值和相应的协方差平方根因子；和分别表示k时刻第i个粒子中关于第m个路标特征路标的位置均值和相应的协方差平方根因子。

步骤2，将运动噪声和测量噪声同时增广至机器人位姿状态向量中，保证协方差的半正定性；

假设运动噪声和测量噪声均服从零均值高斯分布，其协方差矩阵分别为Q_k和R_k，k-1时刻第i个粒子的机器人位姿状态向量均值及其协方差矩阵平方根因子分别增广为：

其中，S_Q，k和S_R，k分别表示Q_k和R_k的平方根因子，且满足：

其中，为S_Q，k的转置，为S_R，k的转置。

步骤3，根据增广后的机器人位姿状态向量均值和协方差矩阵的平方根因子，计算转换无味Sigma点集：

其中，表示增广状态向量的维度，表示第j个转换无味Sigma点，每个转换Sigma点包含了机器人位姿状态分量运动噪声分量和测量噪声分量 为n_a维向量，其元素由下式确定：

其中，r表示向量中的元素序号，表示取下整运算符，当n_a为奇数时，最后一个元素为

步骤4，根据机器人运动函数生成预测转换无味Sigma点：

其中，f(·)表示给定的机器人运动函数，u_k表示当前时刻的控制输入；

步骤5，根据预测的转换无味Sigma点计算机器人位姿状态的预测均值及其协方差平方根因子：

其中，qr(·)表示矩阵的QR分解运算，∏_v，k|k-1定义为以下加权中心偏差矩阵：

步骤6，当第l个路标特征被机器人再次成功观测到时，根据测量函数h(·)生成测量值Sigma点：

其中，表示第l个路标特征在k-1时刻的位置状态估计值；f(·)表示给定的机器人观测函数。

步骤7，根据测量值Sigma点计算预测测量值均值：

步骤8，利用QR分解，计算预测测量值均值的协方差矩阵的平方根因子：

其中，∏_z，k|k-1定义为以下加权中心偏差矩阵：

步骤9，计算机器人位姿状态的交叉协方差矩阵和卡尔曼增益：

步骤10，根据路标特征的测量值z_k，计算机器人位姿状态后验估计均值及其协方差矩阵平方根因子：

其中，cholupdate(·)函数表示矩阵的Cholesky更新分解运算，‘-’表示对参数先进行矩阵减法运算。

步骤11，计算粒子的提议分布，并根据此分布采样生成新一代粒子：

步骤12，利用平方根容积卡尔曼滤波器对新路标特征位置状态进行初始化，新路标特征测量值在传入非线性函数h^-1(·)之前的容积点为：

其中表示第j个容积点；

新路标特征测量值在传入非线性函数h^-1(·)之后的容积点为：

其中，n_z＝dim(z_n)为测量值的维度，h^-1(·)表示测量函数的逆函数，用来将测量值转换为2维空间的坐标值。

步骤13，计算新路标特征对应的位置状态估计均值及其协方差矩阵平方根因子：

其中，∏_n，k定义为以下加权中心偏差矩阵：

当同一时刻有多个新路标特征被观测到时，对每一个新路标特征均按照步骤12和步骤13计算其位置状态均值及其协方差矩阵平方根因子。

步骤14，利用平方根容积卡尔曼滤波器对已知路标特征的位置状态进行更新，对于每一个已知路标特征，根据其先验状态均值及其协方差矩阵平方根因子生成如下容积点：

步骤15，计算已知路标特征的预测测量均值及其协方差矩阵平方根因子：

其中，∏_z，k定义为以下加权中心偏差矩阵：

步骤16，计算已知路标特征的交叉协方差矩阵及卡尔曼增益

步骤17，处理所有当前时刻被观测到的已知特征对应的测量值，计算所有粒子的重要性权值，将其表示为多个高斯概率密度函数的乘积形式：

其中，det(·)表示矩阵行列式运算，L表示被观测到的已知特征的个数，为第l个已知特征对应的测量预测协方差，z_l，k和分别为第l个已知特征对应的实际观测值和预测观测值。

步骤18，根据粒子重要性权值从当前粒子集中随机选择一个粒子，并更新已被重采样粒子个数；

步骤19，将被选粒子对应的机器人位姿状态进行子空间划分，判断它对应的状态值是否落入到某个空子空间上，如果被选粒子对应的机器人位姿状态落入某空子空间，则更新当前非空子空间总数，并将该子空间的标志置为非空，同时计算当前所需粒子个数：

其中，B表示非空子空间的个数，e表示给定的误差上限阈值，σ表示给定的显著性水平值，Z_1-σ表示标准正态分布的1-σ上分位点。

步骤20，当被重采样的粒子总数同时小于所需个数N_k和预设的最大值N_max时，执行步骤18至步骤19，当被重采样的粒子总数超过所需个数N_k或者预设的最大值N_max时，此次粒子重采样过程结束，执行步骤21；

步骤21，将所有被重采样的粒子对应的权值做归一化处理，转入k+1时刻的粒子滤波过程。

步骤22，从自适应重采样后的粒子集中选取粒子重要性权值最大的粒子，并将其对应的机器人位姿状态和路标特征地图作为当前时刻的估计值。

以上所述自适应粒子重采样步骤中，相对熵的计算复杂度与粒子个数成线性关系。假设粒子个数为路标特征个数为L，那么粒子提议分布估计、粒子重要性权值计算以及路标特征状态更新的计算复杂度均为而自适应粒子重采样的计算复杂度则为或重采样过程中实际采用的粒子个数随位姿状态估计的不确定度而动态变化，并且当不确定度较小时算法的计算复杂度较低。因而，当机器人多次沿着同一路径对路标特征进行观测或者机器人能够重新观测到大量路标特征时，本发明方法相对于基于固定粒子个数的FastSLAM算法在计算效率上具有非常大的优势。

如图2所示为对本发明算法性能评估的测试环境，图中蓝色实线和蓝色星号分别表示真实的机器人运动轨迹和路标特征的空间位置，用于对比的算法包括FastSLAM2.0算法、UFastSLAM算法及CFastSLAM算法。对所有算法均运行20次蒙特卡洛试验后，不同算法关于机器人空间位置和航向角的估计均方根误差对比如图3和图4所示。不同算法在粒子重要性重采样步骤后粒子数目的变化过程对比如图5所示。

在机器人定位估计精度的比较上，由图3和图4可知，本发明算法采用数值稳定性更高的滤波器对提议分布进行估计，同时在粒子重采样过程中产生足够多的粒子来保证算法的估计精度，其估计精度要明显优于其他对比算法。由图5可知，其他对比算法由于均采用固定粒子数目的粒子采样策略，因而整个过程中粒子的个数保持100不变，而本发明算法在粒子重采样后，平均粒子个数随时间动态变化，对所有时刻的粒子个数求平均值后约为92。

Claims (5)

1.一种基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，包括以下步骤：

(1)将运动噪声和测量噪声同时增广至机器人位姿状态向量中，利用平方根转换无味卡尔曼滤波器对最优粒子提议分布进行估计，并根据该粒子提议分布对机器人位姿状态进行采样，具体过程为：

(1-1)将k时刻的机器人位姿和路标特征地图的联合后验概率表示为如下N_k个粒子组成的粒子集：

其中，i表示粒子序号，表示k时刻第i个粒子的权重，和分别表示k时刻第i个机器人位姿状态的估计值及其协方差平方根因子；和分别表示k时刻第i个粒子中关于第m个路标特征路标的位置均值和相应的协方差平方根因子；

(1-2)将运动噪声和测量噪声信息同时增广至机器人位姿状态向量及其协方差矩阵的平方根因子中，保证协方差的半正定性；

(1-3)根据机器人运动函数、增广后的机器人位姿状态向量均值及其协方差矩阵的平方根因子，计算机器人位姿状态的预测均值及其协方差平方根因子，具体过程为：

(1-3-1)根据增广后的机器人位姿状态向量均值和协方差矩阵的平方根因子，计算转换无味Sigma点集：

其中，和分别为增广后的机器人位姿状态向量均值和协方差矩阵的平方根因子，表示增广状态向量的维度，表示第j个转换无味Sigma点，每个转换无味Sigma点包含了机器人位姿状态分量运动噪声分量和测量噪声分量 为n_a维向量，其元素由下式确定：

其中，r表示向量中的元素序号，表示取下整运算符，当n_a为奇数时，最后一个元素为

(1-3-2)根据机器人运动函数生成机器人位姿状态的预测转换无味Sigma点：

其中，f(·)表示给定的机器人运动函数，u_k表示当前时刻的控制输入；

(1-3-3)根据预测的转换无味Sigma点计算机器人位姿状态的预测均值及其协方差平方根因子：

其中，qr(·)表示矩阵的QR分解运算，Π_v，k|k-1定义为以下加权中心偏差矩阵：

(1-4)当第l个路标特征被机器人再次成功观测到时，根据路标特征测量值z_k，计算机器人位姿状态后验估计均值及其协方差矩阵平方根因子；

(1-5)计算粒子的提议分布，并根据此分布采样生成新一代粒子；

(2)利用平方根容积卡尔曼滤波器更新每个粒子对应的路标特征地图信息；

(3)在粒子重采样阶段，利用基于相对熵的自适应粒子重采样方法确定当前时刻所需的粒子个数；

(4)根据重采样后的粒子集确定机器人的位姿状态和路标特征地图信息。

2.如权利要求1所述基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，其特征在于：步骤(1-4)的具体步骤为：

(1-4-1)当第l个路标特征被机器人再次成功观测到时，根据测量函数h(·)生成测量值Sigma点：

其中，表示第l个路标特征在k-1时刻的位置状态估计值；h(·)表示给定的机器人观测函数；

(1-4-2)根据测量值Sigma点计算预测测量值均值：

(1-4-3)利用QR分解，计算预测测量值均值的协方差矩阵的平方根因子：

其中，Π_z，k|k-1定义为以下加权中心偏差矩阵：

(1-4-4)计算机器人位姿状态的交叉协方差矩阵和卡尔曼增益：

(1-4-5)根据路标特征的测量值z_k，计算机器人位姿状态后验估计均值及其协方差矩阵平方根因子：

其中，cholupdate(·)函数表示矩阵的Cholesky更新分解运算，‘-’表示对参数先进行矩阵减法运算。

3.如权利要求1所述基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，其特征在于：步骤(2)包括对新发现路标特征进行初始化操作和对已知路标特征进行位置状态更新操作，具体包括如下步骤：

(2-1)利用平方根容积卡尔曼滤波器对新路标特征位置状态进行初始化，新路标特征测量值在传入非线性函数h^-1(·)之前的容积点为：

其中,表示第j个容积点，S_R，k表示测量噪声Q_k的平方根因子；

新路标特征测量值在传入非线性函数h^-1(·)之后的容积点为：

其中，n_z＝dim(z_n)为测量值的维度，h^-1(·)表示测量函数的逆函数，用来将测量值转换为2维空间的坐标值；

(2-2)计算新路标特征对应的位置状态估计均值及其协方差矩阵平方根因子：

其中，Π_n，k定义为以下加权中心偏差矩阵：

当同一时刻有多个新路标特征被观测到时，对每一个新路标特征均按照步骤(2-1)和步骤(2-2)计算其位置状态均值及其协方差矩阵平方根因子；

(2-3)利用平方根容积卡尔曼滤波器对已知路标特征的位置状态进行更新，对于每一个已知路标特征，根据其先验状态均值及其协方差矩阵平方根因子生成如下容积点：

(2-4)计算已知路标特征的预测测量均值及其协方差矩阵平方根因子：

其中，Π_z，k定义为以下加权中心偏差矩阵：

(2-5)计算已知路标特征的交叉协方差矩阵及卡尔曼增益：

4.如权利要求1所述基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，其特征在于：步骤(3)具体包括如下步骤：

(3-1)处理所有当前时刻被观测到的已知特征对应的测量值，计算所有粒子的重要性权值，将其表示为多个高斯概率密度函数的乘积形式：

其中，det(·)表示矩阵行列式运算，L表示被观测到的已知特征的个数，为第l个已知特征对应的测量预测协方差，为测量预测协方差矩阵的平凡根因子，z_l，k和分别为第l个已知特征对应的实际观测值和预测观测值；

(3-2)根据粒子重要性权值从当前粒子集中随机选择一个粒子，并更新已被重采样粒子个数；

(3-3)将被选粒子对应的机器人位姿状态进行子空间划分，判断其对应的状态值是否落入到某个空子空间上，如果被选粒子对应的机器人位姿状态落入某空子空间，则更新当前非空子空间总数，并将该子空间的标志置为非空，同时计算当前所需粒子个数：

其中，B表示非空子空间的个数，e表示给定的误差上限阈值，σ表示给定的显著性水平值，Z_1-σ表示标准正态分布的1-σ上分位点；

(3-4)当被重采样的粒子总数同时小于所需个数N_k和预设的最大值N_max时，执行步骤(3-2)至步骤(3-4)，当被重采样的粒子总数超过所需个数N_k或者预设的最大值N_max时，此次粒子重采样过程结束，执行步骤(3-5)；

(3-5)将所有被重采样的粒子对应的权值做归一化处理，转入k+1时刻的粒子滤波过程。

5.如权利要求1所述基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，其特征在于：在步骤(4)中，从自适应重采样后的粒子集中选取粒子重要性权值最大的粒子，并将其对应的机器人位姿状态和路标特征地图作为当前时刻的估计值。