基于WIFI指紋定位的仿生SLAM研究

秦國威，孫新柱，陳孟元

（安徽工程大學(xué) 電氣傳動與控制重點實驗室，安徽 蕪湖 241000）

同步定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)是移動機器人目前面臨的重大難題，解決此問題就是在一個未知環(huán)境中創(chuàng)建地圖的過程。放置機器人處在一個陌生環(huán)境中，在探索環(huán)境，了解環(huán)境的同時，運用地圖追蹤機器人在該環(huán)境中的位置[1-4]。但是，在實際的復(fù)雜場景下，解決SLAM問題非常困難。因為移動機器人實質(zhì)上就是移動的傳感器平臺，傳感器雖然類型和能力各有不同，但是廣泛存在里程計漂移和不同的噪聲等問題[5]。后來經(jīng)過學(xué)者們的不斷探究，仿生機器人逐漸凸顯出良好的應(yīng)用前景，表現(xiàn)出完美的生物合理性和對自然環(huán)境的高度適應(yīng)性[6]。其中研究最多的為基于嚙齒類動物模型RatSLAM[7-10]，RatSLAM將路徑積分和視覺場景信息集成到位姿感知細胞模型中，從而使得移動機器人具備一定的更新預(yù)測能力，與此同時建立了存儲時間、空間以及視覺等信息的經(jīng)歷制圖算法，目前已經(jīng)廣泛用于機器人的定位導(dǎo)航工作中，解決了眾多SLAM難以解決的問題。但是RatSLAM模型獲取的視覺場景信息以及測距里程計信息均存在一定程度的誤差，文獻[11]針對視覺里程計的誤差引入FAB-MAP(fast appearance based mapping)，這種基于歷史模型的閉環(huán)檢測算法，通過實時關(guān)鍵幀的匹配，可以提高系統(tǒng)的穩(wěn)定性，但是定位的精度不穩(wěn)定。所以RatSLAM模型定位的精度有待進一步提高。

隨著科學(xué)技術(shù)的發(fā)展，目前提高定位精度的方法多種多樣，主流的定位技術(shù)有無線局域網(wǎng)(wireless fidelity, WIFI)、藍牙、無線個域網(wǎng)ZigBee、慣性定位技術(shù)和dici等[12]。文獻[13]針對提高室內(nèi)精度的需求,提出了基于地圖匹配技術(shù)和指紋技術(shù)的藍牙定位方法，但是此定位方法傳輸距離太近。文獻[14]針對現(xiàn)階段礦井井下難以實現(xiàn)真三維人員定位等問題,提出了一種將三維度可視化與Zigbee無線網(wǎng)絡(luò)技術(shù)相融合的新穎算法，即真三維的煤礦人員定位系統(tǒng)，然而此方法定位抗干擾的能力較差。文獻[15]為了精確地判斷設(shè)備是否在特定建筑物內(nèi)部或外部，提出了一種基于接收WIFI信號的二進制分類器，來推斷室內(nèi)或室外條件，精度較高，但此二進制分類器模型算法的復(fù)雜性較大，難以完成。文獻[16]針對地下礦山環(huán)境的復(fù)雜性，以及定位井下救援人員的困難性，對現(xiàn)有定位技術(shù)也進行了研究和比較，相比較而言，WIFI傳輸速率更快，定位精度更高，應(yīng)用的價值更大[17-19]。文獻[20]就RatSLAM定位精度問題，提出了將地磁場與RatSLAM相結(jié)合的方法，根據(jù)地磁區(qū)域的畸變，利用模板匹配定位的精度雖然比單獨的RatSLAM定位取得了更好的效果，但精度仍達不到預(yù)期。

綜合考慮,本文擬采用RatSLAM與WIFI結(jié)合的方法，解決原有RatSLAM定位精確度不高的問題。

1 RatSLAM模型

RatSLAM模型是將頭方向細胞和位置細胞合并，形成一個稱為位姿細胞的新細胞類型。頭方向細胞反映機器人的朝向，位置細胞則反映機器人的位置。機器人的位姿信息即被編碼在位姿感知細胞單獨的競爭性吸引子網(wǎng)絡(luò)(CAN)模塊中，經(jīng)過位姿感知細胞各自的加工處理，機器人自中心信息和非中心信息刺激影響位姿細胞的活動。RatSLAM完整模型的結(jié)構(gòu)如圖1所示。

圖1 RatSLAM系統(tǒng)結(jié)構(gòu)Fig. 1 RatSLAM system structure

1.1 位姿表征

RatSLAM模型為了形成位姿感知細胞群，將頭方向細胞和位置細胞模型化為位姿細胞的競爭性吸引子網(wǎng)絡(luò)結(jié)構(gòu)綜合起來形成一個三維立方體。位姿細胞以 (x ,y,θ)的樣式進行排列，可以同時實現(xiàn)機器人的多個位姿估計，如圖2所示。

圖2 3維位姿感知細胞模型Fig. 2 Model of three-dimensional posture-sensing cell

對于頭方向和位置感知網(wǎng)絡(luò)，競爭性吸引子網(wǎng)絡(luò)的動力學(xué)系統(tǒng)始終操縱位姿感知細胞網(wǎng)絡(luò)內(nèi)部的活動，內(nèi)部動態(tài)過程可分為以下3個階段。

1)興奮度更新

每個位姿感知細胞用興奮度權(quán)重矩陣來表述對該細胞矩陣中其他細胞的投射行為，創(chuàng)建的興奮度權(quán)重矩陣 εdef為 位姿細胞平面 (x ,y)中對應(yīng)著興奮性的一個二維高斯分布和 θ中對應(yīng)著一維分布的乘積，如式(1)所示：

式中： kxy和 kθ分 別為 (x ,y)平 面與 θ維 的方差常數(shù)；d、e 、 f是 x、 y、θ的分布系數(shù)。

因興奮性而使位姿感知細胞活動產(chǎn)生變化的表示如式(2):

式中： Nx、 Ny、 Nθ是 在 (x ,y,θ)空間中的位姿感知細胞3維矩陣元素。

2)全局抑制

因為多個活性包表示的多個位姿假設(shè)的完成需要一段時間，而且會不段加強，因此抑制是比較緩慢的，即在相當長的一段時間內(nèi)相互競爭的活性包會同時存在，全局抑制后的活動水平為

式中抑制常數(shù) δ控制全局抑制水平，興奮度水平僅限為一個非負數(shù)。

3)歸一化

歸一化就是保持位姿感知細胞在獲得視覺等輸入后的總活性，歸一化后姿態(tài)細胞的活性水平為

1.2 視覺場景的學(xué)習(xí)

局部視覺細胞和位姿感知細胞之間存在相互作用，通過建立局部場景細胞和位姿感知細胞之間相關(guān)的連接，將激活的局部場景細胞中的能量注入到位姿感知細胞中，局部場景與可用的機器人位置連接起來，被更新的連接強度則可表示為

式中： Vi為局部視覺場景細胞的活性水平； Qxyθ為位姿感知細胞的活性水平。

1.3 經(jīng)歷地圖的構(gòu)建

經(jīng)歷都具有一個活性水平，活性水平由位姿感知細胞和局部場景網(wǎng)絡(luò)中能量峰與每一個經(jīng)歷相關(guān)單元之間接近程度所決定。每一個經(jīng)歷在位姿感知細胞和視覺局部場景細胞中有一個相關(guān)的活性區(qū)域。當能量峰處于這些活性區(qū)域時，該經(jīng)歷立刻被激活，這些區(qū)域在位姿感知細胞內(nèi)部是連續(xù)的，而在視覺局部場景細胞中的相關(guān)區(qū)域卻是非連續(xù)的。一個經(jīng)歷能級 Exyθ和 第i個 經(jīng)歷的總能級水平 Ei由式(6)、(7)計算可得：

式中： xpc、 ypc和 θpc為最大活性姿態(tài)細胞的坐標；xi、yi、 θi為與該經(jīng)歷相關(guān)的位姿感知細胞的坐標；ra為(x,y)平 面的區(qū)域常數(shù)； θa為 θ維 上的區(qū)域常數(shù)； Vcurr為當前場景； Vi為 與經(jīng)歷i相關(guān)的場景。

2 WIFI與RatSLAM結(jié)合

2.1 WIFI指紋定位的原理

本文采用的WIFI定位方法為位置指紋定位方法。與通常意義下的指紋識別相似，位置指紋定位識別主要依靠表示目標特征的數(shù)據(jù)庫進行辨認?；谛盘枏姸?R SSI的位置指紋定位方式分為兩個階段：離線采集階段和在線定位階段。具體WIFI指紋定位原理圖如圖3所示。

圖3 WIFI指紋定位原理圖Fig. 3 WIFI positioning schematic

2.1.1 離線采集階段

離線采集階段的主要任務(wù)是采集定位區(qū)域各參考點的特征信息，建立位置指紋數(shù)據(jù)庫。首先繪制定位區(qū)域地圖，規(guī)劃一系列的測試參考點，依次測量各參考點來自不同AP的RSSI值(多次測量取平均值)作為該參考點的信號特征數(shù)，并按一定格式記錄在位置指紋數(shù)據(jù)庫中，該數(shù)據(jù)庫也稱為位置指紋地圖。

室內(nèi)環(huán)境下WIFI指紋圖的建立方法如下：在定位環(huán)境中按照一定規(guī)則選取參考點，并在每個參考點上對各個AP的信號強度連續(xù)采樣一段時間，得到每個AP的均值mLi存入數(shù)據(jù)庫中，構(gòu)成定位指紋圖IM:

式中：φ表示所有參考點的位置集合， φ ={L1,L2,···,Lk}，Li=(xi,yi)表 示參考點的位置, k為參考點的數(shù)量；A={AP1,AP2,···,APR}表示地圖中所有觀測到AP的組成集合； M ACi表 示第i個 參考點的 M AC地址值，M為定位指紋中所有均值的集合，即

式中 mLiAPj為第 j個 A P 在 參考點 Li處的均值。

2.1.2 在線定位階段

1)AP的選取策略

①假設(shè)室內(nèi)定位環(huán)境可用的參考點AP個數(shù)為T，選取其中S個AP的優(yōu)化子集則可以將信號空間的維度從T維降到S維，因而可以減少計算量。對于選取的S個AP進行兩兩組合，按照式(10)計算每個組合的互信息，查找出互信息最小的組合，其對應(yīng)的 A Pm、A Pn作為兩個初始參考點AP。

式中： M I(APm,APn)表示兩個不同AP的互信息；H(APm,APn)表示兩個AP的組合信息熵。

②按照式(11)分別計算余下的S–2個AP與兩個初始AP組合的互信息。

找出能使MI最小的AP作為最優(yōu)化AP子集的第3個AP。

③依次按照②的形式選取下一個最優(yōu)的AP，依次迭代，直到選取出S個最優(yōu)AP為止。第R個最優(yōu)的AP的選取算式為

2)貝葉斯位置估計策略

針對上述的互信息最小化AP選取策略，進一步使用貝葉斯后驗估計進行組合優(yōu)化，使得WIFI指紋定位算法的位置估計精度和可靠度大大提升。

貝葉斯后驗估計的基本原理為

式中： R SSI表 示多個 A P 在 位置估計點的 R SSI觀測值； p (Li|RSSI)表 示位置 Li在 給定 R SSI條件下的概率，即在觀測到 R SSI向量的情況下，定位點出現(xiàn)在Li的 概率； p (RSSI|Li)表 示 R SSI觀測值在給定位置Li條 件下的概率； p (Li)表 示位置 Li的概率，通常不考慮指紋點之間的差異，即指紋點等概率； p (RSSI)表示 R SSI出現(xiàn)的全概率，其算式為

式中： C (RSSI1,RSSI2,···,RSSIM)表示指紋點觀測到的指定RSSI向量的個數(shù)； K 表示指紋點觀測歷元數(shù)。

將上述全概率算式帶回貝葉斯后驗估計式，從而計算出后驗條件概率。采用多個指紋點的貝葉斯權(quán)重位置估計算式能夠在較短時間內(nèi)算出位置估計點的位置。

式中： ( x,y)表 示位置估計點的二維坐標； ( xi,yi)表示第i個指紋點的坐標; ωi表 示第i個指紋點的加權(quán)權(quán)重，即貝葉斯后驗條件的概率; K表示鄰近點個數(shù)。

2.2 WIFI與RatSLAM的融合

將無線信號網(wǎng)絡(luò)WIFI作為一種傳感器用到RatSLAM當中，如圖4所示，其定位模型由3個主要部分組成，分別為局部視覺網(wǎng)絡(luò)、位姿細胞網(wǎng)絡(luò)和經(jīng)歷制圖。局部視覺細胞網(wǎng)絡(luò)除了獲取場景的視覺信息外，還存儲環(huán)境WIFI信號強度的感應(yīng)快照，被稱為局部場景模板。局部視覺場景模板用來辨識熟悉的環(huán)境。當新輸入的局部場景的信息與已存在的局部場景模板匹配時，位姿細胞網(wǎng)絡(luò)的活性因子被激活，此時利用前文所述的WIFI指紋定位算法進行準確定位，二者結(jié)合能夠很大程度上阻止錯誤匹配的發(fā)生。

圖4 基于WIFI的RatSLAM結(jié)構(gòu)圖Fig. 4 WIFI-based RatSLAM structure diagram

3 仿真實驗與分析

為了驗證本文的可行性與正確性，本文在PC機上進行多次仿真實驗，處理器為i3-3240，主頻為3.4 GHz，內(nèi)存為4 GB，用MATLAB2014a軟件進行實驗仿真。

3.1 WIFI模板匹配

WIFI模板匹配結(jié)果如圖5所示。模板認知點是由機器人捕捉的獨特的局部場景組成，每次獲取的局部場景即為一個經(jīng)歷認知點。當經(jīng)歷認知點與模板認知點足夠相似時，就會完成模板匹配。圖5中，每個灰色圓圈代表此時創(chuàng)建或匹配一個特定的模板認知點；每個黑色菱形表示獲得一個新的經(jīng)歷認知點。由圖5可知，此實驗循環(huán)3次，兩次重定位。圖5(a)為RatSLAM算法的模板匹配圖。圖5(b)為RatSLAM與WIFI算法結(jié)合產(chǎn)生的模板匹配圖，當黑色菱形與灰色圓圈重合或十分接近時，表示匹配成功，受外界不確定因素的影響，偶爾也會出現(xiàn)一些誤匹配。對比圖5(a)和(b)，圖5(b)的匹配點更多，誤匹配更少，所以圖5(b)匹配的效果要好于圖5(a)。因此可以得出：結(jié)合后的算法比單獨的RatSLAM算法重定位的效果更好，定位的準確率更高。

圖5 模板經(jīng)歷匹配圖Fig. 5 Template of experience-matching graph

3.2 算法經(jīng)歷制圖的對比

實驗的地點為一會議室，平面圖及移機器人的路徑如圖6所示，起點為A，終點為B。RatSLAM+WIFI及RatSLAM算法的經(jīng)歷制圖如圖7所示。對比兩種算法的經(jīng)歷制圖可知，融合算法后的經(jīng)歷制圖在拐角處路徑誤差明顯比融合前算法要大，而且，融合后算法整個經(jīng)歷制圖要更平滑，誤差維持在一個較小值且變化幅度很小，效率更高。所以，融合后算法的經(jīng)歷制圖優(yōu)于融合前。

圖6 會議室平面圖及路徑Fig. 6 Floor plan and path of conference room

圖7 經(jīng)歷制圖Fig. 7 Experience mapping

3.3 算法定位準確度對比

算法定位準確度是指在給定的誤差的前提下，算法成功定位的概率。當給定誤差都較小時，兩種算法定位的準確度都較低，如圖8所示。隨著給定誤差的逐漸增大，定位成功率也隨之提高。RatSLAM+WIFI算法在0.2 m之內(nèi)就達80%，然而單獨的RatSLAM算法在0.3m以上才可以達到這個指標。不難得出，結(jié)合后的算法比結(jié)合前的算法準確度更高，連續(xù)性更好，誤差波動性更小。

圖8 兩種算法定位準確度對比Fig. 8 Comparison of positioning accuracy of two algorithms

3.4 算法定位誤差的比較

表1為兩種算法精度指數(shù)比較，由表1可知，融合之后算法的最大絕對誤差和平均誤差都比單個的RatSLAM定位小，準確率更高。所以，融合后的算法的精確度更高。

表1 算法精度指數(shù)對比表Table 1 Comparison of algorithm accuracy index

4 結(jié)束語

本文系統(tǒng)地研究了基于WIFI無線信號強度的移動機器人RatSLAM問題，利用離線采樣方式建立了環(huán)境的WIFI指紋圖，實現(xiàn)了WIFI指紋算法用于RatSLAM模型的局部場景網(wǎng)絡(luò)中，實現(xiàn)在線精準定位，通過實驗得出，此種融合算法使位置定位精確度得到了很大提高。與此同時，本文提出的算法無需在環(huán)境中架設(shè)專用的定位網(wǎng)絡(luò)或改造現(xiàn)有的WIFI信號發(fā)射裝置，因此，提高了算法的實用性，同時降低了定位系統(tǒng)的成本。雖然此文的融合算法對RatSLAM定位精度等方面有些許的貢獻，但還有提升空間，未來多種無線信號網(wǎng)絡(luò)定位技術(shù)結(jié)合的方法與RatSLAM結(jié)合將會是一個熱門的研究方向。