基于Wi-Fi指紋定位的智能車仿生聲納SLAM算法研究
加入技術(shù)交流群
掃碼加入
和技術(shù)大咖面對(duì)面交流
海量資料庫(kù)查詢
摘要
仿真實(shí)驗(yàn)表明，采用基于Wi-Fi指紋定位的BatSLAM模型，不僅實(shí)現(xiàn)了智能車室內(nèi)二維經(jīng)驗(yàn)圖的構(gòu)建，而且提高了定位的準(zhǔn)確性和精度，改善經(jīng)驗(yàn)圖的失真問(wèn)題，實(shí)現(xiàn)經(jīng)驗(yàn)圖的優(yōu)化效果。
引言
同步定位與地圖構(gòu)建(Simultaneous Location and Mapping，SLAM) 就是運(yùn)動(dòng)載體利用自身攜帶的傳感器獲得周圍環(huán)境的地圖，同時(shí)確定自己在地圖中的位置[1]。智能車在很多領(lǐng)域得到了大量應(yīng)用，例如室內(nèi)進(jìn)行快遞的運(yùn)送，餐廳內(nèi)食物的傳送等，均使用了SLAM技術(shù)。
SLAM問(wèn)題應(yīng)用最成功的是概率的方法，如卡爾曼濾波算法，擴(kuò)展卡爾曼濾波算法，粒子濾波算法[2]等。雖然有些傳統(tǒng)的SLAM算法使用聲納傳感器采集信息，但是更多被視覺(jué)傳感器[3]、激光測(cè)距傳感器[4]所替代。使用聲納傳感器采集信息，常常得到的是周圍環(huán)境的粗略信息。生物聲納，能夠?qū)崿F(xiàn)在復(fù)雜的環(huán)境下的定位和導(dǎo)航，因?yàn)樗鼜幕芈曋刑崛「嗟男畔?，可以模仿蝙蝠聲納的發(fā)射與接收過(guò)程，利用仿生聲納系統(tǒng)實(shí)現(xiàn)復(fù)雜環(huán)境下的定位與導(dǎo)航。
概率方法廣泛應(yīng)用于SLAM的同時(shí)，澳大利亞昆士蘭大學(xué)Michael等提出了一種基于復(fù)合位姿表征的嚙齒動(dòng)物海馬區(qū)擴(kuò)展模型（RatSLAM）[5]。RatSLAM的路徑積分是通過(guò)里程計(jì)獲取速度與角度信息，同時(shí)使用視覺(jué)傳感器對(duì)里程計(jì)得到的信息進(jìn)行修正與更新，繪制精確的經(jīng)驗(yàn)圖。文獻(xiàn)[6]已經(jīng)證明在RatSLAM基礎(chǔ)上，可以使用仿生聲納模板來(lái)代替RatSLAM系統(tǒng)中的視覺(jué)模板，用雙耳平滑耳蝸圖來(lái)代替視覺(jué)圖像，使用BatSLAM模型來(lái)構(gòu)建二維經(jīng)驗(yàn)圖。
科學(xué)技術(shù)在不斷發(fā)展，目前提高定位精度的方法越來(lái)越多，當(dāng)前主流定位技術(shù)主要是無(wú)線局域網(wǎng)（Wi-Fi）、藍(lán)牙、慣性定位等。文獻(xiàn)[7]提出了基于RSSI的室內(nèi)定位技術(shù)來(lái)解決當(dāng)前定位系統(tǒng)中成本高、定位精度低、不能適應(yīng)室內(nèi)環(huán)境多樣性等問(wèn)題。文獻(xiàn)[8]提出了在RSSI技術(shù)的基礎(chǔ)上引入TOA（Time of Arrive，到達(dá)時(shí)間）技術(shù)，從而實(shí)現(xiàn)基于TOA的指紋定位系統(tǒng)，利用無(wú)線信號(hào)的連續(xù)性改進(jìn)定位精度。
本文將BatSLAM與Wi-Fi指紋定位相結(jié)合，通過(guò)提高定位的準(zhǔn)確性和精度，改善經(jīng)驗(yàn)圖的失真問(wèn)題，實(shí)現(xiàn)經(jīng)驗(yàn)圖的優(yōu)化效果。
1 算法原理與設(shè)計(jì)
本文設(shè)計(jì)了1種可以提高智能車室內(nèi)定位精度的改進(jìn)仿生聲納SLAM的算法，在原有的BatSLAM的基礎(chǔ)上，使用Wi-Fi指紋模板對(duì)位姿細(xì)胞進(jìn)行輔助修正，從而提高定位精度。首先使用所設(shè)計(jì)的仿生聲納硬件系統(tǒng)來(lái)采集智能車接收到的回波信息，對(duì)采集到的回波信息進(jìn)行處理后，將耳蝸能量譜作為仿生聲納模板，構(gòu)建的仿生聲納模板生成局部場(chǎng)景細(xì)胞。智能車的位姿狀態(tài)用位姿感知細(xì)胞進(jìn)行表示。利用智能車的里程計(jì)和轉(zhuǎn)角傳感器來(lái)不斷更新位姿感知細(xì)胞，利用局部場(chǎng)景細(xì)胞來(lái)實(shí)現(xiàn)對(duì)位姿細(xì)胞的修正，當(dāng)仿生聲納模板識(shí)別失敗時(shí)，此時(shí)使用Wi-Fi指紋模板來(lái)實(shí)現(xiàn)位姿細(xì)胞的修正。本文首先使用仿生聲納模板進(jìn)行修正，輔助使用Wi-Fi指紋模板修正，不但可以提高定位精度，而且可以實(shí)現(xiàn)經(jīng)驗(yàn)圖的優(yōu)化。
改進(jìn)后的仿生聲納SLAM定位算法的結(jié)構(gòu)框圖如圖1所示。
圖1 改進(jìn)后的仿生聲納SLAM算法的結(jié)構(gòu)框圖
1.1 仿生聲納系統(tǒng)采集回波信號(hào)
蝙蝠在進(jìn)化過(guò)程中形成了完善的聲納系統(tǒng)。該系統(tǒng)中必不可少的是發(fā)聲系統(tǒng)和聽(tīng)覺(jué)系統(tǒng)。連續(xù)發(fā)射頻率可調(diào)(CTFM)聲納是1種寬帶主動(dòng)式聲學(xué)探測(cè)方式，在機(jī)器人導(dǎo)航，以及導(dǎo)盲等領(lǐng)域得到廣泛應(yīng)用。從仿生學(xué)的角度模仿蝙蝠的嘴巴和耳朵，利用CTFM模型構(gòu)建仿生聲納系統(tǒng)來(lái)進(jìn)行聲納信號(hào)的發(fā)射和數(shù)據(jù)采集。仿生聲納系統(tǒng)主要由信號(hào)發(fā)生模塊，回波采集模塊組成。
1.1.1 信號(hào)發(fā)生模塊
仿生聲納系統(tǒng)的關(guān)鍵是模仿蝙蝠聲納脈沖來(lái)設(shè)計(jì)適應(yīng)于該系統(tǒng)的探測(cè)波源。基于STM32可以快捷地構(gòu)造所需波形。信號(hào)發(fā)生器使用直接數(shù)字式（DDS），首先對(duì)波形進(jìn)行采樣，將采樣后得到的數(shù)據(jù)點(diǎn)進(jìn)行量化并存入存儲(chǔ)器，將存儲(chǔ)其中地址對(duì)應(yīng)的波形數(shù)據(jù)點(diǎn)，通過(guò)DAC轉(zhuǎn)換成模擬量進(jìn)行輸出[8]。
蝙蝠發(fā)出的聲音信號(hào)，脈沖主要分為3種類型：恒頻（CF）、調(diào)頻（FM）和復(fù)頻(CF-FM)。在對(duì)生物聲納探測(cè)的研究過(guò)程中，CF脈沖和FM脈沖的使用最為普遍。CF脈沖持續(xù)時(shí)間長(zhǎng)，具有能量值大的優(yōu)點(diǎn)，適合對(duì)目標(biāo)的遠(yuǎn)距離探測(cè)。FM脈沖屬于寬頻帶脈沖，頻率一般由高到低，適合對(duì)近距離物體的識(shí)別和定位。為獲得偵測(cè)、識(shí)別和定位的能力，蝙蝠很多時(shí)候發(fā)出的脈沖是由CF和FM的混合而成。CF脈沖的頻率為c，具有長(zhǎng)度和發(fā)送速度可調(diào)的特性。FM脈沖頻率為(c-d)-(c+d)（c>d），頻率隨時(shí)間線性變換。為更加符合蝙蝠啁啾脈沖的特征，將FM脈沖的幅值進(jìn)行半波調(diào)制。FM脈沖的函數(shù)表達(dá)式如式（1）所示。
（1）
其中， 為（c+d），a為頻率相對(duì)于時(shí)間的變換率，T為脈沖時(shí)間長(zhǎng)度。這2種波形通過(guò)STM32的DA芯片轉(zhuǎn)換成模擬量，經(jīng)過(guò)電壓放大與驅(qū)動(dòng)電路后使用超聲波換能器進(jìn)行輸出。
1.1.2 回波采集模塊
信號(hào)發(fā)生器發(fā)出仿生聲納信號(hào)來(lái)對(duì)前方的障礙物進(jìn)行檢測(cè)，通過(guò)超聲波換能器來(lái)接收反射波并將反射波信號(hào)轉(zhuǎn)化為電信號(hào)，電信號(hào)經(jīng)過(guò)信號(hào)調(diào)理電路進(jìn)行電壓放大以及濾波處理，通過(guò)數(shù)模轉(zhuǎn)換器轉(zhuǎn)換為數(shù)字信號(hào)送至STM32，STM32接收到的回波信號(hào)分別為 
，
。
1.2 仿生聲納模板的構(gòu)建
STM32接收到的信號(hào)分別為 
，
。耳蝸圖作為局部視圖模板，其構(gòu)建過(guò)程如下：對(duì)于接收到的信號(hào)使用1組Gammachirp濾波器處理，利用Gammachirp聽(tīng)覺(jué)濾波器組來(lái)模擬人類耳蝸基底膜，分別得到左耳與右耳的耳蝸能量譜，計(jì)算公式如式（2）式（3）所示。
（2）
（3）
其中 ，
，
分別為左耳與右耳耳蝸能量譜，
為一階低通濾波，。
在使用耳蝸?zhàn)V作為仿生聲納模板時(shí)，這里使用高斯濾波器進(jìn)行平滑處理，進(jìn)一步排除細(xì)小位置改變帶來(lái)的影響。2個(gè)平滑和二次采樣的單聲道耳蝸圖連接起來(lái)，形成1個(gè)雙耳耳蝸圖如式（4）所示。
（4）
其中， 為左側(cè)二次采樣耳蝸圖， 為右側(cè)二次采樣耳蝸圖， 高斯平滑濾波器。
使用標(biāo)準(zhǔn)化雙耳平滑耳蝸圖，計(jì)算公式如式（5）所示。
（5）
將標(biāo)準(zhǔn)化雙耳平滑耳蝸圖作為當(dāng)前仿生聲納模版，計(jì)算公式如式（6）所示。
（6）
1.3 位姿感知細(xì)胞
智能車到達(dá)室內(nèi)具體位置時(shí)，其處于空間中的位姿感知細(xì)胞網(wǎng)絡(luò)（X，Y和Θ）中。此時(shí)會(huì)利用連續(xù)吸引子網(wǎng)絡(luò)（CAN）來(lái)實(shí)時(shí)控制智能車在（X，Y和Θ）空間中的活動(dòng)情況。路徑積分是指通過(guò)不斷地獲取智能車的里程計(jì)、轉(zhuǎn)角傳感器的數(shù)據(jù)以及智能車在（X，Y和Θ）空間中的位姿變化情況來(lái)的實(shí)現(xiàn)位姿感知細(xì)胞的更新，并且利用生成的仿生聲納模板來(lái)實(shí)現(xiàn)位姿感知細(xì)胞的位姿修正。
1.3.1 CAN模型
位姿細(xì)胞內(nèi)部動(dòng)態(tài)過(guò)程主要包括興奮度更新，對(duì)所有細(xì)胞的全局抑制。使用1個(gè)三維離散高斯分布來(lái)創(chuàng)建興奮度權(quán)重矩陣
，如式(7)所示。
(7)
其中，km，kn 分別表示位置與方向的常數(shù)。a 、b 、c 為 x`、y` 、Θ` 的分布系數(shù)；
由于興奮性造成的位姿感知細(xì)胞的活動(dòng)變化如式(8)所示。
(8)
其中Nx`，Ny` , Nθ`, 為 空間中的位姿感知細(xì)胞三維矩陣元素。對(duì)于位姿感知細(xì)胞的活動(dòng)變化的全局抑制如式(9)所示。
(9)
其中km，kn抑制常數(shù)φ控制全局抑制水平，且興奮度水平被限制在非負(fù)數(shù)范圍。
1.3.2 路徑積分
路徑積分主要對(duì)位姿感知細(xì)胞活性進(jìn)行更新。路徑積分令位姿感知細(xì)胞的性能可以獨(dú)立于感知變化速率和里程計(jì)速度與角速傳感器獲取的角速度，不但可以獲取較為準(zhǔn)確的智能車運(yùn)行軌跡，而且不需要改變具體的參數(shù)。通過(guò)路徑積分，位姿感知細(xì)胞活性得到更新如式（10）所示。
(10)
其中
為在x` ，y` ，θ` 方向被四舍五入后的整數(shù)偏差
由式(11)計(jì)算得到。
(11)
其中
是路徑積分常數(shù)，v表示加速度計(jì)速度，w表示陀螺儀得到的角速度。
1.3.3 仿生聲納模板修正位姿感知細(xì)胞
采用雙耳平滑耳蝸圖作為仿生聲納模板，局部場(chǎng)景細(xì)胞由1個(gè)一維細(xì)胞陣列組成，每個(gè)細(xì)胞單元對(duì)應(yīng)1個(gè)特定的仿生聲納模板。在路徑積分時(shí)，由于位置和方向上的累計(jì)誤差，使用仿生聲納模板來(lái)進(jìn)行修正。對(duì)于每個(gè)新的場(chǎng)景，算法會(huì)建立新的仿生聲納模板，同時(shí)建立局部場(chǎng)景細(xì)胞與位姿感知細(xì)胞間的聯(lián)系。進(jìn)行更新的連接強(qiáng)度如式(12)所示。
(12)
其中，Gi為局部場(chǎng)景細(xì)胞的活動(dòng)水平，
為位姿感知細(xì)胞的活動(dòng)水平。
對(duì)于每一個(gè)雙耳平滑耳蝸圖使用絕對(duì)差值和(SAD)模塊進(jìn)行處理產(chǎn)生局部場(chǎng)景。SAD模塊將當(dāng)前耳蝸圖與模板庫(kù)中的耳蝸圖進(jìn)行比較，通過(guò)計(jì)算所有模板中耳蝸圖的像素亮度與當(dāng)前耳蝸圖的像素亮度之差，如式(13)所示。
(13)
其中， 表示當(dāng)前耳蝸圖的像素亮度， 表示數(shù)據(jù)庫(kù)中的第i個(gè)仿生聲納模板的像素亮度。如果像素亮度之差低于某一閾值，當(dāng)前耳蝸圖被識(shí)別出來(lái)，對(duì)位姿感知細(xì)胞進(jìn)行校正，如式(14)所示。
(14)
如果像素亮度之差高于某一閾值 ，當(dāng)前耳蝸圖成為新的仿生聲納模板并加入到模板庫(kù)中。閾值 如式(15)所示。
(15)
其中，nG為數(shù)據(jù)庫(kù)中的仿生聲納模板的數(shù)量，αt為調(diào)整系統(tǒng)的比例因子，αt設(shè)置為0.5。
1.4 Wi-Fi指紋模版修正位姿感知細(xì)胞
本文所設(shè)計(jì)Wi-Fi指紋定位算法與傳統(tǒng)的位置指紋定位算法相類似，是將實(shí)際環(huán)境中的具體位置和Wi-Fi指紋相聯(lián)系，具體的位置與特定的Wi-Fi指紋相對(duì)應(yīng)。本文選取的Wi-Fi指紋特征是具體位置檢測(cè)得到的基站信號(hào)的RSS(信號(hào)接收強(qiáng)度)?；赗SS(信號(hào)接收強(qiáng)度)的Wi-Fi指紋定位算法主要包括離線采集階段和在線定位階段。
1.4.1離線訓(xùn)練階段
離線訓(xùn)練階段的主要工作是完成離線指紋數(shù)據(jù)庫(kù)的構(gòu)建，主要包括3個(gè)部分：選取參考點(diǎn)、采集指紋、構(gòu)建指紋數(shù)據(jù)庫(kù)。首先繪制出室內(nèi)環(huán)境的地圖，接下來(lái)合理的選取進(jìn)行測(cè)試的參考點(diǎn)，同時(shí)對(duì)各個(gè)參考點(diǎn)接收到的不同AP（無(wú)線接入點(diǎn)）的RSSI(信號(hào)接收強(qiáng)度的平均值)進(jìn)行統(tǒng)計(jì)，不同AP的RSSI作為參考點(diǎn)的特征，并將其記錄在Wi-Fi指紋數(shù)據(jù)庫(kù)中。
離線指紋數(shù)據(jù)庫(kù)的具體構(gòu)建過(guò)程：在Wi-Fi裝置調(diào)試完成后首先設(shè)置N個(gè)AP，接下來(lái)在室內(nèi)環(huán)境中保持相同的距離設(shè)置參考點(diǎn),共設(shè)置w個(gè)參考點(diǎn)，參考點(diǎn)的坐標(biāo)為 ，最后在每個(gè)參考點(diǎn)上采集不同AP的RSSI，并且將每個(gè)AP的均值存儲(chǔ)于Wi-Fi指紋模板數(shù)據(jù)庫(kù)中，Wi-Fi指紋數(shù)據(jù)庫(kù)如式（16）所示：
(16)
其中， 表示所選取參考點(diǎn)的集合， , 為參考點(diǎn)的位置，參考點(diǎn)的數(shù)目為w； 為所設(shè)置的無(wú)線接入點(diǎn)，即觀測(cè)點(diǎn)的集合；N為觀測(cè)點(diǎn)的數(shù)目， 是Wi-Fi指紋全部均值的集合，如式（17）所示。
（17）
其中，
為第j個(gè)AP在位置Vi處的均值。
離線階段的原理圖如圖2所示。
圖2 離線訓(xùn)練階段原理圖
1.4.2在線定位階段
在線定位階段主要是利用智能車自身攜帶的通訊設(shè)備在室內(nèi)空間中的具體位置對(duì)各個(gè)AP的RSSI進(jìn)行采集，采集完成后使用一定的匹配算法，將采集到的實(shí)時(shí)數(shù)據(jù)與離線的Wi-Fi指紋數(shù)據(jù)庫(kù)中的數(shù)據(jù)進(jìn)行對(duì)比，通過(guò)對(duì)比可以在離線的Wi-Fi指紋數(shù)據(jù)庫(kù)中得到與實(shí)時(shí)數(shù)據(jù)相似的Wi-Fi指紋，從而完成智能車的實(shí)時(shí)定位，本文采用的在線指紋匹配算法是K加權(quán)近鄰算法。
K加權(quán)近鄰算法是將采集到的實(shí)時(shí)數(shù)據(jù)與離線的Wi-Fi指紋數(shù)據(jù)庫(kù)中的數(shù)據(jù)進(jìn)行對(duì)比，通過(guò)求解采集的實(shí)時(shí)數(shù)據(jù)（不同AP的RSSI）與Wi-Fi指紋數(shù)據(jù)庫(kù)中相應(yīng)AP的RSSI間的歐式距離，然后將匹配獲取的K個(gè)與實(shí)時(shí)定位點(diǎn)相似的Wi-Fi指紋，得到這K個(gè)Wi-Fi指紋的位置坐標(biāo)，并且將這些位置坐標(biāo)根據(jù)相似度屬性給予不同的權(quán)重值，最后根據(jù)這些權(quán)重值對(duì)位置坐標(biāo)進(jìn)行加權(quán)求和，從而獲取最終的定位。K加權(quán)近鄰算法如圖所示。
首先采集到實(shí)時(shí)的第i個(gè)Wi-Fi位置指紋，其與指紋數(shù)據(jù)庫(kù)中的離線Wi-Fi指紋間的歐式距離為Di，如式（18）所示。
（18）
其中，
是第j個(gè)AP在第i個(gè)參考點(diǎn)的信號(hào)接收強(qiáng)度的平均值， 是實(shí)時(shí)采集的參考點(diǎn)在第j個(gè)AP的信號(hào)接收強(qiáng)度的平均值。
接下來(lái)將獲取的 進(jìn)行排序，得到K個(gè)歐式距離最小的參考點(diǎn)，再根據(jù)這些參考點(diǎn)的相似度屬性賦予不同的加權(quán)系數(shù)ui，如式（19）所示。
（19）
最后對(duì)這K個(gè)參考點(diǎn)進(jìn)行加權(quán)求和，定位坐標(biāo)如式（20）所示。
（20）
(Xi,Yi)是K個(gè)Wi-Fi指紋對(duì)應(yīng)的坐標(biāo)。在線階段原理圖如圖3所示。
圖3 在線定位階段原理圖
1.4.3 Wi-Fi指紋模板修正
對(duì)于每個(gè)新的場(chǎng)景，算法會(huì)建立新的Wi-Fi指紋模板。同時(shí)建立Wi-Fi指紋與位姿感知細(xì)胞間的聯(lián)系。進(jìn)行更新的連接強(qiáng)度如式(21)所示。
(21)
其中，Yi指的是第i個(gè)Wi-Fi指紋模板。將當(dāng)前Wi-Fi指紋和數(shù)據(jù)庫(kù)中Wi-Fi指紋模板進(jìn)行對(duì)比，若低于閾值則對(duì)位姿感知細(xì)胞進(jìn)行修正，如式(22)所示。
(22)
如果高于閾值y，當(dāng)前Wi-Fi指紋成為新的Wi-Fi指紋模板并加入到Wi-Fi指紋模板庫(kù)中。
1.5 經(jīng)驗(yàn)圖的構(gòu)建與校正
每個(gè)經(jīng)驗(yàn) 
關(guān)聯(lián)經(jīng)驗(yàn)?zāi)芗?jí)Ei，位姿細(xì)胞 Pxi,yi,θi和局部場(chǎng)景細(xì)胞 Gi，與Wi-Fi指紋模板 Yi，第i個(gè)經(jīng)驗(yàn)如式(23)所示。
(23)
當(dāng)前位姿感知細(xì)胞的活性或當(dāng)前的Wi-Fi 指紋決定經(jīng)驗(yàn)?zāi)芗?jí) Ei的各組成要素，如式（24）所示。
(24)
其中，X`pc ,Y`pc , θ`pc為最大活性姿態(tài)細(xì)胞坐標(biāo)；
,為與該經(jīng)驗(yàn)相關(guān)的位姿感知細(xì)胞坐標(biāo)；Ra為(X`,Y`)平面的區(qū)域常數(shù)；θa為θ`維上區(qū)域常數(shù)：
為當(dāng)前局部場(chǎng)景細(xì)胞； Gi為與經(jīng)驗(yàn)i相關(guān)的局部場(chǎng)景細(xì)胞, 為當(dāng)前的音頻感知哈希， 與經(jīng)驗(yàn)i相關(guān)的音頻感知哈希。
使用當(dāng)前位姿信息、Wi-Fi指紋模板和仿生聲納模板與經(jīng)驗(yàn)圖進(jìn)行匹配時(shí)，當(dāng)?shù)竭_(dá)經(jīng)驗(yàn)過(guò)的地方時(shí)，進(jìn)行1次閉環(huán)檢測(cè)，并對(duì)經(jīng)驗(yàn)圖進(jìn)行校正，經(jīng)驗(yàn)位姿改變?nèi)缡?25)所示。
(25)
其中， 為校正常數(shù)， 為經(jīng)驗(yàn)i到其他經(jīng)驗(yàn)的連接數(shù)； 為從其他經(jīng)驗(yàn)到經(jīng)驗(yàn)i的連接數(shù)。
評(píng)論