张彤 陈利民 吴育新 陈玉祥

关键词： 回环检测; 格拉斯曼; 信号子空间; 投影尺度函数; 地图建模; 重定位精度

中图分类号： TN911.1?34; TP391.9                 文献标识码： A                   文章编号： 1004?373X（2019）05?0066?04

Indoor loop detection algorithm based on multipath fingerprint signal subspace

ZHANG Tong， CHEN Limin， WU Yuxin， CHEN Yuxiang

（School of Information Engineering， Nanchang University， Nanchang 330000， China）

Abstract： The position and posture traced by the camera will produce the error in the SLAM process of mobile robot， and with the accumulation of errors， the scale drift is generated in the obtained track and cartography in the world coordinate system. Therefore， an indoor single station location algorithm based on the multipath fingerprint signal subspace is fused with visual odometer location algorithm for loop detection. The radio fingerprint localization algorithm based on Glassman discriminant projection analysis is used to identify the location information feature. The Glassman projection scaling function is introduced to construct the cost function， and each location space is discretized to reduce the feature similarity between two classes. The simulation experiment results show that the radio fingerprint relocation algorithm can improve the location ambiguity， robustness of location fingerprint and corrected identification rate， determine whether the robot moves to the location nearby the previous location， and eliminate the cumulative errors.

Keywords： loop detection; Glassman; signal subspace; projection scaling function; cartographic modeling; repositioning accuracy

0  引  言

回環检测是SLAM研究中一个基础而又关键的问题，有效地识别出机器人当下所在的位置是否为已经探索过的区域，能够极大地抑制传感器在测量过程中由于累积误差的增长对成图质量的影响。当误差累积到一定程度之后，很有可能会出现环路不能闭合的问题。回环检测为全局校正提供控制，保证场景建图的一致性。当需要对大范围场景建图时，回环检测会显得尤为重要，且准确检测的难度更大。良好的检测结果依赖于使用与机器人内部位姿估计相独立的回环检测系统。回环检测的难点主要有：室内环境中可能会存在比较相似的物体，在二维视觉特征匹配时，产生感知混淆的问题[1]。随着测量时间的增长，测得数据不断增加，因此提取特征和特征匹配所需的计算量也会越来越大，很难满足机器人实时建图的速率要求。

根据检测回环时所需的数据关联对方法进行分类。回环检测主要分为以下三类：图到图（map?to?map）、帧到帧（image?to?image）、图到帧（map?to?image）[2]。

文献[3]提出寻找不同子图中共同特征的相关关系来检测回环的方法。系统同时使用图像外观的相似性和特征距离相近来寻找两张子图中共同特征的最大相容集，如果能寻找到这样一组一致集，那么很容易确定这两张图的相对尺度、旋转和平移。Williams提出一种基于重定位技术的回环检测方法[1]，使用相似点分类器在地图中找寻与当前帧中特征相似的特征，然后根据特征间的相关关系重新计算位姿。目前视觉SLAM中多采用基于外观的回环检测。它和前端、后端的估计都无关，仅根据两幅图像的相似性确定回环检测关系。通常使用基于二维图像特征的匹配算法，所选用的特征描述算子常见的有SIFT[4]，SURF[5]，ORB[6]等。

Cummins改进了基于识别先前已出现场景的回环检测算法[7]。算法对每帧提取SURF特征，然后根据训练所得的SURF特征字典，将帧转化为特征单词向量。系统认为特征相似性越高，那么两帧处于世界中相同位置的概率就越高。这种方法不是基于两帧所处位置，仅仅依据其视觉特征。因此在计算机视觉领域中更多相关问题被提出和研究。

ORB?SLAM[8]使用ORB特征结合视觉词典来检测回环候选帧，提高了回环检测的速率。这些二维特征描述子容易受光照以及视角改变等环境影响，而当机器人运动一段时间后，所处的环境可能发生较大的改变。

文献[9]基于RGB?D信息，根据深度图先进行一次轮廓匹配，在匹配候选中再根据改进的PCA?SIFT提取彩色图信息，使用BOW算法计算图像相似度，以此做闭环检测。虽然该方法使用深度信息，但相似度计算仍使用基于特征描述算子的二维字典算法。同时仅依赖局部二维特征，在相似场景较多的室内环境下发生误匹配的概率很大。

本文提出一种基于格拉斯曼[10]判别分析的无线电指纹定位算法用于回环检测。采用无线电定位中的无累积误差的快速定位算法、判别分析算法将各位置信号子空间之间进行离散度化，减弱各位置之间信号子空间的相似性，提高了在大尺度室内环境地图与重定位的精度。

1  关键帧特征估计

1.1  地图建模

首先在列队中取一帧位置信息，判断距离上一次回环检测帧是否超过10帧，确定回环候选帧以后，需要对当前帧与相邻帧之间进行相似度计算，判断是否出现回环。定位过程的每一帧都可视为定位点，利用信号的线性子空间估计室内信号的多径特征信息，并在回环检测过程中对回环候选帧进行精确定位。在基于信号空间谱的基础上，文献[11]提出了利用信号的线性子空间来估计室内信号的多径特征信息，其信息矩阵可以表示为：

[xtm=Aσtm+ntm] （1）

式中：[σtm]为多射线的传播系数，维度为[q×1];[ntm]为噪声，维度为[pN×1]。本文用信号的线性子空间对各位置上的信号特征进行估计来构建室内定位指纹矩阵。所有位置的多径特征信息矩阵子空间可表示为：

[V=V1V2…Vi…Vn] （2）

假设室内的区域被划分为[n]个定位点，每一个定位点上采取[m]次快拍，并且把每个定位点周围的[ni]个定位点视为同一个定位点。每次快拍得到的定位点个数可以减少到[c]个，定位点的数据集为[X=xni1xni2…xnic]，其中每个定位点都包括[ni]个周围邻近定位点[xnii=x1x2…xni]，并且满足以下关系：

[X=xni1?xni2…?xnic] （3）

[n=c×ni] （4）

所以，数据集的线性子空间可表示为[V=Vni1Vni2…Vnic]，且[Vnii=V1V2…Vni]。由于格拉斯曼流形是一个非线性的空间，大部分的欧氏空间的技术都不能直接用于处理格拉斯曼流形上的数据，所以在地图建模后，构造一个非线性投影映射框架[ΦVnii=Vnii（Vnii）H]，将格拉斯曼流形上的基本元素表示成对应的投影矩阵。在格拉斯曼流形上的每个元素只对应唯一的一个投影矩阵，在格拉斯曼流形上的线性子空间[Vnii]是信号特征的原始数据，其中包含信号位置地点特征，在不提高线性子空间维数的前提条件下，保持定位算法的快速性，就使得需要找出低维的信号线性子空间去替代原始数据的信号特征，即需要寻找一种映射：

[fVnii（Vnii）H=WHVnii（Vnii）HW=WHVnii（WHVnii）H] （5）

式中[W]的维数为[pN×d]（[d

[f：GpN，cl→GpN，d]。

1.2  构建投影尺度空间

由于多径信息矩阵[A]中包含的信号特征信息能够充分地描述某个信号的位置特征，将矩阵[A]表示為空间暂态矩阵，作为包含信号基于室内环境特征的信号指纹。所以空间暂态矩阵[A]可以用于室内定位的定位指纹，并且用基于信号子空间[V]的多径特征信息来提取和估计位置信息。由于在格拉斯曼流形中，两个子空间在不同的表示下的投影尺度是不变的，格拉斯曼流形是可以嵌入在高维空间上的一个光滑曲面，每一个线性子空间对应格拉斯曼流形上的一个点，所以可以用投影尺度来求解格拉斯曼流形上线性子空间之间的距离，投影距离可表示为：

[dpVnii，Vnij=dpVniiVniiH，VnijVnijH=2-12VniiVniiH-VnijVnijHF] （6）

2  无线电指纹回环检测模型

2.1  关键帧的投影判别模型

回环检测是实时地对机器人的位置信息进行重定位，并且可以在位置信息缺少或者路径未知时判断当前区域是否为之前已经访问过的位置信息。基于格拉斯曼投影判别分析的回环检测算法是离线的重定位过程，首先需要解决实时更新位置的指纹信息问题。由于机器人的运动方向是随机的，周围环境信息的变化会对提前建立的离线指纹库产生影响，从而导致重定位的不准确。在本文的无线电指纹回环检测模型中，基于1.1节中的地图建模，建立不同分辨率下的栅格地图，机器人在高分辨率下低曲率的曲线运动可以近似地看成在低分辨率栅格地图下的直线运动。

为了提高无线电重定位运算的速度，得到有效的位置信息，在高分辨率下直线运动上的连续几个点可以视为同一个定位点。通过增加滑动窗口对同一条直线上的连续位置信息点进行分类，并通过计算无线电指纹的相似度进行判别。如图1所示，其中空心和实心的点均为高分辨率场景下的定位点，空心点所连成的直线是机器人在低分辨率运动下的一条轨迹。滑窗A和B区域中的点为机器人运动经过的位置，用于无线电指纹算法进行特征提取和重定位估计。可以判断出该访问区域在低分率下的场景地图是否为已访问过的区域，达到回环检测的目的。

为了达到更加精确的定位效果，检测到回环候选帧之后，对当前帧和回环候选帧之间进行相似度计算。基于格拉斯曼判别分析的无线电指纹重定位算法利用类内和类间的样本位置信息，通过降低类间位置信息的相似性，同时增强类内位置特征信息之间的相似性来增加定位的精确度以及位置指纹的鲁棒性。根据投影尺度公式，提出一个新的构建同位和异位离散程度，低纬度线性子空间的目标函数为：

[U=arg minUJ（U）=arg minU（tr（USwU）-tr（USbU））] （7）

类内和类间的离散度函数表示为：

[Sw=1nwi=1n j：ci=cj2-12tr（AijAHij）] （8）

[Sb=1nbi=1n j：ci≠cj2-12tr（AijAHij）] （9）

式中：[Aij=Pi-Pj]，[P=VVH];[nw]和[nb]分别表示同位定位点个数和异位定位点个数。为了得到变换矩阵[U]，本文用共轭梯度下降法更新迭代式（8）、式（9）。在第[k]次迭代中，找到了使得判别函数最小化的变换矩阵[Uk]，联合第[k-1]次迭代的方向和第[k]次得到的梯度构建出新的搜索方向，在新的搜索方向上得出新的变换矩阵[Uk+1]，直到梯度下降收敛，输出变换矩阵[U]。

2.2  位置估计

利用共轭梯度下降法得到最优的[U]，可以得到变换阵[W]，通过原始子空间投影到[W]得到低维矩阵，再利用改进的位置估计式进行定位。投影尺度用于求解信号子空间的距离，可以大幅度降低定位过程中的位置模糊问题。对于某一待定定位点，其位置估计策略为：

[i～=minidVi，Vj=minitrVHiVj] （10）

3  实验与分析

为了验证基于格拉斯曼判别分析的回环检测算法的有效性，仿真实验中假设机器人运动的轨迹是随机过程。在随机的路径过程中，分别对滑窗的尺度和高低分辨率场景地图的尺度倍数两个方面对回环检测的结果进行分析。室内仿真环境范围均为256 m2，在随机路径下分别设置滑窗尺度为0.5 m和0.3 m，高低分辨率尺度为5倍和10倍。利用控制变量法进行4次不同的实验，并对不同变量下的回环检测重定位精度进行计算。

四组实验结果如图2～图5所示，基于格拉斯曼判别分析算法的重定位精确度如表1所示。

滑窗的宽度固定不变时，例如图2和图3两组实验滑窗的宽度均为0.5，高低分辨率地图尺度倍数不同，图2的重定位精度达到0.5 m，图3重定位精度为0.2 m，在其他条件相同的情况下，高低分辨率尺度越高重定位越精确。当高低分辨率地图尺度倍数固定不变时，例如图3和图5两组实验高低分辨率尺度均为10，由于高分辨率地图区域分辨率越高，其可以进行精定位的位置点就更多，滑窗尺度越大，造成类内容量越高，进而位置模糊性就会降低，偏移量就越小，从而会增加回环检测重定位的精确度。

4  结  论

本文提出基于格拉斯曼判别分析的信号无线电指纹定位算法应用于机器人随机运动过程中的回环检测，解决大尺度场景下基于视觉图像的回环检测产生的累积误差问题。通过格拉斯曼流形上的投影尺度函数，构建投影同位和异位离散程度函数，加大了各信号子空间的投影矩阵之间的差异。同时，建立的路径模型和实时位置指纹库用来增强指纹定位精度。通过仿真实验验证了路径滑窗和高低分辨率定位精度的影响，可以看出格拉斯曼投影判别分析定位算法定位精度小于1 m，指纹定位算法在回环检测中具有较高的精确度和鲁棒性。

参考文献

[1] WILLIAMS B， CUMMINS M， NEIRA J， et al. An image?to?map loop closing method for monocular SLAM [C]// 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. Nice， France： IEEE， 2008： 2053?2059.

[2] WILLIAMS B， CUMMINS M， NEWMAN P， et al. A comparison of loop closing techniques in monocular SLAM [J]. Robo?tics & autonomous systems， 2009， 57（12）： 1188?1197.

[3] BURGARD W， BROCK O， STACHNISS C. Mapping large loops with a single hand?held camera [C]// Proceedings of 2007 Robotics， Science & Systems Conference. Atlanta， Georgia， USA： DBLP， 2007： 297?304.

[4] LOWE D G. Object recognition from local scale?invariant features [C]// Proceedings of the Seventh IEEE International Conference on Computer Vision. Kerkyra： IEEE， 1999： 1150?1157.

[5] BAY H， TUYTELAARS T， GOOL L V. SURF： speeded up robust features [C]// 2006 European Conference on Computer Vision. Graz， Austria： Springer?Verlag， 2006： 404?417.

[6] RUBLEE E， RABAUD V， KONOLIGE K， et al. ORB： an efficient alternative to SIFT or SURF [C]// 2012 IEEE International Conference on Computer Vision. Firenze， Italy： IEEE， 2012： 2564?2571.

[7] CUMMINS M J， NEWMAN P M. FAB?MAP： probabilistic localization and mapping in the space of appearance [J]. International journal of robotics research， 2008， 27（6）： 647?665.

[8] MUR?ARTAL R， MONTIEL J M M， TARD?S J D. ORB?SLAM： a versatile and accurate monocular SLAM system [J]. IEEE transactions on robotics， 2015， 31（5）： 1147?1163.

[9] 孙旻喆.基于RGB?D图像的SLAM闭环检测方法研究[D].秦皇岛：燕山大学，2014.

SUN Minzhe. Research on loop?closing for robot SLAM based on RGB?D images [D]. Qinhuangdao： Yanshan University， 2014.

[10] HAMM J， DONG H Y， VERMA R， et al. GRAM： a framework for geodesic registration on anatomical manifolds [J]. Medical image analysis， 2010， 14（5）： 633?642.

[11] KUPERSHTEIN E， WAX M， COHEN I. Single?site emitter localization via multipath fingerprinting [J]. IEEE transactions on signal processing， 2012， 61（1）： 10?21.