一种移动机器人SLAM中的多假设数据关联方法

陈白帆，蔡自兴，邹智荣

(中南大学 信息科学与工程学院, 湖南 长沙，410083)

移动机器人同时定位与建图(Simultaneous localization and mapping，SLAM)问题是移动机器人研究领域的基本问题与研究热点，也是移动机器人真正实现自主的最重要的条件之一。所谓同时定位与建图，是指机器人在移动过程中根据位姿估计和传感器数据进行自身定位，同时建造增量式地图[1]。数据关联问题也称一致性问题，本是目标跟踪中的问题，用于确定传感器的测量信息和目标源之间的对应关系[2]。在移动机器人SLAM中数据关联是指建立在不同时间、不同地段获得的传感器测量之间、传感器测量与地图特征之间或者地图特征之间的对应关系，以确定它们是否源于环境中同一物理实体的过程。它是SLAM本身面临的挑战之一，对于SLAM的状态估计至关重要，并直接影响到SLAM的计算复杂度和结果的正确性。目前，移动机器人SLAM中的数据关联的研究主要有3个方面：

(1) 局部数据关联。当前观测信息与已有地图中的某个特征匹配或 2个连续的观测帧间的匹配(Scan matching)从而进行特征或目标的跟踪问题的匹配问题，这是SLAM过程中必不可少的基本问题，也是本文的主要研究问题。

(2) 循环闭合。主要针对环境中存在循环(Loop)地形的情况。当移动机器人绕循环地形1周时，综合应用各方面信息确认机器人回到了循环起点，并获得当前观测与循环起点处地图特征间的关联关系，对机器人定位及地图进行误差修正。

(3) 地图合并。对于大范围的环境探索，需要用到多个机器人协作以减少环境探索时间、提高环境建图的精度。在多个机器人SLAM时，为了生成全局地图，就出现了各机器人的局部地图间相同物理实体的路标关联问题，即地图合并问题。

数据间的关联通常利用统计估计的方法来确定。目前，在SLAM领域中也提出了许多数据关联算法，大多采用门限法，其中比较经典的算法包括单匹配最近邻(Individual compatibility nearest neighbor，ICNN)、分枝限界联合匹配(Joint compatibility branch and bound，JCBB)[3]。此外，还有Bailey等[4]引入图论思想，通过寻找2幅完全图间的最大公共子图获得观测与地图间的数据关联。该方法具很强的抗干扰能力，但是，搜索最大公共子图问题很难，且构造完全图需要提取观测特征和观测间的约束，当观测增多时计算量会显著增加。Zhang等[5]将多维分配数据关联算法应用于SLAM中单帧观测的数据关联，Wijesoma等[6]又进一步将该方法应用于多帧观测的数据关联。Hahnel等[7]提出了一种惰性数据关联方法，该方法通过回溯修正过去错误的数据关联，但需要计算维数较大矩阵的逆，很难实时实现。黄庆成等[8]应用基于KD树的最近邻算法实现了局部地图间的特征点数据关联；王婷婷[9]研究了模拟退火算法求解SLAM中的数据关联方法；Ji等[10]提出了一种关联树模型，并对关联树进行有限深度回溯搜索实现数据关联，该方法适用于基于最小二乘的完全SLAM。在目标跟踪方面，Reid[11]针对多目标跟踪问题提出了一种多假设跟踪的数据关联方法，该方法对所有满足约束条件后可能的关联进行假设，并对假设进行跟踪，在一定时间以后才真正确定最优或次优关联对集合。在理想条件下，多假设跟踪方法被认为是处理数据关联的最优方法[12]，并且由于其独有的跟踪特性，使得在动态环境下也能保证其有效性。上述数据关联方法中，一旦观测量和路标间的关联假设被确定，大多数方法就不能进行修改。由于当前时刻的信息量缺乏导致关联假设错误，而这个错误将影响后面的移动机器人位置估计，从而导致后续的数据关联错误。如果能在一段时间后修正之前发生错误观测，就可以获得更好的SLAM结果。少数方法能通过回溯进行修正，但需要大量的计算，实时性不强。如果能够维持多个关联假设，便可增加获得最优或次优关联假设的概率。在此，本文作者提出一种多假设的数据关联方法。在某时刻维持多个数据关联假设，设定每个假设计算代价的函数，选择当前时刻数据关联假设代价最小的为当前时刻SLAM系统的数据关联结果。该关联确定后，并没有丢弃其他的假设。实际上，采用该方法要经过一段时间才获得真正的关联。为了减少计算量，用基本剪枝技术滤除错误的数据关联假设。在此，本文作者利用多个粒子维持多种数据关联假设，即采用粒子滤波器来实现多假设数据关联算法。

1 SLAM中数据关联问题描述

在移动机器人SLAM过程中，数据关联发生在预测阶段，用于判断已构建的地图中与当前观测信息相匹配的特征子集。设移动机器人构建的地图中有n个特征{F1，F2，…，Fn}，传感器的测量值E有m个即{E1，E2，…，Em}。数据关联就是找到地图特征与测量值间的关系，可描述为

其中：ji为地图中第j个特征Fj与第i个测量值Ei的相关值，当两者完全不相关时，ji=0。

在SLAM过程中，根据前一时刻移动机器人的位姿和运动模型可以预测当前时刻移动机器人位姿：

其中：f为状态传递函数；vk是预测噪声，包括系统的随机噪声和模型本身的不确定性，一般采用服从高斯分布噪声。

根据k时刻观测模型可以预测地图特征的位置：

其中：h为测量函数；wk,j为测量噪声，包括表示测量过程中的传感器的误差和模型本身的不确定性。第 i个观测值和已有地图中第j个特征间的距离可用新息及其协方差表示：

Ei和 Fj之间是否相关就要看它们之间的 Mahalanobis距离是否小于某个阈值，即

若新息υk,ij服从高斯分布，则标准化后的距离应满足2χ分布。

为了很好地维持多种数据关联假设，并能从多种假设中寻找最优的关联假设，首先需要建立描述多数据关联假设方法。本文不采用式(1)所示的数据关联描述方法，而另外定义关联假设变量为rij，当观测量与地图中某个特征相关联时即为1，否则为0，则SLAM中的数据关联问题就转化成了0−1整数规划问题。在一般情况下设定关联变量应满足单源约束条件，即 1个观测量最多只源于1个物理路标特征，1个特征最多只产生1个观测量，因此，数据关联变量定义如下：

其中：r0j=1表示特征j在当前帧中没有与之相关联的观测量；ri0=1表示观测量i与所有特征都不匹配，有可能是新路标或虚警(不是真实物理路标的反映，而是传感器噪声或镜面反射等引起的)。

设k时刻观测量与路标间有L种关联假设集合，其关联假设的域为Rk，Rk,l为其中的一组关联假设集，即

Rk,l可看成由0和1组成的满足约束条件的二维矩阵；l=1，…，L。数据关联问题就变成了寻找关联假设集合中最优的一组假设集，即可等价为以下离散优化问题：

rk,ij满足式(7)～(9)。

2 基于粒子滤波的多假设数据关联方法

在基于粒子滤波的移动机器人SLAM方法中，采用粒子来保存机器人位姿的可能分布和地图中路标的可能分布，通过不断采样和更新跟踪整个SLAM过程，按粒子权重获得最接近真实情况的机器人路径轨迹和地图。若将移动机器人的位姿和环境中路标位置看作移动机器人SLAM系统状态，则该方法的实质就是对移动机器人SLAM状态进行多种假设并跟踪。本文将利用粒子滤波器来实现多假设数据关联。

采用多假设数据关联方法，在每次观测后将生成当前时刻的多个数据关联假设，并计算每种数据关联假设的代价，以获得当前时刻SLAM的数据关联结果。基于粒子滤波的多假设数据关联算法，设定每个粒子包含了当前的假设关联集 Rk,l和粒子权重 w，即s={Rk,l，w}。若有N个粒子，则可维持并跟踪N种假设关联。多假设数据关联方法流程中主要模块对应于粒子滤波的模块如图1所示。

图1 基于粒子滤波的多假设数据关联算法流程Fig.1 Multiple hypotheses data association algorithm flow based on particle filter

获得假设生成是多假设数据关联方法中最关键的步骤，也决定了整个算法的复杂度，而数据关联假设的代价计算则决定了当前时刻SLAM的地图和机器人位姿更新的正确性。

在数据关联过程中，每帧观测和地图的关联与之前的数据关联并非完全独立，有一定关系。假设第 k时刻所有的关联假设集合Rk是由k−1时刻的关联假设集合Rk-1和k时刻获取的观测数据Zk相关形成：新的关联假设集合Rk,l由Rk-1,l中每个先验的假设与观测量zk,i相关形成，如此循环，直到所有的先验假设和观测量集处理完成为止，形成最终的假设集Rk。其中，zk,i有可能是源于已构建地图中的路标、新观测到的路标或者虚警。因此，可设关联假设集合 Rk,l为先验假设Rk−1,l与 φk的联合假设：

其中：φk定义为Zk中所有观测量与先验假设之间的数据关联。关联假设集合Rk,l的概率为：

通常，先验分布p(Rk,l)为常数，则式(11)的数据关联整数规划的极大化问题就变成了极小化问题。代价函数越小，则其相对应的关联假设越接近真实的数据关联。

基于粒子滤波的多假设数据关联算法是一个包含粒子采样、权重计算和重采样步骤不断迭代的过程：首先对观测数据进行聚类；根据式(13)对当前粒子集合进行采样，获得下一时刻的粒子集合。通过下式计算粒子的权重：

粒子中保存的假设数据关联集的代价函数值越小，该粒子权重越高；在粒子重采样过程中，利用剪枝技术去掉错误的数据关联假设。

3 实验结果与分析

实验采用经典的 Neira的仿真实验数据[13]。为简化，在实验过程中将本文提出的基于粒子滤波的多假设数据关联方法简称为PFMH。机器人通过前向运动和转向运动进行控制，前向运动速度为0.312 5 m/s，转速为9 °/s。设定机器人的传感器的感知距离为3.5 m，角度为180°，传感器观测周期为1 s。设机器人初始运动控制误差的方差 σveh=(σv，σθ)(其中：前向运动速度控制误差方差 σv=0.01，转向运动控制误差方差σθ=2)。传感器初始观测误差的方差 σsensor=(σρ，σφ)(其中：观测距离误差方差σρ=0.01，转向运动控制误差方差σφ=0.125)。图2和图3所示分别为采用基于粒子滤波的多假设数据关联方法的 SLAM(实验中简称PFMH-SLAM)的定位误差和数据关联结果，其中SLAM解决方法采用扩展卡尔曼滤波方法。

图2 PFMH-SLAM的定位误差Fig.2 Localization errors of PFMH-SLAM

图3 PFMH-SLAM的数据关联结果Fig.3 Data association results of PFMH-SLAM

从图2和图3可以看出：PFMH-SLAM在实验中表现出很高的定位精度和数据关联正确率。较大的关联漏检率主要是由于机器人角度预测出现较大误差。

图4所示为3倍初始运动控制误差方差时，基于单匹配最近邻的SLAM(ICNN-SLAM)、基于分枝限界联合匹配的 SLAM(JCBB-SLAM)和本文提出的PFMH-SLAM这3种方法的数据关联正确率、错误率和漏检率。从图4可以看出：在运动控制误差增大到3倍时，ICNN数据关联正确率低，错误率很高，JCBB和PFMH则保持着较高的数据关联正确率和较低的错误率。由于漏检率仅与传感器观测的误差密切相关，因此，3种方法的漏检率基本相同。

图4 3倍σveh时3种方法数据关联结果比较Fig.4 Experimental results comparison of three times σveh

在不同的运动误差和观测误差下，以初始的运动控制误差方差和初始观测误差为基础分别进行倍增调整，对上述3种方法进行多次实验。在相同误差条件下，对每种方法进行20次实验，每次实验重新随机设置仿真环境中的路标，其实验结果如图5所示。

从图5(a)可见：3种方法的平均数据关联正确率都会随着观测误差的增大而下降。这是由于前2种方法本质上都是一种门限约束的关联判断方法，PFMH在计算关联代价时也使用了这一约束。由于PFMH方法对过去错误数据关联有一定修正能力，因此，在同样观测误差条件下会获得比前两者更高的正确率。从图5(b)可见：当机器人运动控制误差大于2倍初始误差时，ICNN方法的平均数据关联正确率急速下降；当大于4倍初始误差时，JCBB方法的平均数据关联正确率也开始迅速下降，而PFMH方法在运动控制误差大于6倍初始误差时才开始下降，并保持最高的正确率。机器人运动控制误差方差不断增加时，基于这3种方法的SLAM对机器人在X方向、Y方向和角度的定位误差的变化见图5(c)。从图5(c)可见：与ICNN方法相比，JCBB方法和PFMH方法平均定位误差增长较缓慢，且PFMH方法获得的平均定位误差最小。

图5 3种数据关联方法结果比较Fig.5 Experimental results comparison of three data association methods

4 结论

(1) 针对经典数据关联方法一旦关联假设确定就不能修改的不足，将数据关联问题转换成离散优化问题，利用多个粒子来维持多种数据关联假设。

(2) 基于粒子滤波的多假设数据关联方法实际上是在一段时间后才获得真正最优或次优的关联结果，具有正确的数据关联结果和更高的定位精度。

(3) 采用该方法在复杂环境下如何减小计算量同时维持其准确度有待进一步研究。

[1]王璐, 蔡自兴. 未知环境中移动机器人并发建图与定位(CML)的研究进展[J]. 机器人, 2004, 26(4): 380−384.WANG Lu, CAI Zi-xing. Progress of CML for mobile robots in unknown environments[J]. Robot, 2004, 26(4):380−384.

[2]韩崇昭, 朱洪艳, 段战胜, 等. 多源信息融合[M]. 北京: 清华大学出版社, 2006: 290−334.HAN Chong-zhao, ZHU Hong-yan, DUAN Zhan-sheng. Multisource information fusion[M]. Beijing: Tsinghua University Press, 2006: 290−334.

[3]Neira J, Tardos J D. Data association in stochastic mapping using the joint compatibility test[J]. IEEE Transactions on Robotics and Automation, 2001, 17(6): 890−897.

[4]Bailey T, Nebot E M, Rosenblatt J K, et al. Data association for mobile robot navigation: A graph theoretic approach[C]//Proceedings of the IEEE International Conference on Robotics and Automation. San Francisco, USA, 2000: 2512−2517.

[5]ZHANG Sen, XIE Li-hua, Martin A. An efficient data association approach to simultaneous localization and map building[C]// Proceedings of the IEEE International Conference on Robotics and Automation. New Orleans, USA, 2004:854−859.

[6]Wijesoma W S, Perera L D L, Adams M D. Toward multidimensional assignment data association in robot localization and mapping[J]. IEEE Transactions on Robotics and Automation, 2006, 22(2): 350−365.

[7]Hahnel D, Thrun S, Wegbreit B, et al. Towards lazy data association in SLAM[C]//Proceedings of the International Symposium on Robotics Research. Sienna, Italy, 2003: 421−431

[8]黄庆成, 洪炳熔, 厉茂海, 等. 基于主动环形闭合约束的移动机器人分层同时定位和地图创建[J]. 计算机研究与发展,2007, 44(4): 636−642.HUANG Qing-cheng, HONG Bing-rong, LI Mao-hai. Mobile robot hierarchical simultaneous localization and mapping based on active loop closure constraint[J]. Journal of Computer Research and Development, 2007, 44(4): 636−642.

[9]王婷婷. 移动机器人SLAM中的数据关联算法研究[D]. 天津:南开大学软件学院, 2007: 10−50.WANG Ting-ting. Research on data association algorithm in mobile robot SLAM[D]. Tianjin: Nankai University. College of Software, 2007: 10−50.

[10]JI Xiu-cai, ZHANG Hui, HAI Dan, et al. Incremental simultaneous localization and mapping with backtracking data association for mobile robots[C]//Proceedings of the IEEE International Conference on Information and Automation.Zhangjiajie, China, 2008: 634−639.

[11]Reid D. An algorithm for tracking multiple targets[J]. IEEE Transactions on Automatic Control, 1979, 24(6):843−854.

[12]Stone L D, Corwin T L, Barlow C A. Bayesian multiple target tracking[M]. Boston: Artech Print on Demand, 1999: 123−140.

[13]Neira J. Continuous SLAM [EB/OL]. http://webdiis.unizar.es/～neira/5007439/dalab.zip. 2008−12−10.