一種基于物體追蹤的改進(jìn)語義SLAM算法

杜小雙，施 展，華云松

（上海理工大學(xué) 光電信息與計(jì)算機(jī)工程學(xué)院，上海 200093）

0 引言

隨著機(jī)器人技術(shù)、無人駕駛、增強(qiáng)現(xiàn)實(shí)等領(lǐng)域的發(fā)展與普及，視覺SLAM 作為其應(yīng)用的基礎(chǔ)技術(shù)之一［1］，得到了學(xué)者們的廣泛關(guān)注與研究，并成為機(jī)器人定位與建圖研究領(lǐng)域的一個(gè)熱點(diǎn)［2］。

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

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

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

1 系統(tǒng)設(shè)計(jì)

1.1 系統(tǒng)框架

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

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

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

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

1.3 物體追蹤

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

1.4 三重動(dòng)靜態(tài)檢測(cè)

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

1.4.1 重投影誤差

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

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

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

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

F1相機(jī)坐標(biāo)系中的MP點(diǎn)坐標(biāo)通過變換矩陣到F2幀相機(jī)坐標(biāo)系下的坐標(biāo)[X2，Y2，Z2]T為：

得到P'1點(diǎn)的投影坐標(biāo)后與匹配點(diǎn)P2坐標(biāo)求得匹配后的重投影誤差e為：

得到重投影誤差后與設(shè)定的誤差閾值對(duì)比則可進(jìn)行判斷，如果其重投影誤差大于閾值該特征點(diǎn)為動(dòng)態(tài)點(diǎn)。

1.4.2 極線約束

極線約束是攝像機(jī)運(yùn)動(dòng)學(xué)的重要組成部分，可用于檢查特征點(diǎn)的運(yùn)動(dòng)狀態(tài)。對(duì)極幾何示意圖如圖3所示。

Fig.3 Schematic diagram of epipolar geometry圖3 對(duì)極幾何示意圖

對(duì)于匹配特征點(diǎn)，相鄰幀間基礎(chǔ)矩陣F為：

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

則F2圖像平面上的匹配特征點(diǎn)P2對(duì)應(yīng)極線L計(jì)算如下：

最后，當(dāng)前幀的特征點(diǎn)到對(duì)應(yīng)極線距離D如下：

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

1.5 局部建圖

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

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

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

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

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

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

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

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

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

3 實(shí)驗(yàn)與分析

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

3.1 物體追蹤實(shí)驗(yàn)

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

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

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

3.2 軌跡估計(jì)誤差實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

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

4 結(jié)語

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