李宁 章飞

（江苏科技大学 镇江 212003）

1 引言

同是定位与地图创建（Simultaneous Localization And Mapping，SLAM）方法可以描述为移动机器人在未知环境中，从一个未知的位置出发，通过携带传感器获取自身周围的环境信息，逐步构建环境地图，再根据环境地图实现机器人自身定位［1～2］。不同于许多经典的陆上传统导航方法，SLAM方法不需要机器人获取准确的先验环境信息，成为水下机器人自主导航领域的一个研究热点［3］。

由于水下环境复杂，杂波密集，导致传统SLAM算法的数据关联精度降低，计算复杂度大大提高，从而使得滤波算法的精度大幅度降低。Mahler提出了一种基于随机有限集（Random Finite Set，RFS）的多目标贝叶斯（Multi-target Bayesian filter，MTB）滤波器［4］，该滤波方法通过对测量更新中的所有可能关联求平均来规避数据关联问题。RFS是集值随机变量，即状态向量的数量以及状态向量本身是随机的。因此，RFS自然地捕获了地图的不确定性。在后续的研究中，Mahler根据这个理论先后提出了概率假设密度（PHD）［4］、势概率假设密度（CPHD）［5］、多目标多伯努利（MeMBer）［6］等滤波方法。Mullane J等首次将RFS理论应用到SLAM领域［7～9］，提出了基于Rao-Blackwellised粒子滤波器的PHD-SLAM算法，并给出了该算法高斯混合的实现。

PHD-SLAM算法在线性条件采用高斯混合实现，在非线性条件下采用序贯蒙特卡洛实现［10～11］，但这两种实现方法都采用了近似的策略来计算粒子权重，虽然避免了数据关联，但其算法估计误差较大。Mahler提出的MeMBer滤波器不同于PHD和CPHD，它是对多目标的后验概率密度进行估计，而且不用任何近似策略计算粒子权重，因此该算法的精度较高。B.-T.Vo在文献［12］中指出MeMBer滤波器会过多估计目标数目，针对这个问题，B.-T.Vo修改了多伯努利滤波器更新阶段的公式，提出了势均衡多伯努利（Cardinality Balanced Multi-Bernoulli，CBMBer，CBMBer）滤波器，使得目标数的估计更加准确。在上述方法的启发下，本文提出了基于势均衡多伯努利滤波的SLAM方法（Cardinality Balanced Multi-Bernoulli SLAM，CBMBer-SLAM）。该方法不用进行数据关联，也不需要用近似策略进行粒子权重计算，降低了算法计算复杂度，并提高了滤波算法的精度，采用无偏差的势估计避免了过多估计地图特征数目的问题，提高了地图特征估计精度。

2 SLAM的随机有限集模型

2.1 特征地图的RFS模型

在基于RFS的SLAM算法中，k时刻环境的特征地图用一个随机集合Mk表示，其RFS模型为

其中：Mk-1=M∩FoV(X0:k-1)，表示0到k-1时刻地图特征的RFS，M为整个地图特征的RFS；Xk表示k时刻机器人的位姿，表示k时刻新生地图特征。地图特征数目在机器人运动过程中单调增加。

2.2 观测的RFS模型

在实际情况中，针对地图特征观测的无序性，用集合Zk表示k时刻机器人的特征观测集合，此时机器人位姿为Xk，其RFS模型为

其中，Dk(m,Xk)表示机器人在m处路标特征产生的测量RFS，其模型为一个伯努利型RFS。Dk(m,Xk)={z}的概率密度为P（Dm|Xk)gk(z|m,Xk)，P（Dm|Xk)表示当机器人位姿为Xk时，能够检测到位于m处的地图特征概率，即检测概率；gk(z|m,Xk)表示机器人的观测模型。Ck(Xk)表示机器人位姿为Xk时由环境杂波产生的量测RFS。

2.3 RFS框架下的SLAM问题描述

在RFS-SLAM中，使用RFS表示地图特征Mk及观测信息Zk，其中，Zk=表示地图的地标，zki表示时间k的特征量测。集合表示不像向量表示那样（例如FastSLAM）假设地标或量测是顺序存在的，因此可以避免地图特征关联导致的算法复杂问题。在RFS框架下，SLAM问题可以转化成Zk条件下，对Mk和X1:k联合后验概率密度的估计过程。其表示形式为

由于水下环境的制约，机器人在SLAM过程中自身的真实位姿无法得知，因此无法直接对Mk和X1:k的联合后验概率密度进行直接估计，只能利用条件概率公式，将联合后验估计分解成两个部分进行，这两个部分分别为机器人位姿估计及机器人位姿估计下的地图特征估计，分解后的联合后验估计的表示形式为

3 CBMBer-SLAM算法

CBMBer-SLAM方法可分为两个部分：一是对机器人的位姿进行估计；二是在粒子位姿采样估计的基础上进行地图特征后验概率密度估计。本文算法在机器人位姿估计部分采用的是Rao-Blackwellised粒子滤波器，滤波过程中，机器人轨迹的可能路径由各个粒子进行描述。在粒子位姿的基础上，采用CBMBer-SLAM滤波器进行地图特征后验概率密度估计，利用高斯混合实现方式。算法在递归的过程中传递的是多伯努利RFS参数，即多伯努利元素的概率密度函数和各元素的存在概率。最后利用地图特征的数目和位置更新结果来进行粒子的重要性采样，运用粒子加权平均来更新机器人的位姿。

在CBMBer-SLAM滤波器中，地图特征的后验概率密度被认为是一个多目标状态，由多伯努利RFS表示。多伯努利RFS是多个相互独立的伯努利RFS的联合，其表示形式可以简记为π=，r表示存在概率，p表示概率密度，L表示多伯努利项目数。在k-1时刻，CBMBer-SLAM的强度函数为

采用高斯混合实现CBMBer-SLAM地图估计时，要满足以下三个条件：

1）每个粒子都服从线性高斯的运动模型和观测模型为

fk|k-1(x|ξ)表示k时刻的状态转移函数，gk(z|x)表示K时刻地图特征的拟然函数；N(·;m,P)表示高斯分布，其中m表示高斯均值，P表示高斯协方差矩阵；Fk-1、Qk-1分别表示状态转移矩阵和过程噪声协方差矩阵；Hk、Rk分别表示观测矩阵和测量噪声的协方差矩阵。

2）目标的检测概率、存活概率与状态无关

目标的检测概率为pD,k(x)=pD,k，存活概率为pS,k(x)=pS,k。

3）新生地图特征也为一个多伯努利RFS。

3.1 势均衡多伯努利地图估计

采用GM-CBMBer实现对地图的估计时，地图的特征数目和位置由后验概率密度进行描述，在机器人位姿已知的情况下，每个粒子都对应一个机器人的可能路径，地图估计需要对每个可能的机器人路径进行CBMBer预测和更新。

1）预测

假设k-1时刻，机器人的第i个位姿采样粒子为，则多伯努利参数集为，该单个粒子在地图条件下对应的概率密度为，其高斯混合形式为

在k时刻，新生地图特征的多伯努利概率密度参数集为，在位姿粒子条件下，其中概率密度的高斯混合表达形式如下

预测的多伯努利概率密度参数集为新生特征的多伯努利概率密度参数集并上k-1时刻的后验概率密度参数集，则预测的多伯努利RFS参数集为

其中：

上式中：

2）更新

由于多伯努利RFS的并集仍然为一个多伯努利RFS，因此在k时刻，预测的多伯努利概率密度参数集记为，式中Lk|k-1=Lk-1+Lb,k。此时，单个粒子概率密度的高斯混合表达示形式为

更新后的地图特征密度可由如下的多伯努利随机集表示：

由上式可知，更新后的地图特征密度由两部分组成，前半部分表示预测阶段的幸存部分，后半部分表示由观测得到的更新部分。其中：

上式中

式（18）、（19）采用了势均衡多伯努利的更新方式获得地图特征的后验概率密度，该更新方法有效解决了多伯努利滤波带来的过多估计地图特征数目的问题，提高了估计精度。

3）高斯项的修剪、合并

随着新生地图特征的出现以及更新步骤的增加，表示伯努利项的高斯项目数会不断增加，这样就大大增加了计算量，因此我们需要对伯努利项目数以及伯努利项对应的高斯分量进行剪切、合并。首先，设置一个存在概率门限Tr，每一次更新过后，剔除地图特征存在概率小于门限值的伯努利项。然后，对剔除筛选过后的伯努利项的高斯分量进行剪切、合并，即设定一个权重阈值Tp，将权重小于阈值的高斯分量剪切掉；设定一个高斯分量距离门限Tm，如果剪切过后的高斯分量相互之间的位置距离小于门限Tm，则进行高斯合并。

4）地图特征提取

地图特征的估计主要包括对特征点数目的估计和对特征点位置的估计两方面。设定特征提取门限Tf，高斯项在剪切、合并之后，对权重大于特征提取门限值Tf的高斯分量进行提取，地图特征的数目即为大于门限Tf的高斯分量数，高斯分量对应高斯项的均值表示地图特征在地图中的位置，为提高地图特征位置估计的精确度，最后用高斯分量的加权平均表示地图特征的位置。

3.2 机器人位姿估计

本文采用Rao-Blackwellised粒子滤波实现机器人位姿估计。实现过程主要包括粒子重要性采样和粒子重采样两个部分［13～14］。在k时刻，用所有粒子的加权平均和的形式表示机器人位姿的后验分布。

1）粒子重要性采样

2）粒子重采样

在粒子重要性采样结束后需要对粒子进行重采样，得到新的粒子集。这样做可以避免粒子退化导致估计精度降低的问题。

4 仿真结果及分析

仿真实验是在Matlab 2016b版本下进行的，地图大小为45m*60m，共有25个地图特征。CBMBer-SLAM仿真结果如图1所示。

图1 地图仿真结果

图1中，地图特征的真实位置用“〇”表示，本文算法估计地图特征的位置用“+”表示，“×”表示地图环境中的杂波。机器人实际运行轨迹由实线表示，机器人的估计轨迹虚线表示。机器人视域内的杂波服从Poisson分布，机器人传感器探测特征的概率为PD=0.98。由上图可知，CBMBer-SLAM地图构建方面性能优秀。

水下机器人携带的传感器为声呐传感器，仿真中噪声假设为高斯噪声，运动模型参数具体如表1所示。

表1 水下机器人运动模型参数

在仿真过程中将本文提出的方法与RB-PHD-SLAM方法的性能进行对比，从机器人定位误差和机器人地图特征的估计精度两个方面验证本文方法的有效性。

1）机器人定位

用机器人位置均方根误差（RMSE）作为本文算法和RB-PHD-SLAM算法在机器人定位方面的性能对比指标。在本文仿真实验中，机器人位姿的采样粒子数为30个，一共进行10次滤波。通过计算每个时刻机器人位姿的RMSE值来对比两种方法，仿真结果如图2所示。

图2 机器人位置误差

由图2机器人位置误差仿真结果可以对比看出，随着仿真步数增加，本文方法在相同的条件下机器人位置误差变化幅度明显低于RB-PHDSLAM，误差均保持在1m以下，而RB-PHD-SLAM方法的机器人位置误差越来越大，说明本文算法在机器人位置估计方面的精度更好。

2）地图特征估计

本文采用两种方式对机器人地图特征估计性能进行评估。一是对每个时刻地图特征估计的数目与真实地图特征数目的对比，二是每时刻机器人特征位置估计的误差对比。仿真结果如图3、图4所示。

图3 地图特征数目对比

图4 地图特征估计数目误差对比

从图3可以看出，在前30步两种算法在特征数目估计上相差不大，都与真实特征数目相接近。在30步之后，由于算法模型误差以及计算误差的累积，机器人运动步数逐渐增加的同时，RB-PHDSLAM的地图特征估计数目开始与真实数目逐渐偏离，估计数目远远多于真实特征数目。而本文采用的势均衡多伯努利滤波的方法，克服了多伯努利滤波过多估计目标数目的缺陷，使得地图特征数目估计始终与真实地图特征数目相接近。图4将两种方法的特征数目误差进行对比，可以更加直观看出两种算法在地图特征数目估计方面的优劣，证明了本文算法在地图特征估计精度方面优于RB-PHD-SLAM方法。

在基于RFS的SLAM方法中，地图特征信息不像FastSLAM一样采用有序向量表示，而是采用无序的集合形式表示，导致传统方法在多目标系统求距离误差时并不适用，文献［15］提出的OSPA距离方法是一种用来评价集合之间差异程度的方法，适用于评价本文算法。图5采用OSPA误差评价两种算法的地图特征位置估计误差。仿真实验中，两种算法各运行10次，取OSPA距离均值。其中OSPA截断距离取值为2，距离的阶数为1阶。

图5 OSPA误差对比

从图5可看出，前30步本文方法OSPA误差与RB-PHD-SLAM方法相差不大，但是30步之后，本文算法的OSPA误差仍保持稳定，都在1.5m之下；而RB-PHD-SLAM方法的OSPA误差则随着机器人运动步数增加而增大，误差远远超过本文方法。对比两种方法的OSPA可以发现，本文方法的地图特征估计精度更高而且算法性能方面更加稳定。

5 结语

本文所提的势均衡多伯努利SLAM算法算法是基于随机有限集理论的SLAM算法，有效克服了传统算法数据关联的问题。在递归的过程中，该算法是对多目标的后验概率密度进行估计，而不用任何近似策略计算粒子权重，因此该算法的精度较高。将无偏差的势估计方法运用到多伯努利滤波更新中，有效解决了传统SLAM算法会过多估计地图特征数目的缺点，并有效提高了算法的整体估计精度。通过仿真实验，验证了本文在位姿估计精度和地图特征估计精度方面的有效性。