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

权利要求书

1.一种基于多机器人的势均衡多伯努利滤波SLAM方法，其特征在于：包括以下步骤：

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

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

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

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

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

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

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

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

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

2.根据权利要求1所述的基于多机器人的势均衡多伯努利滤波SLAM方法，其特征在于：

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

其中，集合表示k时刻第n个机器人的特征观测集合，为机器人位姿，表示机器人在m处对真实地图特征的观测，表示第n个机器人在其位姿为观测到的杂波RFS；

所述地图特征的多伯努利随机有限集RFS模型

其中，表示第n个机器人平台0到k-1时刻地图特征的RFS，M表示整个地图特征的RFS；表示第n个机器人第i个粒子在k时刻的机器人的位姿，表示k时刻第n个机器人第i个粒子新生地图特征；

新生地图的多伯努利随机有限集RFS模型为

其中，表示k时刻新生地图特征多伯努利RFS，表示k时刻新生地图特征多伯努利RFS表现形式下的存在概率参数r，表示k时刻新生地图特征多伯努利RFS表现形式下的概率密度参数p，L_b,k表示k时刻新生地图特征多伯努利项的项数，表示所有机器人平台所滤波得到的高斯项信息中靠近各自自身位姿的高斯项信息。

3.根据权利要求1所述的基于多机器人的势均衡多伯努利滤波SLAM方法，其特征在于：步骤(2)中分为机器人位姿状态估计和在机器人位姿已知情况下对地图特征的状态估计，分解后联合后验估计表示为

π_k(M_k,X_1:k|Z_0:k,u_0:k-1,X_0:k)＝π_k(X_1:k|Z_0:k,u_0:k-1,X₀)π_k(M_k|Z_0:k,X_0:k) (7)

其中，π_k(X_1:k|Z_0:k,u_0:k-1,X₀)表示机器人在k时刻观测量、k-1时刻的控制量和初始位姿量下的联合后验估计；π_k(M_k|Z_0:k,X_0:k)表示机器人在位姿为X_0:k时进行观测量Z_0:k和位姿为X_0:k条件下的地图特征联合后验估计。

4.根据权利要求1所述的基于多机器人的势均衡多伯努利滤波SLAM方法，其特征在于：步骤(3)中所述滤波器为Rao-Blackwellised粒子滤波器。