基于WiFi指紋序列匹配的機(jī)器人同步定位與地圖構(gòu)建

秦正泓，劉冉，肖宇峰，陳凱翔，鄧忠元，鄧天睿

（西南科技大學(xué) 信息工程學(xué)院，四川 綿陽(yáng) 621010）

0 引言

近年來(lái)，同步定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）逐漸成為機(jī)器人領(lǐng)域研究的熱點(diǎn)問(wèn)題，被認(rèn)為是實(shí)現(xiàn)移動(dòng)機(jī)器人自主化的核心技術(shù)［1-2］。Standford大學(xué)的Smith等［3］首先發(fā)表了關(guān)于SLAM 問(wèn)題的開(kāi)創(chuàng)性論文。針對(duì)SLAM 問(wèn)題，國(guó)內(nèi)外研究人員提出了很多解決方案，其中基于圖優(yōu)化的SLAM（Graph-based SLAM）把這一問(wèn)題轉(zhuǎn)化為最大似然估計(jì)，被認(rèn)為是目前解決SLAM 最有效的方法之一［4-8］。閉環(huán)檢測(cè)是實(shí)現(xiàn)圖優(yōu)化SLAM 的關(guān)鍵，也是SLAM 問(wèn)題的一個(gè)難點(diǎn)。正確的閉環(huán)檢測(cè)可以修正里程計(jì)的累計(jì)誤差，從而得到一致性地圖；錯(cuò)誤的閉環(huán)檢測(cè)結(jié)果則會(huì)對(duì)后續(xù)圖優(yōu)化造成干擾，導(dǎo)致地圖創(chuàng)建的失?。?］。

目前進(jìn)行閉環(huán)檢測(cè)的主流方法是通過(guò)視覺(jué)（特征匹配）或激光（掃描匹配）。Ryu等［10］提出基于一幀激光數(shù)據(jù)與另一幀激光數(shù)據(jù)的掃描匹配算法（幀—幀），通過(guò)旋轉(zhuǎn)和平移判斷兩幀激光數(shù)據(jù)的相似性，達(dá)到閉環(huán)檢測(cè)的效果。Cho等［11］通過(guò)迭代最近點(diǎn)（Iterative Closest Point，ICP），獲得一幀激光數(shù)據(jù)到另一幀的轉(zhuǎn)換關(guān)系，通過(guò)判斷相似性檢測(cè)是否存在閉環(huán)。這種檢測(cè)方法雖然能保證每幀數(shù)據(jù)都被檢測(cè)到，但容易因局部相似性高產(chǎn)生錯(cuò)誤的匹配結(jié)果和數(shù)據(jù)量的增多而延緩檢測(cè)速度。Campos等［12］提出了一個(gè)相對(duì)完整的單目SLAM系統(tǒng)ORBSLAM（Oriented FAST and Rotated BRIEF SLAM）。該系統(tǒng)的各個(gè)線程均采用圖像的ORB（Oriented FAST and Rotated BRIEF）特征，并引入本質(zhì)圖的概念來(lái)加速校驗(yàn)閉環(huán)檢測(cè)，然而在此過(guò)程中需要提取大量的圖像特征，計(jì)算量大，且在光線變化的情況下成功率較低。雖然通過(guò)視覺(jué)或激光來(lái)進(jìn)行閉環(huán)檢測(cè)的SLAM 方案已經(jīng)非常成熟，但在大范圍的復(fù)雜環(huán)境下，機(jī)器人所采集到的環(huán)境信息量龐大，對(duì)于激光傳感器，大量的相似場(chǎng)景可能導(dǎo)致錯(cuò)誤閉環(huán)的出現(xiàn)；對(duì)于視覺(jué)傳感器，在光照多變和視野受限的條件下魯棒性較低，且匹配算法復(fù)雜，計(jì)算開(kāi)銷(xiāo)巨大，從而大幅降低了圖優(yōu)化SLAM 的算法精度。因此，如何減小傳感器匹配誤差，提高SLAM 精度，是當(dāng)前研究的重點(diǎn)之一。

早期的SLAM 研究是通過(guò)濾波實(shí)現(xiàn)機(jī)器人的狀態(tài)估計(jì)?；跒V波器的方法主要是利用遞歸貝葉斯估計(jì)原理，在假定機(jī)器人所有時(shí)刻的觀測(cè)信息以及控制信息已知的條件下，對(duì)系統(tǒng)狀態(tài)的后驗(yàn)概率進(jìn)行估計(jì)。根據(jù)后驗(yàn)概率表示方式的不同，常用的濾波方法包括擴(kuò)展卡爾曼濾波器［13］、Rao-Blackwellised 粒子濾波器（Rao-Blackwellised Particle Filter，RBPF）［14］等?；跒V波器的方法存在更新效率低的問(wèn)題，難以應(yīng)用于大規(guī)模環(huán)境的地圖創(chuàng)建［15-16］。

隨著SLAM 的發(fā)展，基于濾波器的方法逐漸被基于圖優(yōu)化的方法取代?；趫D優(yōu)化的SLAM 方法采用了全局優(yōu)化理論，能夠獲得更好的建圖效果。Kschischang等［17］在2001年提出利用因子圖對(duì)SLAM 問(wèn)題進(jìn)行建模。Carlevaris-Bianco等［18］提出了通過(guò)刪除圖的節(jié)點(diǎn)來(lái)降低后端優(yōu)化計(jì)算的復(fù)雜性，節(jié)點(diǎn)被刪除的順序可能會(huì)影響圖的連通性。Vallvé等［19］結(jié)合因子圖建模與矩陣稀疏化理論，提出了節(jié)點(diǎn)下降優(yōu)化方法，減少迭代的次數(shù)，但其收斂性不高，在大范圍的復(fù)雜環(huán)境下表現(xiàn)較差。Golfarelli等［20］將基于圖優(yōu)化的SLAM 系統(tǒng)看作是質(zhì)量—彈簧模型系統(tǒng)，將SLAM 問(wèn)題轉(zhuǎn)換為最小二乘問(wèn)題；但當(dāng)彈簧受力變形時(shí)，系統(tǒng)是不穩(wěn)定的，且不適用于大范圍的復(fù)雜環(huán)境。

雖然視覺(jué)傳感器和激光傳感器是SLAM 算法中最常用的兩種傳感器，但是這些設(shè)備往往價(jià)格比較昂貴，而且受周?chē)h(huán)境變化的干擾較大。與激光傳感器和視覺(jué)傳感器相比，無(wú)線傳感器在大范圍復(fù)雜環(huán)境的情況下有更好的表現(xiàn)，其安裝簡(jiǎn)便，價(jià)格便宜；并且，WiFi 的廣泛普及給室內(nèi)定位提供了新的機(jī)遇。市場(chǎng)上大量的移動(dòng)通信設(shè)備（智能手機(jī)、筆記本電腦）都內(nèi)嵌了廉價(jià)多樣的傳感器，例如WiFi 模塊、運(yùn)動(dòng)傳感器、全球定位系統(tǒng)（Global Positioning System，GPS）和攝像頭等。而大部分室內(nèi)環(huán)境都部署有WiFi 接入點(diǎn)（Access Point，AP），利用WiFi 進(jìn)行SLAM 具有了很大的可行性。Ferris等［21］使用高斯過(guò)程潛變量模型將高維信號(hào)強(qiáng)度映射到二維空間，從而實(shí)現(xiàn)WiFi SLAM 的系統(tǒng)構(gòu)建。Huang等［22］提出利用圖優(yōu)化SLAM 方法來(lái)解決WiFi SLAM 的問(wèn)題，該方法通過(guò)高斯模型構(gòu)建信號(hào)強(qiáng)度地圖，但設(shè)備的差異以及應(yīng)用環(huán)境的不同為高斯模型的建立帶來(lái)了極大的困難。因此研究人員融合慣性測(cè)量單元（Inertial Measurement Unit，IMU）信息和指紋信息構(gòu)建指紋地圖，例如：Gu等［23］將腳戴式慣性定位系統(tǒng)與圖優(yōu)化SLAM 相結(jié)合進(jìn)行指紋地圖的構(gòu)建；但該方法需要使用者在腳上單獨(dú)佩戴IMU，不利于大范圍的推廣使用。Liu等［24］提出利用WiFi 指紋閉環(huán)和航位推算信息解決SLAM 問(wèn)題；但運(yùn)行時(shí)間較長(zhǎng)時(shí)，航位推算與指紋閉環(huán)會(huì)出現(xiàn)不可避免的偏差。

針對(duì)大范圍復(fù)雜環(huán)境下的圖優(yōu)化SLAM 問(wèn)題，為提高指紋閉環(huán)的準(zhǔn)確性，本文提出了基于WiFi 指紋序列匹配的圖優(yōu)化SLAM 算法。本文的主要工作如下：1）采用動(dòng)態(tài)時(shí)間規(guī)劃（Dynamic Time Warping，DTW）算法實(shí)現(xiàn)WiFi 指紋序列的匹配，保證了閉環(huán)檢測(cè)的準(zhǔn)確性；2）結(jié)合WiFi 指紋信息進(jìn)行圖優(yōu)化SLAM，滿(mǎn)足了SLAM 在大范圍復(fù)雜環(huán)境下的算法精度要求。本文采用兩組實(shí)驗(yàn)數(shù)據(jù)對(duì)算法進(jìn)行驗(yàn)證，結(jié)果表明：與高斯相似度的方法相比，本文算法第一組數(shù)據(jù)的精度提高了22.94%；第二組數(shù)據(jù)的精度提高了39.18%。

1 算法設(shè)計(jì)

本文算法主要分為兩部分：1）不確定性模型的生成；2）Graph-based SLAM 算法。整個(gè)算法的流程如圖1 所示。

首先，利用高斯相似度來(lái)計(jì)算兩個(gè)近距離指紋點(diǎn)的相似度，并通過(guò)一個(gè)訓(xùn)練過(guò)程來(lái)建立相似度和距離的不確定性模型；然后，計(jì)算兩個(gè)遠(yuǎn)距離指紋點(diǎn)的相似度，如果其相似度大于某一閾值，得到一個(gè)高斯閉環(huán)，并將這兩個(gè)指紋點(diǎn)作為兩個(gè)指紋序列的起點(diǎn)，采用DTW 算法計(jì)算兩個(gè)指紋序列的相似度，如果指紋序列的相似度滿(mǎn)足一定閾值，再得到一個(gè)指紋序列的閉環(huán)；最后，將高斯閉環(huán)與指紋序列閉環(huán)作為約束加入到位姿圖中，利用圖優(yōu)化算法對(duì)所構(gòu)建的位姿圖進(jìn)行優(yōu)化，得到優(yōu)化后的機(jī)器人軌跡。

1.1 Graph-based SLAM算法

本文在未知環(huán)境中利用WiFi 數(shù)據(jù)進(jìn)行SLAM，通過(guò)智能設(shè)備在運(yùn)動(dòng)過(guò)程中重復(fù)觀測(cè)到的地圖特征（WiFi 指紋）對(duì)自身進(jìn)行定位?；趫D優(yōu)化的SLAM 從采集的傳感器數(shù)據(jù)中構(gòu)建一個(gè)位姿圖，其中頂點(diǎn)表示位姿，邊用于表示頂點(diǎn)之間的約束。約束分為兩種：一種約束由連續(xù)時(shí)刻的里程計(jì)數(shù)據(jù)得到；另外一種約束可通過(guò)不同時(shí)刻觀測(cè)數(shù)據(jù)的匹配得到（又稱(chēng)為閉環(huán)檢測(cè)）。因?yàn)橛^測(cè)數(shù)據(jù)有一定的誤差，所有的約束都附加有一個(gè)不確定性參數(shù)，由此將基于圖優(yōu)化的SLAM問(wèn)題轉(zhuǎn)化為尋找優(yōu)化位姿使得約束帶來(lái)的誤差最小。

用(xt，yt)表示t時(shí)刻機(jī)器人的二維位置信息，θt表示方位信息。用C表示所構(gòu)建約束的集合。Graph-based SLAM的目的是找到位姿的最佳配置，從而最小化以下函數(shù)：

其中：zij代表頂點(diǎn)i和j之間的剛體變換；Σ表示此變換不確定性的協(xié)方差矩陣。對(duì)于激光雷達(dá)，變換zij可以通過(guò)掃描匹配（例如ICP 算法）進(jìn)行估算；對(duì)于視覺(jué)傳感器，這一變換可以通過(guò)特征匹配來(lái)獲取。給定某一AP 的接收信號(hào)強(qiáng)度（Received Signal Strength，RSS），可以很容易判定某一用戶(hù)是否重新訪問(wèn)了某塊區(qū)域，因?yàn)槊總€(gè)AP 具有唯一的硬件地址。但是通過(guò)信號(hào)強(qiáng)度估算兩個(gè)測(cè)量點(diǎn)的相對(duì)位置關(guān)系則非常困難，因?yàn)镽SS 本身沒(méi)有包含任何距離或方向信息。

信號(hào)強(qiáng)度往往與距離有很大的關(guān)聯(lián)，因此一些學(xué)者通過(guò)信號(hào)衰減模型來(lái)推算距離，典型的系統(tǒng)包括WiFi SLAM［25］和一些基于信號(hào)衰弱模型的室內(nèi)定位系統(tǒng)。但是這一模型的獲取非常困難，由于制造廠商的不一樣，每一個(gè)AP 的參數(shù)都不一樣；另外多徑效應(yīng)和環(huán)境中的物體對(duì)信號(hào)的傳播都有很大的影響。使用位置指紋的方法來(lái)進(jìn)行閉環(huán)檢測(cè)可以克服環(huán)境對(duì)信號(hào)傳播的影響，某個(gè)指紋點(diǎn)描述了在某一位置檢測(cè)到的AP 以及對(duì)應(yīng)的信號(hào)強(qiáng)度；就如人體的指紋和DNA（Deoxyribo Nucleic Acid）一樣，WiFi 指紋可以作為特征用于唯一地描述某一位置，而兩個(gè)位置的遠(yuǎn)近可以通過(guò)指紋點(diǎn)對(duì)的相似度來(lái)進(jìn)行衡量。但在大范圍的復(fù)雜環(huán)境下，僅僅采用指紋點(diǎn)對(duì)的匹配遠(yuǎn)遠(yuǎn)不夠，由于高斯相似度計(jì)算方法只考慮了兩個(gè)指紋點(diǎn)所掃描到的相同AP，因此可能導(dǎo)致指紋點(diǎn)對(duì)的匹配會(huì)出現(xiàn)閉環(huán)的誤判（距離很近的兩點(diǎn)但相似度卻很低），將傳統(tǒng)的基于指紋點(diǎn)對(duì)的匹配擴(kuò)展到指紋序列的匹配，可以減小閉環(huán)誤判的幾率，從而確保算法的準(zhǔn)確性。

1.2 WiFi指紋序列的匹配

由于指紋序列中包含多個(gè)指紋數(shù)據(jù)，信息量比單個(gè)指紋點(diǎn)對(duì)的數(shù)據(jù)豐富，所以，提出通過(guò)WiFi 指紋序列的匹配來(lái)進(jìn)行閉環(huán)檢測(cè)，利用DTW 算法實(shí)現(xiàn)兩個(gè)指紋序列的閉環(huán)檢測(cè)，在很大程度上可以減小定位誤差。DTW 算法是一種彈性匹配算法，通過(guò)對(duì)兩個(gè)指紋序列進(jìn)行相互關(guān)系的搜索，運(yùn)用動(dòng)態(tài)規(guī)劃思想調(diào)整兩序列之間的對(duì)應(yīng)關(guān)系，獲取一條最優(yōu)路徑，使沿該路徑兩序列間的相似度最大。

首先采用高斯函數(shù)計(jì)算兩個(gè)指紋點(diǎn)的相似度，將t時(shí)刻的指紋表示為Ft=(ft，xt)，其中xt=(xt，yt，θt)表示機(jī)器人在t時(shí)刻的里程計(jì)位姿；ft表示位置xt處采集的指紋信息。具體來(lái)說(shuō)，指紋信息包括t時(shí)刻所掃描到AP 的硬件地址以及信號(hào)強(qiáng)度f(wàn)t=(ft，1，ft，2，…，ft，Z)，其中Z表示t時(shí)刻掃描到AP 的數(shù)目。使用高斯函數(shù)來(lái)計(jì)算兩個(gè)指紋點(diǎn)Fi和Fj的相似度：

如果兩個(gè)指紋點(diǎn)的高斯相似度大于一定的閾值vs，將這兩個(gè)指紋點(diǎn)作為兩個(gè)指紋序列的起點(diǎn)，通過(guò)計(jì)算這兩個(gè)指紋序列的相似度來(lái)進(jìn)行閉環(huán)檢測(cè)。假設(shè)兩個(gè)指紋序列分別為L(zhǎng)=l1，l2，…，ln和K=k1，k2，…，km，為了計(jì)算這兩個(gè)指紋序列的相似度，首先是要找到兩個(gè)序列之間的對(duì)應(yīng)關(guān)系，以指紋序列L為橫軸，指紋序列K為縱軸，構(gòu)造一個(gè)n×m的相似度矩陣S，見(jiàn)式（3）。在相似度矩陣中，兩個(gè)指紋序列存在多條對(duì)應(yīng)的路徑，以其中的一條為例，用黑點(diǎn)來(lái)表示該路徑，如圖2 所示，即W=w1，w2，…，wr-1，wr，wr+1，…，wR-1，wR，R表示路徑W的長(zhǎng)度，滿(mǎn)足max(n，m) ≤R≤n+m-1；其中wk為該路徑第r個(gè)點(diǎn)的坐標(biāo)wr=(ir，jr)，它表示指紋序列L的第ir個(gè)指紋點(diǎn)與指紋序列K的第jr個(gè)指紋點(diǎn)相對(duì)應(yīng)。則兩點(diǎn)間的相似度可得兩指紋序列所有對(duì)應(yīng)點(diǎn)間的相似度矩陣S為：

相似度矩陣中的有效路徑需滿(mǎn)足以下約束條件，其約束條件示意圖如圖3 所示。

1）邊界條件：起點(diǎn)為S(1，1)，終點(diǎn)為S(n，m)，也就是說(shuō)，有效路徑必定是從左下角出發(fā)，在右上角結(jié)束。

2）連續(xù)性：圖3 中的路徑從wr到下一點(diǎn)wr+1，需滿(mǎn)足ir+1-ir≤1，jr+1-jr≤1。如圖3 中的圓圈Ⅰ所示，為保證連續(xù)性，當(dāng)沿該路徑至點(diǎn)S(i，j)時(shí)，前一點(diǎn)須經(jīng)過(guò)S(i-1，j-1)、S(i-1，j)、S(i，j-1)其中的一點(diǎn)，也就是說(shuō)某個(gè)時(shí)刻的點(diǎn)只能和相同時(shí)刻以及相鄰時(shí)刻的點(diǎn)匹配，無(wú)法跨越匹配，如圖3 中的圓圈Ⅱ處所示。

3）單調(diào)性：從wr到下一點(diǎn)wr+1時(shí)，需滿(mǎn)足ir≤ir+1、jr≤jr+1，這使得路徑必須隨著時(shí)間單調(diào)進(jìn)行，不能出現(xiàn)時(shí)間上倒退的現(xiàn)象，如圖3 中的圓圈Ⅲ處所示。

顯然滿(mǎn)足約束條件的有效路徑W有多條，而DTW 算法需要尋找其中的最優(yōu)路徑，使沿該路徑兩序列間的相似度累計(jì)值最大。用Sseq(L，K)來(lái)表示指紋序列L和K之間的相似度，即最優(yōu)路徑所對(duì)應(yīng)的相似度。Sseq(L，K)的求解過(guò)程為：

其中：r(i，j)表示在相似度矩陣S中從S(1，1)出發(fā)到S(i，j)路徑上的相似度累計(jì)值；Sgauss(i，j)表示當(dāng)前格點(diǎn)S(i，j)對(duì)應(yīng)的高斯相似度Sgauss(li，kj)，也就是li和kj兩指紋點(diǎn)間的高斯相似度。要到達(dá)點(diǎn)S(i，j)，只能從S(i-1，j-1)、S(i-1，j)或S(i，j-1)出發(fā)，max{r(i-1，j-1)，r(i-1，j)，r(i，j-1)}表示選擇S(i-1，j-1)、S(i-1，j)和S(i，j-1)三點(diǎn)中相似度累計(jì)值最大的一個(gè)點(diǎn)作為出發(fā)點(diǎn)。重復(fù)上述過(guò)程，遍歷整個(gè)相似度矩陣S，就可以找到一條從S(1，1)出發(fā)到S(n，m)的最優(yōu)路徑，最終得到r(n，m)的值，也就得到了指紋序列L和K之間的相似度Sseq(L，K)。

在求解Sseq(L，K) 之后，得到的最優(yōu)路徑用來(lái)表示，所以 兩個(gè)指紋序列L和K之間的相似度可以表示為：

如果兩個(gè)指紋序列的相似度值高于某一閾值vs，認(rèn)為這兩個(gè)位置是同一處，從而檢測(cè)到了一個(gè)指紋序列閉環(huán)。然而，在實(shí)際過(guò)程中，這兩個(gè)位置往往不可能在同一處，因此通過(guò)增加一個(gè)不確定性協(xié)方差參數(shù)來(lái)對(duì)這一誤差進(jìn)行補(bǔ)償。不確定性協(xié)方差矩陣通常是一個(gè)對(duì)稱(chēng)矩陣，在實(shí)際的應(yīng)用中可以選擇一個(gè)數(shù)值很大的對(duì)角矩陣，對(duì)角陣元素的大小表明對(duì)這一誤差的忽視程度。如果兩指紋序列的相似度很高，并且對(duì)應(yīng)的地面真實(shí)值小于3 m，表明兩個(gè)指紋序列的實(shí)際距離很近，認(rèn)為此時(shí)的誤差很小，可以對(duì)其忽視，所以將對(duì)角陣元素設(shè)為1；由于將高斯閉環(huán)與指紋序列閉環(huán)都作為約束加入到位姿圖中，所以對(duì)于高斯閉環(huán)與指紋序列閉環(huán)，把對(duì)角陣元素設(shè)為相似度樣本方差的倒數(shù)。為了提高系統(tǒng)的精度，并且得到相似度樣本的均值與方差，通過(guò)一個(gè)訓(xùn)練過(guò)程來(lái)建立相似度和距離的不確定性模型。

1.3 不確定性模型的訓(xùn)練

Graph-based SLAM 中的每一個(gè)邊都需要指定其不確定性，對(duì)于里程計(jì)所構(gòu)建的邊，其不確定性可以由運(yùn)動(dòng)模型得到。為了提高系統(tǒng)的精度，通過(guò)一個(gè)訓(xùn)練過(guò)程來(lái)建立相似度和距離的不確定性模型。雖然里程計(jì)在長(zhǎng)時(shí)間內(nèi)會(huì)有累計(jì)誤差，但里程計(jì)在短時(shí)間內(nèi)是非常精確的，據(jù)所知一般里程計(jì)在30 m 的范圍仍然很精確。因此計(jì)算所有距離小于30 m指紋的相似性，這樣得到了H個(gè)訓(xùn)練數(shù)據(jù)表示兩個(gè)指紋的相似度，dh表示兩個(gè)指紋之間的距離。然后使用分箱方法（binning）來(lái)對(duì)樣本進(jìn)行訓(xùn)練，得到一個(gè)模型用于表示某一相似度對(duì)應(yīng)距離的不確定性。也就是說(shuō)，給定一個(gè)相似度S，計(jì)算與相似度S間隔為b的所有樣本的均值與方差var(s)，通過(guò)均值與方差計(jì)算式（1）中的協(xié)方差矩陣Σ。

其中：c(b)統(tǒng)計(jì)相似度落在間隔b的樣本的個(gè)數(shù)。通過(guò)此方法建立的不確定性模型可以參見(jiàn)圖4。

2 實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證本文算法的性能，在新加坡南洋理工大學(xué)的2 號(hào)地下停車(chē)場(chǎng)（約5 100 m2）進(jìn)行了兩組實(shí)驗(yàn)。設(shè)計(jì)數(shù)據(jù)采集平臺(tái)如圖5 所示，選取機(jī)器人開(kāi)發(fā)平臺(tái)Clearpath Husky Automated Guided Vehicle 作為實(shí)驗(yàn)驗(yàn)證平臺(tái)。機(jī)器人通過(guò)底盤(pán)得到里程計(jì)信息，數(shù)據(jù)采集頻率設(shè)為10 Hz。在該機(jī)器人身上配置一個(gè)激光傳感器Hokuyo UST-20LX 和5 個(gè)小米Max 3 智能手機(jī)，激光傳感器用于實(shí)現(xiàn)自適應(yīng)蒙特卡洛定位（Adaptive Monte Carlo Localization，AMCL），并將其所得姿態(tài)作為移動(dòng)機(jī)器人在環(huán)境中的地面真實(shí)值；小米手機(jī)以0.5 Hz的頻率采集WiFi 指紋數(shù)據(jù)。機(jī)器人從不同的起點(diǎn)開(kāi)始，以0.4 m/s 的平均速度移動(dòng)，共采集了兩組實(shí)驗(yàn)數(shù)據(jù)。

在Graph-based SLAM中，不同的閉環(huán)檢測(cè)方法對(duì)算法結(jié)果有很大的影響。本文以定位誤差（即優(yōu)化后的機(jī)器人軌跡與真實(shí)值的距離差值）衡量算法精度，定位誤差越小，算法精度越高。首先，將指紋序列閉環(huán)的方法（n=20，m=25）與僅用高斯閉環(huán)的方法相比較，實(shí)驗(yàn)結(jié)果見(jiàn)表1。從表1 可以看出，vs太大或太小都會(huì)導(dǎo)致算法精度下降，對(duì)于不同的實(shí)驗(yàn)數(shù)據(jù)，其最適的相似度閾值也不同。當(dāng)vs大于等于0.70時(shí)，系統(tǒng)還沒(méi)有檢測(cè)到閉環(huán)，導(dǎo)致其誤差較大，都為18.99 m（第一組數(shù)據(jù)）和10.12 m（第二組數(shù)據(jù)）；當(dāng)vs=0.10時(shí)，第一組數(shù)據(jù)的SLAM 算法精度最高，可以達(dá)到3.23 m；當(dāng)vs=0.20時(shí)，第二組數(shù)據(jù)的SLAM 算法精度最高，可以達(dá)到2.67 m。然后，將高斯閉環(huán)與指紋序列閉環(huán)都作為約束加入到位姿圖中，使用該方法所得結(jié)果：第一組數(shù)據(jù)為3.09 m，與僅使用高斯的方法（精度為4.01 m）相比提高了22.94%；第二組數(shù)據(jù)為2.36 m，與僅使用高斯的方法（精度為3.88 m）相比提高了39.18%。

為了驗(yàn)證實(shí)驗(yàn)環(huán)境在受到干擾之后其算法精度是否會(huì)受到影響，對(duì)原始的WiFi 信號(hào)強(qiáng)度加了3 種干擾：1）在原本所掃描到的AP 基礎(chǔ)上刪除某部分AP，實(shí)驗(yàn)一共采集了8 614 個(gè)指紋點(diǎn)，每個(gè)指紋點(diǎn)所掃描到AP 的個(gè)數(shù)在（40，130），所以在每個(gè)指紋點(diǎn)所掃描AP 中隨機(jī)刪除5 個(gè)AP；2）對(duì)信號(hào)強(qiáng)度增加方差為3 的高斯噪聲；3）對(duì)信號(hào)強(qiáng)度增加方差為5 的高斯噪聲。實(shí)驗(yàn)結(jié)果見(jiàn)表1，在3 種干擾情況之下，第一組實(shí)驗(yàn)數(shù)據(jù)得到的最優(yōu)SLAM 算法精度為3.35 m、3.02 m 和3.11 m，第二組實(shí)驗(yàn)數(shù)據(jù)得到的最優(yōu)SLAM 算法精度為2.74 m、2.41 m 和2.61 m。實(shí)驗(yàn)結(jié)果表明，整個(gè)SLAM 算法在環(huán)境受到干擾的情況下，其穩(wěn)定性較高，雖然對(duì)原始的WiFi 數(shù)據(jù)（信號(hào)強(qiáng)度、AP 的數(shù)目）做了一定的處理，但因?yàn)楹笃谟锌煽康闹讣y閉環(huán)檢測(cè)對(duì)數(shù)據(jù)的偏差進(jìn)行消除，再加上圖優(yōu)化算法對(duì)機(jī)器人的軌跡進(jìn)行優(yōu)化，所以整個(gè)SLAM 算法的適用性和穩(wěn)定性得到了驗(yàn)證。

表1 不同的閉環(huán)檢測(cè)方法與vs對(duì)SLAM算法精度的影響Tab.1 Influence of different loop closure detection methods and vs on SLAM algorithm accuracy

從表2 可以看出，指紋序列對(duì)應(yīng)的閉環(huán)數(shù)目少于高斯對(duì)應(yīng)的閉環(huán)數(shù)目，這是由于指紋序列包含了多個(gè)連續(xù)指紋點(diǎn)，原來(lái)的單個(gè)指紋點(diǎn)即可組成一個(gè)閉環(huán)，而現(xiàn)在需要多個(gè)指紋點(diǎn)才能組成一個(gè)閉環(huán)，也就導(dǎo)致算法得到的閉環(huán)數(shù)量大大減少，因此加入位姿圖中的“指紋-閉環(huán)”邊也就更少。并且，相似度閾值vs也對(duì)閉環(huán)數(shù)據(jù)有一定的影響，相似度閾值vs越小，閉環(huán)數(shù)目越多，相似度閾值vs越大，其閉環(huán)數(shù)目也越少，當(dāng)相似度閾值vs大于等于0.70時(shí)，系統(tǒng)檢測(cè)不到閉環(huán)，所以閉環(huán)數(shù)目為0。

表2 不同的閉環(huán)檢測(cè)方法與vs對(duì)閉環(huán)數(shù)目的影響Tab.2 Influence of loop closure detection methods and vs on loop closure number

換言之，運(yùn)用指紋序列的方法，通過(guò)減少閉環(huán)數(shù)目來(lái)減小閉環(huán)誤判的幾率，從而保證了圖優(yōu)化結(jié)果的正確性。如圖6所示，將優(yōu)化后的機(jī)器人軌跡與地面真實(shí)值作對(duì)比，通過(guò)兩組實(shí)驗(yàn)數(shù)據(jù)的結(jié)果可以看出優(yōu)化后的機(jī)器人軌跡與地面真實(shí)值基本一致。其中高斯+指紋序列與地面真實(shí)值最接近。

除此之外，還研究了指紋序列的長(zhǎng)度對(duì)所提算法精度的影響。采用高斯+指紋序列的方法，分別取5 組數(shù)據(jù)，其對(duì)應(yīng)的結(jié)果如表3 所示，從表3 數(shù)據(jù)可以看出，當(dāng)n=20且m=25時(shí)，其算法精度最高。當(dāng)指紋序列太短時(shí)，因?yàn)槠浒闹讣y信息量不夠，導(dǎo)致閉環(huán)檢測(cè)不準(zhǔn)確。當(dāng)指紋序列的長(zhǎng)度太長(zhǎng)時(shí)，由于指紋數(shù)據(jù)量太大會(huì)增加計(jì)算復(fù)雜度，從而導(dǎo)致SLAM 算法精度降低。在對(duì)實(shí)驗(yàn)數(shù)據(jù)加3 種干擾的情況下，第一組實(shí)驗(yàn)數(shù)據(jù)得到的最優(yōu)SLAM 算法精度為3.29 m、3.02 m 和3.11 m，第二組實(shí)驗(yàn)數(shù)據(jù)得到的最優(yōu)SLAM 算法精度為2.74 m、2.39 m 和2.58 m。實(shí)驗(yàn)結(jié)果表明，優(yōu)化后的算法在受干擾的情況下的穩(wěn)定性較高，雖然最優(yōu)算法精度對(duì)應(yīng)的序列長(zhǎng)度值有所變化，但與基于單個(gè)指紋點(diǎn)的高斯方法相比，仍然有很大提高。

表3 高斯+指紋序列方法下指紋序列的長(zhǎng)度對(duì)SLAM算法精度的影響 單位：mTab.3 Influence of length of fingerprint sequence on SLAM algorithm accuracy under Gauss+fingerprint sequence method unit：m

3 結(jié)語(yǔ)

本文提出了基于WiFi 指紋序列匹配的圖優(yōu)化SLAM 算法。首先在機(jī)器人上搭載小米手機(jī)采集WiFi 指紋信息，通過(guò)DTW 算法實(shí)現(xiàn)指紋序列的閉環(huán)檢測(cè)，然后將高斯閉環(huán)和指紋序列的閉環(huán)都作為約束加入到位姿圖中，并通過(guò)圖優(yōu)化算法對(duì)機(jī)器人的軌跡進(jìn)行優(yōu)化，最后得到優(yōu)化后的機(jī)器人軌跡。通過(guò)兩組實(shí)驗(yàn)數(shù)據(jù)對(duì)所提算法進(jìn)行了驗(yàn)證，定位精度均有很大提高，分別可以達(dá)到3.09 m 和2.36 m，與僅使用高斯的方法相比提高了22.94%和39.18%?？梢悦黠@看出，本文算法在大范圍復(fù)雜環(huán)境下極大地提高了SLAM 精度。在后續(xù)的研究工作中，考慮加入其他傳感器信息的融合獲取更高的定位精度。