[发明专利]一种基于多机器人的势均衡多伯努利滤波SLAM方法

说明书

本发明公开基于多机器人的势均衡多伯努利滤波SLAM方法，包括(1)建立观测和地图特征多伯努利RFS模型；(2)SLAM问题转换为独立机器人位姿状态估计和地图特征状态估计；(3)获得k时刻机器人位姿状态估计预测值；(4)用势均衡策略获得k时刻地图特征状态估计；(5)修剪合并更新后高斯项；(6)用高斯项修正地图特征状态估计；(7)将高斯项融合到每一个机器人观测集合中带入k+1时刻机器人先验信息集合；(8)用(4)中滤波器对机器人每个粒子地图估计调整权重,用粒子加权平均更新k时刻位姿状态估计预测值；(9)k＝k+1执行步骤(3)。本发明改善SLAM中特征数量过估计问题，提高多机器人SLAM精度。

技术领域

本发明涉及一种SLAM方法，尤其涉及一种基于多机器人的势均衡多伯努利滤波SLAM方法。

背景技术

导航技术对于移动机器人在未知环境中工作必不可少，而同步定位与地图构建(SLAM)是近年来的主要导航技术。导航算法精度、速度、实时性和稳定性的不断提高一直是研究的重点。在一些复杂的环境中，如水下勘探环境、室内消防救援环境等，由于杂波密集，传统SLAM算法的数据关联精度会降低，计算量会大大增加，导致传统SLAM方法的准确性下降。为了解决复杂的数据关联问题，一种方法是避免数据关联问题，另一种方法是使用更有效的算法，比如基于图的SLAM。

基于随机有限集(RFS)的多目标贝叶斯滤波器通过对测量更新中所有可能的关联进行平均来避免数据关联的问题。RFS是一个设置值的随机变量，即状态向量的个数和状态向量本身都是随机的，所以RFS自然获得了地图的不确定性。在随后的研究中，将RFS应用到SLAM领域，提出了假设概率密度滤波器-同时定位与建图(PHD-SLAM)算法。PHD-SLAM算法在线性条件下通过高斯混合实现，在非线性条件下通过顺序蒙特卡罗实现，但这两种方法都使用近似策略计算粒子权重。虽然避免了数据关联，但其算法估计误差较大。单机器人多伯努利滤波器SLAM方法存在对地图特征数目过估计的问题。

多机器人联合估计策略，目前只有基于PHD-SLAM的多机器人联合估计方法，即MR-PHD-SLAM，但是由于PHD-SLAM信噪比较低以及在强非线性情况下会存在精度不高的问题。并且MR-PHD-SLAM采用特征级的信息融合，虽然可以提升单机器人SLAM精度，但是对于复杂环境来说，精度仍然不够，且实时性不好。

发明内容

发明目的：本发明旨在提供一种基于多机器人的势均衡多伯努利滤波SLAM方法，解决多机器人的SLAM精度低，实时性不好的问题。

技术方案：本发明的基于多机器人的势均衡多伯努利滤波SLAM方法，包括以下步骤：

(1)建立观测的多伯努利随机有限集RFS模型和地图特征的多伯努利随机有限集RFS模型；

(2)将SLAM问题转换为独立机器人位姿状态估计和地图特征状态估计；

(3)采用滤波器获得k时刻机器人位姿状态估计预测值；

(4)采用势均衡策略获得k时刻地图特征状态估计,即获得多伯努利参数存在概率参数r和概率密度参数p估计值；

(5)修剪和合并更新后的高斯项；

(6)根据高斯项修正地图特征的状态估计；

(7)将步骤(5)中各个机器人处理完的高斯项信息融合到每一个机器人的观测集合中，带入k+1时刻的机器人的先验信息集合中进行k+1时刻的处理过程；

(8)根据步骤(4)获得的势均衡多伯努利滤波器对每个机器人所对应的每个粒子地图估计结果调整粒子权重,然后通过采样粒子的加权平均，更新机器人k时刻位姿状态估计预测值；

(9)令k＝k+1，执行步骤(3)。

所述观测的多伯努利随机有限集RFS模型：