基于WiFi指紋序列匹配的機器人同步定位與地圖構建

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

（西南科技大學 信息工程學院，四川 綿陽 621010）

0 引言

近年來，同步定位與地圖構建（Simultaneous Localization And Mapping，SLAM）逐漸成為機器人領域研究的熱點問題，被認為是實現移動機器人自主化的核心技術［1-2］。Standford大學的Smith等［3］首先發表了關于SLAM 問題的開創性論文。針對SLAM 問題，國內外研究人員提出了很多解決方案，其中基于圖優化的SLAM（Graph-based SLAM）把這一問題轉化為最大似然估計，被認為是目前解決SLAM 最有效的方法之一［4-8］。閉環檢測是實現圖優化SLAM 的關鍵，也是SLAM 問題的一個難點。正確的閉環檢測可以修正里程計的累計誤差，從而得到一致性地圖；錯誤的閉環檢測結果則會對后續圖優化造成干擾，導致地圖創建的失敗［9］。

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

早期的SLAM 研究是通過濾波實現機器人的狀態估計。基于濾波器的方法主要是利用遞歸貝葉斯估計原理，在假定機器人所有時刻的觀測信息以及控制信息已知的條件下，對系統狀態的后驗概率進行估計。根據后驗概率表示方式的不同，常用的濾波方法包括擴展卡爾曼濾波器［13］、Rao-Blackwellised 粒子濾波器（Rao-Blackwellised Particle Filter，RBPF）［14］等。基于濾波器的方法存在更新效率低的問題，難以應用于大規模環境的地圖創建［15-16］。

隨著SLAM 的發展，基于濾波器的方法逐漸被基于圖優化的方法取代。基于圖優化的SLAM 方法采用了全局優化理論，能夠獲得更好的建圖效果。Kschischang等［17］在2001年提出利用因子圖對SLAM 問題進行建模。Carlevaris-Bianco等［18］提出了通過刪除圖的節點來降低后端優化計算的復雜性，節點被刪除的順序可能會影響圖的連通性。Vallvé等［19］結合因子圖建模與矩陣稀疏化理論，提出了節點下降優化方法，減少迭代的次數，但其收斂性不高，在大范圍的復雜環境下表現較差。Golfarelli等［20］將基于圖優化的SLAM 系統看作是質量—彈簧模型系統，將SLAM 問題轉換為最小二乘問題；但當彈簧受力變形時，系統是不穩定的，且不適用于大范圍的復雜環境。

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

針對大范圍復雜環境下的圖優化SLAM 問題，為提高指紋閉環的準確性，本文提出了基于WiFi 指紋序列匹配的圖優化SLAM 算法。本文的主要工作如下：1）采用動態時間規劃（Dynamic Time Warping，DTW）算法實現WiFi 指紋序列的匹配，保證了閉環檢測的準確性；2）結合WiFi 指紋信息進行圖優化SLAM，滿足了SLAM 在大范圍復雜環境下的算法精度要求。本文采用兩組實驗數據對算法進行驗證，結果表明：與高斯相似度的方法相比，本文算法第一組數據的精度提高了22.94%；第二組數據的精度提高了39.18%。

1 算法設計

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

首先，利用高斯相似度來計算兩個近距離指紋點的相似度，并通過一個訓練過程來建立相似度和距離的不確定性模型；然后，計算兩個遠距離指紋點的相似度，如果其相似度大于某一閾值，得到一個高斯閉環，并將這兩個指紋點作為兩個指紋序列的起點，采用DTW 算法計算兩個指紋序列的相似度，如果指紋序列的相似度滿足一定閾值，再得到一個指紋序列的閉環；最后，將高斯閉環與指紋序列閉環作為約束加入到位姿圖中，利用圖優化算法對所構建的位姿圖進行優化，得到優化后的機器人軌跡。

1.1 Graph-based SLAM算法

本文在未知環境中利用WiFi 數據進行SLAM，通過智能設備在運動過程中重復觀測到的地圖特征（WiFi 指紋）對自身進行定位。基于圖優化的SLAM 從采集的傳感器數據中構建一個位姿圖，其中頂點表示位姿，邊用于表示頂點之間的約束。約束分為兩種：一種約束由連續時刻的里程計數據得到；另外一種約束可通過不同時刻觀測數據的匹配得到（又稱為閉環檢測）。因為觀測數據有一定的誤差，所有的約束都附加有一個不確定性參數，由此將基于圖優化的SLAM問題轉化為尋找優化位姿使得約束帶來的誤差最小。

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

其中：zij代表頂點i和j之間的剛體變換；Σ表示此變換不確定性的協方差矩陣。對于激光雷達，變換zij可以通過掃描匹配（例如ICP 算法）進行估算；對于視覺傳感器，這一變換可以通過特征匹配來獲取。給定某一AP 的接收信號強度（Received Signal Strength，RSS），可以很容易判定某一用戶是否重新訪問了某塊區域，因為每個AP 具有唯一的硬件地址。但是通過信號強度估算兩個測量點的相對位置關系則非常困難，因為RSS 本身沒有包含任何距離或方向信息。

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

1.2 WiFi指紋序列的匹配

由于指紋序列中包含多個指紋數據，信息量比單個指紋點對的數據豐富，所以，提出通過WiFi 指紋序列的匹配來進行閉環檢測，利用DTW 算法實現兩個指紋序列的閉環檢測，在很大程度上可以減小定位誤差。DTW 算法是一種彈性匹配算法，通過對兩個指紋序列進行相互關系的搜索，運用動態規劃思想調整兩序列之間的對應關系，獲取一條最優路徑，使沿該路徑兩序列間的相似度最大。

首先采用高斯函數計算兩個指紋點的相似度，將t時刻的指紋表示為Ft=(ft，xt)，其中xt=(xt，yt，θt)表示機器人在t時刻的里程計位姿；ft表示位置xt處采集的指紋信息。具體來說，指紋信息包括t時刻所掃描到AP 的硬件地址以及信號強度ft=(ft，1，ft，2，…，ft，Z)，其中Z表示t時刻掃描到AP 的數目。使用高斯函數來計算兩個指紋點Fi和Fj的相似度：

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

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

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

2）連續性：圖3 中的路徑從wr到下一點wr+1，需滿足ir+1-ir≤1，jr+1-jr≤1。如圖3 中的圓圈Ⅰ所示，為保證連續性，當沿該路徑至點S(i，j)時，前一點須經過S(i-1，j-1)、S(i-1，j)、S(i，j-1)其中的一點，也就是說某個時刻的點只能和相同時刻以及相鄰時刻的點匹配，無法跨越匹配，如圖3 中的圓圈Ⅱ處所示。

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

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

其中：r(i，j)表示在相似度矩陣S中從S(1，1)出發到S(i，j)路徑上的相似度累計值；Sgauss(i，j)表示當前格點S(i，j)對應的高斯相似度Sgauss(li，kj)，也就是li和kj兩指紋點間的高斯相似度。要到達點S(i，j)，只能從S(i-1，j-1)、S(i-1，j)或S(i，j-1)出發，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)三點中相似度累計值最大的一個點作為出發點。重復上述過程，遍歷整個相似度矩陣S，就可以找到一條從S(1，1)出發到S(n，m)的最優路徑，最終得到r(n，m)的值，也就得到了指紋序列L和K之間的相似度Sseq(L，K)。

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

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

1.3 不確定性模型的訓練

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

其中：c(b)統計相似度落在間隔b的樣本的個數。通過此方法建立的不確定性模型可以參見圖4。

2 實驗與結果分析

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

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

為了驗證實驗環境在受到干擾之后其算法精度是否會受到影響，對原始的WiFi 信號強度加了3 種干擾：1）在原本所掃描到的AP 基礎上刪除某部分AP，實驗一共采集了8 614 個指紋點，每個指紋點所掃描到AP 的個數在（40，130），所以在每個指紋點所掃描AP 中隨機刪除5 個AP；2）對信號強度增加方差為3 的高斯噪聲；3）對信號強度增加方差為5 的高斯噪聲。實驗結果見表1，在3 種干擾情況之下，第一組實驗數據得到的最優SLAM 算法精度為3.35 m、3.02 m 和3.11 m，第二組實驗數據得到的最優SLAM 算法精度為2.74 m、2.41 m 和2.61 m。實驗結果表明，整個SLAM 算法在環境受到干擾的情況下，其穩定性較高，雖然對原始的WiFi 數據（信號強度、AP 的數目）做了一定的處理，但因為后期有可靠的指紋閉環檢測對數據的偏差進行消除，再加上圖優化算法對機器人的軌跡進行優化，所以整個SLAM 算法的適用性和穩定性得到了驗證。

表1 不同的閉環檢測方法與vs對SLAM算法精度的影響Tab.1 Influence of different loop closure detection methods and vs on SLAM algorithm accuracy

從表2 可以看出，指紋序列對應的閉環數目少于高斯對應的閉環數目，這是由于指紋序列包含了多個連續指紋點，原來的單個指紋點即可組成一個閉環，而現在需要多個指紋點才能組成一個閉環，也就導致算法得到的閉環數量大大減少，因此加入位姿圖中的“指紋-閉環”邊也就更少。并且，相似度閾值vs也對閉環數據有一定的影響，相似度閾值vs越小，閉環數目越多，相似度閾值vs越大，其閉環數目也越少，當相似度閾值vs大于等于0.70時，系統檢測不到閉環，所以閉環數目為0。

表2 不同的閉環檢測方法與vs對閉環數目的影響Tab.2 Influence of loop closure detection methods and vs on loop closure number

換言之，運用指紋序列的方法，通過減少閉環數目來減小閉環誤判的幾率，從而保證了圖優化結果的正確性。如圖6所示，將優化后的機器人軌跡與地面真實值作對比，通過兩組實驗數據的結果可以看出優化后的機器人軌跡與地面真實值基本一致。其中高斯+指紋序列與地面真實值最接近。

除此之外，還研究了指紋序列的長度對所提算法精度的影響。采用高斯+指紋序列的方法，分別取5 組數據，其對應的結果如表3 所示，從表3 數據可以看出，當n=20且m=25時，其算法精度最高。當指紋序列太短時，因為其包含的指紋信息量不夠，導致閉環檢測不準確。當指紋序列的長度太長時，由于指紋數據量太大會增加計算復雜度，從而導致SLAM 算法精度降低。在對實驗數據加3 種干擾的情況下，第一組實驗數據得到的最優SLAM 算法精度為3.29 m、3.02 m 和3.11 m，第二組實驗數據得到的最優SLAM 算法精度為2.74 m、2.39 m 和2.58 m。實驗結果表明，優化后的算法在受干擾的情況下的穩定性較高，雖然最優算法精度對應的序列長度值有所變化，但與基于單個指紋點的高斯方法相比，仍然有很大提高。

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

3 結語

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