余東應(yīng)，劉桂華，曾維林，馮 波，張文凱

（西南科技大學(xué)信息工程學(xué)院，四川綿陽 621010）

0 概述

SLAM（Simultaneous Localization and Mapping）在機(jī)器人位姿估計(jì)及環(huán)境感知過程中起著至關(guān)重要的作用，主要應(yīng)用于機(jī)器人的導(dǎo)航和避障任務(wù)以及自動(dòng)駕駛［1-3］和增強(qiáng)現(xiàn)實(shí)［4-5］等領(lǐng)域。視覺傳感器體積小、使用方便，并且圖像存儲(chǔ)了豐富的環(huán)境紋理和色彩信息，從而使得V-SLAM（Visual-Simultaneous Localization and Mapping）得到廣泛關(guān)注和研究。雖然目前的V-SLAM系統(tǒng)框架比較成熟，解決了V-SLAM 任務(wù)中的很多問題，許多先進(jìn)的SLAM 算法［6-8］也相繼被提出，但是，這些算法均在靜態(tài)的場(chǎng)景或數(shù)據(jù)集上進(jìn)行實(shí)驗(yàn)測(cè)試，可以達(dá)到很好的定位精度與魯棒性，但并未解決場(chǎng)景中存在動(dòng)態(tài)物體的問題。當(dāng)場(chǎng)景中存在動(dòng)態(tài)物體，如人、車等，會(huì)導(dǎo)致V-SLAM 出現(xiàn)定位誤差較大或跟蹤失敗的問題，因此，在動(dòng)態(tài)環(huán)境中進(jìn)行位姿估計(jì)與地圖構(gòu)建依然是一個(gè)挑戰(zhàn)。目前用于消除動(dòng)態(tài)物體影響的方法主要有直接篩選剔除法和語義分割法2 種。

直接篩選剔除法可以分為2 類，一類是根據(jù)圖像幀提取的關(guān)鍵點(diǎn)，如Shi-Tomasi［9］、FAST［10］和ORB［11］等，直接進(jìn)行關(guān)鍵點(diǎn)篩選，最具代表性的算法是RANSAC［12］，該算法針對(duì)低動(dòng)態(tài)場(chǎng)景具有較好的魯棒性，但是，當(dāng)場(chǎng)景中存在運(yùn)動(dòng)的車或人時(shí)，依然會(huì)導(dǎo)致位姿估計(jì)錯(cuò)誤或跟蹤失?。涣硪活惙椒ǜ鶕?jù)連續(xù)圖像幀之間的運(yùn)動(dòng)關(guān)系，采用光流法進(jìn)行動(dòng)態(tài)物體檢測(cè)，如文獻(xiàn)［13］結(jié)合最優(yōu)估計(jì)和均勻采樣的方式進(jìn)行動(dòng)態(tài)物體檢測(cè)，相比于傳統(tǒng)的光流法，其速度快，但是精度較低，文獻(xiàn)［14］采用RGB 光流法進(jìn)行場(chǎng)景中的動(dòng)態(tài)物體分割，文獻(xiàn)［15］根據(jù)雙目立體的場(chǎng)景流進(jìn)行動(dòng)態(tài)物體檢測(cè)，但是上述2 個(gè)系統(tǒng)均需要提供動(dòng)態(tài)物體的先驗(yàn)信息，難以適用于長(zhǎng)期的位姿跟蹤。

語義分割法使用深度學(xué)習(xí)的方式，檢測(cè)場(chǎng)景中的動(dòng)態(tài)物體，并對(duì)其所處區(qū)域進(jìn)行標(biāo)注與分割，基于該方法已取得諸多研究成果［16-18］。文獻(xiàn)［18］在ORB-SLAM2 的基礎(chǔ)上，采用Segnet［19］網(wǎng)絡(luò)進(jìn)行語義分割，獲得圖像中的語義信息，同時(shí)結(jié)合運(yùn)動(dòng)一致性檢查剔除動(dòng)態(tài)物體上的特征點(diǎn)，該算法構(gòu)建了帶有語義信息的八叉樹地圖，具有較好的實(shí)時(shí)性，但是定位精度不高。文獻(xiàn)［20］采用Mask R-CNN［21］進(jìn)行圖像的實(shí)例分割，對(duì)于算法中的RGB-D 相機(jī)部分，采用多視角幾何檢測(cè)動(dòng)態(tài)特征點(diǎn)并將其移除，同時(shí)結(jié)合圖像修復(fù)技術(shù)實(shí)現(xiàn)靜態(tài)三維稠密地圖構(gòu)建，該算法具有較高的定位精度，但是系統(tǒng)實(shí)時(shí)性不高。文獻(xiàn)［22］采用稀疏場(chǎng)景流和語義分割進(jìn)行動(dòng)態(tài)目標(biāo)識(shí)別，采用雙目立體匹配構(gòu)建靜態(tài)的稠密三維地圖，但是該算法沒有對(duì)定位精度進(jìn)行評(píng)價(jià)，并且系統(tǒng)運(yùn)行速度僅為2.5 Hz。

現(xiàn)有的SLAM系統(tǒng)在動(dòng)態(tài)環(huán)境下普遍存在實(shí)時(shí)性較差或定位誤差較大的問題，導(dǎo)致其無法應(yīng)用于現(xiàn)實(shí)場(chǎng)景中。針對(duì)以上問題，本文結(jié)合自適應(yīng)窗隔匹配模型及深度學(xué)習(xí)算法，將兩者應(yīng)用于SLAM系統(tǒng)中。在本文系統(tǒng)前端，采用自適應(yīng)隔窗匹配模型進(jìn)行匹配點(diǎn)篩選，得到靜態(tài)區(qū)域的特征匹配點(diǎn)對(duì)，實(shí)現(xiàn)V-SLAM系統(tǒng)的初始化。在位姿跟蹤部分，通過最小化重投影誤差進(jìn)行當(dāng)前幀位姿跟蹤，并且僅對(duì)關(guān)鍵幀實(shí)現(xiàn)特征匹配，同時(shí)向局部地圖中添加靜態(tài)三維點(diǎn)，更新稀疏點(diǎn)云地圖。在系統(tǒng)后端，將前端獲取的關(guān)鍵幀位姿信息以及RGB-D 相機(jī)獲得的圖像信息傳送至深度學(xué)習(xí)模塊進(jìn)行語義分割，以消除場(chǎng)景中的動(dòng)態(tài)物體，實(shí)現(xiàn)靜態(tài)三維稠密地圖構(gòu)建。

1 算法框架

動(dòng)態(tài)場(chǎng)景下的RGB-D SLAM 算法結(jié)構(gòu)框圖如圖1 所示，該算法主要包括初始化、位姿跟蹤、局部地圖構(gòu)建、閉環(huán)檢測(cè)、深度學(xué)習(xí)語義分割及三維地圖構(gòu)建等模塊。

圖1 RGB-D SLAM 算法結(jié)構(gòu)框圖Fig.1 Block diagram of RGB-D SLAM algorithm structure

在RGB-D SLAM 算法中，首先進(jìn)行圖像幀的構(gòu)建，圖像幀的構(gòu)建方式和ORB-SLAM2［6］一致，將每一幀的圖像信息和提取的ORB 特征點(diǎn)保存在系統(tǒng)中，用于初始化、位姿跟蹤和地圖優(yōu)化等。

若系統(tǒng)未初始化，則采用自適應(yīng)隔窗匹配的方式實(shí)現(xiàn)動(dòng)態(tài)場(chǎng)景中靜態(tài)特征點(diǎn)的匹配，然后根據(jù)BA（Bundle Adjustment）優(yōu)化得到最終的初始化靜態(tài)3D地圖點(diǎn)。在初始化成功之后，將當(dāng)前幀作為初始位位姿，根據(jù)構(gòu)建的靜態(tài)局部地圖實(shí)現(xiàn)連續(xù)圖像幀的位姿估計(jì)。

在跟蹤過程中同時(shí)進(jìn)行關(guān)鍵幀的選擇，以保存機(jī)器人定位過程中的關(guān)鍵信息。通過關(guān)鍵幀之間的特征匹配，向局部地圖中添加三維點(diǎn)，用于位姿跟蹤、閉環(huán)檢測(cè)及重定位等。同時(shí)，對(duì)關(guān)鍵幀中的圖像信息進(jìn)行語義分割，消除場(chǎng)景中動(dòng)態(tài)物體的影響，構(gòu)建靜態(tài)的三維稠密地圖，從而實(shí)現(xiàn)地圖的可重復(fù)使用。

2 系統(tǒng)前端設(shè)計(jì)

在本文算法的前端，首先對(duì)每幅圖像進(jìn)行均勻分布的ORB 特征提取，然后使用自適應(yīng)窗隔匹配模型消除場(chǎng)景中動(dòng)態(tài)物體的影響，得到靜態(tài)區(qū)域的特征匹配點(diǎn)對(duì)，最后進(jìn)行位姿跟蹤與關(guān)鍵幀篩選。

2.1 ORB 特征提取

首先將原始圖像按照尺度為1.2 的大小構(gòu)成層數(shù)為8 的圖像金字塔，將每層圖像分成30×30 的網(wǎng)格區(qū)域；然后設(shè)置FAST［10］角點(diǎn)提取的閾值為9，在每層圖像上提取FAST 角點(diǎn)，并且生成256 維的BRIEF［23］特征點(diǎn)描述子。如果當(dāng)前網(wǎng)格區(qū)域沒有提取到特征點(diǎn)，減少FAST 角點(diǎn)提取的閾值繼續(xù)提取特點(diǎn)，閾值最小值為5。將所有層提取到的特征點(diǎn)進(jìn)行保存得到當(dāng)前圖像幀的ORB 特征點(diǎn)。

2.2 自適應(yīng)窗隔匹配模型

普通的特征點(diǎn)法SLAM系統(tǒng)一般采用連續(xù)的兩幀圖像進(jìn)行特征匹配，以實(shí)現(xiàn)系統(tǒng)的初始化和位姿跟蹤。但是，當(dāng)場(chǎng)景中存在動(dòng)態(tài)物體時(shí)，由于相機(jī)幀率較高，對(duì)于連續(xù)的兩幀圖像，運(yùn)動(dòng)物體并沒有發(fā)生明顯的移動(dòng)，難以消除動(dòng)態(tài)物體的影響。另外，對(duì)于非連續(xù)的兩幀圖像，傳統(tǒng)的匹配方式會(huì)造成大量的誤匹配。連續(xù)圖像幀及非連續(xù)圖像幀的匹配區(qū)別如圖2 所示。

圖2 圖像幀之間的特征匹配Fig.2 Feature matching between image frames

在圖2 中，A 和B 為場(chǎng)景中的部分運(yùn)動(dòng)區(qū)域，C為場(chǎng)景中的靜態(tài)區(qū)域。圖2（a）描述了連續(xù)圖像幀之間的特征匹配，由于動(dòng)態(tài)區(qū)域移動(dòng)較小，因此A、B 和C 區(qū)域均能得到較好的特征匹配，但是會(huì)造成SLAM系統(tǒng)累計(jì)誤差持續(xù)增大的問題，影響定位精度。圖2（b）所示為非連續(xù)圖像幀之間的特征匹配，由于動(dòng)態(tài)區(qū)域移動(dòng)較大，使A 和B 區(qū)域的特征匹配發(fā)生錯(cuò)誤，同樣會(huì)影響系統(tǒng)的定位精度。

針對(duì)以上問題，為方便理論分析，本文定義如下的集合關(guān)系式，記圖像中全局特征點(diǎn)集合為U，靜態(tài)背景及物體上的特征點(diǎn)集合為S，動(dòng)態(tài)物體上的特征點(diǎn)集合為D，三者的關(guān)系為：

所有圖像幀集合表示為F={f1，f2，…，fN}，固定窗口圖像幀集合為W={f1，f2，…，f8}，其中，W?F。

為消除動(dòng)態(tài)物體的影響，引入自適應(yīng)窗隔匹配模型，該模型通過幀間篩選策略將窗口中首幀圖像與第x（x≤8）幀圖像進(jìn)行特征匹配，x-1 為窗隔大小，窗口大小設(shè)置為固定值8。自適應(yīng)窗隔匹配模型算法步驟如下：

1）選取窗口W中兩幀圖像f1、fx，提取均勻分布的ORB 特征點(diǎn)，并使用暴力匹配法獲得初步的匹配關(guān)系，然后進(jìn)行圖像區(qū)域劃分。圖像區(qū)域劃分主要流程如圖3 所示。

圖3 圖像區(qū)域劃分流程Fig.3 Procedure of image area division

將圖像幀劃分為16×16 大小的像素塊，以fx圖像左上角像素塊為起點(diǎn)，在f1圖像中執(zhí)行匹配搜索，搜索步長(zhǎng)設(shè)置為8，優(yōu)先選擇含匹配角點(diǎn)的像素塊。匹配正確之后以該像素塊為基準(zhǔn)，在橫向和縱向依次進(jìn)行匹配擴(kuò)展，逐漸擴(kuò)大區(qū)域范圍，若該區(qū)域塊能夠占據(jù)圖像區(qū)域的絕大部分，則視為靜態(tài)區(qū)域塊，記作T區(qū)域。匹配搜索過程中檢測(cè)到不連續(xù)區(qū)塊，則將其單獨(dú)標(biāo)記，并對(duì)該區(qū)域進(jìn)行局部擴(kuò)展，若該區(qū)域占據(jù)圖像范圍較小，視其為動(dòng)態(tài)區(qū)域塊，記為F區(qū)域。在實(shí)際應(yīng)用中，設(shè)定T區(qū)域必定出現(xiàn)且唯一，F(xiàn)區(qū)域可為0 個(gè)或者多個(gè)。

2）圖像中的場(chǎng)景隨著相機(jī)運(yùn)動(dòng)而不斷更新，兩幀圖像間的變換可視為仿射變換。記第一幀圖像中某區(qū)域第i個(gè)像素塊坐標(biāo)的向量形式為viI，第x幀（x≤8）圖像中與之對(duì)應(yīng)的像素塊坐標(biāo)向量為viP，則有如下關(guān)系：

其中，A為二階可逆矩陣，t為二維平移向量，m為對(duì)應(yīng)區(qū)域中像素塊的總數(shù)?？梢酝ㄟ^最小化均方根誤差來求解A、t，其公式為：

考慮到區(qū)域中所有的像素塊，將式（2）寫成矩陣形式可得：

記VI=(v1I，v2I，…，vmI)，VP=(v1P，v2P，…，vmP)，則式（4）可以記作VI=AVP+tLT，L為m維單位列向量。因此，對(duì)式（3）的求解可以等價(jià)為無約束極值問題，如式（5）所示：

其中，ψ(A，t)是關(guān)于A和t的矩陣函數(shù)，tr()運(yùn)算符表示求矩陣的跡。在函數(shù)ψ(A，t)取極小值時(shí)，對(duì)應(yīng)的參數(shù)A*、t*滿足如下關(guān)系式：

由式（8）、式（9）可以得到T區(qū)域及F區(qū)域?qū)?yīng)的仿射變換參數(shù)。由于相機(jī)運(yùn)動(dòng)是平滑的，為簡(jiǎn)化運(yùn)算，對(duì)窗口中的圖像只分析平移向量t的變化。

在窗口圖像幀集合W中，將圖像幀f1與圖像幀f2～f8逐一進(jìn)行匹配搜索來確定窗隔的大小。定義tT為T區(qū)域的平移向量，tF為F區(qū)域的平移向量。存在如下3 種情況：

（1）圖像幀中僅存在T區(qū)域，則將其視為靜態(tài)場(chǎng)景，若表示場(chǎng)景中在某段時(shí)間內(nèi)沒有出現(xiàn)動(dòng)態(tài)物體且相機(jī)已經(jīng)發(fā)生明顯移動(dòng)，則認(rèn)定兩幀圖像符合篩選條件。

（2）當(dāng)圖像幀中存在一個(gè)F區(qū)域，判斷tT與tF的大小，若表示圖像幀中動(dòng)態(tài)物體區(qū)域發(fā)生明顯變化，則認(rèn)為兩幀圖像符合篩選條件。

（3）當(dāng)圖像幀中出現(xiàn)多個(gè)F區(qū)域，僅對(duì)平移向量較小的T區(qū)域做出判斷，若其滿足則認(rèn)為兩幀圖像符合篩選條件。

窗隔圖像幀的更新策略為：若圖像幀f1在初始窗口圖像幀中沒有匹配到符合篩選條件的圖像幀，則保留f1，將f2～f8幀移出，再向窗口中移入7 幀圖像，重新進(jìn)行匹配搜索。若圖像幀f1與窗口中第x幀圖像符合篩選條件，則窗隔大小即為x-1，然后將窗口中前x-1幀圖像移出，并將第x幀圖像設(shè)置為f1幀，再向窗口中補(bǔ)充x-1 幀圖像，并開啟新一輪匹配搜索。

3）在篩選出符合條件的圖像幀之后，本文使用改進(jìn)的GMS［24］特征匹配算法來獲得高質(zhì)量的匹配點(diǎn)對(duì)，該算法是一種高效且實(shí)時(shí)的匹配點(diǎn)篩選算法，其基于運(yùn)動(dòng)網(wǎng)格統(tǒng)計(jì)模型對(duì)原始的特征匹配進(jìn)行篩選，得到靜態(tài)區(qū)域的特征匹配。

根據(jù)GMS 特征匹配算法的原理，得出如下關(guān)系：

其中，s表示兩幀圖像中對(duì)應(yīng)區(qū)域得分，|X|表示該區(qū)域中特征匹配點(diǎn)對(duì)的數(shù)量，如果一個(gè)特征匹配是正確的，那么它的周圍依然存在其他特征匹配點(diǎn)對(duì)，此時(shí)s的值較大。

對(duì)于連續(xù)的2 個(gè)圖像幀，如圖2（a）所示，區(qū)域A、B、C 中均可得到較多的匹配點(diǎn)對(duì)，從而使得動(dòng)態(tài)區(qū)域A、B 造成假得分的情況。假設(shè)A、B 和C 區(qū)域中的特征匹配點(diǎn)對(duì)得分為sA、sB和sC，那么有如下關(guān)系：

在這種情況下，無法通過設(shè)定合適的閾值將動(dòng)態(tài)區(qū)域（區(qū)域A 和B）和靜態(tài)區(qū)域（區(qū)域C）的特征匹配區(qū)分開。因此，GMS 算法不適用于場(chǎng)景中動(dòng)態(tài)物體移動(dòng)較小的情況。

本文系統(tǒng)采用自適應(yīng)窗隔匹配模型，得到場(chǎng)景中動(dòng)態(tài)物體發(fā)生明顯移動(dòng)的兩幀圖像。以圖2（b）為例，靜態(tài)區(qū)域C 有較高的匹配得分，但是，區(qū)域A 和B受到動(dòng)態(tài)物體的影響發(fā)生了明顯變化，使得這些區(qū)域的特征匹配出現(xiàn)較大的錯(cuò)誤，sA和sB的值很小，則有如下關(guān)系：

這樣就可以通過設(shè)置合適的閾值，消除動(dòng)態(tài)物體上特征匹配點(diǎn)的影響，解決假得分的問題，獲得靜態(tài)區(qū)域的特征匹配點(diǎn)集S。

為提高特征匹配點(diǎn)對(duì)的篩選速度，將圖像進(jìn)行網(wǎng)格劃分。由網(wǎng)格的可分性度量值可知，P正比于網(wǎng)格數(shù)量K及網(wǎng)格區(qū)域中特征點(diǎn)數(shù)量M，且K與M互補(bǔ)。因此，綜合考慮特征匹配點(diǎn)對(duì)的篩選速度和可分精度，采用增大網(wǎng)格數(shù)量K，即縮小網(wǎng)格大小的方式，將網(wǎng)格大小設(shè)置為10×10。然后，直接判斷每個(gè)網(wǎng)格中所有特征匹配點(diǎn)對(duì)的正確性，如圖4 所示。

圖4 圖像網(wǎng)格劃分Fig.4 Grid division of image

圖4 為經(jīng)過窗隔匹配篩選的兩幀圖像，圖4（a）為窗隔模型中的首幀圖像，圖4（b）為第x幀圖像，在以網(wǎng)格A 為中心的3×3 網(wǎng)格中，統(tǒng)計(jì)左右圖像對(duì)中區(qū)域A 的匹配得分sij，其定義如下：其中表示網(wǎng)格對(duì){ik，jk}中對(duì)應(yīng)的特征點(diǎn)匹配數(shù)量，根據(jù)運(yùn)動(dòng)的平滑性及特征點(diǎn)匹配的一致性，sij值越大，表示網(wǎng)格A 中特征匹配的正確性越高，反之，則正確性越低。此時(shí)，設(shè)置閾值t來對(duì)網(wǎng)格A 中的特征匹配是否正確進(jìn)行判斷：

其中，α為常量（一般設(shè)置為6），n表示以A 為中心的3×3 網(wǎng)格中特征匹配的數(shù)量。

對(duì)于靜態(tài)區(qū)域，如圖4 中白色方框所示，網(wǎng)格A可以獲得較高的匹配得分sij，而對(duì)圖4 中黑色方框所示的動(dòng)態(tài)區(qū)域，由于存在運(yùn)動(dòng)物體，使得左圖像提取的特征點(diǎn)無法在右圖像中找到對(duì)應(yīng)的匹配點(diǎn)，甚至?xí)谟覉D像中找到錯(cuò)誤的匹配點(diǎn)，因此該區(qū)域的匹配得分值sij較低。由式（14）能夠看出，可以根據(jù)區(qū)域內(nèi)匹配特征點(diǎn)的數(shù)量設(shè)置自適應(yīng)閾值。若sij≥t，則區(qū)域匹配正確，保留匹配點(diǎn)；若sij＜t，則區(qū)域匹配錯(cuò)誤，舍棄匹配點(diǎn)。通過自適應(yīng)閾值判斷匹配點(diǎn)對(duì)，可以剔除動(dòng)態(tài)區(qū)域的誤匹配，得到靜態(tài)區(qū)域的匹配點(diǎn)對(duì)，從而提高全局的匹配正確率。

2.3 位姿跟蹤

使用自適應(yīng)窗隔匹配模型進(jìn)行系統(tǒng)的初始化，初始化成功后得到當(dāng)前幀的位姿信息，將當(dāng)前幀設(shè)置為關(guān)鍵幀和參考幀。當(dāng)新的圖像幀創(chuàng)建后，得到n組匹配成功的靜態(tài)點(diǎn)對(duì)，其對(duì)應(yīng)于三維空間點(diǎn)Pi，pi1、pi2（i∈n）為該點(diǎn)在參考幀和當(dāng)前幀中的投影為在初始位姿下空間點(diǎn)Pi在當(dāng)前幀中的投影，ei為重投影誤差，如圖5 所示。然后，可通過BA 優(yōu)化方式，即最小化重投影誤差求解相機(jī)位姿。引入李代數(shù)中ξ表示相機(jī)位姿，有如下關(guān)系：

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

其中，“∧”運(yùn)算符號(hào)表示將李代數(shù)元素轉(zhuǎn)換為變換矩陣。該點(diǎn)重投影誤差可表示為：

將n組匹配點(diǎn)的誤差項(xiàng)進(jìn)行平方求和，構(gòu)建最小二乘問題，并使其最小化：

使用列文伯格-馬夸爾特優(yōu)化算法求解式（17），可求得最優(yōu)相機(jī)位姿ξ*。

在進(jìn)入跟蹤模塊后，使用參考幀模型或恒速模型對(duì)當(dāng)前幀位姿進(jìn)行估計(jì)。對(duì)于參考幀模型，設(shè)置最新的關(guān)鍵幀作為參考幀，對(duì)參考幀和當(dāng)前幀進(jìn)行窗隔匹配，如果參考幀匹配的特征點(diǎn)在局部地圖中存在對(duì)應(yīng)的三維點(diǎn)，就將這些三維點(diǎn)投影到當(dāng)前幀。將上一幀的位姿信息作為當(dāng)前幀位姿的初始值，根據(jù)BA 優(yōu)化得到當(dāng)前幀的位姿信息與觀測(cè)到的三維地圖點(diǎn)。與參考幀模型不同，恒速模型假設(shè)兩幀之間的運(yùn)動(dòng)與之前兩幀相同，以此來設(shè)定BA 優(yōu)化的位姿初始值。

關(guān)鍵幀的篩選策略不同于ORB-SLAM2，本文首先采用自適應(yīng)窗隔匹配法得到兩圖像幀，再對(duì)靜態(tài)區(qū)域的匹配點(diǎn)數(shù)進(jìn)行判斷，若匹配點(diǎn)數(shù)大于50 且距離上一次所加入的關(guān)鍵幀超過8 幀，則加入新的關(guān)鍵幀。該方法既可確保圖像中的動(dòng)態(tài)物體出現(xiàn)顯著運(yùn)動(dòng)，又可避免引入過多的冗余信息，而且在為局部地圖添加新的地圖點(diǎn)時(shí)，能夠消除場(chǎng)景中動(dòng)態(tài)物體的影響。

3 系統(tǒng)后端設(shè)計(jì)

為消除場(chǎng)景中的動(dòng)態(tài)物體，構(gòu)建可供實(shí)際應(yīng)用的靜態(tài)稠密地圖，本文系統(tǒng)采用Mask R-CNN 進(jìn)行圖像的語義分割，然后將分割得到的動(dòng)態(tài)物體對(duì)應(yīng)的深度信息從該幀的深度圖中移除，從而達(dá)到僅對(duì)靜態(tài)場(chǎng)景進(jìn)行稠密地圖構(gòu)建的目的。

3.1 圖像語義分割

本文采用Mask R-CNN 對(duì)圖像幀進(jìn)行語義分割，使用在COCO［25］數(shù)據(jù)集上訓(xùn)練的深度模型，該模型可以分割常見的運(yùn)動(dòng)物體（如人、自行車、汽車、火車、卡車、鳥、馬、羊等）。Mask R-CNN 算法的輸入是原始的RGB 圖像，輸出圖像中包含矩形選擇框、類別標(biāo)簽以及實(shí)例的掩膜區(qū)域。然而，在本文系統(tǒng)的應(yīng)用中，僅需用到實(shí)例的掩膜信息，因此，在算法中移除了關(guān)于類別預(yù)測(cè)的模塊，分割結(jié)果如圖6 所示，該圖來自COCO 數(shù)據(jù)集。

圖6 Mask R-CNN 分割示意圖Fig.6 The segmentation diagram of Mask R-CNN

由于Mask R-CNN 是在Faster R-CNN［26］的框架下進(jìn)行設(shè)計(jì)的，因此相對(duì)于其他語義分割算法具有較快的預(yù)測(cè)速度，在GPU 設(shè)備上預(yù)測(cè)COCO 數(shù)據(jù)集上一張圖片的語義信息僅需200 ms。

V-SLAM系統(tǒng)均有很高的實(shí)時(shí)性要求，為了提高系統(tǒng)運(yùn)行速度，將語義分割部分作為一個(gè)單獨(dú)的線程運(yùn)行。假設(shè)輸入圖像大小為m×n×3，則輸出矩陣的大小為m×n×l，其中，l表示圖像中目標(biāo)的數(shù)目。對(duì)圖像的每一個(gè)通道進(jìn)行語義分割，最終得到單幀圖像中所有的動(dòng)態(tài)目標(biāo)。

3.2 靜態(tài)三維地圖構(gòu)建

利用深度學(xué)習(xí)算法得到動(dòng)態(tài)物體的掩膜信息，根據(jù)對(duì)應(yīng)關(guān)系可移除動(dòng)態(tài)物體在深度圖中的深度數(shù)據(jù)，然后結(jié)合SLAM系統(tǒng)得到的關(guān)鍵幀位姿信息進(jìn)行靜態(tài)三維稠密地圖構(gòu)建。

為了保證系統(tǒng)的運(yùn)行速度，將可視化點(diǎn)云信息界面作為一個(gè)單獨(dú)的線程運(yùn)行，當(dāng)圖像幀為關(guān)鍵幀時(shí)，將此時(shí)的深度圖和RGB 圖像結(jié)合相機(jī)內(nèi)部參數(shù)恢復(fù)得到相機(jī)坐標(biāo)系下的三維點(diǎn)云坐標(biāo)(x，y，z)，如式（18）所示：

其中，d與深度圖在像素坐標(biāo)(u，ν)的值成正相關(guān)，其余參數(shù)均為相機(jī)內(nèi)參，由相機(jī)自身決定，可以通過相機(jī)標(biāo)定的方式得到。

4 實(shí)驗(yàn)結(jié)果與分析

本文使用TUM［27］RGB-D 動(dòng)態(tài)序列數(shù)據(jù)集進(jìn)行特征匹配、定位精度及跟蹤時(shí)間的對(duì)比分析，并在動(dòng)態(tài)數(shù)據(jù)集及實(shí)際環(huán)境中進(jìn)行場(chǎng)景稠密重建測(cè)試。實(shí)驗(yàn)環(huán)境配置為4 核i7 處理器，8 GB 內(nèi)存，GTX 1060，Ubuntu16.04 操作系統(tǒng)。

4.1 特征匹配效果分析

基于TUM 的fr3_w_xyz 動(dòng)態(tài)數(shù)據(jù)集，對(duì)本文系統(tǒng)的自適應(yīng)窗隔匹配策略與BF、FLANN 匹配方式作對(duì)比分析，在前后兩幀中均提取1 000 個(gè)特征點(diǎn)進(jìn)行匹配，匹配結(jié)果如圖7 所示。

圖7 匹配效果對(duì)比Fig.7 Comparison of matching effect

從圖7（a）、圖7（b）可以看出，BF 匹配和FLANN匹配不能有效移除動(dòng)態(tài)物體上的匹配點(diǎn)對(duì)，同時(shí)導(dǎo)致靜態(tài)區(qū)域部分出現(xiàn)較多的誤匹配。圖7（c）為自適應(yīng)窗隔匹配算法的匹配效果，該算法不僅移除了動(dòng)態(tài)物體上的匹配點(diǎn)對(duì)，也使靜態(tài)區(qū)域特征匹配的正確率得到提升。

對(duì)窗隔匹配算法、BF 算法及FLANN 算法的匹配耗時(shí)及匹配點(diǎn)對(duì)個(gè)數(shù)進(jìn)行對(duì)比分析，結(jié)果如表1所示。

表1 匹配耗時(shí)及匹配數(shù)量對(duì)比Table 1 Comparison of matching time and matching quantity

由表1 可知，F(xiàn)LANN 算法的匹配耗時(shí)最長(zhǎng)，且匹配點(diǎn)對(duì)數(shù)量最少，BF 算法匹配耗時(shí)最短，且匹配點(diǎn)對(duì)個(gè)數(shù)最多，但是該算法會(huì)帶來大量的誤匹配。因此，綜合考慮匹配速度及匹配正確性，自適應(yīng)窗隔匹配算法達(dá)到最優(yōu)。

4.2 定位精度及實(shí)時(shí)性分析

使用TUM RGB-D 數(shù)據(jù)集評(píng)估本文算法的定位精度和實(shí)時(shí)性，分析包含多動(dòng)態(tài)對(duì)象的低動(dòng)態(tài)（fr3_s_*）和高動(dòng)態(tài)（fr3_w_*）場(chǎng)景序列的測(cè)試結(jié)果，并與ORBSLAM2 及DynaSLAM系統(tǒng)進(jìn)行對(duì)比。以下均以O(shè)UR_SYSTEM來指代本文系統(tǒng)，實(shí)驗(yàn)結(jié)果如表2和表3所示。在表2 中使用絕對(duì)軌跡誤差（ATE）中的平方根誤差（RMSE）對(duì)定位精度進(jìn)行評(píng)估，表3 中使用圖像幀平均跟蹤時(shí)間進(jìn)行實(shí)時(shí)性能評(píng)估，圖像幀跟蹤時(shí)間為從構(gòu)建圖像幀到獲取其位姿的耗時(shí)，圖像幀平均跟蹤時(shí)間為所有圖像幀跟蹤時(shí)間總和取平均值。其中，“*”表示跟蹤丟失且重定位失敗。

表2 系統(tǒng)定位精度對(duì)比Table 2 Comparison of system positioning accuracy cm

表3 系統(tǒng)跟蹤時(shí)間對(duì)比Table 3 Comparison of system tracking time s

由表2 可得，在靜態(tài)場(chǎng)景（fr1_*）及單一動(dòng)態(tài)對(duì)象序列（fr2_*）中，本文系統(tǒng)（OUR_SYSTEM）定位精度最高。在低動(dòng)態(tài)的場(chǎng)景序列（fr3_s_*）中，OUR_SYSTEM 以及其他SLAM系統(tǒng)定位誤差均較小。但是，在多數(shù)數(shù)據(jù)集上，OUR_SYSTEM 達(dá)到了最高的定位精度。在高動(dòng)態(tài)場(chǎng)景序列（fr3_w_*）中，OUR_SYSTEM 的定位精度不僅遠(yuǎn)高于ORBSLAM2系統(tǒng)，而且也優(yōu)于DynaSLAM系統(tǒng)。

由表3 可得，在靜態(tài)場(chǎng)景序列及全部動(dòng)態(tài)場(chǎng)景序列中，OUR_SYSTEM 的跟蹤速度快于ORBSLAM2 及DynaSLAM，并且跟蹤速度比DynaSLAM系統(tǒng)快10 倍以上。計(jì)算表中OUR_SYSTEM 跟蹤時(shí)間的均值，可得平均時(shí)間為0.025 s，即跟蹤速率為40 Hz，滿足視覺SLAM 的實(shí)時(shí)性要求。

綜合以上關(guān)于定位精度和跟蹤速度的分析，OUR_SYSTEM 在靜態(tài)及動(dòng)態(tài)場(chǎng)景中均可實(shí)現(xiàn)較高的定位精度，而且具有較好的實(shí)時(shí)性，總體效果優(yōu)于ORB-SLAM2 和DynaSLAM系統(tǒng)。

為進(jìn)一步驗(yàn)證OUR_SYSTEM 的定位精度，繪制絕對(duì)位姿誤差A(yù)PE（Absolute Pose Error）的分布示意圖，如圖8 所示。

圖8 絕對(duì)位姿誤差分布Fig.8 APE distribution

圖8 所示為位姿跟蹤過程中OUR_SYSTEM、ORB-SLAM2 及DynaSLAM 在動(dòng)態(tài)數(shù)據(jù)集fr3_s_hs、fr3_s_xyz、fr3_w_hs 及fr3_w_xyz 上的APE 分布，其中，矩形區(qū)域表示3/4 的APE 數(shù)據(jù)分布，其余部分表示剩余的APE 數(shù)據(jù)分布。從圖8 可以看出，在低動(dòng)態(tài)數(shù)據(jù)集（fr3_s_*）的定位過程中，這3 種SLAM 算法均可達(dá)到較小的絕對(duì)位姿誤差，并且OUR_SYSTEM 絕對(duì)位姿誤差最小。在高動(dòng)態(tài)場(chǎng)景數(shù)據(jù)集（fr3_w_*）中，原始ORB-SLAM2系統(tǒng)的絕對(duì)位姿誤差最大，而OUR_SYSTEM 在最大限度上消除了運(yùn)動(dòng)物體的影響，達(dá)到了最高的定位精度。

OUR_SYSTEM 在數(shù)據(jù)集fr3_s_hs、fr3_s_xyz、fr3_w_hs 及fr3_w_xyz 中實(shí)際運(yùn)動(dòng)軌跡與真實(shí)軌跡的熱力對(duì)比如圖9 所示。

圖9 3D 軌跡熱力圖Fig.9 3D trajectory thermal map

從圖9 可以看出，OUR_SYSTEM 在動(dòng)態(tài)環(huán)境下能夠?qū)崿F(xiàn)對(duì)相機(jī)位姿的精確估計(jì)。

4.3 靜態(tài)地圖構(gòu)建及算法有效性分析

在TUM 動(dòng)態(tài)數(shù)據(jù)集和實(shí)際場(chǎng)景中分別進(jìn)行三維場(chǎng)景重建效果測(cè)試，并在實(shí)際環(huán)境下測(cè)試OUR_SYSTEM特征提取及定位的效果。TUM 動(dòng)態(tài)數(shù)據(jù)集上的實(shí)驗(yàn)結(jié)果如圖10 所示。圖10（a）為原始RGB 圖像，對(duì)原始圖像通過深度學(xué)習(xí)算法進(jìn)行動(dòng)態(tài)目標(biāo)剔除，得到圖10（b），同時(shí)，將對(duì)應(yīng)的深度圖中動(dòng)態(tài)目標(biāo)的深度信息也剔除，得到圖10（c）。使用不含動(dòng)態(tài)目標(biāo)的RGB 圖像及深度圖像，結(jié)合圖像幀的位姿信息進(jìn)行靜態(tài)三維稠密地圖構(gòu)建，結(jié)果如圖10（d）所示，該地圖不含動(dòng)態(tài)物體，可以重復(fù)使用，有較高的實(shí)用性。

圖10 TUM 數(shù)據(jù)集的三維地圖構(gòu)建效果Fig.10 3D map construction effect of TUM dataset

為進(jìn)一步驗(yàn)證算法的有效性，使用RGB-D相機(jī)（樂視Letv）在動(dòng)態(tài)的實(shí)驗(yàn)室環(huán)境中進(jìn)行場(chǎng)景三維重建。在場(chǎng)景中存在移動(dòng)人員的情況下，通過RGB-D 相機(jī)連續(xù)采集圖像進(jìn)行實(shí)驗(yàn)，使相機(jī)足夠平穩(wěn)且出現(xiàn)閉環(huán)。圖11 所示為ORB-SLAM2 與OUR_SYSTEM 提取ORB特征的對(duì)比效果，圖11（a）為ORB_SLAM2 算法效果，其中有大量的特征點(diǎn)分布在行走的人員身上，圖11（b）為OUR_SYSTEM 效果，其中絕大多數(shù)的特征點(diǎn)分布在背景中。

圖11 特征點(diǎn)提取效果對(duì)比Fig.11 Effect comparison of feature points extraction

圖12 所示為ORB_SLAM2 與OUR_SYSTEM 相機(jī)預(yù)測(cè)軌跡對(duì)比結(jié)果，圖中實(shí)線軌跡比較平滑，且有閉環(huán)出現(xiàn)，這與相機(jī)的真實(shí)運(yùn)動(dòng)軌跡相符，表明OUR_SYSTEM 在實(shí)際動(dòng)態(tài)場(chǎng)景中精度較高。而黑色虛線軌跡由于受到動(dòng)態(tài)物體的影響，出現(xiàn)了大幅度的偏差，定位誤差較大，雖然有閉環(huán)出現(xiàn)，但未能實(shí)現(xiàn)全局軌跡矯正。

圖12 相機(jī)預(yù)測(cè)軌跡對(duì)比Fig.12 Comparison of camera prediction trajectory

與圖10 類似，圖13（a）、圖13（b）、圖13（c）為視頻序列中某一幀圖像的處理效果，圖13（d）為最終的重建效果。可以看出，最終效果消除了實(shí)際場(chǎng)景中的動(dòng)態(tài)物體，構(gòu)建動(dòng)態(tài)場(chǎng)景的靜態(tài)三維稠密地圖能夠?qū)崿F(xiàn)地圖的可重復(fù)使用。

圖13 實(shí)際場(chǎng)景三維地圖構(gòu)建效果Fig.13 Construction effect of 3D map of actual scene

5 結(jié)束語

本文針對(duì)現(xiàn)有RGB-D 視覺SLAM系統(tǒng)在動(dòng)態(tài)環(huán)境下存在的精度不足、實(shí)時(shí)性較差等問題，提出一種動(dòng)態(tài)場(chǎng)景下的RGB-D SLAM 算法，該算法采用自適應(yīng)窗隔匹配模型來實(shí)現(xiàn)動(dòng)態(tài)環(huán)境下的運(yùn)動(dòng)估計(jì)。在定位過程中，相比于ORB-SLAM2 及DynaSLAM方法，本文算法在實(shí)時(shí)性和定位精度上均有所提升。在場(chǎng)景重建過程中，本文算法使用Mask R-CNN 檢測(cè)運(yùn)動(dòng)物體，根據(jù)所提供的掩膜信息消除動(dòng)態(tài)目標(biāo)。實(shí)驗(yàn)結(jié)果表明，該算法可以對(duì)場(chǎng)景進(jìn)行完整的三維重建。但是，當(dāng)相機(jī)移動(dòng)過快或者處于低紋理場(chǎng)景中時(shí)，本文算法容易出現(xiàn)跟蹤丟失的情況。因此，下一步將考慮添加IMU 傳感器，使用多傳感器融合的方法來實(shí)現(xiàn)更加魯棒的位姿跟蹤。