基于區(qū)域映射深度優(yōu)化的動態(tài)多特征RGBD-SLAM 算法

陳孟元，郭行榮，錢潤邦，程 浩

（1.安徽工程大學(xué) 電氣工程學(xué)院，蕪湖 241000；2.高端裝備先進感知與智能控制教育部重點實驗室，蕪湖 241000）

近年來，隨著機器人技術(shù)與計算機技術(shù)的發(fā)展，同時定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）受到越來越多的機器人及計算機行業(yè)從事人員的關(guān)注，SLAM 主要研究機器人在未知環(huán)境中認(rèn)知自身位姿以及構(gòu)建周圍環(huán)境地圖[1]。根據(jù)搭載傳感器的不同，SLAM 技術(shù)主要分為激光和視覺兩大類，早期的SLAM 技術(shù)主要采用激光雷達來實現(xiàn)，但由于其高昂價格的局限性，很難大規(guī)模應(yīng)用于各種工業(yè)場景下；隨著技術(shù)的發(fā)展，視覺傳感器逐步進入大眾視野，由于視覺傳感器具有使用場景廣、價格便宜、能夠獲取豐富的信息等優(yōu)點，使得視覺傳感器逐漸成為SLAM 研究的主流[2,3]。

目前基于特征點法的視覺SLAM系統(tǒng)主要分為以單一點特征和以多特征為主兩大類。Mur-Artal 等人[4]提出基于單一特征點的ORB-SLAM2 算法，通過提取角點信息完成特征匹配，并通過特征數(shù)據(jù)關(guān)聯(lián)得到物體運動位姿，但此系統(tǒng)在稀疏紋理場景下極易出現(xiàn)因特征數(shù)量過少而跟蹤丟失的問題。Campos 等人[5]提出的ORB-SLAM3 依賴最大后驗估計，并利用所有先前信息進行重定位，使系統(tǒng)可以在特征稀少場景中長期運行，但依然無法有效解決稀疏紋理場景下跟蹤易丟失的問題。Von Gioi 等人[6]提出了線段檢測器（Line Segment Detector,LSD），該算法計算圖像中所有像素點的梯度大小和方向，將梯度方向變化小且相鄰的點作為一個連通域，并改善和篩選連通域為直線檢測結(jié)果。基于此系統(tǒng)引入了多特征提取的方法，Gomez-Ojeda 等人[7]提出了PL-SLAM 算法，在圖像點特征稀少或分布不均勻的情況下引入LSD 檢測器，并在所有線程中使用兩種特征，采用兩種特征的組合描述能力作為閉環(huán)檢測的依據(jù)，使得該算法在稀疏紋理場景下取得了不錯的效果，但當(dāng)環(huán)境中出現(xiàn)動態(tài)物體時容易出現(xiàn)特征提取紊亂的問題。Li 等人[8]針對稀疏結(jié)構(gòu)化場景下傳統(tǒng)SLAM無法有效提取特征點進行準(zhǔn)確跟蹤的問題，提出了基于點、線和面的解耦-精化方法，并在位姿估計模塊中使用曼哈頓關(guān)系的RGBD-SLAM。該算法通過從周圍環(huán)境中提取幾何特征來提高跟蹤和映射精度，但在特征提取環(huán)節(jié)由于深度相機得到的深度圖在物體邊界出現(xiàn)深度不連續(xù)以及場景中闖入動態(tài)物體的問題，使得RGBD-SLAM 算法無法運行。為了減少動態(tài)物體的影響，Zhang 等人[9]利用點和線特征計算攝像機姿態(tài)，并結(jié)合深度學(xué)習(xí)語義分割與深度信息K-均值聚類算法來檢測潛在的動態(tài)特征，采用兩種一致性檢查策略，檢測和過濾動態(tài)特征。該算法能夠有效解決低動態(tài)稀疏場景下的動態(tài)物體，但易因深度信息的不準(zhǔn)確以及幾何信息提取過少而出現(xiàn)動態(tài)物體剔除不完全的問題。

針對上述問題，本文提出基于區(qū)域映射深度優(yōu)化的動態(tài)多特征 RGBD-SLAM 算法（Dynamic Multi-feature RGBD-SLAM Algorithm based on Region Mapping Depth Optimization,DR-SLAM）。該算法在RGBD-SLAM 算法的基礎(chǔ)上，引入?yún)^(qū)域映射深度優(yōu)化以解決深度不連續(xù)的問題。通過聯(lián)合深度圖與多視圖幾何剔除動態(tài)物體，改善了現(xiàn)有RGBD-SLAM 算法深度信息不準(zhǔn)確的問題，提高了動態(tài)場景下系統(tǒng)運行的穩(wěn)定性。在公開數(shù)據(jù)集中對本文算法進行驗證，測試結(jié)果表明DR-SLAM 算法與ORB-SLAM3 算法、RGBD-SLAM 算法以及DS-SLAM 算法相比，在抗干擾性、定位精度、軌跡精度等方面有較大優(yōu)勢，表現(xiàn)出了良好的構(gòu)圖能力。

1 系統(tǒng)整體框架

DR-SLAM 算法框架圖如圖1 所示。

圖1 DR-SLAM 算法框架圖Fig.1 DR-SLAM algorithm framework diagram

本文算法包含深度圖優(yōu)化和動態(tài)物體剔除兩個環(huán)節(jié)。深度圖優(yōu)化環(huán)節(jié)通過輕量級的AFFormer[10]網(wǎng)絡(luò)提取語義信息，并將提取的語義信息映射到其對應(yīng)的深度圖上進行近鄰修復(fù)，從而完成深度圖的優(yōu)化。動態(tài)物體剔除環(huán)節(jié)包含點特征剔除動態(tài)物體線程和點線面特征提取線程。首先利用點特征信息，聯(lián)合優(yōu)化圖像序列與多視圖幾何完成動態(tài)物體的剔除，并利用雙向映射背景修復(fù)模型補全剔除區(qū)域的靜態(tài)信息，然后進行點線面特征的提取，為SLAM 系統(tǒng)定位與建圖提供準(zhǔn)確的信息。

2 深度圖優(yōu)化

良好的深度圖像是特征信息準(zhǔn)確跟蹤的前提。在點線面SLAM 系統(tǒng)中，使用D435i 相機，用紅外結(jié)構(gòu)光進行深度測量時，容易受到環(huán)境和設(shè)備的影響。由于RGB-D 相機得到的深度圖在物體邊界處有深度不連續(xù)的先天性弱勢，將特征根據(jù)深度值和相機內(nèi)參投影至相機坐標(biāo)系時，會出現(xiàn)由于深度不連續(xù)導(dǎo)致投影不準(zhǔn)確的情況，進而降低幀與幀之間的特征跟蹤精度。因此，高質(zhì)量的深度圖像是提高SLAM 系統(tǒng)性能的重要因素。為了提高深度圖像的準(zhǔn)確度，本文算法采用了AFFormer 語義分割網(wǎng)絡(luò)。AFFormer 語義分割網(wǎng)絡(luò)是采用Transformer 架構(gòu)的新型語義分割網(wǎng)絡(luò)，具有全視野、高效準(zhǔn)確的優(yōu)點。通過實時輕量級的網(wǎng)絡(luò)對RGB 圖像進行檢測，得到圖像中物體的類別集合。

其中，Clxi表示的第i個類別；n表示算法中檢測到當(dāng)前幀中的物體類別總數(shù)；C是類別集合。對于每個Clxi∈C，網(wǎng)絡(luò)對其進行特征紋理的提取，得到各種特征紋理的集合。

其中，Rdepth表示的是RGB 圖像深度特征紋理信息；ClV表示的是顏色值信息，以及一些其他的特征信息。

AFFormer 語義分割網(wǎng)絡(luò)對RGB 圖像中的類別進行標(biāo)定，使得Clxi在RGB 圖中生成對應(yīng)的掩碼Mxi。本文將掩碼映射到深度圖上對應(yīng)區(qū)域，此時RGB 圖像中所有類別的深度特征紋理信息通過掩碼映射輸送到對應(yīng)的深度圖像區(qū)域，得到單個類別對應(yīng)的優(yōu)化區(qū)域，數(shù)學(xué)表達式為：

其中，Ddepth表示深度圖像；Ri表示單個類別對應(yīng)的優(yōu)化區(qū)域。

通常認(rèn)為由深度相機生成的深度圖像同一類別的深度值相差不大，為了更好地解決物體邊界處出現(xiàn)深度不連續(xù)現(xiàn)象，本文采用物體內(nèi)修復(fù)方法。根據(jù)隨機抽樣一致[11]（Random Sample Consensus,RANSAC）算法和Ri檢測出正常深度值點以及異常深度值點并剔除掉異常深度值，最后通過近鄰值替代法完成深度圖的優(yōu)化。具體做法如下：

1）通過深度相機得到在Ri的深度值Dpv，其中Dpv={Dpv1,Dpv2,Dpv3…}，其中包含正常深度值點和異常深度值點。

2）運用RANSAC 算法剔除異常值。通過步驟1）得到單個類別的所有深度值，為了得到最準(zhǔn)確的深度值，本文選擇優(yōu)化區(qū)域的所有深度值作為模型的數(shù)據(jù)點。若任意兩個數(shù)據(jù)點滿足式(5)，則可以認(rèn)為是正常深度值點，反之為異常的深度值點，并將異常值剔除。

3）近鄰值替代法完成深度圖的優(yōu)化。通過步驟2）得到異常值點，以此點為原點四周索引，直至索引到深度正常值，用其替代異常值點，即完成深度圖優(yōu)化。

深度相機在不同實驗場景下得到的深度值一般都是不相同的，本文經(jīng)實驗得出在TUM[12]數(shù)據(jù)集上的閾值X通常取0.8。

3 動態(tài)物體剔除

3.1 聯(lián)合深度圖與多視圖幾何剔除動態(tài)物體

動態(tài)物體對SLAM算法的定位精度和地圖的構(gòu)建有著重要影響，當(dāng)前大多數(shù)點線面SLAM 系統(tǒng)都在靜態(tài)環(huán)境中實現(xiàn)了高精度的定位與構(gòu)圖，但當(dāng)環(huán)境中闖入動態(tài)物體時極易造成軌跡誤差增大問題，使得整體算法缺乏克服機器人在動態(tài)環(huán)境下穩(wěn)定運行的能力，從而影響整個SLAM 系統(tǒng)的定位與建圖。因此，本文提出一種聯(lián)合深度圖與多視圖幾何剔除動態(tài)物體的方法，剔除動態(tài)物體示意圖如圖2 所示，其中Ck-1和Ck表示前后兩幀的相機中心。

圖2 剔除動態(tài)物體Fig.2 Culling dynamic objects

SLAM 系統(tǒng)在運行過程中，在已知相機內(nèi)參和深度圖像深度值的前提下，將經(jīng)過深度優(yōu)化的深度圖像轉(zhuǎn)化為點云，計算公式見式(6)。

其中，(x,y,z)是點云坐標(biāo)；(u,v)是圖像坐標(biāo)；fx、fy為RGB-D 相機的內(nèi)參。

通過式(6)獲取當(dāng)前圖像幀與后一幀的點云地圖點X={x1,x2···xn}和Y={y1,y2···yn}，并通過迭代最近點[13]（Iterative Closest Point,ICP）得到相機前后的位姿差，從而求解出相機位姿。得到相機前后的位姿差，采用最小二乘法求解旋轉(zhuǎn)矩陣R∈R3×3和平移向量t∈R3，最終得到Hc，計算公式見式(7)。

其中，xi∈R3為前一幀點云；yi∈R3為后一幀點云；Mr為最小殘差；Hc∈SE(3)為相機姿態(tài)相對變換矩陣；hc∈ SE(3)為相機位姿相對變換向量；Ny為匹配的點數(shù)。

其中，hc*∈R6為最小二乘解；ρh為懲罰因子；ΣP∈R4×4為R、t協(xié)方差矩陣；j為殘差運算所需3D點投影至2D 點數(shù)量；e(hc)∈R4×4為hc的分解矩陣。

由相機位姿和多視圖幾何確定物體位姿，將所有潛在動態(tài)物體建模為位姿變換矩陣Ho∈R4×4。通過對應(yīng)的RGB 相鄰幀間幾何關(guān)系將空間中的點從參考幀F(xiàn)k-1關(guān)聯(lián)到后一幀F(xiàn)k，計算公式見式(9)。

其中，mk是Fk中的平面坐標(biāo)；ε和ε-1分別為投影函數(shù)和反向投影函數(shù)；Ik-1是空間點投影到Fk-1中的三維點；mk-1為Ik-1在Fk-1中的平面坐標(biāo)；mz-1為Ik-1在Fk-1中的深度值。

多視圖幾何得到的物體變換矩陣存在誤差，本文通過重投影誤差與最小二乘法對物體姿態(tài)變換矩陣Ho進行優(yōu)化處理。同理，通過∈R6從SE(3)到 R6的映射運算，得到準(zhǔn)確的物體運動估計矩陣ho*。

其中，e(Ho)為位姿變換矩陣優(yōu)化解（最小化重投影誤差）；ho為位姿變換向量，e(ho)為ho的分解矩陣；為Fk-1中mk-1的坐標(biāo)投影到Fk中的坐標(biāo)。

SLAM 系統(tǒng)在運行過程中受環(huán)境影響較大，僅通過物體運動估計矩陣難以準(zhǔn)確判斷物體是否運動。本文采用平面幾何映射判斷物體狀態(tài)，空間中的點投影到Fk-1幀中得到2D像素坐標(biāo)mk-1，此時假設(shè)mk-1通過幾何關(guān)系投影到Fk中的點為，與Fk中真實投影點mk的像素距離d1為動態(tài)視覺誤差權(quán)重。為了減少誤差，同時將Fk中的2D像素坐標(biāo)mk也投影到Fk-1中，與Fk-1中真實投影點mk-1形成動態(tài)視覺誤差權(quán)重d2。由于動態(tài)物體具有豐富的像素點，故物體的動態(tài)視覺誤差可由像素動態(tài)視覺誤差代替。為了得到準(zhǔn)確的物體動態(tài)視覺誤差，本文采用深度值類比權(quán)重法，計算公式見式(11)。

其中，mz為空間中的點到Fk中mk的深度值；θ為物體動態(tài)視覺誤差。

姿態(tài)不確定性是判斷物體是否運動的重要指標(biāo)，姿態(tài)不確定性可以通過微分熵值從光度殘差最小化導(dǎo)出，高姿態(tài)不確定性的三維空間點運動觀測將導(dǎo)致圖像上某物體產(chǎn)生較大的位移，反之低姿態(tài)不確定性將產(chǎn)生較小圖像差異，因此本文采用運動熵作為判斷物體運動的依據(jù)，計算公式見式(12)。

其中，H(θ)為運動熵；e為自然指數(shù)；Pθ為不確定誤差，服從ω維高斯分布，通常Pθ=（JT∑-1J）-1，∑-1為對角矩陣，J為雅各比矩陣。

基于此，將物體動態(tài)視覺誤差與一個由運動微分熵f(H(θ))引導(dǎo)并隨熵緩慢變大的動態(tài)閾值Δθ=f(H(θ))進行對比，其中f(·) 為求微分。若θ＞Δθ，則判斷為動態(tài)物體并將其剔除。動態(tài)物體剔除方法如表1 所示。

表1 動態(tài)物體剔除算法Tab.1 Dynamic object elimination algorithm

3.2 背景修復(fù)與點線面特征提取

剔除動態(tài)物體后用于特征提取的信息減少，使得本算法在多動態(tài)物體場景中位姿估計精度降低，繼而影響回環(huán)檢測和全靜態(tài)場景地圖構(gòu)建。因此，本文提出一種基于雙向映射模型的背景修復(fù)方法，借助前后幀中的靜態(tài)信息修復(fù)剔除區(qū)域RGB 圖像和深度圖靜態(tài)背景。完成靜態(tài)背景修復(fù)后，進行點線面特征的提取以完成SLAM 定位與建圖的任務(wù)。

基于雙向映射的背景修復(fù)示意圖如圖3 所示，以待修復(fù)關(guān)鍵幀F(xiàn)t為起點，向前后箭頭方向共移動8 幀的時間窗口。窗口內(nèi)的關(guān)鍵幀圖像依次與待修復(fù)關(guān)鍵幀圖像對齊，當(dāng)所有圖像對齊后，將待修復(fù)關(guān)鍵幀圖像的剔除區(qū)域沿著黑色虛線箭頭方向索引所對應(yīng)的像素。若索引到一個對應(yīng)像素，直接進行缺失區(qū)域像素補全，若索引到多個對應(yīng)像素值，則對索引到的像素取平均值，然后再進行缺失區(qū)域的像素補全。

圖3 基于雙向映射的背景修復(fù)Fig.3 Bidirectional mapping based background repair

4 實驗結(jié)果與分析

本文實驗所用電腦設(shè)備：CPU 為 Inter Core i7-10080F、8 核處理器、主頻2.9 GHz、內(nèi)存16 GB；系統(tǒng)為Ubuntu18.04。為了驗證本文算法的有效性，使用TUM 公開數(shù)據(jù)集進行驗證。

4.1 深度優(yōu)化

深度優(yōu)化效果如圖4 所示，其中圖4(a)～圖4(d)為TUM 數(shù)據(jù)集中四幀原始圖像幀，圖4(e)～圖4(h)為四幀經(jīng)過圖像對比度處理的原始深度圖像幀，圖 4(i)～ 圖4(l)為四幀采用本文算法優(yōu)化后得到的深度圖像幀。由于深度相機在運行過程中易受環(huán)境中噪音影響，使產(chǎn)生的深度圖像存在深度缺失，尤其是在動態(tài)場景及兩個物體的連接處。從圖4(i)-圖4(l)可以看出，采用本文算法優(yōu)化得到的深度圖像中存在的黑洞值大多數(shù)都被修復(fù)，而且從圖4(e)和圖4(i)中能夠看出本文算法能夠粗略重現(xiàn)深度圖像中一部分深度完全丟失的地方，如圖4(a)中的手臂和后背就被重現(xiàn)出來，深度優(yōu)化算法提高了特征跟蹤的精度，為后續(xù)動態(tài)物體剔除提供了良好的深度依據(jù)。

由于RGB-D 相機自身的原因，得到的深度圖會出現(xiàn)深度不連續(xù)的問題，影響特征跟蹤精度。當(dāng)特征點根據(jù)深度值和相機內(nèi)參投影至相機坐標(biāo)系時，存在由于深度錯誤影響投影精度的情況，進而降低幀與幀之間的特征跟蹤的準(zhǔn)確率。為了體現(xiàn)深度優(yōu)化算法與SLAM 系統(tǒng)的聯(lián)系以及在SLAM 線程中的優(yōu)勢，本文采用特征跟蹤準(zhǔn)確率（Feature Tracking Accuracy）作為深度優(yōu)化算法的評價指標(biāo)。特征跟蹤準(zhǔn)確率指的是當(dāng)前幀中提取的特征點投影到下一幀的成功率，特征跟蹤的準(zhǔn)確率越高表示跟蹤精度越高。本文在TUM數(shù)據(jù)集上選取了四個高動態(tài)序列（fr3_walking_xyz、f r3_walking_static、fr3_walking_rpy、fr3_walking_half sphere）以及兩個無動態(tài)序列（fr2_xyz、fr2_rpy），并將每個序列圖像幀數(shù)據(jù)的中位數(shù)（Median）與平均值（Mean）作為實驗數(shù)據(jù)指標(biāo)。表2 給出ORB-SLAM3、RGBD-SLAM、不加優(yōu)化的本文算法（DR-SLAM wit hout Optimization,DR-SLAM-WO）和本文DR-SLA M 算法在TUM 數(shù)據(jù)集下的特征跟蹤準(zhǔn)確率，表中加粗黑色字體表示最優(yōu)值。從表2 可以看出，在四個高動態(tài)序列中，DR-SLAM 的特征跟蹤準(zhǔn)確率是最高的，DR-SLAM-WO 的特征跟蹤準(zhǔn)確率是次高的；在兩個無動態(tài)序列中，DR-SLAM 的特征跟蹤準(zhǔn)確率與ORB-SLAM3 幾乎相同且都高于DR-SLAM-WO 和RGBDSLAM，驗證了本文深度優(yōu)化算法能夠提高動態(tài)SLA M 系統(tǒng)特征跟蹤精度，從而提升動態(tài)SLAM 系統(tǒng)的位姿估計準(zhǔn)確率，為減少軌跡誤差提供良好基礎(chǔ)。

表2 四種算法在TUM 數(shù)據(jù)集下的特征跟蹤準(zhǔn)確率（單位：%）Tab.2 Feature tracking accuracy of four algorithms in TUM dataset (Unit: %)

4.2 點特征剔除動態(tài)物體效果圖

點特征動態(tài)物體剔除效果如圖5 所示，其中圖5(a)～圖5(d)為在TUM 數(shù)據(jù)集上任意選取的四個動態(tài)原始幀；圖5(e)～圖5(h)為四個動態(tài)物體剔除圖像幀，從圖中可以看出，DR-SLAM 在動態(tài)物體場景下能有效剔除動態(tài)物體；圖5(i)～圖5(l)為與動態(tài)物體剔除幀對應(yīng)的稀疏地圖點幀。如圖5(j)所示，其中紅色點為靜態(tài)特征點，黑色點為本文標(biāo)記的動態(tài)特征點。隨著動態(tài)物體的減少，黑色點的數(shù)量也逐漸減少，驗證了DR-SLAM 能夠有效區(qū)分動態(tài)特征點，為后續(xù)的多特征信息提取減少了動態(tài)物體的影響。

圖5 點特征動態(tài)物體剔除效果Fig.5 Point feature dynamic object elimination effect

4.3 背景修復(fù)與點線面特征提取

背景修復(fù)與點線面特征提取效果如圖6 所示，其中圖6(a)～圖6(d)為在TUM 數(shù)據(jù)集上任意選取的四個動態(tài)原始幀；圖6(e)～圖6(h)為四個原始圖像幀經(jīng)修復(fù)后的圖像幀，可以看出雙向映射模型能有效地進行剔除區(qū)域靜態(tài)背景的修復(fù)；圖6(i)～圖(l)為靜態(tài)背景修復(fù)后的點線面特征提取幀。本文通過剔除動態(tài)物體后進行背景修復(fù)，并進行點線面特征的提取，能夠有效降低動態(tài)物體對構(gòu)建地圖的影響，提供了良好的位姿估計精度，從而減少軌跡誤差。

圖6 背景修復(fù)與點線面特征提取效果Fig.6 Effect of background restoration and point,line and surface feature extraction

4.4 TUM 數(shù)據(jù)集驗證與評估

為了驗證DR-SLAM 剔除動態(tài)物體的能力，本文選取TUM 數(shù)據(jù)集的三個動態(tài)序列fr3_walking_xy（z圖中為W_xyz：相機原地左右上下轉(zhuǎn)動拍攝的數(shù)據(jù)集）、fr3_walking_rpy（圖中為W_rpy：相機上下左右移動拍攝的數(shù)據(jù)集）、fr3_walking_halfsphere（圖中為W_half：相機左右上下以及翻轉(zhuǎn)拍攝的數(shù)據(jù)集）進行驗證評估。圖7 為ORB-SLAM3、RGBD-SLAM、DS-SLAM[21]和DR-SLAM 四種算法在這三個序列上運行的軌跡對比，圖中黑色線為相機的真實運動軌跡，藍色線為系統(tǒng)依據(jù)算法估計出的相機運動軌跡，紅色線為相機運行軌跡和真實軌跡之間的誤差。ORB-SLAM3 和RGBD-SLAM 為靜態(tài)環(huán)境中的SLAM算法，DS-SLAM 和DR-SLAM 為動態(tài)環(huán)境中的SLAM算法。從圖7(a)和圖7(b)可以看出，由于ORB-SLAM3和RGBD-SLAM 無法識別環(huán)境中存在的動態(tài)物體，因此所生成的軌跡與真實軌跡相比存在較大的軌跡誤差；從圖7(d)可以看出，由于DR-SLAM 減小了深度不連續(xù)對動態(tài)物體的影響，所以DR-SLAM 生成的軌跡誤差小于其他三種算法。圖7(e)～圖7(l)為四種算法在另兩組動態(tài)序列的軌跡對比，從圖中可以看出ORB-SLAM3、RGBD-SLAM 和DS-SLAM 算法所生成的軌跡誤差依舊較大，而DR-SLAM 所生成的軌跡誤差還是小于其他三種算法，驗證了DR-SLAM 對于環(huán)境中的動態(tài)物體具有良好的剔除能力，能夠有效減少軌跡誤差。

圖7 TUM 數(shù)據(jù)集驗證評估Fig.7 TUM data set validation evaluation

為了進一步驗證DR-SLAM 在動態(tài)環(huán)境以及靜態(tài)環(huán)境中軌跡精度高、誤差小的性能，在上述實驗的三個序列上增加fr3_walking_static（相機拍攝場景中的人物是靜止的，沒有動態(tài)物體）序列來評測算法的定位與建圖能力。為了消除偶然誤差，本文在每個序列上進行5 次實驗，并且取均方根誤差（Root Mean Square Error,RMSE）、平均值（Mean）、標(biāo)準(zhǔn)差（Standard Deviation,SD）作為衡量單位。表3 給出ORB-SLAM3、RGBD-SLAM、DS-SLAM 和本文DR-SLAM 四種算法的絕對軌跡誤差（Absolute Trajectory Error,ATE），表中加粗黑體表示最優(yōu)數(shù)值。由于ORB-SLAM3、RGBD-SLAM 和DS-SLAM 三種算法都是直接通過深度相機獲得深度圖，存在深度值丟失的問題，易出現(xiàn)跟蹤精度下降，導(dǎo)致無法準(zhǔn)確剔除動態(tài)點，從而無法有效完整地剔除動態(tài)物體。而DR-SLAM 通過深度圖優(yōu)化以及多視圖幾何雙向投影進行動態(tài)物體剔除并進行背景修復(fù)，既避免了深度黑洞值的影響又保留了大量的特征信息，該算法四個序列的所有衡量指標(biāo)都達到最好。通常SLAM 系統(tǒng)中RMSE 指標(biāo)最能反映軌跡精度優(yōu)劣，本文選取RMSE指標(biāo)做定量分析。在 fr3_walking_xyz 序列上，DR-SLAM 相較于ORB-SLAM3、RGBD-SLAM 以及DS-SLAM 的絕對軌跡誤差RMSE 分別減少92.6%、87.5%和 28.2%；在 fr3_walking_static 序列上，DR-SLAM 相較于ORB-SLAM3、RGBD-SLAM 以及DS-SLAM 的絕對軌跡誤差RMSE 分別減少91.5%、91.8%和37.5%；在fr3_walking_rpy 序列上，DR-SLAM相較于ORB-SLAM3、RGBD-SLAM 以及DS-SLAM的絕對軌跡誤差RMSE 分別減少76.4%、65.3%和3.3%；在fr3_walking_halfsphere 序列上，DR-SLAM相較于ORB-SLAM3、RGBD-SLAM 以及DS-SLAM的絕對軌跡誤差RMSE 分別減少52.3%、80.9%和1.4%。總的來說，DR-SLAM 相較于ORB-SLAM3、RGBD-SLAM 以及DS-SLAM 的平均絕對軌跡誤差RMSE 分別減少78.2%、81.4%和17.6%，驗證了本文動態(tài)剔除算法能夠有效剔除環(huán)境中的動態(tài)物體，為SLAM 系統(tǒng)的定位與建圖提供了良好基礎(chǔ)。

表3 四種算法在TUM 數(shù)據(jù)集下的ATE 對比（單位：米）Tab.3 Comparison of ATE of four algorithms in TUM dataset (Unit: m)

圖8 為ORB-SLAM3、RGBD-SLAM、DS-SLAM和DR-SLAM 四種算法在TUM 數(shù)據(jù)集運行時間的對比。

圖8 四種算法在TUM 數(shù)據(jù)集下運行時間對比Fig.8 Comparison of running time of four algorithms in TUM dataset

從圖8 可以看出，四種算法運行時間的平均值分別為105 s、101 s、537 s 和458 s。由于DR-SLAM 減少了深度不連續(xù)對SLAM 系統(tǒng)的影響，使DR-SLAM運行時間平均值與DS-SLAM 相比減少了15%；與RGBD-SLAM 和ORB-SLAM3 相比，由于DR-SLAM增加了語義分割網(wǎng)絡(luò)映射對深度圖的優(yōu)化以及動態(tài)物體的剔除，使得DR-SLAM 運行時間較RGBD-SLAM有較大的增加，但仍可滿足SLAM系統(tǒng)正常運行需求。

4.5 真實場景實驗

實驗平臺為Husky 輪式機器人，其硬件配置為：CPU 為i7-10875H 處理器，內(nèi)存8 GB；GPU 為GTX1080，操作系統(tǒng)為Ubuntu18.04。真實環(huán)境如圖9(a)所示，場地大小為12 m×5 m。圖9(b)為真實場景布局，圖中黃色和藍色部分為實驗室工作臺。

圖9 真實場景設(shè)置Fig.9 Real scene settings

圖10 為真實場景下 RGBD-SLAM 算法與DR-SLAM 算法的定位軌跡對比。本文算法是針對RGBD-SLAM 的問題進行研究的，而ORB-SLAM3 一般是在靜態(tài)場景中運行，無法有效地在動態(tài)場景中工作，又因為DS-SLAM 的分割算法只能分割簡單的物體，無法在實驗室復(fù)雜的環(huán)境中運行（易分割失敗），故本文只與RGBD-SLAM 作對比。從圖中可以看出，DR-SLAM 與RGBD-SLAM 的絕對軌跡誤差分別為0.078 m 和0.635 m，這是由于RGBD-SLAM 算法無法有效剔除動態(tài)物體，導(dǎo)致算法運行軌跡誤差較大，而本文算法加入深度優(yōu)化修復(fù)深度不連續(xù)以及聯(lián)合多視圖幾何剔除動態(tài)物體，提高了特征匹配精度，減小了軌跡誤差，因此本文算法在動態(tài)場景下能穩(wěn)定運行，具有較高的魯棒性。

圖10 軌跡對比Fig.10 Trajectory comparison

5 結(jié)論

本文提出了一種DR-SLAM 算法，該算法具有以下創(chuàng)新點：1）提出一種基于語義信息的深度圖優(yōu)化，改善了點線面SLAM在特征提取環(huán)節(jié)由于深度相機得到的深度圖易出現(xiàn)深度不連續(xù)影響特征跟蹤的問題，提高了SLAM 系統(tǒng)的位姿估計精度。2）針對突然出現(xiàn)動態(tài)物體造成軌跡誤差增大的問題，提出一種聯(lián)合深度信息與多視圖幾何完成動態(tài)物體剔除的方法，并利用雙向映射模型進行靜態(tài)背景的修復(fù)，提高了定位的精度并減少軌跡誤差。本文算法在公開數(shù)據(jù)集TUM上進行測試，結(jié)果表明，由于DR-SLAM 采用區(qū)域映射深度優(yōu)化獲取了良好的深度信息，以及聯(lián)合多視圖幾何剔除動態(tài)物體和背景修復(fù)，使得本文算法與RGBD-SLAM 和DS-SLAM 算法相比，在位姿和軌跡精度方面具有良好的優(yōu)勢，體現(xiàn)了良好的抗干擾能力與動態(tài)物體剔除能力。