基于局部熵的SLAM 視覺里程計優化算法

于雅楠 衛紅 陳靜

在移動機器人研究領域,同步定位與地圖創建(Simultaneous localization and mapping,SLAM)技術使機器人從未知環境的未知地點出發,在運動過程中通過機器人搭載視覺、激光等傳感器重復觀測到的地圖特征定位自身位置和姿態,再根據自身位置增量式地構建地圖,從而達到同時定位和地圖構建的目的,為機器人自主定位、避障、導航、路徑規劃等任務提供必要的支持.鑒于SLAM 技術是使機器人真正實現完全自動化的關鍵技術之一,其應用已廣泛覆蓋無人駕駛汽車、無人機、人機交互、虛擬現實、增強現實等人工智能領域[1?3],并且結合深度學習的SLAM 也成為近兩年的研究熱點[4?6].在SLAM 應用中,相比能夠測試環境范圍的激光傳感器而言,可直接獲取圖像信息的視覺傳感器(包括單目、雙目、深度相機)在環境的可視化上具備更多優勢,對移動機器人進行場景識別以及感知理解具有更廣泛的應用前景.

在視覺SLAM 中,視覺里程計作為SLAM 系統前端,其意義在于依靠圖像信息來增量式地估計相機運動.視覺里程計根據其原理不同主要分為非直接方法與直接方法兩類[7],最本質區別在于,傳統的非直接方法借助于人工提取的圖像特征信息,通過優化特征的重投影誤差來計算相機的姿態運動,而直接方法借助于圖像包含的全部像素信息,即優化像素灰度誤差來計算相機的姿態運動.除此之外,完整的SLAM系統還包括運動回環檢測、精確地圖創建、軌跡與地圖的全局優化.SLAM 系統框架如圖1 所示.

圖1 SLAM 系統框架Fig.1 SLAM framework

SLAM 研究仍然存在諸多挑戰,在利用稀疏特征的非直接方法中,存在如何設計一種最優表示圖像信息的特征、如何使特征在光照變化、相機參數改變或紋理信息不足的環境中具有較高魯棒性等問題.在直接方法中,雖然圖像信息可以最大化被利用,但如果存在遮擋、漫反射或光線變化等特殊情況,以優化像素值為目標函數的前提假設則不成立.除上述討論的關鍵技術外,SLAM 研究對于應用環境也有特別的要求,在復雜以及多動態目標運動環境中SLAM 系統顯得更為脆弱[8],長時間或遠距離的實時應用對系統的魯棒性需求更高.另外,SLAM 位置及姿態跟蹤結果對于突然的環境亮度變化極為敏感[9],過曝光或欠曝光等情況使成像區域出現飽和現象,從而影響特征提取以及深度估計.

本文提出一種基于局部圖像熵的細節增強優化算法,改善視覺里程計中由于特征不充分而造成的圖像匹配失敗以及跟蹤丟失等問題,提高移動機器人視覺定位精度和系統魯棒性.首先介紹了SLAM 關鍵技術及可行性,結合應用中面臨的實際問題,即圖像紋理或結構特征不豐富、視覺傳感器快速運動或大角度轉動時出現跟蹤丟失等問題,提出基于信息熵的圖像特征提取方法,構建SLAM 系統,對比ORBSLAM2 系統進行了開源數據集驗證及分析,最后得出相關結論.

1 相關研究

1.1 SLAM 技術

2007 年Davison 等開發的基于單目攝像頭的Mono-SLAM[10]系統成功實現了機器人視覺SLAM.2007 年英國牛津大學Klein 和Murray 發布了獨特創新的雙線程并行追蹤與建圖SLAM 算法[11],即前端跟蹤后端建圖的PTAM,引入關鍵幀的特征分析和姿態跟蹤后,使得SLAM 在實時性及穩定性上有了較大的突破.2014 年德國弗賴堡大學Endres等開發的RGBD-SLAM[12]與2015 年西班牙薩拉戈薩大學Mur-Artal 等開發的ORB-SLAM[13]開源系統都是目前非常成功的PTAM 的延伸.其中,ORB-SLAM2[14]基于orb特征完成圖像序列的匹配及跟蹤,甚至回環檢測和再定位,并且提供的接口可以實現在單目、雙目、RGB-D 及ROS(機器人操作系統)下的使用.由于ORB-SLAM2 構建的是稀疏三維點圖,在避障和導航等實際任務中無法直接使用,2017年Lv 等基于該系統利用八叉樹結構以及空間預測擴展了密集型三維地圖[15].

與基于稀疏特征的非直接方法不同,2011 年Newcombe等提出的DTAM 解決特征不豐富情況下最大化地利用圖像的全部像素信息進行精細化建圖[16].為了減小稠密SLAM計算量并提高同步性,作為半稠密型的代表,Engel 等先后于2014 年發布的LSD-SLAM[17]和2018 年發布的DSO 視覺里程計[18]只關注有信息區域或有紋理區域的像素信息估計,解決大尺度空間機器人定位及稠密地圖創建的系統性能問題.2014 年Forster 等開發的SVO-SLAM[19]結合稀疏特征與直接法,只在前端的圖像配準部分根據特征點周圍信息采用直接法計算,又稱半直接法.

1.2 相機運動估計

視覺里程計作為SLAM 前端能夠計算任意兩幀圖像之間的轉換關系,進而估計出粗略的相機運動.相比直接法,基于稀疏特征的非直接法更加穩定,并且對光照變化的魯棒性更高.通過依次提取相鄰幀圖像特征點、計算特征描述子、匹配特征以確定圖像之間的關聯性,估計相機運動姿態.

基于RGB-D 相機獲取的彩色圖像以及深度信息,通常采用迭代最近點方法(ICP)將稀疏特征點對齊,并求解位姿估計問題.假設已根據提取的RGB-D 數據特征點找到兩幅圖像之間的特征匹配關系,計算出空間坐標分別為P1={p11,p12,···,p1n}與P2={p21,p22,···,p2n}.兩組空間點之間存在姿態變換R、t,使得任意特征點有

構造目標函數對投影誤差作最小化處理,得到最優的位姿估計,即:

此處,n表示參與優化的圖像特征點個數.

將當前幀特征點與臨近幀特征點以及關鍵幀特征點進行數據關聯,從而得到機器人局部和全局的位姿估計值.

1.3 本文貢獻

在實際應用中,目前主流的基于稀疏特征的SLAM 系統無論是在視覺里程計環節、閉環檢測環節亦或是重定位環節,都依靠從圖像提取的具有表征性的大量特征點,無論采用何種特征提取方法,在特征定位上都需要耗費大量時間.除此之外,由于特征不足而造成的軌跡跟蹤丟失也是基于稀疏法的SLAM 系統常常遇到的問題.例如,在使用ORB-SLAM2系統對開源TUM RGB-D 數據集fr1_desk 相機運動進行跟蹤時,在已與深度圖像關聯的RGB 圖像序列中,第158 幀與第159 幀之間,如圖2 所示,由于相機發生了快速的轉動,導致圖像序列中的144 幀數據跟蹤失敗.

圖2 幀間相機大角度轉動Fig.2 Big camera rotation in adjacent images

為解決此類問題,本文提出了一種圖像關聯準確率相對較高的SLAM 視覺里程計優化算法,提出根據局部信息熵值來篩選圖像有效信息,解決系統在提取特征點、匹配特征點、定位特征點上消耗大量的時間問題.另外,本文對局部圖像進行細節增強,改善視覺里程計中由于特征不足而造成的圖像匹配失敗等問題,提高移動機器人視覺定位精度和系統魯棒性.

2 系統構建

2.1 算法改進

本文采用基于圖像熵的視覺里程計優化算法,其系統流程如圖3 所示.

圖3 視覺里程計優化算法Fig.3 Optimization visual odometry algorithm

首先,構建圖像金字塔,增加特征的尺度不變特性.由于每層圖像都根據尺度因子進行了縮放處理,為了使圖像特征實現均勻化提取,并且降低后續圖像匹配時的復雜度,在不同的尺度空間下,根據圖像塊的尺寸定義(width,height)對圖像進行區域分割.

逐層對每個圖像塊進行角點特征的快速提取,通過四叉樹方式對關鍵點進行分配和管理,根據預先設定的閾值選取影響度大的特征(最強壯的特征)作為該結點的特征進行保留,以此實現圖像特征的均勻分布.同時,增加特征的旋轉不變特性,計算特征點在圖像中的實際位置,并生成特征描述子進行保存.

在原有算法中,為了使特征在整幅圖像中均勻分布,算法對每個圖像塊均做提取角點的操作.這樣做的結果是,既增加了無紋理或像素梯度不明顯區域的計算量,又可能造成某些區域保留下來的角點與相鄰幀圖像該區域角點并不能建立有效的匹配關系,反而其他區域中表征紋理或結構信息的角點被刪除掉以滿足預先設定的特征參數閾值.

另外,原算法對光照變化的容忍度低,一幅圖像中存在曝光不足或過曝光時,算法中特征提取能力并不會因此而改變,從而影響特征的檢測能力.在圖3 中,虛線框中為視覺里程計的優化方法,增加基于局部信息熵的細節增強模塊,改善連續的幀間圖像匹配效果.

2.2 基于局部信息熵篩選

信息熵最先由美國工程師Shannon 提出[20],在圖像處理技術層面上,圖像的信息熵能夠直接表示圖像中含有的信息量,也是對圖像中信息的不確定性的一種度量方式.本文中,為更有效提取能夠表征圖像紋理或結構信息的特征,同時減小特征提取計算量,利用圖像熵的信息特征統計形式,對圖像塊進行灰度分布及概率計算:

其中,圖像一維熵H(x)表示圖像灰度分布的聚集特征,p(xi)表示灰度為i(i=0,···,255)的像素在圖像中出現的概率.如果像素灰度值分布范圍大,則信息熵值較大,圖像紋理信息豐富或圖像像素梯度變化明顯.反之,圖像灰度單一,信息熵較小,則認為該圖像無紋理信息或圖像像素梯度變化不明顯,在該圖像塊中提取的特征對圖像匹配貢獻不大,可進行區域剔除.

通過對圖像塊進行信息特征統計,H(x)越接近0,表明該圖像塊內像素梯度值越小,圖像內紋理或結構信息不足.故將H(x)與塊剔除閾值R進行比較,完成篩選.這里閾值R為經驗值,可根據視頻序列中特征的豐富程度來定義.

2.3 光照自適應調整

提出一種光照自適應優化方法,對保留的局部圖像進行非線性調整,降低圖像局部的陰影和光照變化所造成的影響,增強該區域中亮與暗的對比度,增加圖像細節并抑制噪音干擾.目前,Gamma 矯正是成像設備中普遍使用的一種亮度非線性逐點調整方法[21?22].圖像亮度Gamma 矯正函數:

其中,I(x,y)是像素(x,y)的灰度值,G(x,y)是像素(x,y)的校正值,γ為校正參數.如圖4 所示,γ取值小于1 可提高圖像亮度;相反,γ取值大于1 可降低圖像亮度.

圖4 Gamma 非線性曲線Fig.4 Gamma nonlinear curves

分別選取γ=0.4 和γ=2.2 對兩幅圖片進行亮度調整,與原圖對比結果如圖5 所示,圖中局部區域對比度在Gamma 修正后有所增加,但同時其他區域的圖像也相應受到抑制.

圖5 Gamma 修正Fig.5 Gamma correction

為了只對局部圖像進行自適應調整,這里,γ由圖像塊的平均灰度值決定:

其中,μ為圖像灰度校正閾值,取值范圍在(0,1)之間,當圖像平均灰度值大于255·μ時根據γ值暗化圖像,相反,當圖像平均灰度值小于255·μ時根據γ值亮化圖像.當μ取值0.5 時,γ參數曲線如圖6 所示.

圖6 γ 參數曲線Fig.6 γ parameter curve

3 實驗結果

系統分別采用RGB-D 數據集fr1_desk、fr1_360、fr1_floor 和fr1_room 進行實驗驗證,圖像序列讀取速度設為30 幀/秒,跟蹤結果與標準軌跡數據進行誤差計算,同時與開源ORB-SLAM2 系統進行對比與分析.所有實驗在Intel CoreTMi5-3470 CPU、主頻3.20 GHz、四核、內存15.6 GB 配置的臺式機上完成,系統后端采用G2O 進行基于姿態圖的位姿優化,生成運動軌跡.

3.1 基于圖像熵的視覺里程計

以fr1_desk 數據集為例,提取數據集中彩色單幀圖像,首先進行灰度化處理,采用雙線性插值采樣方法建立圖像金字塔.在金字塔圖像中劃分圖像塊,基于信息熵閾值R=5對圖像塊進行篩選,減小無紋理或像素梯度不明顯區域對計算資源的占用,在保留的圖像區域中通過自適應光照調整,增強對比度與圖像細節.如表1 所示,通過分析圖像灰度校正閾值μ對跟蹤精度以及跟蹤效果的影響,系統選用μ=0.5.

表1 閾值選取Table 1 Threshold selection

圖像塊篩選及局部細節增強結果如圖7 所示.

圖7 處理結果Fig.7 Processing results

提取計算速度較快的orb 特征,根據生成的描述算子進行相鄰幀圖像匹配.優化后任意相鄰圖像的匹配結果如圖8所示.

圖8 匹配結果Fig.8 Matching results

如前文所述,運行測試數據集fr1_desk 時,相機在第158幀與第159 幀之間發生了快速的轉動,由于缺少有效特征作為匹配依據,從而導致圖像序列中的144 幀數據跟蹤失敗.采用本文提出的圖像塊篩選及局部細節增強技術,在相機發生大角度轉動位置,相鄰幀之間的特征匹配效果發生明顯的改善,優化前后匹配結果如圖9 所示.

圖9 匹配結果對比Fig.9 Matching results compared

圖9 中,由于利用了圖像塊的信息熵值判斷其信息量大小,對比度低以及梯度變化小的圖像塊被自動剔除,對保留的圖像塊進行亮度自適應調整,增強局部圖像細節,盡可能多地提取能夠表征圖像信息的局部特征點作為相鄰幀匹配以及關鍵幀匹配的關聯依據,進一步提高相機運動的魯棒性,避免由于幀間匹配失敗而造成的運動跟蹤丟失問題.

根據匹配結果計算幀間的R,t變換關系,后端采用G2O進行基于姿態圖的位姿優化,最終生成運動軌跡,如圖10 所示.

圖10 運動軌跡Fig.10 Motion trajectory

3.2 SLAM 結果分析

3.2.1 跟蹤精度

在跟蹤精度上,絕對軌跡誤差反映了運動估計值與標準值之間的差異.以fr1_desk 數據集為例,算法優化后的運動軌跡與標準值之間的誤差如圖11 所示.

圖11 絕對軌跡誤差Fig.11 Absolute trajectory error

采用反映測量精密度的絕對軌跡誤差均方根(Root mean square error,RMSE)作為評價標準,絕對軌跡誤差均方根RMSE(x)定義如下:

其中,xe,i表示圖像序列中第i幀的位置估計值,xs,i表示圖像序列中第i幀的位置標準值.

除此之外,相對位姿誤差是一種相對位姿轉換的標準度量,由于相對位姿包括平移分量和旋轉分量,故通常將相對位姿分解為位姿平移部分和位姿旋轉部分分別進行評價.算法優化后的相對位姿與標準相對位姿之間的誤差如圖12 所示.

圖12 相對位姿誤差Fig.12 Relative pose error

同樣,采用反映測量精密度的相對位姿誤差均方根(RMSE)作為評價標準,相對位姿誤差均方根RMSE(T)定義如下:

其中,Te,ij=Te,i ?Te,j表示估計位姿在圖像序列中第i幀和第j幀之間的相對變換,Ts,ij=Ts,i ?Ts,j表示標準位姿在圖像序列中第i幀和第j幀之間的相對變換.

通過對比ORB-SLAM2 算法與優化SLAM 算法的運動軌跡誤差,本文提出的方法在運動跟蹤精度上有所改善,基于RGB-D 數據集的測試結果如表2 所示.其中,由于對局部圖像進行細節增強,平均處理時間略有增加,但在跟蹤軌跡誤差、平移誤差和旋轉誤差等指標上均有所下降.

表2 軌跡分析結果Table 2 Trajectory analysis results

3.2.2 光照自適應

本文提出的優化算法對于環境光照變化具有自適應性,提高算法在低暗區域和高亮區域中的特征提取效果,有效突出或增強更多的紋理細節信息,解決由于特征不足而造成的匹配失敗以及運動跟蹤丟失等問題.在圖13 和圖14 中,圖(a)為正常光照條件下提取的orb 特征點分布情況,當光照條件發生變化后,圖像中存在欠曝光或過曝光成像時,與圖(a)相比,部分區域無法提取足夠的特征信息,圖像處理結果如圖(b)和圖(c)所示.然而,經過算法中的局部光照自適應調整和細節增強后,特征提取效果有所改善,圖像處理結果如圖(d)和圖(e)所示,減小了環境光照變化對系統性能的影響.

圖13 光照自適應調整效果AFig.13 Effect A of adaptive illumination adjustment

圖14 光照自適應調整效果BFig.14 Effect B of adaptive illumination adjustment

若圖像中存在過曝光或欠曝光區域,將無法產生帶有信息的特征用于視覺SLAM 當中,故選取不同亮度值的相同數據集作為測試驗證對象,對比分析局部光照自適應調整前后的系統跟蹤精度.以fr1_room 為例,測試結果如圖15 所示,當環境光照發生變化時,系統跟蹤精度隨之受到影響,而本文提出的方法可以改善光照條件不理想情況下的絕對軌跡誤差,提高系統性能.

圖15 不同光照條件下跟蹤精度Fig.15 Tracking accuracy in different illumination conditions

3.2.3 運動魯棒性

本文提出的優化算法在視覺傳感器快速運動方面表現出了較強的魯棒性,可以解決由于幀間匹配失敗而造成的運動跟蹤丟失問題.

以fr1_desk 數據集為例,其SLAM 跟蹤結果如圖16 所示.圖16(a)中由于無法利用幀間特征進行有效匹配,所以只有當相機重新定位到原來的位置時才能繼續跟蹤.圖16(b)中由于增強了基于信息熵篩選后保留下來的圖像細節信息,增加有用特征提高匹配成功率,所以跟蹤成功.多組測試結果顯示,使用ORB-SLAM2 對fr1_desk 數據集進行跟蹤時的成功率不足10%,而應用優化算法進行SLAM 的運動跟蹤成功率可提升至60% 以上.

圖16 軌跡跟蹤結果Fig.16 Trajectory tracking result

4 結論

在移動機器人同步定位與地圖構建中,本文提出了一種基于局部信息熵的細節增強視覺里程計優化算法,利用局部信息熵值判斷圖像信息大小,剔除對比度低以及梯度變化小的圖像區域,對保留的區域進行亮度自適應調整,增強局部圖像細節,將其作為相鄰幀匹配以及關鍵幀匹配的關聯依據,提高機器人運動跟蹤精度.

雖然本優化算法在處理時間上有所增加,但能夠實現移動機器人在視覺傳感器發生大角度轉動時的有效運動估計.分別在TUM RGB-D 數據集fr1_desk、fr1_360、fr1_floor、fr1_room 上進行驗證,應用本文提出的優化算法使運動跟蹤成功率最大可提升至60% 以上,同時使預測軌跡誤差值減小.該算法對于環境的光照變化具有自適應性,并且對于相機的運動具有較強魯棒性,當視覺傳感器快速運動時,可以解決移動機器人在紋理信息不足情況下的正常軌跡跟蹤問題.