摘要

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

引言

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

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

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

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

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

1   算法原理與設計

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

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

圖1 改進后的仿生聲納SLAM算法的結構框圖

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

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

1.1.1 信號發(fā)生模塊

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

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

（1）

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

1.1.2 回波采集模塊

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

1.2 仿生聲納模板的構建

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

 （2）

 （3）

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

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

   （4）

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

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

   （5）

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

   （6）

1.3 位姿感知細胞

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

1.3.1 CAN模型

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

    (7)

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

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

    (8)

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

    (9)

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

1.3.2 路徑積分

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

    (10)

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

    (11)

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

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

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

   (12)

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

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

   (13)

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

   (14)

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

   (15)

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

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

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

1.4.1離線訓練階段

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

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

   (16)

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

   （17）

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

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

圖2 離線訓練階段原理圖

1.4.2在線定位階段

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

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

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

   （18）

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

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

    （19）

最后對這K個參考點進行加權求和，定位坐標如式（20）所示。

   （20）

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

圖3 在線定位階段原理圖

1.4.3 Wi-Fi指紋模板修正

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

   (21)

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

   (22)

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

1.5 經驗圖的構建與校正

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

   (23)

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

   (24)

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

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

    (25)

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