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

摘要

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

引言

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

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

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

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

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

1   算法原理與設計

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

改進(jìn)后的仿生聲納SLAM定位算法的結構框圖如圖1所示。

圖1 改進(jìn)后的仿生聲納SLAM算法的結構框圖

1.1 仿生聲納系統采集回波信號

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

1.1.1 信號發(fā)生模塊

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

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

（1）

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

1.1.2 回波采集模塊

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

1.2 仿生聲納模板的構建

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

 （2）

 （3）

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

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

   （4）

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

使用標準化雙耳平滑耳蝸圖，計算公式如式（5）所示。

   （5）

將標準化雙耳平滑耳蝸圖作為當前仿生聲納模版，計算公式如式（6）所示。

   （6）

1.3 位姿感知細胞

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

1.3.1 CAN模型

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

    (7)

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

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

    (8)

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

    (9)

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

1.3.2 路徑積分

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

    (10)

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

    (11)

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

1.3.3 仿生聲納模板修正位姿感知細胞

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

   (12)

其中，G_i為局部場(chǎng)景細胞的活動(dòng)水平，為位姿感知細胞的活動(dòng)水平。

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

   (13)

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

   (14)

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

   (15)

其中，n_G為數據庫中的仿生聲納模板的數量，α_t為調整系統的比例因子，α_t設置為0.5。

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

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

1.4.1離線(xiàn)訓練階段

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

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

   (16)

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

   （17）

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

離線(xiàn)階段的原理圖如圖2所示。

圖2 離線(xiàn)訓練階段原理圖

1.4.2在線(xiàn)定位階段

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

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

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

   （18）

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

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

    （19）

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

   （20）

(X_i,Y_i)是K個(gè)Wi-Fi指紋對應的坐標。在線(xiàn)階段原理圖如圖3所示。

圖3 在線(xiàn)定位階段原理圖

1.4.3 Wi-Fi指紋模板修正

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

   (21)

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

   (22)

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

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

每個(gè)經(jīng)驗 關(guān)聯(lián)經(jīng)驗能級E_i，位姿細胞 P_xi,yi,θi和局部場(chǎng)景細胞 G_i，與Wi-Fi指紋模板 Yi，第i個(gè)經(jīng)驗如式(23)所示。

   (23)

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

   (24)

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

使用當前位姿信息、Wi-Fi指紋模板和仿生聲納模板與經(jīng)驗圖進(jìn)行匹配時(shí)，當到達經(jīng)驗過(guò)的地方時(shí)，進(jìn)行1次閉環(huán)檢測，并對經(jīng)驗圖進(jìn)行校正，經(jīng)驗位姿改變如式(25)所示。

    (25)

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