高斯混合概率假设密度SLAM算法

辛菁，贾渭娟，苟蛟龙

(1.西安理工大学 自动化与信息工程学院，陕西 西安 710048;2.重庆大学 城市科技学院 电气信息学院, 重庆 永川 402167)

移动机器人同步定位与地图创建(Simultaneous Localization and Mapping, SLAM)问题定义为机器人在未知环境中从一个未知的位置开始移动，在移动过程中根据其自身所带传感器感知所处环境附近路标相对自身的位置信息增量式构建环境地图，然后利用这个地图确定自己的绝对位姿[1]。在SLAM过程中，数据关联技术的使用不仅可以提高机器人系统的性能，而且使机器人在长时间、大范围内保持系统的收敛性。数据关联最初应用在目标跟踪领域，即是用于确定传感器的测量信息和目标源之间的对应关系，其中经典的数据关联算法包括最近邻数据关联算法(NNDA)[2]、概率数据关联算法(PDA)[3]、联合概率数据关联(JPDA)[4-5]和多假设法[6](MHT)等。

SLAM中数据关联是指利用当前传感器探测到的m个观测值对地图中已经存在的n个地图特征进行更新时，必须明确指出哪个观测值对应于哪个特征。如果机器人对观测值和特征的关联发生错误，则会导致错误的更新，并且此后的预测也会发生错误，从而增大定位和地图构建的误差，严重时甚至会导致定位与构图误差发散，并进一步增加了此后数据关联的困难。因此，数据关联是SLAM的一个难点问题。近几年，随着SLAM问题研究的深入，提出了许多SLAM数据关联算法[7]。如独立兼容最近邻算法(ICNN，individual compatibility nearest neighbour)、连续兼容最近邻算法(SCNN，sequential compatibility nearest neighbour)、联合兼容分枝限界(JCBB，joint compatibility branch and bound)和联合最大可能性算法(JML，joint maximum likelihood)。2007年王永清[8]提出对JCBB进行线性搜索优化，并将优化后的JCBB对特征线段的观测进行门限过滤，完成观测和预测间的最终匹配，有效提高了移动机器人应用的可行性。2008年季秀才[9]提出了一种关联树模型，并对关联树进行有限深度回溯搜索实现数据关联算法，但该方法仅适用于基于最小二乘的完全SLAM。2009年曾文静[10]提出了一种基于蚁群优化算法的最大可能性算法，该算法利用蚁群算法解决组合优化问题的优越性来搜索最优的可能性关联集合。上述SLAM中数据关联算法均假设环境地图中的特征个数是已知的情况，然而现实中移动机器人运行过程中其周围的环境地图中的特征个数往往是未知的。2010年John Mullane[11]等人提出Rao-Blackwellised PHD SLAM算法，该算法采用PHD(概率假设密度)来解决FastSLAM中数据关联问题，不仅增强了数据关联的准确率，而且也提高了机器人的定位精度，但是由于PHD算法本身数值积分存在着“维数灾”计算问题使得该方法计算量较大。

为了进一步提高SLAM算法的定位精度，本文将改进PHD算法—GMPHD(高斯混合概率假设密度)算法应用于SLAM过程中解决其数据关联问题。同时针对FastSLAM2.0中粒子退化和耗尽的问题，采用将无迹卡尔曼滤波(UKF)算法应用到FastSLAM2.0中经过改进而得到的UFastSLAM算法。

1 UFastSLAM算法简介

1.1 基于粒子滤波的FastSLAM算法

Montemerlo等人将Rao-Blackwellized粒子滤波[12]应用到SLAM中于2003年提出了FastSLAM2.0算法[13]。其基本思想是将SLAM问题分解为机器人的定位问题和机器人所在环境地图的创建问题，其中，机器人定位用一个粒子滤波器实现；由于地图中的特征是相互独立的，所以用N个EKF实现环境地图的构建。与其本人2002年提出的FastSLAM算法不同之处在于FastSLAM2.0算法同时考虑了控制信息和观测信息，进一步提高了机器人的定位精度。FastSLAM2.0是一种粒子滤波和EKF的混合算法，其优点表现为：

①较低的计算时间复杂度。当采用树状的数据结构时FastSLAM2.0的计算时间复杂度为O(MlogN)明显优于EKFSLAM的O(N2)，其中M为粒子数，N是地图中的特征数。

②当后验概率为非高斯模型时，多模型分布情况下的EKFSLAM方法通常会产生数据关联错误，而FastSLAM2.0能很好地处理这种状况，同时具有更强的鲁棒性。

1.2 UFastSLAM算法

虽然FastSLAM2.0有上述优点，但是该算法存在以下问题:

1)由于机器人位姿估计是采用粒子滤波算法，因此存在着粒子退化问题。

2)在引入重采样方法后，会出现粒子耗尽问题。

3)在长时间运行过程中，FastSLAM存在由于环境地图协方差累积所带来的计算复杂度增加的问题。

基于上述缺点，文献[14]中提出一种改进的FastSLAM算法——UFastSLAM算法，与FastSLAM算法相比，UFastSLAM有如下两处改进：

②在环境特征估计方面。地图特征的估计是采用UKF方法的，较之EKF具有更高的估计精度。而且，在FastSLAM算法的框架下，地图中的每个特征都是服从二维高斯分布的，计算量较小，因此采用UKF并不会造成更新的缓慢。

2 GMPHD-UFastSLAM算法基本原理

标准的UFastSLAM算法假设传感器观测值和地图特征间的数据关联是已知的来回避数据关联问题，然而在实际环境中，特征存在着很大的不确定性并且地图的特征数也是未知的，基于此本文在UFastSLAM算法基础上提出了高斯混合概率假设密度SLAM算法，即GMPHD-UFastSLAM算法。该算法的基本思想是将UFastSLAM算法中的数据关联问题转换成有限集统计理论跟踪算法的高斯混合问题，即高斯混合概率假设密度(Gaussian Mixture Probability Hypothesis Density, GMPHD)，然后采用GMPHD解决UFastSLAM算法中的观测值和地图特征间的数据关联。这样不仅可以提高数据关联的准确性而且也提高了机器人位姿估计的精度。

2.1 SLAM中数据关联问题描述

SLAM中数据关联是指建立安装在移动机器人上传感器观测到的信息和其移动机器人运行过程中周围环境地图特征之间的对应关系，以确定它们是否有公共源。在k时刻，假设机器人上的传感器获得m个环境特征Ei(i=1,…,m)，其对应的观测值为zk,i。此时所构建的地图中的特征F有n个Fj(j=1,…,n)。则SLAM中数据关联可以描述为：

R={j1,j2,…,ji,…,jm}

(1)

其中ji表示地图中第j个特征Fj和第i个观测值Ei的关联性，当两者完全不相关时ji=0。

(2)

k时刻传感器第i个实际观测值与其估计值之间残差vk和对应的估计协方差Sk为：

(3)

(4)

(5)

2.2 GMPHD-UFastSLAM算法基本原理

实际环境中地图特征个数往往是未知的，GMPHD算法在目标数未知时可以很好的跟踪目标，并且具有较高的跟踪精度。GMPHD算法是根据Mahler提出的随机有限集理论[15]，在概率假设密度(PHD，Probability Hypothesis Density)[16]的基础上经过改进而得到的一种多目标跟踪算法[17-18]。其基本思想是：当目标初始的先验概率密度满足高斯分布的形式时，通过将状态噪声、观测噪声、目标的繁衍、新目标的产生、目标的存活概率和检测概率表示成高斯混合的形式，之后每个时刻的后验概率密度均表示成高斯混合的形式，GMPHD就是利用混合高斯成分来预测和更新随机集的PHD，并估计出目标状态。基于此，本文采用GMPHD算法来解决UFastSLAM中的数据关联问题，提出了GMPHD-UFastSLAM算法。整个算法主要由机器人位姿估计和地图特征估计两部分组成。

(1)机器人位姿估计

采用粒子滤波和UKF相结合的方法来实现，首先定义

(6)

(7)

计算sigma点集，即：

(8)

(9)

(10)

(11)

(12)

(13)

更为精确机器人位姿还需进一步考虑观测信息，即可根据式(12)的预测通过非线性观测方程来计算k时刻的观测量的均值、协方差、交互协方差矩阵和卡尔曼增益，即：

(14)

(15)

(16)

(17)

(18)

(19)

(20)

(2)地图特征估计

Step 1 初始化

在k-1时刻，多目标后验密度的一阶统计矩即GMPHD强度函数为：

(21)

Step 2 GMPHD强度预测

(22)

即地图中新产生的特征表示为：

Step 3 GMPHD强度更新

GMPHD更新表达式如下：

(23)

其中：

(24)

(25)

(26)

(27)

(28)

(29)

Step 4 剪枝和合并

(30)

(31)

数据关联过程结束。

将经过GMPHD计算后的特征估计值代入UKF过程中进行更新计算，得到地图特征的更新值。

重采样方法和UFastSLAM算法相同。

综上，整个GMPHD-UFastSLAM算法实现步骤如下：

第一步 初始化。包括初始SLAM状态和协方差矩阵以及GMPHD算法相关参数。

第二步 预测阶段。根据机器人的运动模型预测机器人的位姿、速度和转向角等。

第三步 数据关联。将机器人传感器观测到的特征信息与此时该特征的预测值进行数据关联，这里采用GMPHD算法来实现，因为此时的特征个数是未知的。

第四步 更新地图。采用UKF算法更新每个粒子的地图。

第五步 重采样得到新的粒子集。

GMPHD-UFastSLAM算法实现伪代码如下：

1 初始化参数

2 fori=1 toM

4 预测机器人位姿的均值和方差式

5 观测信息数据关联

6 fort=1 toN

7 将特征的观测值式根据式(21)～ (30)顺序递推出式(31)

8 endfor

9 for 新观测到的特征

10 更新机器人的均值和方差

11 更新sigma点集

12 计算粒子权重

13 endfor

14 if特征j为新特征

15 初始化该特征

16 else

17 更新该特征

18 endif

19 for 不再观测到的特征

20 将上一时刻信息赋予当前时刻

21 endfor

23 endfor

24 fori=1 toM

25 归一化权重并计算ωneff

26 ifωneff<ωλ

27 进入重采样

28 else

29 保持原粒子权重

30 endif

31 endfor

32 获得k时刻粒子集Xk

需要进一步说明的是原始的PHD是一个集合估计的概念，而且其不产生航迹。而多目标跟踪中，当目标数很大时，目标状态的联合分布的计算量会非常大。如果目标独立运动，可用各目标分别滤波来代替，但这要求考虑数据关联问题。所以为了可以解决计算量问题的方法，在处理多目标跟踪问题时，采用PHD滤波来得到目标的状态和数目。因此可将UFastSLAM算法中的数据关联问题转换成有限集统计理论跟踪算法的高斯混合问题，即高斯混合概率假设密度(Gaussian Mixture Probability Hypothesis Density, GMPHD)，然后采用PHD算法的改进算法GMPHD解决UFastSLAM算法中的地图特征和其对应的观测值的数据关联。

3 仿真实验

为了验证本文所提出的高斯混合概率假设密度SLAM算法的有效性和可靠性，在MATLAB环境下进行了如下三个仿真实验：

① 基于GMPHD的UFastSLAM算法定位性能比较实验；

② 不同噪声环境下GMPHD-UFastSLAM定位性能比较实验；

③ 不同地图环境下 GMPHD-UFastSLAM算法定位性能比较实验。

实验中所用的移动机器人运动模型为：

(32)

其中，X=[x,y,θ]T表示机器人的状态；v和γ分别表示移动机器人的前向速度和角速度，即控制量为uk=(v,γ)T;wx,wy和wθ分别为系统噪声，本实验采用均值为零的高斯白噪声，用来描述机器人车轮打滑等未知条件的影响。

移动机器人观测模型表示为：

zi(k+1)=h(x(k+1),xi)+v(k+1)

(33)

其中，xi=(xi,yi)T表示环境特征i的坐标；zi(k+1)表示k+1时刻传感器提取环境特征i的观测量；v(k+1)表示观测噪声，实验中采用均值为零的高斯噪声来模拟。测量函数h(x(k+1),xi)是机器人位姿信息和观测特征坐标的函数。若令ρi(k)和φi(k)分别表示环境特征i与传感器的距离和夹角，则观测方程可进一步表示如下：

(34)

实验一：基于 GMPHD的UFastSLAM算法定位性能比较

为验证本文提出的GMPHD-UFastSLAM算法在数据关联准确性和定位精度方面的性能，进行了基于GMPHD的UFastSLAM算法定位性能比较实验。

仿真中相关参数设置如下：

协方差矩阵初始值P(0)=diag(0,0,0)；机器人的前向速度为v=3 m/s；最大转向角为maxG=π/2；机器人所环绕的圈数为1，粒子数为10；传感器观测的最大距离为30 m；控制噪声为(σv=0.3 m/s,σr=3°);观测噪声为(σρ=0.1 m,σφ=1°)；实验采用MATLAB仿真环境实现，移动机器人在规定的运动范围内从原点(0,0)开始运动。

定位结果如图1～3所示，其中图1和图2中，“*”表示实际的地图特征，“+”表示估计出的地图特征；实线表示实际的机器人运动轨迹(状态)，虚线表示估计出的机器人运动轨迹(状态)。

从图1和图2可以看出，本文所提出的算法(如图1所示)所估计出的地图特征与实际的地图特征的距离比UFastSLAM(如图2所示)中的距离小，即本文所提出的算法中的数据关联准确性相比较于UFastSLAM有所改善。上述两种算法的定位误差曲线如图3所示。从该图可以看出本文提出的GMPHD-UFastSLAM算法定位精度亦优于UFastSLAM算法。

图1 GMPHD-UFastSLAM定位结果

图2 UFastSLAM定位结果

图3 GMPHD-UFastSLAM和UFastSLAM算法定位误差曲线

为了进一步验证不同粒子数对MPHD-UFastSLAM算法定位性能的影响，本文在粒子数N=10和20时分别进行了10次定位实验，机器人位姿RMSE和时间的平均值如表1所示。从表1可以看出，当粒子数增加时，这两种算法的定位精度都有所提高；但GMPHD-UFastSLAM算法定位的均方根误差均小于UFastSLAM算法。

表1 不同粒子数情况下的10次定位结果

综上所述，与UFastSLAM算法相比较，本文提出的GMPHD-UFastSLAM算法不但可以应用于地图中特征个数未知的情况，而且还解决了粒子退化和耗尽问题，并且具有较高的数据关联的准确性和机器人定位精度。

实验二：不同噪声环境下 GMPHD-UFastSLAM算法定位性能比较

为了验证GMPHD-UFastSLAM算法在噪声环境下的定位性能，本文进行了不同强度噪声下的机器人定位性能实验。实验中控制噪声和观测噪声取值如下(其他参数与实验一相同)：

①噪声情况一：相同控制噪声不同观测噪声

控制噪声为(σv=0.3 m/s,σr=3°)，与实验一相同；观测噪声为(σρ=0.1 m,σφ=3°)与实验一(σρ=0.1 m,σφ=1°)不同。GMPHD-UFastSLAM算法定位结果如图4～5所示。

图4 GMPHD-UFastSLAM定位结果(噪声情况一)

图5 GMPHD-UFastSLAM定位误差曲线(噪声情况一)

相同控制噪声，不同观测噪声时GMPHD-UFastSLAM算法在10次定位实验中的机器人定位RMSE和时间的平均值如表2所示。从图1～5及表1～2可以看出，当机器人控制噪声一定，观测噪声增大时，两种算法的定位性能均有所下降，但与UFastSLAM相比，GMPHD-UFastSLAM仍具有较好的定位性能。

表2 不同观测噪声时GMPHD-UFastSLAM算法定位误差(RMSE)

② 噪声情况二：不同控制噪声和不同观测噪声

控制噪声为(σv=0.4 m/s,σr=5°)；观测噪声为(σρ=0.2m,σφ=3°)。GMPHD-UFastSLAM算法和UFastSLAM算法的定位结果分别如图6～7所示，定位误差曲线如图8所示。

图6 GMPHD-UFastSLAM定位结果(噪声情况二)

图7 UFastSLAM定位定位结果(噪声情况二)

图8 两种算法定位误差曲线图(噪声情况二)

不同控制噪声和观测噪声条件下，两种算法在10次定位实验中的机器人定位误差RMSE和时间的平均值如表3所示。

表3 不同控制噪声和不同观测噪声时两种算法定位误差(RMSE)

从图6～ 8及表3可以看出，当机器人控制噪声和观测噪声增大时，两种算法的定位性能均有所下降，但与UFastSLAM相比，GMPHD-UFastSLAM仍具有较好的定位性能。

实验三：不同地图环境下 GMPHD-UFastSLAM算法定位性能比较

为了验证GMPHD-UFastSLAM算法能在不同地图环境下的定位性能，本文针对稀疏地图特征和密集地图特征两种情况进行了机器人定位实验。

①密集地图特征。参数设置和定位结果见实验一。

②稀疏地图特征。传感器观测的最大距离为25 m；控制噪声为(σv=0.2 m/s,σr=2°)；观测噪声为(σρ=0.1 m,σφ=1°)，定位结果图如图9～11所示。

图9 稀疏地图特征情况下的GMPHD-UFastSLAM定位结果

从密集地图特征情况下的定位结果(如图1～ 3所示)和稀疏地图特征情况下的定位结果(如图9～ 11所示)对比可以看出，在不同地图环境下，GMPHD-UFastSLAM均能对机器人进行有效的定位和地图特征估计。

图10 稀疏地图特征情况下的UFastSLAM定位结果

图11 稀疏地图特征情况下两种算法的定位误差曲线图

4 结 语

针对地图特征个数未知的情况，本文将SLAM算法中的数据关联问题转换成有机集统计理论跟踪算法的高斯混合问题，提出利用GMPHD算法来解决UFastSLAM中的观测值和地图特征间的数据关联，即GMPHD-UFastSLAM算法。仿真结果表明GMPHD-UFastSLAM算法不但解决了粒子退化和耗尽问题，也提高了数据关联的正确性和移动机器人的定位精度。

参考文献：

[1]Dissanayake M G, Newman P,Clark S, et al.A aolution to the simultaneous localization and map building (SLAM) problem[J].IEEE Transaction on Robotics and Automation,2001,17(2):229-241.

[2]Singer R A, See R G.A new filter for optimal tracking in dense multitarget environments[C]∥Proceedings of the Ninth Aileron Conference on Circuit and System Theory,197l:201-211.

[3]Y-Bar-Shalom.Extension of the probabilistic data association filter to multitar environment[C]∥Proceedings Fifth Symposium on Nonlinear Estimation.1974:16-21.

[4]Y-Bar-Shalom, Fortmann T E, Schefie M.Joint probabilistic data association for multiple targets in clutter[C]∥Proceedings 1980 Conference Information Sciences and Systems, Princeton University，March 1980.

[5]冯洋.多目标跟踪的数据关联算法研究[D].西安：西安电子科技大学.2008,11-12.

Feng Yang.The research on data association in multi-target tracking[D].Xi’an：Xidian University.2008,11-12.

[6]Reid D B.An algorithm for tracking multiple targets[J].IEEE Transaction on Automatic Control, 1979,24(10):843-854.

[7]曾文静,张铁栋,姜大鹏.SLAM数据关联方法的比较分析[J].系统工程与电子技术,2010,32(4):860-864.

Zeng Wengjing, Zhang Tiedong, Jiang Dapeng.Analysis of data association methods of SLAM[J].System Engineering and Electronics,2010,32(4):860-864.

[8]王永清.同时定位与地图创建中的数据关联技术研究[D].青岛：中国海洋大学，2007.

Wang Yongqing.Research on data association for simultaneous localization and mapping[D].Qingdao:Ocean University of China,2007.

[9]季秀才.机器人同步定位与建图中数据关联问题研究[D].长沙：国防科技大学.2008:11-15.

Ji Xiucai.Data association problem for simultaneous localization and mapping of mobile mobots[D].Changsha: National University of Defense Technology,2008:11-15.

[10]曾文静.基于水下机器人EKF-SLAM的数据关联算法研究[D].哈尔滨:哈尔滨工程大学, 2009.

Zen Wenjing.Research on data association for the EKF-SLAM of the AUV[D].Harbin: Harbin Engineering University,2009.

[11]Mullan J, Vo B N, Martin D.Rao-Blackwellised PHD SLAM[C]∥ Proceedings of the International Conference on Robotics and Automation, Anchorage, Alaska, 2010:5410-5416.

[12]Arturo G,Oscar R,Monica B,et al.Dealing with data association in visual SLAM[EB/OL].http:∥www.intechopen.com/books/computer_vision/dealing_with_data_association_in_visual_slam,[2008-11-1].

[13]Montemerlo M, Thrun S,Koller D,et al.FastSLAM2.0: an improved particle filtering algorithm for simultaneous localization and mapping that provably converges[C]∥Proceedings of the International Conference on Artificial Intelligence, California, CA, USA, 2003:1151-1156.

[14]兰文华.移动机器人同步定位与地图构建(SLAM)研究[D].西安:西安理工大学,2011.

Lan Wenhua.Study on mobile robot simultaneous localization and map building[D].Xi’an: Xi’an University of Technology,2011.

[15]Goodman I R, Mahler R P, Nguyen H T.Mathematics of data fusion[M].Kluwer Academic Publishers, Dordrecht,1997.

[16]Mahler R.Multi-target bayes filtering via first-order multi-target moments[J].IEEE Transactions on AES,2003,39(4):1152-1178.

[17]郝燕玲，孟凡彬，周卫东，等.多目标跟踪的高斯混合概率假设密度滤波算法[J].弹箭与制导学报,2010,30(3):35-40.

Hao Yanling, Meng Fanbin,Zhou Weidong, et al.Gaussian mixture probability hypothesis density filter algorithm in multi-target tracking[J].Journal of Projectiles, Missiles and Guidance,2010,30(3):35-40.

[18]吴盘龙，任开创，蔡亚东.多目标跟踪的混合高斯PHD滤波[J].计算机工程与应用,2011,47(4):230-232.

Wu Panlong, Ren Kaichuang, Cai Yadong.Gaussian mixture probability hypothesis density filter for multiple target tracking[J].Compter Engineering and Applications,2011,47(4):230-232.