一種改進(jìn)型RatSLAM算法構(gòu)建認(rèn)知地圖的研究

洪濤，史濤,2，任紅格,3

（1.華北理工大學(xué)電氣工程學(xué)院，唐山063210；2.天津理工大學(xué)電子工程學(xué)院，天津300384；3.天津城建大學(xué)控制與機(jī)械工程學(xué)院，天津300384）

0 引言

隨著社會(huì)的智能化發(fā)展，將會(huì)存在越來越多的機(jī)器人與我們生活在一起，它們將面對這個(gè)復(fù)雜且多變的社會(huì)環(huán)境，這就要求機(jī)器人在未知環(huán)境中工作時(shí)需要更精確的定位與識(shí)別，以此來對機(jī)器人進(jìn)行即時(shí)定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）[1]，目前較受歡迎的有基于生物海馬神經(jīng)元機(jī)制的RatSLAM算法，Milford教授通過對大鼠大腦假設(shè)的“位姿細(xì)胞”進(jìn)行數(shù)學(xué)建模，通過視覺里程計(jì)驅(qū)動(dòng)導(dǎo)航系統(tǒng)，建立一個(gè)具有仿生性的實(shí)時(shí)定位與地圖構(gòu)建算法，該算法具有更完善的生物合理性以及強(qiáng)的環(huán)境適應(yīng)性[2-3]。實(shí)驗(yàn)表明這種純視覺的仿生SLAM算法在室內(nèi)靜態(tài)環(huán)境中有著良好的定位導(dǎo)航能力[4]，但在SLAM系統(tǒng)的前端中對于視覺里程計(jì)過于依賴，存在著圖像匹配精度受限，累計(jì)誤差以及環(huán)境適應(yīng)性較差等問題，因此RatSLAM還需要更進(jìn)一步的優(yōu)化[5-8]。Tubman等人曾提出引入RGB-D深度信息到RatSLAM算法中來提高精度，但在具體實(shí)踐中具有較大難度，實(shí)用性還有待提升[9]，張瀟等人也嘗試建立融合光學(xué)雙軸速度傳感器和MIMU信息的航跡推算模型，但系統(tǒng)的魯棒性受到影響[10]。孫新柱等人也提出過結(jié)合SURF算子的RatSLAM的方法，但實(shí)時(shí)性并無具體的提升[11]。也有為解決RatSLAM算法視覺模板匹配較差以及特征點(diǎn)誤匹配的問題而提出的將特征點(diǎn)與RatSLAM相結(jié)合的算法，提取RGB圖像的特征，令FAST關(guān)鍵點(diǎn)與BRIEF描述子像匹配，但在動(dòng)態(tài)環(huán)境中的跟蹤效果不太理想[12-13]。

本文在傳統(tǒng)基于海馬神經(jīng)元的RatSLAM算法基礎(chǔ)上，對O-FAST（Oriented FAST）關(guān)鍵點(diǎn)進(jìn)行提取，再使用漸進(jìn)一致采樣（PROSAC）算法進(jìn)行特征點(diǎn)匹配，實(shí)現(xiàn)圖像幀間匹配并重新定位，提高特征匹配的準(zhǔn)確性以及系統(tǒng)魯棒性，最后再通過仿真環(huán)境進(jìn)行軌跡對比來驗(yàn)證構(gòu)建的地圖的準(zhǔn)確性。

1 RatSLAM算法實(shí)現(xiàn)

RatSLAM系統(tǒng)主要包含:位姿細(xì)胞（Pose Cell）、局部場景細(xì)胞（Local View Cell）、經(jīng)歷地圖（Experience Map）。每個(gè)位姿細(xì)胞代表著機(jī)器人某個(gè)時(shí)刻的位置與方向信息，每個(gè)局部場景細(xì)胞表示著機(jī)器人某時(shí)刻所經(jīng)歷場景的圖像特征信息，而經(jīng)歷地圖是將兩者相結(jié)合，將多個(gè)時(shí)刻的位姿與場景信息整合成相關(guān)地圖[14]。RatSLAM算法流程圖如圖1所示。

圖1 RatSLAM算法流程圖

1.1 位姿動(dòng)態(tài)表達(dá)

1.1.1 興奮性更新（Excitatory update）

構(gòu)建一個(gè)三維的高斯矩陣ε來表示興奮度的權(quán)重，εabc的三維矩陣是由一個(gè)二維的高斯矩陣來表示位姿細(xì)胞的興奮性和一個(gè)θ維的矩陣來表征細(xì)胞興奮一維分布的乘積，其計(jì)算公式為：

式中，vxy和vθ分別為平面（x,y）和表示方向θ維的方差常量，a,b,c是x,y,θ的分布系數(shù)。計(jì)算公式如下：

式中，Dx,Dy,Dθ是位姿細(xì)胞在（x,y,θ）的三維矩陣元素，在位姿檢測時(shí)，對相應(yīng)位姿細(xì)胞進(jìn)行刺激，因刺激而產(chǎn)生的興奮度變化活性ΔE為：

1.1.2 抑制（Inhibition）

位姿細(xì)胞相互間會(huì)產(chǎn)生抑制，在興奮度更新期間，每一次的位姿估計(jì)都會(huì)導(dǎo)致抑制權(quán)值矩陣ξ增加，此時(shí)需要引入一個(gè)全局抑制輸入μ對位姿細(xì)胞活性進(jìn)行抑制，抑制可以看作是一個(gè)迭代的過程，抑制后的細(xì)胞活性為：

1.1.3 標(biāo)準(zhǔn)化（Normalization）

CAN神經(jīng)網(wǎng)絡(luò)上的位姿細(xì)胞總活性需要保持不變，通過標(biāo)準(zhǔn)化可實(shí)現(xiàn)總活性保持為一。標(biāo)準(zhǔn)化公式如下：

1.2 路徑積分

路徑積分表示轉(zhuǎn)換角速度與線速度的信息進(jìn)行迭代更新，在RatSLAM算法中即表示由里程計(jì)與（x,y,θ）三維坐標(biāo)獲取的信息通過轉(zhuǎn)碼來更新當(dāng)前的細(xì)胞活性。更新后的細(xì)胞活性為，計(jì)算公式為：

式中，a表示殘余數(shù)矩陣，它與位姿細(xì)胞單元相乘表 示 被 注 入 活 性 的 總 量，δx,δy,δθ表 示 三 維 空 間（x,y,θ）上的整數(shù)偏差，由以下公式計(jì)算得到：

這里λx、λy和λθ是路徑積分常數(shù)。v為平移速度，ω是傳感器獲得的角速度，θ為細(xì)胞優(yōu)先朝向。

1.3 經(jīng)歷地圖

在經(jīng)歷地圖中，我們通過構(gòu)建一個(gè)獨(dú)立的三維坐標(biāo)系（x,y,θ）來表征每個(gè)ei的空間位置，記Li為經(jīng)歷ei在地圖中的位置，ei的向量描述為：

獲得了經(jīng)歷ei的向量描述，便可以通過對比場景的信息來完善地圖的構(gòu)建。判斷當(dāng)前經(jīng)歷ei對應(yīng)的位姿細(xì)胞狀態(tài)Pi與局部場景細(xì)胞Vi跟之前所存儲(chǔ)的經(jīng)歷ei是否匹配，若是不匹配，則求出插值S，公式如下：

式中，NP為當(dāng)前時(shí)刻與上一時(shí)刻的位姿細(xì)胞之間的權(quán)值，NV是當(dāng)前時(shí)刻與上一時(shí)刻局部感知細(xì)胞之間的權(quán)值匹配。通過插值S與閾值相比來判斷目前的場景是否需要更新，若S大于閾值，則判斷當(dāng)前場景為新場景，記為一個(gè)新的ei經(jīng)歷。

2 對RatSLAM算法的改進(jìn)

基于RatSLAM仿生算法去構(gòu)建認(rèn)知地圖，本文提出了一種結(jié)合ORB特征點(diǎn)的RatSLAM算法，通過在ORB特征點(diǎn)的提取中引入自適應(yīng)閾值來進(jìn)行關(guān)鍵點(diǎn)提取，在匹配階段利用漸進(jìn)一致采樣算法對特征點(diǎn)進(jìn)行圖像匹配，通過減少迭代的次數(shù)來加強(qiáng)算法在動(dòng)態(tài)環(huán)境中的實(shí)時(shí)性與精確性。

2.1 改進(jìn)特征點(diǎn)匹配

為了解決特征點(diǎn)匹配可能會(huì)出現(xiàn)誤匹配、低精度的問題，本文結(jié)合漸進(jìn)一致采樣算法（PROSAC）來進(jìn)行特征匹配，可以減小匹配對的計(jì)算量，優(yōu)化系統(tǒng)的動(dòng)態(tài)實(shí)時(shí)性。

漸進(jìn)一致采樣算法是先對數(shù)據(jù)點(diǎn)進(jìn)行排序，在評價(jià)函數(shù)值最高的數(shù)據(jù)點(diǎn)子集中隨機(jī)采樣，即從不斷增大的最佳對應(yīng)點(diǎn)集合中進(jìn)行采樣，這樣整體的采集樣本中正確的點(diǎn)集就會(huì)比錯(cuò)誤的點(diǎn)集要多，因此可以節(jié)省計(jì)算量，提高匹配速度。

在提取的特征點(diǎn)的匹配對中選取離特征點(diǎn)與最近和第二近相鄰點(diǎn)的漢明距離（Hamming Distance）之比為：

Vp是特征點(diǎn)p的特征向量，Vap是離特征點(diǎn)p最近的特征向量，Vmp是離特征點(diǎn)p第二近的特征向量，分別求得特征向量與最近點(diǎn)以及特征向量與次近點(diǎn)的距離D，得到漢明距離之比R。

漢明距離之比R將決定數(shù)據(jù)點(diǎn)子集中采樣點(diǎn)匹配質(zhì)量的好壞，若R小于動(dòng)態(tài)閾值v，則匹配按照由好到差來排列，此時(shí)得到質(zhì)量函數(shù)如下式：

漸進(jìn)一致采樣算法特征匹配的實(shí)驗(yàn)步驟：

（1）設(shè)定迭代最大次數(shù)為X。

（2）檢測當(dāng)前迭代次數(shù)，若迭代次數(shù)大于迭代最大值，則返回統(tǒng)計(jì)的內(nèi)點(diǎn)數(shù)量最大的一組數(shù)據(jù)；若迭代次數(shù)小于最大迭代次數(shù)，流程進(jìn)入下一步。

（3）將數(shù)據(jù)點(diǎn)子集中的采樣點(diǎn)以匹配質(zhì)量從好到壞排序得到數(shù)據(jù)點(diǎn)集合U，從中選取N個(gè)較好的數(shù)據(jù)子集。

（4）再從N個(gè)數(shù)據(jù)子集中隨機(jī)選取n個(gè)匹配點(diǎn)數(shù)據(jù)來計(jì)算模型的參數(shù)β。

（5）判斷模型的參數(shù)是否大于設(shè)定的誤差閾值ηprosac，若參數(shù)小于閾值，則判定該點(diǎn)為內(nèi)點(diǎn)，若大于閾值，則舍棄匹配點(diǎn)。

（6）設(shè)定數(shù)量閾值ηnumber，判斷內(nèi)點(diǎn)數(shù)量若大于閾值，進(jìn)行特征點(diǎn)誤匹配的剔除，得到數(shù)學(xué)模型；若內(nèi)點(diǎn)數(shù)小于閾值，則返回步驟（2），當(dāng)前迭代次數(shù)加1。

PROSAC算法特征匹配的流程圖如圖2所示。

圖2 PROSAC算法的特征匹配

2.2 閉環(huán)檢測

在實(shí)際的仿真中所使用的視覺里程計(jì)以及傳感器是會(huì)產(chǎn)生累計(jì)誤差的，這些誤差將會(huì)對認(rèn)知地圖的構(gòu)建產(chǎn)生影響，因此為驗(yàn)證改進(jìn)之后的匹配精度效果，我們需要對構(gòu)建的地圖進(jìn)行閉環(huán)檢測，閉環(huán)檢測就是物體運(yùn)動(dòng)的實(shí)際目標(biāo)點(diǎn)與預(yù)測的位置點(diǎn)進(jìn)行對比，閉環(huán)檢測的示意如圖3所示，該示意圖就顯示出了當(dāng)累計(jì)誤差的不斷積累會(huì)使物體運(yùn)動(dòng)出現(xiàn)偏離目標(biāo)點(diǎn)的現(xiàn)象。因此，本文進(jìn)行機(jī)器人仿真實(shí)驗(yàn)的閉環(huán)檢測來減小誤差，提高認(rèn)知地圖構(gòu)建的魯棒性。

圖3 閉環(huán)檢測

本文使用ROS平臺(tái)進(jìn)行機(jī)器人運(yùn)動(dòng)仿真，將采集的視覺圖像進(jìn)行閉環(huán)檢測，方法如下：

（1）使用采集的圖像進(jìn)行灰度處理，再將圖像信息轉(zhuǎn)換成矩陣。

（2）對每一幅圖像與之前所保存的圖像進(jìn)行視覺模板匹配的檢測，這里設(shè)定相似度的閾值為0.75，判斷圖像是否達(dá)到閾值。

（3）當(dāng)圖像匹配度大于等于閾值，判定與該圖像為儲(chǔ)存過的圖像，與之相對應(yīng)的神經(jīng)元細(xì)胞也是達(dá)到了閾值。

（4）判定兩個(gè)圖像場景的采樣間隔是否大于50，此步驟是為避免出現(xiàn)機(jī)器人原地運(yùn)動(dòng)的現(xiàn)象，若采樣間隔大于50且相似匹配度超過閾值，則判定檢測到閉環(huán)點(diǎn)位置；若不滿足二者之中的任一條件，則判定閉環(huán)檢測未完成。

3 實(shí)驗(yàn)分析

本文實(shí)驗(yàn)采用的是Ubuntu操作系統(tǒng)下的開源平臺(tái)ROS—機(jī)器人操作系統(tǒng)進(jìn)行仿真，實(shí)驗(yàn)數(shù)據(jù)來自于德國卡爾斯魯厄理工學(xué)院和豐田美國技術(shù)研究院聯(lián)合構(gòu)建的KITTI開源數(shù)據(jù)集，通過自動(dòng)駕駛汽車以及傳感器對行車環(huán)境進(jìn)行數(shù)據(jù)采集，數(shù)據(jù)采集平臺(tái)裝配有2架灰度攝像機(jī)，2架彩色攝像機(jī)，1個(gè)Velodyne64線3D激光雷達(dá)，4個(gè)光學(xué)鏡頭，1個(gè)GPS導(dǎo)航系統(tǒng)。

3.1 特征點(diǎn)匹配對比實(shí)驗(yàn)

對提取的特征點(diǎn)利用PROSAC算法進(jìn)行匹配。參數(shù)設(shè)定，圖像匹配特征點(diǎn)的漢明距離D為最小距離的兩倍，漢明距離之比R設(shè)閾值為0.6，特征點(diǎn)設(shè)置為300。

實(shí)驗(yàn)結(jié)果如圖4所示，為使顯示較為清晰，匹配的連線通過不同顏色連接?？梢钥吹?，圖4（a）顯示的傳統(tǒng)算法在圖像匹配時(shí)存在匹配對較多，誤匹配概率較大等問題；圖4（b）為改進(jìn)算法匹配圖；圖4（c）顯示的是SURF+RatSLAM算法的匹配效果，匹配率相較于傳統(tǒng)RatSLAM具有一定優(yōu)勢，但在匹配時(shí)間上耗時(shí)較久；圖4（d）為本文算法匹配效果圖。通過對比可以看出相比較于傳統(tǒng)算法與SURF+RatSALM算法，改進(jìn)之后的算法通過約束篩選，能夠降低誤匹配對，只對圖像關(guān)鍵點(diǎn)像素進(jìn)行匹配，并且減少了重疊冗雜的現(xiàn)象，縮短圖像匹配的時(shí)間，提升算法的動(dòng)態(tài)性能。

圖4 RatSLAM算法改進(jìn)前后匹配效果的對比

對實(shí)驗(yàn)的匹配率進(jìn)行分析，結(jié)果如表1所示，本文所提出的改進(jìn)型RatSLAM算法相比較于傳統(tǒng)算法以及另一種結(jié)合SURF算子的算法在匹配成功率上有所提升，改進(jìn)后的RatSLAM算法通過篩選特征點(diǎn)得到了更精簡的匹配對，在匹配的速度與匹配精度上優(yōu)于傳統(tǒng)的RatSLAM以及結(jié)合SURF描述子的RatSLAM算法。

表1 RatSLAM算法改進(jìn)前后特征提取與匹配的時(shí)間對比

3.2 閉環(huán)點(diǎn)檢測實(shí)驗(yàn)

本文通過在ROS中搭建仿真環(huán)境進(jìn)行閉環(huán)檢測驗(yàn)證構(gòu)建地圖的精確性，仿真環(huán)境地圖如圖5所示。實(shí)驗(yàn)分成兩個(gè)對照組，一組僅憑借里程計(jì)自帶算法自主導(dǎo)航構(gòu)建地圖，另一組基于改進(jìn)后RatSLAM算法的經(jīng)歷地圖，機(jī)器人圖像采集0.2s/幀，設(shè)定運(yùn)行速度最大不超過0.1m/s，角速度0.05rad/s，圖6（a）是由里程計(jì)獲取信息得到的地圖，圖6（b）是本文改進(jìn)后RatSLAM構(gòu)建的經(jīng)歷地圖，可以看出傳統(tǒng)的里程計(jì)構(gòu)建的環(huán)境地圖會(huì)因?yàn)槔塾?jì)誤差而產(chǎn)生漂移，而本文所構(gòu)建的地圖在閉環(huán)檢測中的精度更高。

圖5 仿真環(huán)境

圖6 RatSLAM算法改進(jìn)前后的閉環(huán)檢測對比

通過圖像匹配與閉環(huán)檢測得到的誤差數(shù)據(jù)如圖7所示，改進(jìn)后的算法在地圖的定位精度上有著明顯的提升。本文采用均方根誤差（Root Mean Square Error，RMSE）公式進(jìn)行偏差分析，偏差的大小反映算法在實(shí)時(shí)匹配中準(zhǔn)確性。式中N表示采樣的次數(shù)，ui表示第i采樣對應(yīng)的圖像觀測的位置坐標(biāo)，Ui0表示第i次采樣時(shí)的真實(shí)位置坐標(biāo)，通過計(jì)算來驗(yàn)證改進(jìn)前后Rat?SLAM算法在定位匹配上的準(zhǔn)確性與穩(wěn)定性。從表2的結(jié)果可知，傳統(tǒng)算法的均方根誤差值要大于改進(jìn)后的算法，說明本文算法的仿真結(jié)果與真實(shí)軌跡的離散程度更小，更貼近真實(shí)軌跡，也說明本文算法在圖像模板匹配定位上有更高的精確性和魯棒性。

圖7 累計(jì)誤差對比圖

表2 RatSLAM算法改進(jìn)前后均方根誤差對比

4 結(jié)語

本文通過引入ORB特征點(diǎn)對傳統(tǒng)RatSLAM算法的圖像提取與匹配進(jìn)行改進(jìn)，設(shè)置一種自適應(yīng)閾值對圖像的特征點(diǎn)提取進(jìn)行篩選，簡化特征點(diǎn)的冗雜與聚集，實(shí)現(xiàn)匹配的快速性并降低誤匹配的現(xiàn)象。同時(shí)對傳統(tǒng)RatSLAM算法在動(dòng)態(tài)環(huán)境中的適應(yīng)性較差的問題作出改善，使得算法的魯棒性變強(qiáng)。但是在仿真實(shí)驗(yàn)中也出現(xiàn)了因?yàn)閷?shí)時(shí)匹配而導(dǎo)致的定位漂移的現(xiàn)象，這可以成為之后的實(shí)驗(yàn)研究重點(diǎn)，若能夠?qū)崿F(xiàn)突破，對于汽車自動(dòng)駕駛方面將具有良好的應(yīng)用前景。