張彤 陳利民 吳育新 陳玉祥

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

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

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

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

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

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

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

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

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

1.1 ?地圖建模

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

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

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

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

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

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

[n=c×ni] （4）

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

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

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

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

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

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

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

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

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

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

2.2 ?位置估計(jì)

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

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

3 ?實(shí)驗(yàn)與分析

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

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

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

4 ?結(jié) ?論

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

參考文獻(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)檢測(cè)方法研究[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.