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

摘要

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

引言

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

SLAM問題應(yīng)用最成功的是概率的方法，如卡爾曼濾波算法，擴展卡爾曼濾波算法，粒子濾波算法^[2]等。雖然有些傳統(tǒng)的SLAM算法使用聲納傳感器采集信息，但是更多被視覺傳感器^[3]、激光測距傳感器^[4]所替代。使用聲納傳感器采集信息，常常得到的是周圍環(huán)境的粗略信息。生物聲納，能夠?qū)崿F(xiàn)在復(fù)雜的環(huán)境下的定位和導(dǎo)航，因為它從回聲中提取更多的信息，可以模仿蝙蝠聲納的發(fā)射與接收過程，利用仿生聲納系統(tǒng)實現(xiàn)復(fù)雜環(huán)境下的定位與導(dǎo)航。

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

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

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

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

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

改進后的仿生聲納SLAM定位算法的結(jié)構(gòu)框圖如圖1所示。

圖1 改進后的仿生聲納SLAM算法的結(jié)構(gòu)框圖

1.1 仿生聲納系統(tǒng)采集回波信號

蝙蝠在進化過程中形成了完善的聲納系統(tǒng)。該系統(tǒng)中必不可少的是發(fā)聲系統(tǒng)和聽覺系統(tǒng)。連續(xù)發(fā)射頻率可調(diào)(CTFM)聲納是1種寬帶主動式聲學(xué)探測方式，在機器人導(dǎo)航，以及導(dǎo)盲等領(lǐng)域得到廣泛應(yīng)用。從仿生學(xué)的角度模仿蝙蝠的嘴巴和耳朵，利用CTFM模型構(gòu)建仿生聲納系統(tǒng)來進行聲納信號的發(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），首先對波形進行采樣，將采樣后得到的數(shù)據(jù)點進行量化并存入存儲器，將存儲其中地址對應(yīng)的波形數(shù)據(jù)點，通過DAC轉(zhuǎn)換成模擬量進行輸出[8]。

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

（1）

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

1.1.2 回波采集模塊

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

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

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

 （2）

 （3）

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

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

   （4）

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

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

   （5）

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

   （6）

1.3 位姿感知細(xì)胞

智能車到達室內(nèi)具體位置時，其處于空間中的位姿感知細(xì)胞網(wǎng)絡(luò)（X，Y和Θ）中。此時會利用連續(xù)吸引子網(wǎng)絡(luò)（CAN）來實時控制智能車在（X，Y和Θ）空間中的活動情況。路徑積分是指通過不斷地獲取智能車的里程計、轉(zhuǎn)角傳感器的數(shù)據(jù)以及智能車在（X，Y和Θ）空間中的位姿變化情況來的實現(xiàn)位姿感知細(xì)胞的更新，并且利用生成的仿生聲納模板來實現(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ì)胞活性進行更新。路徑積分令位姿感知細(xì)胞的性能可以獨立于感知變化速率和里程計速度與角速傳感器獲取的角速度，不但可以獲取較為準(zhǔn)確的智能車運行軌跡，而且不需要改變具體的參數(shù)。通過路徑積分，位姿感知細(xì)胞活性得到更新如式（10）所示。

    (10)

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

    (11)

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

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

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

   (12)

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

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

   (13)

其中， 表示當(dāng)前耳蝸圖的像素亮度， 表示數(shù)據(jù)庫中的第i個仿生聲納模板的像素亮度。如果像素亮度之差低于某一閾值，當(dāng)前耳蝸圖被識別出來，對位姿感知細(xì)胞進行校正，如式(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)的位置指紋定位算法相類似，是將實際環(huán)境中的具體位置和Wi-Fi指紋相聯(lián)系，具體的位置與特定的Wi-Fi指紋相對應(yīng)。本文選取的Wi-Fi指紋特征是具體位置檢測得到的基站信號的RSS(信號接收強度)?；赗SS(信號接收強度)的Wi-Fi指紋定位算法主要包括離線采集階段和在線定位階段。

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

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

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

   (16)

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

   （17）

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

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

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

1.4.2在線定位階段

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

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

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

   （18）

其中，是第j個AP在第i個參考點的信號接收強度的平均值， 是實時采集的參考點在第j個AP的信號接收強度的平均值。

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

    （19）

最后對這K個參考點進行加權(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)系。進行更新的連接強度如式(21)所示。

   (21)

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

   (22)

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

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

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

   (23)

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

   (24)

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

使用當(dāng)前位姿信息、Wi-Fi指紋模板和仿生聲納模板與經(jīng)驗圖進行匹配時，當(dāng)?shù)竭_經(jīng)驗過的地方時，進行1次閉環(huán)檢測，并對經(jīng)驗圖進行校正，經(jīng)驗位姿改變?nèi)缡?25)所示。

    (25)

其中， 為校正常數(shù)， 為經(jīng)驗i到其他經(jīng)驗的連接數(shù)； 為從其他經(jīng)驗到經(jīng)驗i的連接數(shù)。