基于多徑指紋信號子空間的室內(nèi)回環(huán)檢測算法研究

張彤 陳利民 吳育新 陳玉祥

關(guān)鍵詞： 回環(huán)檢測; 格拉斯曼; 信號子空間; 投影尺度函數(shù); 地圖建模; 重定位精度

中圖分類號： TN911.1?34; TP391.9 ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識碼： 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 ?引 ?言

回環(huán)檢測是SLAM研究中一個基礎(chǔ)而又關(guān)鍵的問題，有效地識別出機(jī)器人當(dāng)下所在的位置是否為已經(jīng)探索過的區(qū)域，能夠極大地抑制傳感器在測量過程中由于累積誤差的增長對成圖質(zhì)量的影響。當(dāng)誤差累積到一定程度之后，很有可能會出現(xiàn)環(huán)路不能閉合的問題?；丨h(huán)檢測為全局校正提供控制，保證場景建圖的一致性。當(dāng)需要對大范圍場景建圖時，回環(huán)檢測會顯得尤為重要，且準(zhǔn)確檢測的難度更大。良好的檢測結(jié)果依賴于使用與機(jī)器人內(nèi)部位姿估計相獨立的回環(huán)檢測系統(tǒng)?；丨h(huán)檢測的難點主要有：室內(nèi)環(huán)境中可能會存在比較相似的物體，在二維視覺特征匹配時，產(chǎn)生感知混淆的問題[1]。隨著測量時間的增長，測得數(shù)據(jù)不斷增加，因此提取特征和特征匹配所需的計算量也會越來越大，很難滿足機(jī)器人實時建圖的速率要求。

根據(jù)檢測回環(huán)時所需的數(shù)據(jù)關(guān)聯(lián)對方法進(jìn)行分類。回環(huán)檢測主要分為以下三類：圖到圖（map?to?map）、幀到幀（image?to?image）、圖到幀（map?to?image）[2]。

文獻(xiàn)[3]提出尋找不同子圖中共同特征的相關(guān)關(guān)系來檢測回環(huán)的方法。系統(tǒng)同時使用圖像外觀的相似性和特征距離相近來尋找兩張子圖中共同特征的最大相容集，如果能尋找到這樣一組一致集，那么很容易確定這兩張圖的相對尺度、旋轉(zhuǎn)和平移。Williams提出一種基于重定位技術(shù)的回環(huán)檢測方法[1]，使用相似點分類器在地圖中找尋與當(dāng)前幀中特征相似的特征，然后根據(jù)特征間的相關(guān)關(guān)系重新計算位姿。目前視覺SLAM中多采用基于外觀的回環(huán)檢測。它和前端、后端的估計都無關(guān)，僅根據(jù)兩幅圖像的相似性確定回環(huán)檢測關(guān)系。通常使用基于二維圖像特征的匹配算法，所選用的特征描述算子常見的有SIFT[4]，SURF[5]，ORB[6]等。

Cummins改進(jìn)了基于識別先前已出現(xiàn)場景的回環(huán)檢測算法[7]。算法對每幀提取SURF特征，然后根據(jù)訓(xùn)練所得的SURF特征字典，將幀轉(zhuǎn)化為特征單詞向量。系統(tǒng)認(rèn)為特征相似性越高，那么兩幀處于世界中相同位置的概率就越高。這種方法不是基于兩幀所處位置，僅僅依據(jù)其視覺特征。因此在計算機(jī)視覺領(lǐng)域中更多相關(guān)問題被提出和研究。

ORB?SLAM[8]使用ORB特征結(jié)合視覺詞典來檢測回環(huán)候選幀，提高了回環(huán)檢測的速率。這些二維特征描述子容易受光照以及視角改變等環(huán)境影響，而當(dāng)機(jī)器人運動一段時間后，所處的環(huán)境可能發(fā)生較大的改變。

文獻(xiàn)[9]基于RGB?D信息，根據(jù)深度圖先進(jìn)行一次輪廓匹配，在匹配候選中再根據(jù)改進(jìn)的PCA?SIFT提取彩色圖信息，使用BOW算法計算圖像相似度，以此做閉環(huán)檢測。雖然該方法使用深度信息，但相似度計算仍使用基于特征描述算子的二維字典算法。同時僅依賴局部二維特征，在相似場景較多的室內(nèi)環(huán)境下發(fā)生誤匹配的概率很大。

本文提出一種基于格拉斯曼[10]判別分析的無線電指紋定位算法用于回環(huán)檢測。采用無線電定位中的無累積誤差的快速定位算法、判別分析算法將各位置信號子空間之間進(jìn)行離散度化，減弱各位置之間信號子空間的相似性，提高了在大尺度室內(nèi)環(huán)境地圖與重定位的精度。

1 ?關(guān)鍵幀特征估計

1.1 ?地圖建模

首先在列隊中取一幀位置信息，判斷距離上一次回環(huán)檢測幀是否超過10幀，確定回環(huán)候選幀以后，需要對當(dāng)前幀與相鄰幀之間進(jìn)行相似度計算，判斷是否出現(xiàn)回環(huán)。定位過程的每一幀都可視為定位點，利用信號的線性子空間估計室內(nèi)信號的多徑特征信息，并在回環(huán)檢測過程中對回環(huán)候選幀進(jìn)行精確定位。在基于信號空間譜的基礎(chǔ)上，文獻(xiàn)[11]提出了利用信號的線性子空間來估計室內(nèi)信號的多徑特征信息，其信息矩陣可以表示為：

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

式中：[σtm]為多射線的傳播系數(shù)，維度為[q×1];[ntm]為噪聲，維度為[pN×1]。本文用信號的線性子空間對各位置上的信號特征進(jìn)行估計來構(gòu)建室內(nèi)定位指紋矩陣。所有位置的多徑特征信息矩陣子空間可表示為：

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

假設(shè)室內(nèi)的區(qū)域被劃分為[n]個定位點，每一個定位點上采取[m]次快拍，并且把每個定位點周圍的[ni]個定位點視為同一個定位點。每次快拍得到的定位點個數(shù)可以減少到[c]個，定位點的數(shù)據(jù)集為[X=xni1xni2…xnic]，其中每個定位點都包括[ni]個周圍鄰近定位點[xnii=x1x2…xni]，并且滿足以下關(guān)系：

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

[n=c×ni] （4）

所以，數(shù)據(jù)集的線性子空間可表示為[V=Vni1Vni2…Vnic]，且[Vnii=V1V2…Vni]。由于格拉斯曼流形是一個非線性的空間，大部分的歐氏空間的技術(shù)都不能直接用于處理格拉斯曼流形上的數(shù)據(jù)，所以在地圖建模后，構(gòu)造一個非線性投影映射框架[ΦVnii=Vnii（Vnii）H]，將格拉斯曼流形上的基本元素表示成對應(yīng)的投影矩陣。在格拉斯曼流形上的每個元素只對應(yīng)唯一的一個投影矩陣，在格拉斯曼流形上的線性子空間[Vnii]是信號特征的原始數(shù)據(jù)，其中包含信號位置地點特征，在不提高線性子空間維數(shù)的前提條件下，保持定位算法的快速性，就使得需要找出低維的信號線性子空間去替代原始數(shù)據(jù)的信號特征，即需要尋找一種映射：

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

式中[W]的維數(shù)為[pN×d]（[d

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

1.2 ?構(gòu)建投影尺度空間

由于多徑信息矩陣[A]中包含的信號特征信息能夠充分地描述某個信號的位置特征，將矩陣[A]表示為空間暫態(tài)矩陣，作為包含信號基于室內(nèi)環(huán)境特征的信號指紋。所以空間暫態(tài)矩陣[A]可以用于室內(nèi)定位的定位指紋，并且用基于信號子空間[V]的多徑特征信息來提取和估計位置信息。由于在格拉斯曼流形中，兩個子空間在不同的表示下的投影尺度是不變的，格拉斯曼流形是可以嵌入在高維空間上的一個光滑曲面，每一個線性子空間對應(yīng)格拉斯曼流形上的一個點，所以可以用投影尺度來求解格拉斯曼流形上線性子空間之間的距離，投影距離可表示為：

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

2 ?無線電指紋回環(huán)檢測模型

2.1 ?關(guān)鍵幀的投影判別模型

回環(huán)檢測是實時地對機(jī)器人的位置信息進(jìn)行重定位，并且可以在位置信息缺少或者路徑未知時判斷當(dāng)前區(qū)域是否為之前已經(jīng)訪問過的位置信息?；诟窭孤队芭袆e分析的回環(huán)檢測算法是離線的重定位過程，首先需要解決實時更新位置的指紋信息問題。由于機(jī)器人的運動方向是隨機(jī)的，周圍環(huán)境信息的變化會對提前建立的離線指紋庫產(chǎn)生影響，從而導(dǎo)致重定位的不準(zhǔn)確。在本文的無線電指紋回環(huán)檢測模型中，基于1.1節(jié)中的地圖建模，建立不同分辨率下的柵格地圖，機(jī)器人在高分辨率下低曲率的曲線運動可以近似地看成在低分辨率柵格地圖下的直線運動。

為了提高無線電重定位運算的速度，得到有效的位置信息，在高分辨率下直線運動上的連續(xù)幾個點可以視為同一個定位點。通過增加滑動窗口對同一條直線上的連續(xù)位置信息點進(jìn)行分類，并通過計算無線電指紋的相似度進(jìn)行判別。如圖1所示，其中空心和實心的點均為高分辨率場景下的定位點，空心點所連成的直線是機(jī)器人在低分辨率運動下的一條軌跡?；癆和B區(qū)域中的點為機(jī)器人運動經(jīng)過的位置，用于無線電指紋算法進(jìn)行特征提取和重定位估計?？梢耘袛喑鲈撛L問區(qū)域在低分率下的場景地圖是否為已訪問過的區(qū)域，達(dá)到回環(huán)檢測的目的。

為了達(dá)到更加精確的定位效果，檢測到回環(huán)候選幀之后，對當(dāng)前幀和回環(huán)候選幀之間進(jìn)行相似度計算。基于格拉斯曼判別分析的無線電指紋重定位算法利用類內(nèi)和類間的樣本位置信息，通過降低類間位置信息的相似性，同時增強(qiáng)類內(nèi)位置特征信息之間的相似性來增加定位的精確度以及位置指紋的魯棒性。根據(jù)投影尺度公式，提出一個新的構(gòu)建同位和異位離散程度，低緯度線性子空間的目標(biāo)函數(shù)為：

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

類內(nèi)和類間的離散度函數(shù)表示為：

[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]分別表示同位定位點個數(shù)和異位定位點個數(shù)。為了得到變換矩陣[U]，本文用共軛梯度下降法更新迭代式（8）、式（9）。在第[k]次迭代中，找到了使得判別函數(shù)最小化的變換矩陣[Uk]，聯(lián)合第[k-1]次迭代的方向和第[k]次得到的梯度構(gòu)建出新的搜索方向，在新的搜索方向上得出新的變換矩陣[Uk+1]，直到梯度下降收斂，輸出變換矩陣[U]。

2.2 ?位置估計

利用共軛梯度下降法得到最優(yōu)的[U]，可以得到變換陣[W]，通過原始子空間投影到[W]得到低維矩陣，再利用改進(jìn)的位置估計式進(jìn)行定位。投影尺度用于求解信號子空間的距離，可以大幅度降低定位過程中的位置模糊問題。對于某一待定定位點，其位置估計策略為：

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

3 ?實驗與分析

為了驗證基于格拉斯曼判別分析的回環(huán)檢測算法的有效性，仿真實驗中假設(shè)機(jī)器人運動的軌跡是隨機(jī)過程。在隨機(jī)的路徑過程中，分別對滑窗的尺度和高低分辨率場景地圖的尺度倍數(shù)兩個方面對回環(huán)檢測的結(jié)果進(jìn)行分析。室內(nèi)仿真環(huán)境范圍均為256 m2，在隨機(jī)路徑下分別設(shè)置滑窗尺度為0.5 m和0.3 m，高低分辨率尺度為5倍和10倍。利用控制變量法進(jìn)行4次不同的實驗，并對不同變量下的回環(huán)檢測重定位精度進(jìn)行計算。

四組實驗結(jié)果如圖2～圖5所示，基于格拉斯曼判別分析算法的重定位精確度如表1所示。

滑窗的寬度固定不變時，例如圖2和圖3兩組實驗滑窗的寬度均為0.5，高低分辨率地圖尺度倍數(shù)不同，圖2的重定位精度達(dá)到0.5 m，圖3重定位精度為0.2 m，在其他條件相同的情況下，高低分辨率尺度越高重定位越精確。當(dāng)高低分辨率地圖尺度倍數(shù)固定不變時，例如圖3和圖5兩組實驗高低分辨率尺度均為10，由于高分辨率地圖區(qū)域分辨率越高，其可以進(jìn)行精定位的位置點就更多，滑窗尺度越大，造成類內(nèi)容量越高，進(jìn)而位置模糊性就會降低，偏移量就越小，從而會增加回環(huán)檢測重定位的精確度。

4 ?結(jié) ?論

本文提出基于格拉斯曼判別分析的信號無線電指紋定位算法應(yīng)用于機(jī)器人隨機(jī)運動過程中的回環(huán)檢測，解決大尺度場景下基于視覺圖像的回環(huán)檢測產(chǎn)生的累積誤差問題。通過格拉斯曼流形上的投影尺度函數(shù)，構(gòu)建投影同位和異位離散程度函數(shù)，加大了各信號子空間的投影矩陣之間的差異。同時，建立的路徑模型和實時位置指紋庫用來增強(qiáng)指紋定位精度。通過仿真實驗驗證了路徑滑窗和高低分辨率定位精度的影響，可以看出格拉斯曼投影判別分析定位算法定位精度小于1 m，指紋定位算法在回環(huán)檢測中具有較高的精確度和魯棒性。

參考文獻(xiàn)

[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閉環(huán)檢測方法研究[D].秦皇島：燕山大學(xué)，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.