一种重尾噪声下鲁棒概率假设密度SLAM方法

本发明涉及同时定位与建图，具体涉及一种重尾噪声下鲁棒概率假设密度slam方法。

背景技术：

1、以fastslam为代表的slam方法的性能极大地依赖于传感器量测与地图特征点的正确关联，即数据关联问题。随机有限集(rfs)方法可以有效避免复杂的数据关联而直接对多目标的个数和状态同时进行估计，并且具有严格的数学理论基础。目前基于rfs的slam已有大量工作，mullane等人提出了phd-slam 1.0算法，将地图建模为泊松rfs，并通过实验验证了基于rfs的slam算法的优越性。gao等人提出了phd-slam 2.0算法，在利用重要性密度函数产生表示机器人位姿的粒子时充分利用当前时刻的量测，从而引导粒子向高似然区域移动，使得粒子的分布更接近状态的后验概率分布，相比phd-slam 1.0计算效率显著提高。

2、上述算法均假设量测噪声为高斯分布，然而在实际应用中，由于传感器退化或恶劣天气等影响，实际的量测噪声往往偏离高斯分布而呈现出一种重尾分布，即重尾噪声。重尾噪声出现在高斯分布的尾部，导致特征点和量测正确关联的概率被低估，这将影响地图的估计，从而进一步影响机器人位姿的估计，随着时间的推移，slam系统可能会失效。

技术实现思路

1、本发明的目的在于提供一种重尾噪声下鲁棒概率假设密度slam方法，旨在解决现有技术在重尾噪声环境下的同时定位与建图估计精度不足的问题。

2、为实现上述目的，本发明提供了一种重尾噪声下鲁棒概率假设密度slam方法，包括下列步骤：

3、步骤1：初始化机器人位姿以及参数；

4、步骤2：对k时刻的机器人位姿粒子进行重要性采样；

5、步骤3：对重要性采样后的粒子进行地图预测，将新生地图特征点加入地图，得到预测地图；

6、步骤4：用量测更新预测地图，得到更新的地图；

7、步骤5：利用sc策略更新粒子的权重；

8、步骤6：筛选权重最大的粒子及其地图作为这一时刻的状态提取；

9、若k＜k，则令k＝k+1并进行粒子重采样，接着循环执行步骤2至步骤6；

10、否则，循环终止。

11、可选的，步骤1中初始化用于近似这一时刻机器人位姿后验分布的粒子集以及参数，参数具体包括采样初始时刻，预设采样周期数，存在概率，检测概率，变分迭代终止阈值，学生t分布的自由度，地图高斯分量修剪阈值和合并阈值。

12、可选的，在步骤2的执行过程中，取重要性密度为状态转移密度，对k时刻的机器人位姿粒子进行重要性采样，得到新粒子。

13、可选的，在步骤3中，假设新生概率假设密度为高斯混和形式，将其加入到地图概率假设密度中，得到每个粒子及其对应的预测地图概率假设密度；采用自适应新生模型，即新生概率假设密度源于上一时刻未和特征点关联上的量测。

14、可选的，在步骤4的执行过程中，将量测建模为学生t分布，利用变分贝叶斯推导出完整的地图更新公式，并运用于量测和地图特征点的卷积过程，得到每个粒子及其对应的更新地图概率假设密度。

15、可选的，步骤5中采用sc策略更新机器人位姿，获得更新后的机器人位姿粒子。

16、可选的，步骤6中从更新的机器人位姿粒子中提取权重最高的粒子及其地图作为这一时刻的状态估计，接着进行粒子的重采样，剔除小权重粒子，复制大权重粒子，得到用于近似下一时刻机器人位姿的后验分布的粒子集。

17、本发明提供了一种重尾噪声下鲁棒概率假设密度slam方法，首先将机器人位姿建模为粒子，环境地图建模为概率假设密度，然后利用ekf算法对所有表示机器人位姿的粒子进行重要性采样；基于每个粒子进行地图预测，将新生概率假设密度添加到后验概率假设密度中；为了更好处理重尾噪声，将量测噪声建模为学生t分布，进而利用基于学生t分布的边缘近似似然函数，推导出完整的地图更新表达式；最后利用sc策略更新机器人位姿粒子的权重，并进行状态提取和粒子重采样。本发明在重尾噪声干扰的slam场景中，在保证计算效率的同时，能有效完成slam任务。

1.一种重尾噪声下鲁棒概率假设密度slam方法，其特征在于，包括下列步骤：

2.如权利要求1所述的重尾噪声下鲁棒概率假设密度slam方法，其特征在于，

3.如权利要求2所述的重尾噪声下鲁棒概率假设密度slam方法，其特征在于，

4.如权利要求3所述的重尾噪声下鲁棒概率假设密度slam方法，其特征在于，

5.如权利要求4所述的重尾噪声下鲁棒概率假设密度slam方法，其特征在于，

6.如权利要求5所述的重尾噪声下鲁棒概率假设密度slam方法，其特征在于，

7.如权利要求6所述的重尾噪声下鲁棒概率假设密度slam方法，其特征在于，