杜小雙，施 展，華云松

（上海理工大學 光電信息與計算機工程學院，上海 200093）

0 引言

隨著機器人技術、無人駕駛、增強現(xiàn)實等領域的發(fā)展與普及，視覺SLAM 作為其應用的基礎技術之一［1］，得到了學者們的廣泛關注與研究，并成為機器人定位與建圖研究領域的一個熱點［2］。

視覺同步定位與建圖（Simultaneous Localization and Mapping，SLAM）可以大致分為兩類：直接法和特征點法。直接法假設相鄰幀間光度值不變，用局部像素的梯度和方向進行計算并優(yōu)化出相機位姿，如LSD-SLAM［3］、DSO［4］等。特征點法通過特征點提取，并使用描述子匹配特征點，通過匹配特征點解出幀間位姿并優(yōu)化多幀位姿。由Mur-Artal 等［6-8］提出的ORB-SLAM 是一個適用于單目、雙目和RGB-D 相機的SLAM 系統(tǒng)，因出色的性能而得到了廣泛應用。

上述SLAM 算法都是基于靜態(tài)場景假設無法解決動態(tài)場景的問題，而隨著深度學習的快速發(fā)展，一些先進深度學習網(wǎng)絡，如DeepLab［9］、SegNet［10］和YOLACT［11］等可以精確進行像素級的語義分割，語義SLAM 即利用語義神經(jīng)網(wǎng)絡分割出圖像物體掩碼，通過剔除動態(tài)物體掩碼上的特征點以實現(xiàn)正確的跟蹤。Yu 等［12］提出的DS-SLAM 在ORBSLAM2 的3 個線程基礎上新增語義分割和稠密建圖兩個新線程，使用SegNet 網(wǎng)絡分割出物體掩碼并結(jié)合移動一致性檢查方法以減少動態(tài)對象影響，從而提高動態(tài)環(huán)境下的跟蹤精度。此外，Bescos 等［13］提出的DynaSLAM 利用mask R-CNN［14］神經(jīng)網(wǎng)絡和多視圖幾何檢測和剔除運動物體從而實現(xiàn)更精準的軌跡估計。

針對語義SLAM 僅停留在剔除動態(tài)物體特征點層面并沒有充分利用語義信息，無法解決分割錯誤對物體追蹤的影響，本文提出了一種基于物體追蹤的改進語義SLAM 算法，實現(xiàn)物體追蹤與定位，并通過剔除運動物體影響提高軌跡估計精度。本文貢獻如下：①提出一種物體動靜態(tài)檢測方法，對物體匹配特征點進行深度、重投影誤差和極線約束三重檢測實現(xiàn)精準的物體動靜態(tài)判斷；②提出一種物體追蹤狀態(tài)判別方法，通過物體的4 種追蹤狀態(tài)相互轉(zhuǎn)換，實現(xiàn)穩(wěn)健的環(huán)境物體追蹤與定位；③在TUM RGB-D 數(shù)據(jù)集上進行實驗，證明本文所提方法的有效性。

1 系統(tǒng)設計

1.1 系統(tǒng)框架

Fig.1 System architecture圖1 系統(tǒng)框架

1.2 語義分割網(wǎng)絡

針對室內(nèi)動態(tài)環(huán)境中的物體分割，本文采用YOLACT［15］++網(wǎng)絡。YOLACT++網(wǎng)絡是一種簡單的全卷積實時語義分割網(wǎng)絡，使用Resnet101-FPN 作為主干網(wǎng)絡，使用在MS COCO 數(shù)據(jù)集上訓練得到的網(wǎng)絡參數(shù)進行物體檢測。假設YOLACT++網(wǎng)絡的輸入是一個x×y×3 的RGB 圖像矩陣，則網(wǎng)絡的輸出就是一個x×y×n的掩碼矩陣，其中n是圖像中物體的數(shù)量。對于網(wǎng)絡最后的每個輸出通道m(xù)∈n，都會得到一個對象的掩碼矩陣。

在實時性上，YOLACT++使用Resnet101-FPN 主干網(wǎng)絡在MS COCO 2017 驗證集上的檢測速度可以達到27.3幀/s［15］。同樣地，在TUM 數(shù)據(jù)集實驗中，使用YOLACT++分割RGB 圖像中的人物、椅子和屏幕3 種物體，速度也能夠滿足算法語義分割任務中的實時性需求。每幀圖像分割完成后，將得到的掩碼與定位框輸入本文算法。在本幀圖片提取特征點時，結(jié)合掩碼得到每個特征點的物體類別。為了特征匹配的準確性，剔除位于物體掩碼邊緣的特征點。

1.3 物體追蹤

物體追蹤模塊為本文核心算法，當出現(xiàn)新的物體時創(chuàng)建物體追蹤對象，其包含物體追蹤狀態(tài)信息、物體相鄰兩幀的物體特征點（當前幀與上一幀）和由物體特征點所創(chuàng)建的物體地圖點。再得到當前幀位姿后將上一幀追蹤物體特征點投影到當前幀上進行匹配，對匹配成功的特征點進行三重動靜態(tài)檢測以判斷其動靜態(tài)，之后根據(jù)特征點匹配點數(shù)量與動靜態(tài)點比例更新物體追蹤狀態(tài)。如果當前幀需要創(chuàng)建關鍵幀則將物體追蹤信息輸入關鍵幀中，在局部建圖中根據(jù)關鍵幀中物體追蹤信息新建替換與刪除地圖點。

1.4 三重動靜態(tài)檢測

在匹配物體特征點時，對匹配成功的物體特征點進行深度、重投影誤差和極線約束三重檢測以判斷是否為動態(tài)點，其中深度檢測為匹配特征點投影到當前幀上的深度值與當前幀的深度計算其差值后與深度閾值對比，差值超過深度閾值則認為該特征點為動態(tài)點。

1.4.1 重投影誤差

重投影誤差即將上一幀的匹配特征點投影到當前幀上與匹配點的直接平面距離差距。重投影誤差如圖2所示。

Fig.2 Schematic diagram of reprojection error圖2 重投影誤差示意圖

圖2 中，P1、P2為匹配特征點，MP為點P1投影到3D 空間中的地圖點和P'1在F2像素平面的投影，相機內(nèi)參矩陣為K，F(xiàn)1到F2之間的旋轉(zhuǎn)矩陣為R，平移向量為t。其中，P1點的像素坐標為P1=[u1，v1]，P1的像素坐標為P2=[u2，v2]。MP在空間中地圖點的坐標為MP=[X1，Y1，Z1]，其中深度Z1通過RGB-D 相機的深度圖獲得。X1、Y1與像素坐標系的關系如下：

后來，朝廷下令，把撫順關馬市關閉了，這一關就是幾個月。馬市不開，女真人沒鹽，沒鐵器，沒馬匹，生活不了，他們就上書朝廷，要求開關，恢復交易。朝廷又重新開市，開市以后，地方上的守關明軍照樣勒索。王杲呢，再帶女真人去打。這樣，把朝廷又惹翻兒了，一面安撫，一面帶兵去圍剿女真人。李成梁受命帶領幾萬明軍，把古埒城團團圍住。

F1相機坐標系中的MP點坐標通過變換矩陣到F2幀相機坐標系下的坐標[X2，Y2，Z2]T為：

得到P'1點的投影坐標后與匹配點P2坐標求得匹配后的重投影誤差e為：

得到重投影誤差后與設定的誤差閾值對比則可進行判斷，如果其重投影誤差大于閾值該特征點為動態(tài)點。

1.4.2 極線約束

極線約束是攝像機運動學的重要組成部分，可用于檢查特征點的運動狀態(tài)。對極幾何示意圖如圖3所示。

Fig.3 Schematic diagram of epipolar geometry圖3 對極幾何示意圖

對于匹配特征點，相鄰幀間基礎矩陣F為：

其中，K為相機內(nèi)參，R、t為幀間的旋轉(zhuǎn)矩陣和平移向量。在得到基礎矩陣F后對每個物體掩碼的匹配特征點計算當前特征點的極線。上一幀的特征點通過基礎矩陣映射到當前幀的區(qū)域即為極線。設前一幀和當前幀中的匹配特征點分別表示為[u1，v1]和[u2，v2]，u、v表示特征點的像素坐標值，而P1=[u1，v1，1]，P2=[u2，v2，1]表示匹配特征點的齊次坐標形式。匹配特征點的極線約束如下：

則F2圖像平面上的匹配特征點P2對應極線L計算如下：

最后，當前幀的特征點到對應極線距離D如下：

判斷匹配特征點極線之間的距離是否大于閾值，如果距離大于閾值，則該特征點被視為動態(tài)點。

1.5 局部建圖

在局部建圖中，本文算法只對靜態(tài)地圖點進行操作。由于動態(tài)地圖點已經(jīng)無法跟蹤優(yōu)化，因此將其設置為壞點不加入關鍵幀與地圖點的局部優(yōu)化，并且動態(tài)物體也不再新建地圖點，僅僅依靠幀間的物體特征點進行追蹤。如果動態(tài)物體在算法運行中由動態(tài)轉(zhuǎn)換為靜態(tài)，就可以正常新建地圖點進行跟蹤優(yōu)化。對于消失在相機視野中的靜態(tài)物體，其物體地圖點得到保留，并且它們將被用于恢復物體跟蹤狀態(tài)。

2 基于語義分割與動靜態(tài)檢測的追蹤狀態(tài)轉(zhuǎn)換

在本文算法中，物體都具有4 種追蹤狀態(tài)：①靜態(tài)是匹配特征點、動靜態(tài)點在閾值范圍內(nèi)；②動態(tài)是匹配特征點、動靜點比例超過閾值范圍；③即將丟失是當前幀未匹配到同類型掩碼；④已丟失是連續(xù)多幀未匹配同類型掩碼。

物體跟蹤狀態(tài)轉(zhuǎn)換過程如圖4所示。具體步驟如下：

Fig.4 Schematic diagram of object tracking state transition圖4 物體追蹤狀態(tài)轉(zhuǎn)換示意圖

（1）當物體首次出現(xiàn)在相機視野時，本文算法會根據(jù)物體的類別創(chuàng)建跟蹤對象，并在創(chuàng)建的初始幀中確定物體所處的運動狀態(tài)。如果判斷為靜態(tài)物體特征點和地圖點，加入后續(xù)匹配跟蹤；反之，判斷為動態(tài)則刪除地圖點，其特征點不參與后續(xù)匹配跟蹤。

（2）正常追蹤的物體會根據(jù)當前幀間物體特征點匹配情況更新當前幀中的跟蹤狀態(tài)。如果匹配成功，則使用動靜態(tài)點比例判斷物體當前幀的運動狀態(tài)；反之，則判斷物體追蹤失敗，將物體轉(zhuǎn)為即將丟失狀態(tài)，并儲存物體信息用于丟失找回。

（3）當物體處于即將丟失狀態(tài)時，會儲存丟失前一幀的物體相關信息，在之后匹配過程中使用丟失幀的儲存信息進行匹配。如果成功匹配上相同類型的掩碼，則物體恢復追蹤狀態(tài)后正常追蹤；而對于超過十幀依然沒有匹配正確的物體掩碼，將物體的追蹤狀態(tài)轉(zhuǎn)為已丟失，不再對物體進行追蹤，動態(tài)物體直接刪除跟蹤信息，靜態(tài)物體則保留全局地圖點等待之后的召回。

在算法物體追蹤過程中，通過物體4 種追蹤狀態(tài)的相互轉(zhuǎn)換，可以有效地避免遮掩與分割掩碼錯誤導致的追蹤失敗，從而穩(wěn)定地實現(xiàn)物體的追蹤與定位。

3 實驗與分析

對于本文所提出的SLAM 算法，將通過實驗證明其算法的準確性與穩(wěn)定性，在公共TUM RGB-D數(shù)據(jù)集上進行兩個部分的實驗：①對TUM RGB-D 數(shù)據(jù)集中的walking_halfsphere 序列進行實驗，通過跟蹤物體定位框?qū)Ρ疚乃惴ǖ奈矬w追蹤能力進行評估；②對TUM RGB-D 數(shù)據(jù)集中的8個sitting序列和walking序列進行定量分析。利用TUM數(shù)據(jù)集的官方評估工具對本文算法得到的軌跡進行評估，同時使用其他SLAM算法結(jié)果與本文算法結(jié)果進行比較。

3.1 物體追蹤實驗

在實驗中使用walking_halfsphere 序列集，此場景中人與相機既有運動又有靜止，物體間存在相互遮蓋和重合，相機自身運動包含平移和旋轉(zhuǎn)，這些要素的結(jié)合對于物體的分割、跟蹤都是一項較大的挑戰(zhàn)，因此選擇此序列集進行實驗能夠較為準確地評估基于本文算法的物體追蹤性能。圖5 是基于本文算法運行walking_halfsphere 序列時的圖像（彩圖掃OSID 碼可見）。

Fig.5 Operation diagram of the object tracking system圖5 物體追蹤系統(tǒng)運行圖

從圖5（a）可以看出，在實驗開始時，人和屏幕都被成功追蹤，其中動態(tài)物體為人，靜態(tài)物體為屏幕。圖5（b）中相機和人都在運動，先前追蹤的人已經(jīng)丟失，此時刪除該追蹤信息，等到人再出現(xiàn)在相機視野中時，重新創(chuàng)建新的追蹤對象并追蹤。圖5（c）和圖5（d）中，兩個椅子進入相機視野，被成功追蹤并編號為一號椅子和二號椅子。之后，圖5（e）、圖5（f）與圖5（g）中兩人重新回到相機視野且其中一人坐回椅子，該人從動態(tài)轉(zhuǎn)變?yōu)殪o態(tài)。圖5（h）是相機與人運動后的圖像。由此證明，本文所提出的算法可正常追蹤物體，同時具有更新物體追蹤狀態(tài)的能力。

3.2 軌跡估計誤差實驗

實驗中將對TUM RGB-D 數(shù)據(jù)集中的siting 和walking兩種類型共8 條序列進行實驗并將結(jié)果與ORB-SLAM3、DS-SLAM 和DynaSLAM 的軌跡估計誤差數(shù)據(jù)進行比較。

圖6 為本文算法和ORB-SLAM3 在高動態(tài)場景中軌跡估計誤差對比圖，其中黑色線段為真實軌跡，藍色線段為估計軌跡，而紅色表示真實軌跡與估計軌跡間的差值，紅色范圍越大表示誤差越大（彩圖掃OSID 碼可見）。

Fig.6 Schematic diagram of estimated trajectory error圖6 ORB-SLAM3系統(tǒng)的估計軌跡誤差示意圖

圖6 與圖7 為本文算法與ORB-SLAM3 在walking_halfsphere 和walking_xyz 兩圖像序列集下的估計軌跡誤差示意圖，可以看出本文算法與ORB-SLAM3 相比在動態(tài)環(huán)境中的軌跡誤差存在較大差距。因此，以ORB-SLAM3 為代表的傳統(tǒng)視覺SLAM 在處理動態(tài)環(huán)境上存在不足，同時表明對動態(tài)物體進行去除可使視覺SLAM 跟蹤精度得到極大提高。

Fig.7 Schematic diagram of the estimated trajectory error of the algorithm in this paper圖7 本文算法的估計軌跡誤差示意圖

在該部分實驗中，將本文算法與ORB-SLAM3、DSSLAM、DynaSLAM 的結(jié)果進行比較，使用TUM 官方提供的誤差分析工具對估計軌跡與真實軌跡進行對比。其測量值為絕對軌跡誤差（Absolute Trajectory Error，ATE）與相對姿態(tài)誤差（Relative Pose Error，RPE），使用均方根誤差（Root Mean Square Error，RMSE）和標準差（Standard Deviation，SD）作為評估指標。結(jié)果如表1—表3 所示（“—”表示相關論文中沒有提供結(jié)果）。

Table 1 Comparison of ATE result of TUM RGB-D dataset表1 TUM RGB-D 數(shù)據(jù)集的絕對軌跡誤差結(jié)果比較

Table 2 Comparison of results of relative pose translation error of TUM RGB-D dataset表2 TUM RGB-D 數(shù)據(jù)集的相對姿態(tài)平移誤差結(jié)果比較

Table 3 Comparison of relative pose rotation error results of TUM RGB-D dataset表3 TUM RGB-D數(shù)據(jù)集的相對姿態(tài)旋轉(zhuǎn)誤差結(jié)果比較

從表1—表3 可以看出，與原版ORB-SLAM3 系統(tǒng)相比，語義SLAM 算法可以極大減少在動態(tài)場景下的絕對軌跡誤差與相對姿態(tài)誤差。本文算法與DS-SLAM 和DynaSLAM 相比，在4 個動態(tài)序列中的3 個（walking_xyz，walking_rpy 和walking_half）中的絕對軌跡誤差和相對姿態(tài)誤差都有不同程度的下降；而在靜態(tài)序列中，由于DS-SLAM與DynaSLAM 缺少相關數(shù)據(jù)，因而本文算法與ORBSLAM3 進行對比，在4 個靜態(tài)序列中由于本文算法在跟蹤中剔除了類別匹配錯誤的特征點，故在靜態(tài)場景下的估計誤差比ORB-SLAM3 更小，但當前視覺SLAM 算法在靜態(tài)場景中已經(jīng)有較好的性能，因此可提升的空間有限。

4 結(jié)語

本文提出了一種基于ORB-SLAM3 的語義SLAM 算法，該算法使用YOLACT++獲取圖像中掩碼與定位框，對物體特征點匹配后通過三重檢測判斷其動靜狀態(tài)，再根據(jù)數(shù)量與動靜態(tài)比例更新當前物體跟蹤狀態(tài)，實現(xiàn)穩(wěn)定的物體追蹤，同時剔除動態(tài)物體的影響以實現(xiàn)精準的軌跡估計。本文算法的實驗結(jié)果與其他視覺SLAM 算法的實驗結(jié)果相比，可以更加準確地估計相機運動軌跡并正確地追蹤到物體的運動狀態(tài)與位置，具有廣泛應用價值。