李延炬1，肖宇峰1，古松2，贺希3，郭正平3

　　（1.西南科技大学 特殊环境机器人技术四川省重点实验室，绵阳 四川 621010；2.西南科技大学教务处，绵阳 四川 621010；3. 苏州中材建设有限公司，苏州 江苏 215300）

摘要：移动机器人同时定位与地图构建（SLAM）过程中的难点问题之一即是数据关联。结合独立兼容最近邻（ICNN）算法计算复杂度低和联合相容分枝定界（JCBB）算法关联准确度高的优点，提出一种基于关联数据预处理的混合数据关联方法。首先经过数据预处理，选取合适的观测特征子集和局部地图特征子集运行ICNN算法进行数据关联，若算法失败，则采用JCBB算法重新计算以保证算法精确度。仿真实验结果表明，该算法运行时间短，精确度高，适用于各种复杂环境。

　　关键词：同时定位与地图构建；数据关联；数据预处理；最近邻；联合相容

　中图分类号：TP301.6文献标识码：ADOI： 10.19358/j.issn.1674-7720.2017.02.024

引用格式：李延炬，肖宇峰，古松，等.基于激光传感器的SLAM数据关联算法的研究［J］.微型机与应用，2017,36（2）：78-82.

0引言

　　*基金项目：四川省教育厅重点项目（14ZA0091）；四川省科技支撑计划项目（2015GZ0035）；四川省科技支撑计划项目（2015GZ0342）移动机器人同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）是指机器人在未知环境中移动时根据传感器收集的信息创建环境地图，同时利用该地图进行自身的定位。SLAM中的数据关联问题是指建立在不同时刻、不同位置通过传感器获得的观测特征之间和地图特征之间的对应关系，以确定它们是否来源于实际环境中的同一物理实体［1］。在SLAM中，数据关联的计算复杂度和准确度直接影响着系统状态估计的时间耗度和精确性，对最终建立的地图有着关键性的影响。故合适的数据关联算法在机器人SLAM过程中至关重要。

　　目前，机器人SLAM领域中主要应用的数据关联方法包括独立兼容最近邻（Individual Compatibility Nearest Neighbor, ICNN）算法、联合相容分枝定界（Joint Compatibility Branch and Bound, JCBB）算法和多假设跟踪（Multiple Hypothesis Tracking, MHT）算法等［2］。在不断优化过程中，MULLANE J等人采用了概率假设密度（Probability Hypothesis Density, PHD）的方法来解决FastSLAM中的数据关联问题，得出了RaoBlackwellised PHD SLAM算法［3］；SEGUNDO P S等人采用了稀疏关联图的方式来描述各集合之间的关联关系，从而将数据关联问题转化为了求解图的最大团问题［4］；曾文静等人采用蚁群算法来搜索量测和特征的关联集合［5］。这些前人的研究工作为数据关联算法的优化提供了理论依据，本文将从算法实时性和精确性方面对数据关联算法的优化进行探讨。

　　当前，SLAM中应用最常见的ICNN算法虽然实现简单、实时性好，但是关联精确性较差，容易导致SLAM算法发散；而JCBB算法虽然关联准确度高，但是计算复杂，实时性较差［6］。在此，考虑两者的优缺点，提出将二者混合使用的方法，通过对量测数据和地图数据进行分集，缩小数据关联空间，再以ICNN算法为基础，求解最优关联解，若关联失败，则采用JCBB算法重新求解解空间，实现关联校正。该算法在保证鲁棒性的同时极大地减少了关联的数据量，在应用中可以满足较高的实时性要求。

1EKF-SLAM算法原理

　　在SLAM问题中，系统的状态向量可以表示为：

　　Xk=［XTv,kMT］T(1)

　　式中：Xv,k=［xv,kyv,kθv,k］T表示机器人的位姿；M=［x1y1…xnyn］T表示建立的地图特征。EKF（Extended Kalman Filter）在此的运用，就是基于机器人的运动模型和观测模型递归高效地估计机器人的精确位姿和路标特征，其过程服从马尔科夫过程，分为预测和更新两个阶段。其步骤如下：

　　（1）预测

　　①建立机器人运动模型如下：

　　Xv,k=fv(Xv,k－1,uv,k－1)+ωk－1(2)

　　Xk=f(Xk－1,uk－1)=fv(Xk－1,uk－1)

　　M(3)

　　其中，Xv,k表示k时刻机器人位姿状态；Xk表示k时刻机器人状态向量，包括地图特征信息；uk－1=［Vk－1αk－1］表示机器人的控制向量，包括机器人速度Vk－1和偏航角度αk－1；ωk－1是方差为Qk－1的高斯白噪声；fv(·)表示一非线性函数，用于计算k时刻机器人位姿状态。

　　②在k－1时刻，计算机器人的k时刻状态估计值和估计协方差如下：

　　k,k－1=E［f(Xv,k－1,uk－1)］=fv(v,k－1,uk－1)

　　M(4)

　　Pk,k－1=E［(Xk－k,k－1)(Xk－k,k－1)T］

　　=fxk－1Pk－1fTxk－1+Qk(5)

　　式中，Jacobian矩阵fxk－1表示非线性函数fv(·)在点xk－1处的一阶Taylor展式线性化的结果。

　　fxk－1fx|(xk－1,uk)(6)

　　（2）观测

　　①建立机器人观测模型如下：

　　k=h(k,k－1)+nk(7)

　　式中，k表示量测值的预测值，h(·)表示一非线性函数，nk表示方差为Rk的高斯白噪声。

　　②在k时刻，得到量测值Zk，并计算新息vk及新息协方差Sk如下：

　　vk=Zk－k(8)

　　Sk=hxkPk,k－1hTxk+Rk(9)

　　式中，Jacobian矩阵hxk表示非线性函数h(·)在点k,k－1处的一阶Taylor展式线性化的结果。

　　hxk=hX(k,k-1 )(10)

　　(3)更新

　　①由式(5)和式(10)计算卡尔曼增益Kk。

　　Kk=Pk,k－1hTxkS－1k(11)

　　②计算更新机器人的状态估计Xk和协方差估计Pk。

　　Xk=k,k－1+Kkvk(12)

　　Pk=(I－Kkhxk)Pk,k－1(13)

2数据关联

　　2.1独立相容最近邻（ICNN）算法

　　假设k时刻机器人观测得到M个环境特征Zk,i(i=1,2,…,m)，地图中保存N个路标特征Fj(j=1,2,…,n)。数据关联的目的就是要确立当前观测量Zk,i和地图中已有特征Fj之间的对应关系，并得到关联假设集Hm(j1,j2,…,jm)，其中jk>0表示第k个观测特征匹配的地图特征的编号。jk=0表示观测特征不是地图已有特征，为新特征：jk=－1表示其为虚警。

　　在关联假设集Hm下，计算观测特征Zk,i与每一个地图特征Fj的预测观测值k之间的新息及新息协方差如下：

　　独立相容的检测准则即满足如下式：

　　D2k,ij=vTkS－1kvk≤χ2d,1－α(16)

　　此时称观测特征Zk,i与地图特征Fj相容，其中，d表示观测量维度，α表示置信度，一般选取置信水平1－α为95％。

　　通过独立相容检测标准后，可以得到与观测特征相容的所有地图特征，最近邻算法即为在所相容的地图特征中选取Mahalanobis距离最短的作为观测特征的最佳匹配。Mahalanobis距离的定义如下：

　　Mk=vTkS－1kvk(17)

　　式(17)表示实际观测值与理论预测值之间的协方差距离。则最近邻算法的选择标准为：

　　Nk=min(Mk+ln|Sk|)(18)

　　2.2联合相容分枝定界（JCBB）算法

　　联合相容分枝定界算法是采用联合相容检验方法同时将获得的所有观测特征与地图特征进行联合关联，并采用分枝定界准则来搜索关联解空间。

　　在关联假设集Hm(j1,j2,…,jm)下，地图特征的联合观测方程为：

　　联合新息及联合新息协方差为：

　　vHm=ZHm－hHm(k,k－1)(20)

　　SHm=HHmPk,k－1HTHm+RHm(21)

　　式中，ZHm=［Zk,1…Zk,m］，HHm = hHm X(k,k-1 )。则联合相容的检测准则为下式成立：

　　D2Hm=vTHmS－1HmvHm≤χ2d,1－α(22)

　　此时称所有观测特征和地图特征是联合相容的。

　　分枝定界准则在数据关联中的应用主要是遍历关联解空间，求解最优解向量。利用式(22)的联合相容条件作为分枝标准，按它们的Mahalanobis距离确定搜索顺序，把关联解空间分解成很多小的子集；在每个子集内，利用配对数目的单调非减规则作为定界标准，将配对数最大的关联假设作为整个解空间的最优关联解。

3优化算法

　　ICNN算法虽然原理简单、计算量小，但是其关联精度不高，各个观测特征的关联结果容易相互冲突，导致关联失败。JCBB算法将所有的观测特征与地图特征进行联合关联，在检验过程中，一个错误匹配的特征可以与其他特征匹配联合相容的概率随着配对个数的增加而降低，所以，它的鲁棒性要强于独立相容检验方法，但是其计算量大，在复杂环境中实时性较差。

　　为了在SLAM过程中保证精度的同时提高数据关联过程的计算效率，提出了局部关联和混合关联相结合的方法。采用局部地图区域内的特征集和经过预处理分类的观测特征子集组成关联空间，使得数据关联过程的计算量大大减少，SLAM实时性较好；同时，在关联空间内，先采用ICNN算法进行数据关联，若关联失败，则采用JCBB算法重新进行数据关联以提高算法精确性。算法具体实现如下：

　　(1)选取局部地图并分集

　　①选取局部地图

　　根据实际应用需要，选取以机器人为中心，涵盖半径略大于激光传感器有效测距范围的地图特征，保证选取的特征少而精，可表示为：

　　式中，(xr,yr)和(xj,yj)分别表示机器人和特征点在全局坐标系中的位置，r为选取的有效半径。如图1所示，其中，黑色实心圆点表示地图中已有特征，黑色实心星点表示新观测的特征点。　

　　②分集

　　依次计算局部地图中的特征点(xk,yk)与其他特征点(xj,yj)(j≠k)之间的相对距离Δd，将距离小于设定阈值的点划分为(xk,yk)代表的子集Fk。再从剩余点中随机抽取一点，继续划分子集，已经有归属子集的点不重复参与划分。

　　D［(xj,yj),(xk,yk)］≤Δd(xk,yk)∈Fk

　　D［(xj,yj),(xk,yk)］>Δd(xk,yk)Fk(24)

　　(2)动态分类观测特征子集

　　在实际机器人导航过程中，并不需要把所有的观测特征都作为地图中的特征，一般噪声和动态特征是不保留在地图特征中的，而这些特征量可能很大，既降低了关联的精确度，又大大增加了计算量，影响系统的整体性能。在此，引入预处理的思想，定义预处理特征集，对获得的观测特征进行去噪分类，保证最后获得的子集里面的特征少而精确，具体实现如下。

　　①去噪

　　将所有的观测特征加入预处理特征集，为有效地滤除噪声点，减少干扰，对特征集里的特征点依次计算相邻距离D(n,n+i)，与设置的阈值ΔD进行比较，若满足D(n,n+i)<ΔD，则认为是特征点，反之，则是噪声点。

　　D(n,n+i)=(xn－xn+i)2+(yn－yn+i)2(25)

　　f(n,n+i)=0,D(n,n+i)<ΔD

　　1,D(n,n+i)≥ΔD (26)

　　式中ΔD依据激光传感器的模型特性确定。

　　②确定阈值ΔD

　　二维激光雷达的测量点在极坐标系下表示形式为(θi,ri) ，对应的直角坐标系下的参数xi=ricosθi，yi=risinθi ，位姿描述为ui=(θi,ri,xi,yi)。其中θ表示测量角度，r表示测量距离，i=(1,2,…,n)表示测量点。根据公式l=|α|r，可以求得相应角度在不同极径的弧长，于是可得ΔD≈l=|α|，其中，α为两点角度差，为参考点(θi,ri)附近j个点中除去最大极径和最小极径的平均值。

　　=∑i+ji=i－jri－max(ri|i+ji=i－j)－min(ri|i+ji=i－j)j－2(27)

　　③分类子集

　　将特征集里特征点按角度顺序依次排列，将首点(xi,yi)依次与本区域点{(xi+2,yi+2),(xi+3,yi+3),…,(xi+j,yi+j)}进行阈值比较，判断各点是否和首点属于同一子集，直到该区域最后一点。

　　（3）关联合适的关联集

　　将每个观测特征子集的首点与局部地图特征各个子集的首点依次做联合相容检验，取检验结果最好的关联解所在的子集作为关联对象进行关联。

　　（4）使用ICNN算法求关联解

　　（5）若步骤（4）求解失败，则使用JCBB算法求关联解

4实验与分析

　　4.1实验模型

　　机器人在k时刻的位姿为：

　　Xv,k=［xv,kyv,kθv,k］T(28)

　　假设机器人在k+1时刻相对k时刻位姿的变化量为ΔXv,k=［△xv,k△yv,k△θv,k］T, 可以由模拟里程计获得。

　　建立轮式机器人运动方程为：

　　其中，输入(xi,yi)为第i个特征的位置坐标，(ωr(k),ωθ(k))表示传感器量测噪声。

　　激光雷达选用的有效测量距离设为0.1~10 m，误差为0.1 m，角度测量范围为［－3π/4,3π/4］，误差为0.1°，系统过程噪声初始值设置为diag(0.09,0.09,180/π)，观测噪声初始值设置为diag(0.01,180/π)，机器人行驶速度为0.4 m/s，地图面积为200 m×200 m。

　4.2实验结果与分析

　　图2简单环境全局地图示意图在地图环境相对良好、特征信息比较清晰、机器人运动路径规划相对简单的条件下，验证混合算法的实现效果，结果如图2和图3所示。

　　从图2中可以看出，机器人在环境状况良好的情况下，SLAM过程中数据关联效果较好，激光观测特征与地图路标特征基本重合，机器人实际运动路径遵从设定的目标路线行驶；如图3所示，在SLAM过程中，机器人到达点与各目标点基本重合，且计算迅速，在机器人速度设定在1.5 m/s的情况下，也可以很好地完成SLAM任务，其详细数据如表1所示。

　　在地图环境相对复杂、特征点排列杂乱无序、机器人路径规划复杂曲折的情况下，验证ICNN、JCBB和混合算法的实现效果,如图4~图7所示。

　　从图4中可以看出，在复杂环境SLAM过程中，ICNN、JCBB和混合算法实现效果差别巨大；图5显示ICNN算法直接失败，机器人观测特征与地图特征匹配结果错误，导致机器人运动路径与理论路径相差甚远，最终无法到达目标点；图6中，使用JCBB算法，观测特征与地图特征关联正确率高，机器人在SLAM过程中，能精确地控制自己的位姿，实际运动路径接近于理论路径，但是运算时间很长，执行一次仿真要数个小时以上，混合算法的效果与JCBB算法大致一样，精度略有降低，但是计算时间少，可以满足机器人实时SLAM，其详细数据如表2所示。在此，分别比较三种数据关联方法在简单环境和复杂环境下SLAM执行的有效性，得到了三种算法的计算相对时间和目标位置精度的比较值。可以看出，三种算法中，混合算法运行时间最短，并且保持了相对较好的精度，在机器人SLAM过程中应用效果良好。

5结论

　　针对二维激光雷达在移动机器人SLAM过程中数据关联问题，提出一种基于ICNN算法和JCBB算法的混合算法，在关联空间内，采用JCBB算法对ICNN算法的结果进行校正补充，提高了算法的精确性；同时，结合二维激光雷达的数据特点，对观测数据和地图数据进行预处理，既保证了数据精度，又大大减少了运算数据，使得算法具有较好的实时执行性。实验结果表明，所提算法适用于不同的环境，是实际应用中的一种可行方案。

　参考文献

　　［1］ 周武．面向智能移动机器人的同时定位与地图创建研究［D］．南京：南京理工大学，2009.

　　［2］ DALLIL A, OUSSALAH M, OULDALI A. Sensor fusion and target tracking using evidential data association［J］. Sensors Journal, IEEE, 2013,13：285-293.

　　［3］ MULLANE J, VO B N, ADAMS M D. RaoBlackwellised PHD SLAM ［C］. IEEE International Conference on Robotics and Automation,2010：5410-5416.

　　［4］ SEGUNDO P S, RODRIGUEZLOSADA D. Robust global feature based data association with a sparse bit optimized maximum clique algorithm［J］.IEEE Transactions on Robotics, 2013, 29：1332-1339.

　　［5］ 曾文静,张铁栋,徐玉如,等．一种基于蚁群算法的SLAM数据关联方法［J］.计算机应用,2009,29(1):136-148.

　　［6］ 郭剑辉,赵春霞,石杏喜.一种改进的联合相容SLAM数据关联方法［J］.仪器仪表学报,2008,29(11):2260-2265.