基于Wi-Fi指紋定位的智能車仿生聲納SLAM算法研究

摘要

仿真實(shí)驗(yàn)表明，采用基于Wi-Fi指紋定位的BatSLAM模型，不僅實(shí)現(xiàn)了智能車室內(nèi)二維經(jīng)驗(yàn)圖的構(gòu)建，而且提高了定位的準(zhǔn)確性和精度，改善經(jīng)驗(yàn)圖的失真問題，實(shí)現(xiàn)經(jīng)驗(yàn)圖的優(yōu)化效果。

引言

同步定位與地圖構(gòu)建(Simultaneous Location and Mapping，SLAM) 就是運(yùn)動載體利用自身攜帶的傳感器獲得周圍環(huán)境的地圖，同時確定自己在地圖中的位置^[1]。智能車在很多領(lǐng)域得到了大量應(yīng)用，例如室內(nèi)進(jìn)行快遞的運(yùn)送，餐廳內(nèi)食物的傳送等，均使用了SLAM技術(shù)。

SLAM問題應(yīng)用最成功的是概率的方法，如卡爾曼濾波算法，擴(kuò)展卡爾曼濾波算法，粒子濾波算法^[2]等。雖然有些傳統(tǒng)的SLAM算法使用聲納傳感器采集信息，但是更多被視覺傳感器^[3]、激光測距傳感器^[4]所替代。使用聲納傳感器采集信息，常常得到的是周圍環(huán)境的粗略信息。生物聲納，能夠?qū)崿F(xiàn)在復(fù)雜的環(huán)境下的定位和導(dǎo)航，因?yàn)樗鼜幕芈曋刑崛「嗟男畔ⅲ梢阅７买鹇暭{的發(fā)射與接收過程，利用仿生聲納系統(tǒng)實(shí)現(xiàn)復(fù)雜環(huán)境下的定位與導(dǎo)航。

概率方法廣泛應(yīng)用于SLAM的同時，澳大利亞昆士蘭大學(xué)Michael等提出了一種基于復(fù)合位姿表征的嚙齒動物海馬區(qū)擴(kuò)展模型（RatSLAM）[5]。RatSLAM的路徑積分是通過里程計獲取速度與角度信息，同時使用視覺傳感器對里程計得到的信息進(jìn)行修正與更新，繪制精確的經(jīng)驗(yàn)圖。文獻(xiàn)^[6]已經(jīng)證明在RatSLAM基礎(chǔ)上，可以使用仿生聲納模板來代替RatSLAM系統(tǒng)中的視覺模板，用雙耳平滑耳蝸圖來代替視覺圖像，使用BatSLAM模型來構(gòu)建二維經(jīng)驗(yàn)圖。

科學(xué)技術(shù)在不斷發(fā)展，目前提高定位精度的方法越來越多，當(dāng)前主流定位技術(shù)主要是無線局域網(wǎng)（Wi-Fi）、藍(lán)牙、慣性定位等。文獻(xiàn)[7]提出了基于RSSI的室內(nèi)定位技術(shù)來解決當(dāng)前定位系統(tǒng)中成本高、定位精度低、不能適應(yīng)室內(nèi)環(huán)境多樣性等問題。文獻(xiàn)[8]提出了在RSSI技術(shù)的基礎(chǔ)上引入TOA（Time of Arrive，到達(dá)時間）技術(shù)，從而實(shí)現(xiàn)基于TOA的指紋定位系統(tǒng)，利用無線信號的連續(xù)性改進(jìn)定位精度。

本文將BatSLAM與Wi-Fi指紋定位相結(jié)合，通過提高定位的準(zhǔn)確性和精度，改善經(jīng)驗(yàn)圖的失真問題，實(shí)現(xiàn)經(jīng)驗(yàn)圖的優(yōu)化效果。

1   算法原理與設(shè)計

本文設(shè)計了1種可以提高智能車室內(nèi)定位精度的改進(jìn)仿生聲納SLAM的算法，在原有的BatSLAM的基礎(chǔ)上，使用Wi-Fi指紋模板對位姿細(xì)胞進(jìn)行輔助修正，從而提高定位精度。首先使用所設(shè)計的仿生聲納硬件系統(tǒng)來采集智能車接收到的回波信息，對采集到的回波信息進(jìn)行處理后，將耳蝸能量譜作為仿生聲納模板，構(gòu)建的仿生聲納模板生成局部場景細(xì)胞。智能車的位姿狀態(tài)用位姿感知細(xì)胞進(jìn)行表示。利用智能車的里程計和轉(zhuǎn)角傳感器來不斷更新位姿感知細(xì)胞，利用局部場景細(xì)胞來實(shí)現(xiàn)對位姿細(xì)胞的修正，當(dāng)仿生聲納模板識別失敗時，此時使用Wi-Fi指紋模板來實(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)采集回波信號

蝙蝠在進(jìn)化過程中形成了完善的聲納系統(tǒng)。該系統(tǒng)中必不可少的是發(fā)聲系統(tǒng)和聽覺系統(tǒng)。連續(xù)發(fā)射頻率可調(diào)(CTFM)聲納是1種寬帶主動式聲學(xué)探測方式，在機(jī)器人導(dǎo)航，以及導(dǎo)盲等領(lǐng)域得到廣泛應(yīng)用。從仿生學(xué)的角度模仿蝙蝠的嘴巴和耳朵，利用CTFM模型構(gòu)建仿生聲納系統(tǒng)來進(jìn)行聲納信號的發(fā)射和數(shù)據(jù)采集。仿生聲納系統(tǒng)主要由信號發(fā)生模塊，回波采集模塊組成。

1.1.1 信號發(fā)生模塊

仿生聲納系統(tǒng)的關(guān)鍵是模仿蝙蝠聲納脈沖來設(shè)計適應(yīng)于該系統(tǒng)的探測波源?；赟TM32可以快捷地構(gòu)造所需波形。信號發(fā)生器使用直接數(shù)字式（DDS），首先對波形進(jìn)行采樣，將采樣后得到的數(shù)據(jù)點(diǎn)進(jìn)行量化并存入存儲器，將存儲其中地址對應(yīng)的波形數(shù)據(jù)點(diǎn)，通過DAC轉(zhuǎn)換成模擬量進(jìn)行輸出[8]。

蝙蝠發(fā)出的聲音信號，脈沖主要分為3種類型：恒頻（CF）、調(diào)頻（FM）和復(fù)頻(CF-FM)。在對生物聲納探測的研究過程中，CF脈沖和FM脈沖的使用最為普遍。CF脈沖持續(xù)時間長，具有能量值大的優(yōu)點(diǎn)，適合對目標(biāo)的遠(yuǎn)距離探測。FM脈沖屬于寬頻帶脈沖，頻率一般由高到低，適合對近距離物體的識別和定位。為獲得偵測、識別和定位的能力，蝙蝠很多時候發(fā)出的脈沖是由CF和FM的混合而成。CF脈沖的頻率為c，具有長度和發(fā)送速度可調(diào)的特性。FM脈沖頻率為(c-d)-(c+d)（c>d），頻率隨時間線性變換。為更加符合蝙蝠啁啾脈沖的特征，將FM脈沖的幅值進(jìn)行半波調(diào)制。FM脈沖的函數(shù)表達(dá)式如式（1）所示。

（1）

其中， 為（c+d），a為頻率相對于時間的變換率，T為脈沖時間長度。這2種波形通過STM32的DA芯片轉(zhuǎn)換成模擬量，經(jīng)過電壓放大與驅(qū)動電路后使用超聲波換能器進(jìn)行輸出。

1.1.2 回波采集模塊

信號發(fā)生器發(fā)出仿生聲納信號來對前方的障礙物進(jìn)行檢測，通過超聲波換能器來接收反射波并將反射波信號轉(zhuǎn)化為電信號，電信號經(jīng)過信號調(diào)理電路進(jìn)行電壓放大以及濾波處理，通過數(shù)模轉(zhuǎn)換器轉(zhuǎn)換為數(shù)字信號送至STM32，STM32接收到的回波信號分別為 ， 。

1.2 仿生聲納模板的構(gòu)建

STM32接收到的信號分別為  ， 。耳蝸圖作為局部視圖模板，其構(gòu)建過程如下：對于接收到的信號使用1組Gammachirp濾波器處理，利用Gammachirp聽覺濾波器組來模擬人類耳蝸基底膜，分別得到左耳與右耳的耳蝸能量譜，計算公式如式（2）式（3）所示。

 （2）

 （3）

其中 ， ， 分別為左耳與右耳耳蝸能量譜， 為一階低通濾波，。

在使用耳蝸?zhàn)V作為仿生聲納模板時，這里使用高斯濾波器進(jìn)行平滑處理，進(jìn)一步排除細(xì)小位置改變帶來的影響。2個平滑和二次采樣的單聲道耳蝸圖連接起來，形成1個雙耳耳蝸圖如式（4）所示。

   （4）

其中， 為左側(cè)二次采樣耳蝸圖， 為右側(cè)二次采樣耳蝸圖， 高斯平滑濾波器。

使用標(biāo)準(zhǔn)化雙耳平滑耳蝸圖，計算公式如式（5）所示。

   （5）

將標(biāo)準(zhǔn)化雙耳平滑耳蝸圖作為當(dāng)前仿生聲納模版，計算公式如式（6）所示。

   （6）

1.3 位姿感知細(xì)胞

智能車到達(dá)室內(nèi)具體位置時，其處于空間中的位姿感知細(xì)胞網(wǎng)絡(luò)（X，Y和Θ）中。此時會利用連續(xù)吸引子網(wǎng)絡(luò)（CAN）來實(shí)時控制智能車在（X，Y和Θ）空間中的活動情況。路徑積分是指通過不斷地獲取智能車的里程計、轉(zhuǎn)角傳感器的數(shù)據(jù)以及智能車在（X，Y和Θ）空間中的位姿變化情況來的實(shí)現(xiàn)位姿感知細(xì)胞的更新，并且利用生成的仿生聲納模板來實(shí)現(xiàn)位姿感知細(xì)胞的位姿修正。

1.3.1 CAN模型

位姿細(xì)胞內(nèi)部動態(tài)過程主要包括興奮度更新，對所有細(xì)胞的全局抑制。使用1個三維離散高斯分布來創(chuàng)建興奮度權(quán)重矩陣，如式(7)所示。

    (7)

其中，k_m，k_n  分別表示位置與方向的常數(shù)。a 、b 、c 為 x`、y` 、Θ` 的分布系數(shù)；

由于興奮性造成的位姿感知細(xì)胞的活動變化如式(8)所示。

    (8)

其中N_{x`</sub>，N<sub>y`} , N_θ`, 為 空間中的位姿感知細(xì)胞三維矩陣元素。對于位姿感知細(xì)胞的活動變化的全局抑制如式(9)所示。

    (9)

其中k_m，k_n抑制常數(shù)φ控制全局抑制水平，且興奮度水平被限制在非負(fù)數(shù)范圍。

1.3.2 路徑積分

路徑積分主要對位姿感知細(xì)胞活性進(jìn)行更新。路徑積分令位姿感知細(xì)胞的性能可以獨(dú)立于感知變化速率和里程計速度與角速傳感器獲取的角速度，不但可以獲取較為準(zhǔn)確的智能車運(yùn)行軌跡，而且不需要改變具體的參數(shù)。通過路徑積分，位姿感知細(xì)胞活性得到更新如式（10）所示。

    (10)

       其中為在x` ，y` ，θ` 方向被四舍五入后的整數(shù)偏差由式(11)計算得到。

    (11)

其中 是路徑積分常數(shù)，v表示加速度計速度，w表示陀螺儀得到的角速度。

1.3.3 仿生聲納模板修正位姿感知細(xì)胞

       采用雙耳平滑耳蝸圖作為仿生聲納模板，局部場景細(xì)胞由1個一維細(xì)胞陣列組成，每個細(xì)胞單元對應(yīng)1個特定的仿生聲納模板。在路徑積分時，由于位置和方向上的累計誤差，使用仿生聲納模板來進(jìn)行修正。對于每個新的場景，算法會建立新的仿生聲納模板，同時建立局部場景細(xì)胞與位姿感知細(xì)胞間的聯(lián)系。進(jìn)行更新的連接強(qiáng)度如式(12)所示。

   (12)

其中，G_i為局部場景細(xì)胞的活動水平，為位姿感知細(xì)胞的活動水平。

對于每一個雙耳平滑耳蝸圖使用絕對差值和(SAD)模塊進(jìn)行處理產(chǎn)生局部場景。SAD模塊將當(dāng)前耳蝸圖與模板庫中的耳蝸圖進(jìn)行比較，通過計算所有模板中耳蝸圖的像素亮度與當(dāng)前耳蝸圖的像素亮度之差，如式(13)所示。

   (13)

其中， 表示當(dāng)前耳蝸圖的像素亮度， 表示數(shù)據(jù)庫中的第i個仿生聲納模板的像素亮度。如果像素亮度之差低于某一閾值，當(dāng)前耳蝸圖被識別出來，對位姿感知細(xì)胞進(jìn)行校正，如式(14)所示。

   (14)

如果像素亮度之差高于某一閾值 ，當(dāng)前耳蝸圖成為新的仿生聲納模板并加入到模板庫中。閾值 如式(15)所示。

   (15)

其中，n_G為數(shù)據(jù)庫中的仿生聲納模板的數(shù)量，α_t為調(diào)整系統(tǒng)的比例因子，α_t設(shè)置為0.5。

1.4 Wi-Fi指紋模版修正位姿感知細(xì)胞

本文所設(shè)計Wi-Fi指紋定位算法與傳統(tǒng)的位置指紋定位算法相類似，是將實(shí)際環(huán)境中的具體位置和Wi-Fi指紋相聯(lián)系，具體的位置與特定的Wi-Fi指紋相對應(yīng)。本文選取的Wi-Fi指紋特征是具體位置檢測得到的基站信號的RSS(信號接收強(qiáng)度)?；赗SS(信號接收強(qiáng)度)的Wi-Fi指紋定位算法主要包括離線采集階段和在線定位階段。

1.4.1離線訓(xùn)練階段

離線訓(xùn)練階段的主要工作是完成離線指紋數(shù)據(jù)庫的構(gòu)建，主要包括3個部分：選取參考點(diǎn)、采集指紋、構(gòu)建指紋數(shù)據(jù)庫。首先繪制出室內(nèi)環(huán)境的地圖，接下來合理的選取進(jìn)行測試的參考點(diǎn)，同時對各個參考點(diǎn)接收到的不同AP（無線接入點(diǎn)）的RSSI(信號接收強(qiáng)度的平均值)進(jìn)行統(tǒng)計，不同AP的RSSI作為參考點(diǎn)的特征，并將其記錄在Wi-Fi指紋數(shù)據(jù)庫中。

離線指紋數(shù)據(jù)庫的具體構(gòu)建過程：在Wi-Fi裝置調(diào)試完成后首先設(shè)置N個AP，接下來在室內(nèi)環(huán)境中保持相同的距離設(shè)置參考點(diǎn),共設(shè)置w個參考點(diǎn)，參考點(diǎn)的坐標(biāo)為 ，最后在每個參考點(diǎn)上采集不同AP的RSSI，并且將每個AP的均值存儲于Wi-Fi指紋模板數(shù)據(jù)庫中，Wi-Fi指紋數(shù)據(jù)庫如式（16）所示：

   (16)

其中， 表示所選取參考點(diǎn)的集合， , 為參考點(diǎn)的位置，參考點(diǎn)的數(shù)目為w； 為所設(shè)置的無線接入點(diǎn)，即觀測點(diǎn)的集合；N為觀測點(diǎn)的數(shù)目， 是Wi-Fi指紋全部均值的集合，如式（17）所示。

   （17）

其中，為第j個AP在位置V_i處的均值。

離線階段的原理圖如圖2所示。

圖2 離線訓(xùn)練階段原理圖

1.4.2在線定位階段

在線定位階段主要是利用智能車自身攜帶的通訊設(shè)備在室內(nèi)空間中的具體位置對各個AP的RSSI進(jìn)行采集，采集完成后使用一定的匹配算法，將采集到的實(shí)時數(shù)據(jù)與離線的Wi-Fi指紋數(shù)據(jù)庫中的數(shù)據(jù)進(jìn)行對比，通過對比可以在離線的Wi-Fi指紋數(shù)據(jù)庫中得到與實(shí)時數(shù)據(jù)相似的Wi-Fi指紋，從而完成智能車的實(shí)時定位，本文采用的在線指紋匹配算法是K加權(quán)近鄰算法。

K加權(quán)近鄰算法是將采集到的實(shí)時數(shù)據(jù)與離線的Wi-Fi指紋數(shù)據(jù)庫中的數(shù)據(jù)進(jìn)行對比，通過求解采集的實(shí)時數(shù)據(jù)（不同AP的RSSI）與Wi-Fi指紋數(shù)據(jù)庫中相應(yīng)AP的RSSI間的歐式距離，然后將匹配獲取的K個與實(shí)時定位點(diǎn)相似的Wi-Fi指紋，得到這K個Wi-Fi指紋的位置坐標(biāo)，并且將這些位置坐標(biāo)根據(jù)相似度屬性給予不同的權(quán)重值，最后根據(jù)這些權(quán)重值對位置坐標(biāo)進(jìn)行加權(quán)求和，從而獲取最終的定位。K加權(quán)近鄰算法如圖所示。

首先采集到實(shí)時的第i個Wi-Fi位置指紋，其與指紋數(shù)據(jù)庫中的離線Wi-Fi指紋間的歐式距離為D_i，如式（18）所示。

   （18）

其中，是第j個AP在第i個參考點(diǎn)的信號接收強(qiáng)度的平均值， 是實(shí)時采集的參考點(diǎn)在第j個AP的信號接收強(qiáng)度的平均值。

接下來將獲取的 進(jìn)行排序，得到K個歐式距離最小的參考點(diǎn)，再根據(jù)這些參考點(diǎn)的相似度屬性賦予不同的加權(quán)系數(shù)u_i，如式（19）所示。

    （19）

最后對這K個參考點(diǎn)進(jìn)行加權(quán)求和，定位坐標(biāo)如式（20）所示。

   （20）

(X_i,Y_i)是K個Wi-Fi指紋對應(yīng)的坐標(biāo)。在線階段原理圖如圖3所示。

圖3 在線定位階段原理圖

1.4.3 Wi-Fi指紋模板修正

       對于每個新的場景，算法會建立新的Wi-Fi指紋模板。同時建立Wi-Fi指紋與位姿感知細(xì)胞間的聯(lián)系。進(jìn)行更新的連接強(qiáng)度如式(21)所示。

   (21)

       其中，Y_i指的是第i個Wi-Fi指紋模板。將當(dāng)前Wi-Fi指紋和數(shù)據(jù)庫中Wi-Fi指紋模板進(jìn)行對比，若低于閾值則對位姿感知細(xì)胞進(jìn)行修正，如式(22)所示。

   (22)

       如果高于閾值y，當(dāng)前Wi-Fi指紋成為新的Wi-Fi指紋模板并加入到Wi-Fi指紋模板庫中。

1.5 經(jīng)驗(yàn)圖的構(gòu)建與校正

每個經(jīng)驗(yàn) 關(guān)聯(lián)經(jīng)驗(yàn)?zāi)芗塃_i，位姿細(xì)胞 P_xi,yi,θi和局部場景細(xì)胞 G_i，與Wi-Fi指紋模板 Yi，第i個經(jīng)驗(yàn)如式(23)所示。

   (23)

當(dāng)前位姿感知細(xì)胞的活性或當(dāng)前的Wi-Fi 指紋決定經(jīng)驗(yàn)?zāi)芗?E_i的各組成要素，如式（24）所示。

   (24)

其中，X`<sub>pc</sub> ,Y`_pc , θ`<sub>pc</sub>為最大活性姿態(tài)細(xì)胞坐標(biāo)；,為與該經(jīng)驗(yàn)相關(guān)的位姿感知細(xì)胞坐標(biāo)；R<sub>a</sub>為(X`,Y`)平面的區(qū)域常數(shù)；θa為θ`維上區(qū)域常數(shù)：為當(dāng)前局部場景細(xì)胞； Gⁱ為與經(jīng)驗(yàn)i相關(guān)的局部場景細(xì)胞, 為當(dāng)前的音頻感知哈希， 與經(jīng)驗(yàn)i相關(guān)的音頻感知哈希。

使用當(dāng)前位姿信息、Wi-Fi指紋模板和仿生聲納模板與經(jīng)驗(yàn)圖進(jìn)行匹配時，當(dāng)?shù)竭_(dá)經(jīng)驗(yàn)過的地方時，進(jìn)行1次閉環(huán)檢測，并對經(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ù)。