室內機器人動態SLAM技術

韓慧妍，韓方正，韓 燮，況立群，曹亞明

(1.中北大學 計算機科學與技術學院，山西 太原 030051；2.機器視覺與虛擬現實山西省重點實驗室，山西 太原 030051；3.山西省視覺信息處理及智能機器人工程研究中心，山西 太原 030051)

0 引 言

同時定位和映射(SLAM)在無人系統導航和人機交互中起著重要作用。在室內場景地圖構建過程中，大多數SLAM假設環境是靜態的，然而室內環境存在人、寵物等動態物體，當運動物體進入相機的視場時，這些物體將保留在構建的地圖中，同時會干擾相機的姿態估計[1]。

針對此類問題，早先的方法將預定義運動物體的語義信息與空間幾何信息相結合[2]，消除運動物體的負面影響。事實上，語義分割方法提供的對象原始掩膜并不完善，無法完全覆蓋運動的物體，尤其是運動物體的輪廓邊界，其邊界信息被泄漏到點云圖中，形成大量的噪聲塊。在本文中，提出了一種在室內動態環境下工作的BN-SLAM系統，該系統具有較為完整的精確特征點匹配，且移動物體去除方法與ORB-SLAM3相結合。在BN-SLAM中，BN網絡是一種實時的深度神經網絡，它可以同時獲得環境中運動物體的語義包圍盒和原始掩膜，利用運動物體的語義包圍盒將深度圖像快速劃分為靜態環境區域和潛在的動態區域。靜態環境區域中的匹配點可以通過構建潛在動態區域中的外極約束來定位，再利用深度圖像的幾何信息對原始掩模進行修正，以更完整地覆蓋運動物體，使用修正后的掩模去除局部點云中的噪聲塊，通過合并這些局部點云，可以得到一個清晰的全局點云圖。本文的主要貢獻如下：第一，提出了一種結合原始掩模和深度圖像信息獲取運動物體修正掩模的方法；第二，提出了一種消除點云圖構造中由運動物體形成的噪聲塊的方法。

1 相關工作

在未獲取運動物體信息時，尋求可靠的靜態匹配特征是動態SLAM系統的基本任務。Zhang等[3]提出了一種基于RGB-D數據的運動去除方法，作為一個預處理模塊來過濾掉運動的物體。DMS-SLAM[4]使用GMS[5]和滑動窗口進行系統初始化，消除運動物體的影響，構建靜態三維地圖，通過重投影得到與局部地圖中當前幀對應的三維點云，將這些點與參考幀模型相結合，實現當前幀的位姿估計并更新局部地圖中三維點云。DynaSLAM[6]結合多視角幾何和初始掩膜，去除預定義的運動目標，并填充該區域。DynaSLAM II[7]估計攝像機位姿，構建稀疏3D地圖并記錄攝像機的運動軌跡，利用光束平差法優化靜態場景結構及動態物體的姿態。MaskFusion[8]基于圖像的實例級語義分割創建語義對象掩碼，識別運動物體。MaskFusion也可以分割語義類標簽，并將其分配給場景中的不同對象，同時進行跟蹤和重建。MIDFusion[9]通過提取幾何、語義和運動特征識別運動物體。DS-SLAM[10]濾除運動物體后，基于物體運動的連續性去除動態匹配點。DP-SLAM[11]基于貝葉斯似然估計架構追蹤動態匹配點，糾正幾何約束和語義分割中的錯誤。以上動態SLAM系統側重于提高攝像機定位精度，本方法的重點是可以在動態環境中獲得干凈和準確的點云圖。深度圖像[12]存在一定的深度缺失區域，不適合使用多模態網絡對RGB-D圖像進行語義分割，在平衡了性能和實時性之后，本系統選擇BN作為語義分割網絡，同時可以進一步消除運動物體產生的噪聲塊，剔除運動物體在場景中的拖影和輪廓邊界。

2 BN-SLAM系統

2.1 BN-SLAM系統概述

基于實驗室已購置的松靈移動機器人、RGB-D攝像機、工控機，搭建BN-SLAM系統，具體研究方案如圖1所示。RGB圖像通過BN網絡，獲得原始掩膜和環境對象的語義包圍盒[13]，原始掩膜與深度掩膜結合可以進行修正得到修正掩膜。在攝像機定位過程中，通過移動物體的語義包圍盒，可以將圖像快速劃分為環境區域和潛在的動態區域，通過環境區域中的匹配點來構建外極約束，以對潛在動態區域中的靜態和動態匹配點進行分類。然后，使用靜態匹配點進行相機定位。在精確估計相機姿態后，通過拼接局部點云，可以得到全局點云圖。

圖1 BN-SLAM系統方案

根據對象在環境中的狀態，對象大致可以分為3類：移動的物體：如行走的人。這些物體不僅直接干擾攝像機的姿態估計，而且在構建的環境地圖中留下大量的噪聲塊。靜態對象：如冰箱、桌子、監視器等。這些對象通常位于環境中的相同位置，并且不會經常被移動?？梢苿拥奈矬w：如椅子、書籍等。這些對象可以是移動的，也可以是靜態的。

由于BN網絡獲得的原始目標掩模不完善，因此利用目標區域的深度信息對原始掩模進行改進，即得到能夠完全覆蓋目標區域的修正掩模。在去除由移動物體引起的噪聲塊后，可以獲取清晰的局部點云。在構建全局點云圖中，即將每一組關鍵幀對應的局部點云進行合并。如式(1)所示

(1)

其中，Pi表示局部點云，PE表示全局點云圖，旋轉矩陣Ri和平移矩陣ti由攝像機所在參考坐標系的位姿決定。

2.2 基于深度信息修復初始掩膜

由于深度攝像機的限制，深度圖像中的一些區域失去了其深度值。當一個物體的表面非常光滑時，這個物體的深度值也會嚴重缺失。針對深度圖像的缺失等問題，使用大小為2×2的滑窗遍歷深度圖像，并記錄滑窗內的深度值，如式(2)所示，其中(u，v)表示滑窗左上角像素對應的圖像坐標，邊緣點可通過式(3)提取，τ1為閾值

(2)

(3)

(4)

(5)

(6)

圖2 人輪廓的修正過程

2.3 移除與運動物體交互的可移動物體

(1)判斷二者是否發生交互

對掩膜修正后，若運動物體的掩膜與可移動物體的掩膜有交集，則認為兩者存在交互，如式(7)所示，其中i=1，…，n，n為圖像中運動物體的總量，j=1，…，m，m為圖像中可移動物體的總量，DObj和SObj分別為運動物體和靜態物體的集合

(7)

如圖3所示，兩個人和兩把椅子修改后的掩膜顯示在方框中。轉椅是C(1)， 普通椅子是C(2)， 兩人與圖2一致。經過判斷，P(1) 與C(1) 發生互動。

圖3 人與椅子交互

(2)移除移動物體的殘留噪聲

pC=HpP

(8)

利用相鄰兩幀殘差邊界的深度值相差較大的特性將邊界噪聲去除，如式(9)所示，其中dP和dC是IP和IC對應的深度圖像，τ4為閾值。最后，通過形態學方法去除深度圖像中的孤立圖像塊，以確保完全去除運動物體所在區域

(9)

基于以上操作，將得到清晰完整的單視角靜態場景點云圖。對應的給出流程如圖4所示。

圖4 殘余噪聲塊清除

如圖5所示，第一行為去除噪聲塊之前的局部點云，噪聲塊用矩形標記。第二行為去除噪聲塊后的局部點云數據。即噪聲塊被完全除去。

圖5 去除運動物體前后點云圖比較

2.4 靜態匹配點位姿

在攝像機定位過程中，基于運動物體的語義包圍盒，將圖像快速劃分為靜態區域和潛在動態區域?；跇O線約束并使用加權RANSAC方法，將潛在動態區域分類為靜態和動態匹配點，為可靠性較高的靜態物體點賦予較高的置信度，以有效消除來自運動物體和可移動物體的不利影響，利用剩余的靜態匹配點計算攝像機位姿。

設前一幀IP和當前幀IC的兩組靜態匹配點為PP={PP1，PP2，…，PPm}和PC={PC1，PC2，…，PCm}，IP與IC之間的攝像機位姿變換可通過最小二乘法求解式(10)得到

(10)

從IP和IC的靜態區域中提取匹配點，其中I為潛在動態區域所對應匹配點的集合。內點查找函數使用輸入模型對樣本進行評估，并返回誤差小于閾值的內點子集。加權RANSAC步驟如圖6所示。

圖6 加權RANSAC步驟流程

步驟1 與傳統算法RANSAC算法一致，從數據集P中隨機選取n個實驗數據，所選取的n個實驗數據需要滿足構成對應圖像數據集模型Q1的數量，并計算模型參數；

步驟2 模型Q1對圖像數據集的所有匹配點進行距離計算，并根據閾值判斷內外點，保留內點，剔除外點；

步驟3 將已獲取的內點集作為初始樣本，進行基于最小二乘法的初始樣本模型，得到Q2；

步驟4 計算內點集與經過最小二乘法優化過的樣本模型Q2間的距離，并通過與閾值比較，篩選內點，剔除外點；

步驟5 對以上4步進行循環迭代，使得迭代次數滿足預期設定迭代值，得到最優模型Qbest。

(11)

在IP和IC的潛在動態區域中提取匹配點集PDP=[uDP，vDP，1]，PDC=[uDC，vDC，1]。 計算潛在動態匹配點到對應極線的距離，如式(12)所示，其中，lx和ly可通過式(13)求得

(12)

(13)

計算潛在動態區域中第i個匹配點狀態的方法如式(14)所示，其中D和S分別表示動態和靜態匹配點集，τ5為閾值。靜態匹配點定位流程如圖7所示。詳細步驟如下：

圖7 靜態匹配點算法流程

步驟1 獲取正確的RGB圖像，選取在非潛在動態區域的靜態環境區域，并提取靜態點；

步驟2 將提取到的特征點通過加權RANSAC算法得出基礎矩陣；

步驟3 選取在潛在動態區域匹配點，通過計算對應點極線的距離，當對應點極線距離小于閾值則加入到靜態匹配點集合；

步驟4 當對應點極線距離大于閾值則剔除；

步驟5 通過上述4步操作，即可得到靜態匹配點集合

(14)

3 實驗和討論

3.1 機器人搭建

本文搭建的地面機器人采用四輪差速底盤，可以四輪驅動，底盤最小轉彎半徑為0 m，爬坡角度接近30度。選用額定功率為300 W的12寸輪轂電機作為驅動電機，采用無刷直流驅動器驅動車輪轉動，如圖8所示。機器人分為3層：頂層包括D435相機[15]、激光雷達；中間層包括各類傳感器和工控機；底層包含電源、Jetson AGX Xavier和顯示器。其中，Jetson AGX Xavier可以進行視覺測距、傳感器融合、定位與地圖繪制、障礙物檢測等工作，擁有32萬億次運算/秒(TOPS)的峰值計算能力和750 Gbps的高速I/O性能，如圖9所示。

圖8 機器人實物

圖9 機器人配件

3.2 實驗環境設置

機器人工控機搭載Ubuntu20.04系統，Ubuntu系統內安裝的機器人操作系統(robot operating system，ROS)作為機器人程序的運行平臺。在ROS中實現用戶界面(userinterface，UI)、人機交互、圖像處理、SLAM、運動決策等功能并通過控制串口收發實現對機器人運動的控制與傳感器信號的獲取，該機器人分別進行兩次實驗，第一次運行標準TUM數據集來驗證各類算法的精確度，并展現本文BN-SLAM系統的優化效率；第二次驗證本文所提出的BN-SLAM系統在實際環境中的效果。為了保證數據的高速傳輸，將D435及各類傳感設備連接到主機的USB3.0端口，所有獲得的數據記錄在機器人內部工控機。

3.3 實驗功能實現

該實驗中，在TUM RGB-D數據集上比較BN-SLAM與ORB-SLAM2[14]，Dyna-SLAM和DS-SLAM的性能。TUM數據集提供了具有相機軌跡和參數的動態場景。選取步行序列代表高度動態的環境，并作為實驗數據集。

相機有4種運動模式：半球形(half)，相機在半球形軌跡上移動；rpy，表示相機繞主軸旋轉；static，表示相機保持固定；xyz，表示相機沿x、y和z軸移動。使用fr3/w/half、fr3/w/rpy、fr3/w/static、fr3/w/xyz來表示4組步行圖像序列，其中fr3、w分別代表參與實驗的人、運動為行走。

3.4 誤差估計

在實驗中，絕對軌跡誤差(ATE)和相對姿勢誤差(RPE)用于定量評估[16]。讓P1，…，Pn∈SE(3) 表示估計的相機位姿，Q1，…，Qn∈SE(3) 表示實際軌跡位姿。ATE測量估計軌跡全局的一致性，ATEFi在時間步長i可以通過式(15)求得

(15)

S表示剛體變換，它將估計軌跡與實際軌跡對齊。RPE測量固定時間內估計軌跡的局部精度Δ，RPEEi在時間步長i可以通過式(16)求得

(16)

首先，對4個系統的ATE和RPE進行定量分析。4個SLAM系統的ATE和RPE的均方根誤差(RMSE)和標準偏差(S.D.)值如表1～表3所示。RMSE測量觀測值與真實值之間的偏差，即反映了系統的魯棒性。S.D.衡量整體數據的偏差程度，則展現了系統的穩定性。表中的改進率計算式(17)如下

(17)

表1 絕對軌跡誤差(ATE)的結果

其中，κ表示改進值，α是ORB-SLAM2的值，β代表本文的值。

表1和表2，BN-SLAM在fr3/w/half和fr3/w/xyz序列中，ATE和平移RPE的效果最好；在fr3/w/rpy序列中，BN-SLAM的結果僅次于Dyna-SLAM。表3顯示了旋轉RPE的結果，BN-SLAM在fr3/w/half序列中的S.D.實現了最佳值；在序列fr3/w/rpy中，BN-SLAM的結果僅次于Dyna-SLAM。

表2 平移相對姿勢誤差(RPE)的結果

表3 旋轉相對姿勢誤差(RPE)的結果

根據表1～表3獲得的結果，在高動態環境中，消除移動物體后，相機的姿態估計將大大提高。Dyna-SLAM、DS-SLAM和BN-SLAM在上述4組序列中取得了良好的姿勢估計結果。對于BN-SLAM，序列中ATE、平移RPE和旋轉RPE的平均RMSE值分別為95.46%、92.45%和90.88%。且平均S.D.值分別為94.88%、94.76%和92.80%。

從圖10可以看出，4個SLAM系統都失去了部分軌跡點。為定量比較4個系統跟蹤的軌跡點，獲取了實際跟蹤軌跡點的結果，見表4。與ORB-SLAM2、Dyna-SLAM和DS-SLAM相比，BN-SLAM跟蹤的軌跡點在序列中是最完整的。在該實驗序列中，BN-SLAM跟蹤軌跡點結果的平均率為98.80%。

表4 實際跟蹤軌跡對比結果

圖10 4種SLAM軌跡對比

3.5 點云圖的估計

圖11顯示了TUM數據集在高動態序列中對運動物體去除的結果。第一行顯示從序列中選取的RGB圖像。第二行和第三行分別顯示了獲得的原始掩膜和該方法修正后生成的掩膜，即原始掩膜的邊界是平滑的，而修改后的掩膜邊界更清晰。原始掩膜無法覆蓋移動物體的某些部分，例如第一列中行人的腿，第三列中坐著的人的腳和背部，以及第四列左側人的手臂。與原始掩膜相比，修正后的掩膜可以更完整地覆蓋移動物體的區域。第四行圖像是去除運動物體的修正掩模所覆蓋區域后獲得的局部點云，其中運動物體形成的拖尾已被去除。

圖11 運動物體去除結果

如圖12所示，ORB-SLAM2、Dyna-SLAM和DS-SLAM在高動態序列fr3/w/xyz中獲得的全局點云圖的三視圖。兩人的大部分信息都未能去除，桌子的木板嚴重變形。Dyna-SLAM和DS-SLAM消除了兩人的干擾，但由于語義分割不完善，兩人的邊界信息被泄露到點云圖中。與ORB-SLAM2相比，Dyna-SLAM和DS-SLAM的點云圖中的噪聲塊相對稀疏，即可以清楚地看到環境中有哪些對象，但仍在點云圖中具有大量噪聲塊。由于環境中移動人員泄露的邊界信息已經被濾除，所以在BN-SLAM獲得的點云圖中幾乎沒有剩余的人員信息。

圖12 高動態序列fr3/w/xyz中點云圖對比

4個SLAM算法在低動態序列fr3/s/xyz中獲得的點云圖的三視圖如圖13所示。在第一列圖像中，由于ORB-SLAM2可以在低動態環境中準確地定位相機，且這些點被映射到正確的位置，所以點云圖中的對象清晰可見，且桌面板不會變形，但兩人的信息仍未能去除。第二列和第三列的圖像中，Dyna-SLAM和DS-SLAM已經濾除了這兩個人的大部分信息，但仍泄露出由噪聲塊造成的兩人輪廓邊界。從第四列的圖像中可以看出，去除了由兩人形成的噪聲塊。與其它3種SLAM系統相比，BN-SLAM在低動態序列中獲得的全局點云圖效果最好。

圖13 低動態序列fr3/s/xyz中點云圖對比

3.6 在現實環境中的評估

為了驗證BN-SLAM在現實環境中的有效性，使用D435相機在實驗室環境中進行實驗。圖14顯示了動態特征點的剔除結果，該實驗將實際場景中靜態環境區域和潛在動態區域快速劃分，精準識別出運動物體，即圖中語義包圍盒中的運動的人。非運動物體上的大部分特征點被判定為靜態點。在實驗中，可以較好識別出各類靜動態物體。圖15分別顯示了在去除人的信息前后所構建的點云圖。在第一行，人體后方的靜態物體被移動的人形成的噪聲塊擋住，墻壁也由于錯誤的相機定位而嚴重變形。在第二行中，人體的干擾被消除，靜態物體清晰可見，房間的幾何結構保存完好。

圖14 動態特征點的剔除結果

圖15 去除人的信息前后所構建的點云圖

4 結束語

在本文中，提出了一個在動態環境中工作的BN-SLAM系統，它將原始掩膜和移動物體的深度信息結合起來得到深度掩膜，其深度掩膜與原始掩膜結合得到的修正掩?？梢杂行Ц采w運動物體的區域。本文使用移動對象的語義包圍盒，將圖像分為環境區域和潛在的動態區域。利用環境區域中的匹配點構建極性約束，以去除潛在動態區域中的異常值。實驗結果表明，BN-SLAM系統能夠在動態環境中獲得準確的相機姿態和清晰的全局點云圖。本文系統目前設計用于處理室內動態場景，尚未擴展到室外場景，如何快速識別并濾除室外場景中的多類移動物體是下一步工作的重心。