秦國(guó)威，孫新柱，陳孟元

（安徽工程大學(xué) 電氣傳動(dòng)與控制重點(diǎn)實(shí)驗(yàn)室，安徽 蕪湖 241000）

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

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

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

1 RatSLAM模型

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

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

1.1 位姿表征

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

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

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

1)興奮度更新

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

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

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

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

2)全局抑制

因?yàn)槎鄠€(gè)活性包表示的多個(gè)位姿假設(shè)的完成需要一段時(shí)間，而且會(huì)不段加強(qiáng)，因此抑制是比較緩慢的，即在相當(dāng)長(zhǎng)的一段時(shí)間內(nèi)相互競(jìng)爭(zhēng)的活性包會(huì)同時(shí)存在，全局抑制后的活動(dòng)水平為

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

3)歸一化

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

1.2 視覺(jué)場(chǎng)景的學(xué)習(xí)

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

式中： Vi為局部視覺(jué)場(chǎng)景細(xì)胞的活性水平； Qxyθ為位姿感知細(xì)胞的活性水平。

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

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

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

2 WIFI與RatSLAM結(jié)合

2.1 WIFI指紋定位的原理

本文采用的WIFI定位方法為位置指紋定位方法。與通常意義下的指紋識(shí)別相似，位置指紋定位識(shí)別主要依靠表示目標(biāo)特征的數(shù)據(jù)庫(kù)進(jìn)行辨認(rèn)?；谛盘?hào)強(qiáng)度 R SSI的位置指紋定位方式分為兩個(gè)階段：離線采集階段和在線定位階段。具體WIFI指紋定位原理圖如圖3所示。

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

2.1.1 離線采集階段

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

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

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

式中 mLiAPj為第 j個(gè) A P 在 參考點(diǎn) Li處的均值。

2.1.2 在線定位階段

1)AP的選取策略

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

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

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

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

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

2)貝葉斯位置估計(jì)策略

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

貝葉斯后驗(yàn)估計(jì)的基本原理為

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

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

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

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

2.2 WIFI與RatSLAM的融合

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

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

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

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

3.1 WIFI模板匹配

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

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

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

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

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

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

3.3 算法定位準(zhǔn)確度對(duì)比

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

圖8 兩種算法定位準(zhǔn)確度對(duì)比Fig. 8 Comparison of positioning accuracy of two algorithms

3.4 算法定位誤差的比較

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

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

4 結(jié)束語(yǔ)

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