[发明专利]一种随机有限集框架下的同步定位与地图构建方法

权利要求书

1.一种随机有限集框架下的同步定位与地图构建方法，其特征在于：区别于基于向量的SLAM模型，将传感器观测信息、环境地图信息、观测的不确定信息都建模为随机有限集形式，利用Rao-Blackwillised分解，将SLAM全概率估计问题分解为机器人位姿估计和以这个估计为条件的环境地图估计：在进行机器人位姿估计时通过无迹卡尔曼滤波融合观测信息生成粒子滤波的建议分布函数，在进行地图状态估计时，将地图状态的后验概率估计转化为其后验PHD函数的估计过程，并利用无迹卡尔曼滤波对组成地图PHD函数的高斯构件进行更新，该方法包括以下步骤：

步骤1、对于任一k时刻，将传感器观测信息、环境地图信息、观测的不确定信息都建模为随机有限集形式，并依据这一模型建立SLAM问题的求解目标，即递归的获得机器人位姿状态和环境地图集合的联合后验概率分布；

步骤2、在有限集模型下，基于Rao-Blackwillised分解，将k时刻SLAM联合后验概率估计转换为两个“独立”的估计过程：即机器人状态后验估计，以及该状态估计条件下的环境地图后验估计；

步骤3、对于机器人状态估计，引入无迹变换，通过无迹粒子滤波实现，即利用无迹卡尔曼滤波融合观测信息，由k-1时刻的后验估计值生成粒子滤波k时刻的建议分布函数，并采样生成粒子，这一过程包含以下步骤：

(1)对k-1时刻的粒子进行UT变换，获得Sigma点集；

(2)利用机器人运动模型对Sigma点集中的Sigma点进行时间预测，获得其一阶矩、二阶矩及相应权重；

(3)利用UKF融合观测信息将Sigma点的统计特性更新到k时刻；

(4)利用更新后的统计特性构建粒子滤波k时刻的建议分布函数，并从中采样新的粒子；

(5)对粒子的重要性权值进行更新；

(6)进行重采样操作，剔除小权值粒子，复制大权值粒子，得到用于近似k时刻机器人状态后验分布的粒子集；

步骤4、进行环境地图状态估计，环境地图集合对应的PHD函数包含两个部分：存活特征的PHD函数和新生特征的PHD函数，首先利用高斯混合模型对这两个PHD函数进行近似，再利用无迹卡尔曼滤波分别对组成这两部分PHD函数的高斯构件进行更新，这一过程包含以下步骤：

(1)对k-1存活特征PHD函数的高斯构件进行UT变换，获得Sigma点集，计算其统计特性，利用传感器观测模型进行时间预测，并计算其更新所需成分；

(2)对k-1新生特征PHD函数的高斯构件进行UT变换，获得Sigma点集，计算其统计特性，利用传感器观测模型进行时间预测，并计算其更新所需成分；

(3)利用UKF融合观测信息对上述全部高斯构件进行更新，获得k时刻环境地图集合后验PHD的高斯混合近似；

步骤5、对SLAM状态估计结果进行输出，包括以下步骤：

(1)将k时刻粒子滤波的后验粒子状态进行加权输出，作为该时刻机器人的状态估计结果；

(2)对k时刻环境地图PHD函数的高斯构件进行合并及剪枝处理：首先，将相似程度高于合并门限的高斯构件合并为一个高斯分布，减少构件数。接着，设定最大高斯构件数，对超过该数量的高斯构件进行剪枝；最后，设定高斯构件的权重阈值，对权重低于阈值的高斯构件进行剪枝处理；

(3)将经过合并及剪枝处理的高斯构件的加权和作为每个环境特征的估计值，并将这些特征估计值以集合形式输出，形成整个环境地图的估计结果；

步骤6、重复步骤1～步骤5，进行k+1时刻的SLAM状态估计。