一種改進型RatSLAM算法構建認知地圖的研究

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

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

0 引言

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

本文在傳統基于海馬神經元的RatSLAM算法基礎上，對O-FAST（Oriented FAST）關鍵點進行提取，再使用漸進一致采樣（PROSAC）算法進行特征點匹配，實現圖像幀間匹配并重新定位，提高特征匹配的準確性以及系統魯棒性，最后再通過仿真環境進行軌跡對比來驗證構建的地圖的準確性。

1 RatSLAM算法實現

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

圖1 RatSLAM算法流程圖

1.1 位姿動態表達

1.1.1 興奮性更新（Excitatory update）

構建一個三維的高斯矩陣ε來表示興奮度的權重，εabc的三維矩陣是由一個二維的高斯矩陣來表示位姿細胞的興奮性和一個θ維的矩陣來表征細胞興奮一維分布的乘積，其計算公式為：

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

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

1.1.2 抑制（Inhibition）

位姿細胞相互間會產生抑制，在興奮度更新期間，每一次的位姿估計都會導致抑制權值矩陣ξ增加，此時需要引入一個全局抑制輸入μ對位姿細胞活性進行抑制，抑制可以看作是一個迭代的過程，抑制后的細胞活性為：

1.1.3 標準化（Normalization）

CAN神經網絡上的位姿細胞總活性需要保持不變，通過標準化可實現總活性保持為一。標準化公式如下：

1.2 路徑積分

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

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

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

1.3 經歷地圖

在經歷地圖中，我們通過構建一個獨立的三維坐標系（x,y,θ）來表征每個ei的空間位置，記Li為經歷ei在地圖中的位置，ei的向量描述為：

獲得了經歷ei的向量描述，便可以通過對比場景的信息來完善地圖的構建。判斷當前經歷ei對應的位姿細胞狀態Pi與局部場景細胞Vi跟之前所存儲的經歷ei是否匹配，若是不匹配，則求出插值S，公式如下：

式中，NP為當前時刻與上一時刻的位姿細胞之間的權值，NV是當前時刻與上一時刻局部感知細胞之間的權值匹配。通過插值S與閾值相比來判斷目前的場景是否需要更新，若S大于閾值，則判斷當前場景為新場景，記為一個新的ei經歷。

2 對RatSLAM算法的改進

基于RatSLAM仿生算法去構建認知地圖，本文提出了一種結合ORB特征點的RatSLAM算法，通過在ORB特征點的提取中引入自適應閾值來進行關鍵點提取，在匹配階段利用漸進一致采樣算法對特征點進行圖像匹配，通過減少迭代的次數來加強算法在動態環境中的實時性與精確性。

2.1 改進特征點匹配

為了解決特征點匹配可能會出現誤匹配、低精度的問題，本文結合漸進一致采樣算法（PROSAC）來進行特征匹配，可以減小匹配對的計算量，優化系統的動態實時性。

漸進一致采樣算法是先對數據點進行排序，在評價函數值最高的數據點子集中隨機采樣，即從不斷增大的最佳對應點集合中進行采樣，這樣整體的采集樣本中正確的點集就會比錯誤的點集要多，因此可以節省計算量，提高匹配速度。

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

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

漢明距離之比R將決定數據點子集中采樣點匹配質量的好壞，若R小于動態閾值v，則匹配按照由好到差來排列，此時得到質量函數如下式：

漸進一致采樣算法特征匹配的實驗步驟：

（1）設定迭代最大次數為X。

（2）檢測當前迭代次數，若迭代次數大于迭代最大值，則返回統計的內點數量最大的一組數據；若迭代次數小于最大迭代次數，流程進入下一步。

（3）將數據點子集中的采樣點以匹配質量從好到壞排序得到數據點集合U，從中選取N個較好的數據子集。

（4）再從N個數據子集中隨機選取n個匹配點數據來計算模型的參數β。

（5）判斷模型的參數是否大于設定的誤差閾值ηprosac，若參數小于閾值，則判定該點為內點，若大于閾值，則舍棄匹配點。

（6）設定數量閾值ηnumber，判斷內點數量若大于閾值，進行特征點誤匹配的剔除，得到數學模型；若內點數小于閾值，則返回步驟（2），當前迭代次數加1。

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

圖2 PROSAC算法的特征匹配

2.2 閉環檢測

在實際的仿真中所使用的視覺里程計以及傳感器是會產生累計誤差的，這些誤差將會對認知地圖的構建產生影響，因此為驗證改進之后的匹配精度效果，我們需要對構建的地圖進行閉環檢測，閉環檢測就是物體運動的實際目標點與預測的位置點進行對比，閉環檢測的示意如圖3所示，該示意圖就顯示出了當累計誤差的不斷積累會使物體運動出現偏離目標點的現象。因此，本文進行機器人仿真實驗的閉環檢測來減小誤差，提高認知地圖構建的魯棒性。

圖3 閉環檢測

本文使用ROS平臺進行機器人運動仿真，將采集的視覺圖像進行閉環檢測，方法如下：

（1）使用采集的圖像進行灰度處理，再將圖像信息轉換成矩陣。

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

（3）當圖像匹配度大于等于閾值，判定與該圖像為儲存過的圖像，與之相對應的神經元細胞也是達到了閾值。

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

3 實驗分析

本文實驗采用的是Ubuntu操作系統下的開源平臺ROS—機器人操作系統進行仿真，實驗數據來自于德國卡爾斯魯厄理工學院和豐田美國技術研究院聯合構建的KITTI開源數據集，通過自動駕駛汽車以及傳感器對行車環境進行數據采集，數據采集平臺裝配有2架灰度攝像機，2架彩色攝像機，1個Velodyne64線3D激光雷達，4個光學鏡頭，1個GPS導航系統。

3.1 特征點匹配對比實驗

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

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

圖4 RatSLAM算法改進前后匹配效果的對比

對實驗的匹配率進行分析，結果如表1所示，本文所提出的改進型RatSLAM算法相比較于傳統算法以及另一種結合SURF算子的算法在匹配成功率上有所提升，改進后的RatSLAM算法通過篩選特征點得到了更精簡的匹配對，在匹配的速度與匹配精度上優于傳統的RatSLAM以及結合SURF描述子的RatSLAM算法。

表1 RatSLAM算法改進前后特征提取與匹配的時間對比

3.2 閉環點檢測實驗

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

圖5 仿真環境

圖6 RatSLAM算法改進前后的閉環檢測對比

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

圖7 累計誤差對比圖

表2 RatSLAM算法改進前后均方根誤差對比

4 結語

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