馬立 徐夢聰 周磊

(上海大學(xué) 機(jī)電工程與自動化學(xué)院，上海 200444)

SLAM(Simultaneous Localization and Mapping，同時(shí)定位與地圖構(gòu)建)作為自主機(jī)器人定位的關(guān)鍵技術(shù)，在機(jī)器人路徑規(guī)劃、陸地海洋探索、高危環(huán)境救援等領(lǐng)域得到了廣泛應(yīng)用[1- 2]。如今SLAM技術(shù)更多地融合進(jìn)了生活服務(wù)、車輛工程等民用行業(yè)。SLAM算法主要解決移動機(jī)器人在進(jìn)入未知環(huán)境時(shí)確定自身位置、同時(shí)構(gòu)建3D環(huán)境地圖的問題[3- 4]。

近年來，隨著相機(jī)技術(shù)的不斷優(yōu)化和革新，視覺SLAM算法也越來越成熟。目前大部分視覺SLAM系統(tǒng)均采用提取ORB(Oriented FAST and Rotated BRIEF)[5]特征點(diǎn)的方法，該方法是一種角點(diǎn)檢測和匹配算法。利用改進(jìn)的FAST(Features from Accelerated Segment Test)關(guān)鍵點(diǎn)檢測器和BRIEF(Binary Robust Independent Elementary Features)描述子模型，ORB方法不僅兼顧了特征提取的快速性，而且增加了角度信息與尺度信息。相較于其他特征提取方法，在保證有相同的匹配性能的情況下，ORB方法受圖像噪聲的影響更小并且運(yùn)行速度更快，實(shí)時(shí)性更強(qiáng)，因此被廣泛用于對實(shí)時(shí)性要求高的應(yīng)用場合[6]。由Mur-Artal等[7]提出的ORB-SLAM算法是一套相對較為完整的SLAM算法，該算法為廣大研究者提供了示范性解決方案。在此基礎(chǔ)上，王丹等[8]引入輪式里程計(jì)數(shù)據(jù)為視覺定位算法提供先驗(yàn)和尺度信息，然后通過最小化局部地圖點(diǎn)、線的重投影誤差得到更準(zhǔn)確的視覺位姿，在視覺定位失效時(shí)，定位系統(tǒng)能根據(jù)輪式里程計(jì)數(shù)據(jù)繼續(xù)工作[8]。陳興華等[9]在2020年提出了一種基于點(diǎn)線不變量的線特征匹配方法，并且引入了加權(quán)的思想對點(diǎn)線特征的權(quán)重進(jìn)行分配，但是在實(shí)驗(yàn)過程中有特征丟失的現(xiàn)象。

ORB-SLAM算法作為目前運(yùn)行較為穩(wěn)定、應(yīng)用廣泛的開源SLAM算法，也存在弱紋理環(huán)境下因ORB特征提取算法提取不到特征點(diǎn)而造成跟蹤定位失敗的問題。當(dāng)場景中紋理信息不足時(shí)，特征點(diǎn)的數(shù)量會急劇減少，進(jìn)而導(dǎo)致算法的精度降低，給SLAM技術(shù)的實(shí)際應(yīng)用帶來了極大的障礙。

針對這一問題，文中基于ORB-SLAM算法提出了一種點(diǎn)線特征融合SLAM算法——PAL(Point and Line)-SLAM。該算法區(qū)別于現(xiàn)有的點(diǎn)線特征融合模型，建立以直線端點(diǎn)到匹配線段所在直線的垂直距離作為重投影誤差的誤差模型。文中還應(yīng)用kinectV2相機(jī)搭建了用于測試PAL-SLAM算法定位精度和誤差的實(shí)驗(yàn)平臺，通過數(shù)據(jù)集仿真并結(jié)合硬件測試完成了兩種算法的對比實(shí)驗(yàn)，對PAL-SLAM算法進(jìn)行了有效性驗(yàn)證。

1 PAL-SLAM算法的設(shè)計(jì)

ORB-SLAM算法一般分為前端和后端，文中提出的PLA-SLAM算法是以主流ORB-SLAM框架為基礎(chǔ)、結(jié)合RGB-D相機(jī)設(shè)計(jì)的點(diǎn)特征與線特征融合的SLAM算法。如圖1所示，該算法的框架分為前端和后端兩部分，前端利用點(diǎn)線特征匹配以及姿態(tài)估計(jì)算法求解相鄰幀之間的姿態(tài)變換，進(jìn)而求解出當(dāng)前幀的初始姿態(tài)值，其過程包括點(diǎn)線特征提取、特征匹配以及初始姿態(tài)估計(jì)。后端在插入關(guān)鍵幀時(shí)對前端求解的姿態(tài)進(jìn)一步優(yōu)化，包括局部地圖優(yōu)化和回環(huán)檢測優(yōu)化。

圖1 基于RGB-D相機(jī)的PAL-SLAM算法流程圖Fig.1 Flow chart of PAL-SLAM algorithm based on RGB-D camera

前端的視覺里程計(jì)能給出一個(gè)短時(shí)間內(nèi)的軌跡和地圖，但不可避免地存在誤差累積，這個(gè)地圖在長時(shí)間內(nèi)是不準(zhǔn)確的。近幾年興起的SLAM算法中使用圖優(yōu)化替代了原來的卡爾曼濾波器，然后通過建立點(diǎn)線詞袋模型判斷當(dāng)前關(guān)鍵幀是否有回環(huán)現(xiàn)象發(fā)生。利用后端消除累計(jì)誤差，可建立全局一致的軌跡和地圖。

在SLAM算法的實(shí)際運(yùn)行中，優(yōu)化處理每一幀圖片會造成大量的內(nèi)存和運(yùn)算資源浪費(fèi)，因此需要對信息量豐富的關(guān)鍵幀進(jìn)行保存，對信息量不足的關(guān)鍵幀進(jìn)行刪除。一組關(guān)鍵幀和該組關(guān)鍵幀所包含的地圖特征組成局部地圖，局部地圖的維護(hù)是將大量地圖特征(即在前端過程里各關(guān)鍵幀中提取的路標(biāo)點(diǎn)和路標(biāo)線)進(jìn)行創(chuàng)建，并將觀測數(shù)量較小和魯棒性不足的地圖特征加以剔除，以減少誤差，節(jié)省計(jì)算資源。圖模型的構(gòu)建是將關(guān)鍵幀姿態(tài)和地圖特征作為圖模型中的頂點(diǎn)，它們之間的觀測模型和運(yùn)動模型作為圖模型的邊構(gòu)建。圖優(yōu)化是將地圖特征在當(dāng)前關(guān)鍵幀的重投影誤差進(jìn)行最小化，從而達(dá)到優(yōu)化局部地圖中的狀態(tài)變量的目的。

PAL-SLAM算法采用的特征點(diǎn)提取算法為ORB算法，特征線提取算法為LSD(Line Segment Detector)算法[10]。LSD算法主要用于檢測圖像中局部直線的輪廓，是一種有效的直線檢測分割算法。因此，文中采用基于BRIEF點(diǎn)特征描述子和LBD線特征描述子的詞袋模型。

1.1 圖像特征的提取與匹配

在基于特征法的視覺SLAM算法中，特征的提取和匹配是整個(gè)算法的重要基礎(chǔ)。文中采用目前應(yīng)用較多的ORB算法提取點(diǎn)特征，并利用LSD算法提取線段描述子獲得線特征[11]。LSD算法作為一種局部算法，能夠快速地提取圖像中的線段區(qū)域，并且利用錯(cuò)誤控制的方法，使得檢測結(jié)果相對于其他線特征提取算法更加準(zhǔn)確。利用非線性優(yōu)化的方式來求解相機(jī)運(yùn)動位姿，將姿態(tài)求解問題轉(zhuǎn)換為最小化重投影誤差問題[12- 13]。

1.2 建立點(diǎn)線重投影誤差模型

定義待優(yōu)化變量向量為φ。向量φ包含當(dāng)前幀姿態(tài)δw，為了方便非線性求導(dǎo)，采用李代數(shù)SE(3)的形式表述姿態(tài)，第j個(gè)地圖特征點(diǎn)在世界坐標(biāo)系中的三維坐標(biāo)為Owj(Xwj,Ywj,Zwj)，第k個(gè)特征線在空間中的始點(diǎn)和終點(diǎn)的三維坐標(biāo)為{Pwk,Qwk}，那么最小化匹配點(diǎn)和地圖點(diǎn)之間的重投影誤差的目標(biāo)函數(shù)為

(1)

式中：Kl、Pl、Ll分別為局部地圖中的關(guān)鍵幀集合、路標(biāo)點(diǎn)集合以及路標(biāo)線集合；Hp和Hl為核函數(shù)；eij為第j個(gè)地圖點(diǎn)在第i幀關(guān)鍵幀的投影誤差，表達(dá)式為

(2)

圖2 點(diǎn)線誤差模型示意圖Fig.2 Schematic diagram of point line error model

則線段的重投影誤差表達(dá)式為

(3)

式中：eik為第k個(gè)地圖線在第i幀關(guān)鍵幀上的重投影誤差；lik為空間線段的表達(dá)方式，可由齊次坐標(biāo)系下線段兩端點(diǎn)的交叉乘積得到。

特征匹配過程中，誤匹配的誤差值要遠(yuǎn)大于正確匹配的誤差值，資源也會傾向于用來優(yōu)化這些誤差，從而使匹配得到不準(zhǔn)確的結(jié)果。為消除此現(xiàn)象，添加核函數(shù)進(jìn)行優(yōu)化，核函數(shù)的形式為

(4)

式中，e為上文提到的點(diǎn)和線段的重投影誤差，γ為核函數(shù)中根據(jù)具體情況定義的閾值。

最后，通過最小化目標(biāo)函數(shù)求解出待優(yōu)化變量的最優(yōu)值φ*。

1.3 線段重投影雅可比矩陣

由前可知，最小化點(diǎn)線重投影誤差目標(biāo)方程的求解過程為：待優(yōu)化變量不斷迭代—縮減重投影誤差—誤差足夠小時(shí)將此時(shí)的優(yōu)化變量值作為該最小二乘問題的最優(yōu)解。文中采用高斯牛頓法作為相機(jī)姿態(tài)的優(yōu)化算法。局部地圖中優(yōu)化的狀態(tài)變量為當(dāng)前幀的姿態(tài)以及局部地圖中包含的局部特征。首先推導(dǎo)利用始末端點(diǎn)為直線建模方法下的目標(biāo)函數(shù)及誤差向量對姿態(tài)和局部特征的雅可比形式。投影線在像素平面中的端點(diǎn)坐標(biāo)形式為

(5)

式中，fx和fy分別表示在u軸及v軸上焦距的縮放，Cx和Cy分別表示由投影平面至像素平面的原點(diǎn)平移，(X′,Y′,Z′)為世界坐標(biāo)系中線段端點(diǎn)到相機(jī)坐標(biāo)系下的端點(diǎn)坐標(biāo)。那么，該線段產(chǎn)生的誤差為

(6)

(7)

(8)

(9)

式中，T為李代數(shù)齊次變換的表達(dá)形式。整理式(7)-(9)可得

(10)

(11)

待優(yōu)化變量中不僅包含相機(jī)姿態(tài)，還包含地圖點(diǎn)和地圖線段。特征點(diǎn)誤差的偏導(dǎo)為

(12)

(13)

(14)

式中，Jik為第k條線段產(chǎn)生的投影誤差eik對φ所在行的具體形式，它只在對對應(yīng)線段求偏導(dǎo)時(shí)有具體數(shù)值，對其他部分求偏導(dǎo)時(shí)則為0。同理可知Jij是雅可比矩陣中eij對φ求偏導(dǎo)所在行的具體形式。

2 實(shí)驗(yàn)及結(jié)果分析

2.1 基于Kinect V2的仿真平臺的搭建

基于Ubuntu16.04 LTS系統(tǒng)設(shè)計(jì)PAL-SLAM算法，使用到的第三方庫為OpenCV 3.3、G2O、Eigen、PCL、ROS Kinetic Kame 機(jī)器人操作系統(tǒng)，使用的RGB-D相機(jī)型號為Kinect V2。RGB-D 相機(jī)可以增強(qiáng)硬件平臺對場景深度的感知能力，避免了以往算法對場景深度進(jìn)行估計(jì)的依賴，簡化了計(jì)算過程。標(biāo)定實(shí)驗(yàn)軟件采用開源Kinect V2驅(qū)動程序及ROS包，配置軟件環(huán)境OpenGL和OpenCL用于加速包的處理，對相機(jī)進(jìn)行標(biāo)定實(shí)驗(yàn)。經(jīng)過實(shí)驗(yàn)可測得該相機(jī)的標(biāo)定參數(shù)，如表1所示。

表1 Kinect V2標(biāo)定結(jié)果Table 1 Kinect V2 calibration results

2.2 數(shù)據(jù)集實(shí)驗(yàn)及結(jié)果分析

使用TUM數(shù)據(jù)集進(jìn)行測試。TUM數(shù)據(jù)集包括RGB-D圖像數(shù)據(jù)、圖像數(shù)據(jù)的真實(shí)軌跡數(shù)據(jù)及測試SLAM軌跡精度的腳本工具等，是對SLAM系統(tǒng)的標(biāo)準(zhǔn)評價(jià)方案。在TUM里選取fr1 xyz、fr1 floor、f3_structure_texture_far、freiburg2_large_no_loop共4組數(shù)據(jù)對ORB-SLAM、PTAM、LSD-SLAM、RGBD-SLAM和PAL-SLAM算法進(jìn)行標(biāo)準(zhǔn)誤差(RMSE)的對比，結(jié)果見表2，其中ORB-SLAM、PTAM、LSD-SLAM、RGBD-SLAM在fr1 xyz、fr1 floor、f3_structure_texture_far幾組數(shù)據(jù)下的結(jié)果來自文獻(xiàn)[14]。由表2可以看出：作為目前應(yīng)用場景最廣泛的SLAM算法，ORB-SLAM算法在大多數(shù)場景下都有較好表現(xiàn)，而文中提出的PAL-SLAM算法在4組實(shí)驗(yàn)數(shù)據(jù)中的運(yùn)行結(jié)果也較為穩(wěn)定；PAL-SLAM在大場景freiburg2_large_no_loop和結(jié)構(gòu)化場景f3_structure_texture_far數(shù)據(jù)集中表現(xiàn)較好，能夠提取出富含結(jié)構(gòu)化的特征線，因此優(yōu)化效果較好；而fr1 xyz數(shù)據(jù)集是室內(nèi)小場景環(huán)境，fr1 floor數(shù)據(jù)集里從地面提取的線特征平行且相似，產(chǎn)生了過匹配的現(xiàn)象，因此PAL-SLAM算法對它們的優(yōu)化效果一般。圖3為實(shí)驗(yàn)過程中fr1 xyz數(shù)據(jù)集的仿真界面，可以看出PAL-SLAM算法能夠有效地刻畫出場景中的幾何結(jié)構(gòu)信息。PAL-SLAM算法主要是針對室內(nèi)大場景下的應(yīng)用環(huán)境，因此文中采用freiburg2_large_no_loop數(shù)據(jù)集對ORB-SLAM和PAL-SLAM兩種算法進(jìn)行進(jìn)一步的對比測試。

表2 TUM 數(shù)據(jù)集下的RMSETable 2 RMSE on TUM dataset cm

圖3 fr1 xyz數(shù)據(jù)集的仿真界面Fig.3 Simulation interface of fr1 xyz dataset

freiburg2_large_no_loop數(shù)據(jù)集場景為工業(yè)廠區(qū)，其軌跡長度為26.086 m，平均平移速度為0.243 m/s，平均角速度為15.09 °/s，總耗時(shí)為21.37 s，其中樣張如圖4所示。從圖4可以看出，該場景中存在一些明暗變化的區(qū)域以及一些弱紋理結(jié)構(gòu)化場景。數(shù)據(jù)集實(shí)際軌跡從工廠內(nèi)部采集。

圖4 工業(yè)廠區(qū)實(shí)景圖Fig.4 Real pictures of industrial plant

此種場景能夠較為全面地測試文中提出的基于點(diǎn)線特征融合的PAL-SLAM算法與只有特征點(diǎn)的ORB-SLAM 算法應(yīng)對不同場景的魯棒性。

實(shí)驗(yàn)結(jié)果如圖5所示?？梢园l(fā)現(xiàn)PAL-SLAM算法能夠較好地跟蹤相機(jī)的軌跡，與真實(shí)軌跡基本貼合。而ORB-SLAM算法在系統(tǒng)剛開始時(shí)能夠取得較為準(zhǔn)確的相機(jī)姿態(tài)值，但隨著系統(tǒng)不斷運(yùn)行，ORB-SLAM算法的軌跡誤差不斷增大，與實(shí)際軌跡線偏離越來越遠(yuǎn)。

圖5 軌跡線的三維重建圖Fig.5 3D reconstruction of trajectories

在廠房場景中，出現(xiàn)了較多的線特征，圖4中紅線標(biāo)注的即為圖像中的部分線特征?；邳c(diǎn)特征的ORB-SLAM算法在該場景中提取的環(huán)境信息較少，特征點(diǎn)個(gè)數(shù)減少，導(dǎo)致姿態(tài)求解誤差較大，誤差逐步累積，便導(dǎo)致定位誤差越來越大。但是這些豐富的線特征使得文中提出的PAL-SLAM算法能夠獲取更加豐富的場景信息，基于這些信息和點(diǎn)線融合局部地圖優(yōu)化使得姿態(tài)求解始終保持較高的精度。

為了更加準(zhǔn)確地比較兩種算法的定位精度，文中進(jìn)一步比較了它們的絕對誤差等數(shù)據(jù)，結(jié)果如表3所示。從表3可以看出：PAL-SLAM算法的最大絕對平移誤差為0.097 793 m，僅為ORB-SLAM算法的24.8%；PAL-SLAM算法的最小絕對平移誤差為0.008 029 m，標(biāo)準(zhǔn)誤差(RMSE)為0.047 838 m，遠(yuǎn)小于ORB-SLAM的標(biāo)準(zhǔn)誤差0.253 454 m，其標(biāo)準(zhǔn)誤差僅為ORB-SLAM的18.9%。由此可知，線特征的融入使得PAL-SLAM在應(yīng)對無回環(huán)大場景時(shí)的定位精度大幅提高。

表3 兩種算法結(jié)果的對比1)Table 3 Comparison two algorithms’ results m

2.3 基于Kinect V2的實(shí)驗(yàn)及結(jié)果分析

使用freiburg2_large_no_loop數(shù)據(jù)集測試完成后，結(jié)合攝像頭、移動底盤測試PAL-SLAM算法與硬件結(jié)合的建圖效果。如圖6，實(shí)驗(yàn)場景為實(shí)驗(yàn)室內(nèi)部，將相機(jī)沿著中間桌面邊際勻速運(yùn)動一周，該桌面長 170 cm，寬72 cm；相機(jī)測幀率為 20 Hz。

圖6 實(shí)驗(yàn)場景圖Fig.6 Experimental scene

圖7所示為PAL-SLAM算法的軌跡重建結(jié)果。可以看出，PAL-SLAM算法能夠有效跟蹤相機(jī)，對軌跡進(jìn)行重建，但是在y和x軸方向上均略有浮動。分析其原因，一是相機(jī)標(biāo)定參數(shù)存在誤差，導(dǎo)致輸入系統(tǒng)的數(shù)據(jù)源頭存在部分偏差；二是Kinect V2的頻率不穩(wěn)定，其提供的數(shù)據(jù)幀率存在一定范圍的波動，但是系統(tǒng)的參數(shù)是基于幀率 20 Hz的，由此導(dǎo)致定位精度出現(xiàn)微小誤差；三是硬件本身運(yùn)動過程中，Kinect V2的底盤無法保證相機(jī)做完全平穩(wěn)的直線運(yùn)動。以上結(jié)果表明,PAL-SLAM算法可與硬件結(jié)合并應(yīng)用于實(shí)際場景。

實(shí)線為PAL-SLAM算法的實(shí)驗(yàn)后重建軌跡點(diǎn)劃線為相機(jī)沿規(guī)定路線的理論運(yùn)動軌跡

3 結(jié)論

文中利用添加提取圖像中線特征的方法，將點(diǎn)線特征進(jìn)行有效融合，通過點(diǎn)線特征融合原理重建地圖重投影誤差模型，對目前較為流行的ORB-SLAM算法框架進(jìn)行改進(jìn)，提出一種PAL-SLAM算法。利用TUM數(shù)據(jù)集進(jìn)行改進(jìn)前后兩種算法的對比實(shí)驗(yàn)，結(jié)果表明：PAL-SLAM算法在室內(nèi)大場景中的定位精度較好，在沒有回環(huán)檢測的大場景環(huán)境下，其標(biāo)準(zhǔn)誤差為0.047 838 m，遠(yuǎn)小于ORB-SLAM算法的標(biāo)準(zhǔn)誤差，僅為ORB-SLAM算法的18.9%；PAL-SLAM算法的最大絕對平移誤差也小于0.1 m，僅為ORB-SLAM算法的24.8%。此結(jié)果證明，線特征的融入能夠更好地提高系統(tǒng)定位精度，同時(shí)PAL-SLAM算法能夠與硬件平臺較好結(jié)合，有效追蹤相機(jī)軌跡。后續(xù)研究中，將針對實(shí)際應(yīng)用中存在的漂移誤差做進(jìn)一步的優(yōu)化。