自適應(yīng)窗隔匹配與深度學(xué)習(xí)相結(jié)合的RGB-D SLAM 算法

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

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

0 概述

SLAM（Simultaneous Localization and Mapping）在機器人位姿估計及環(huán)境感知過程中起著至關(guān)重要的作用，主要應(yīng)用于機器人的導(dǎo)航和避障任務(wù)以及自動駕駛［1-3］和增強現(xiàn)實［4-5］等領(lǐng)域。視覺傳感器體積小、使用方便，并且圖像存儲了豐富的環(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)的場景或數(shù)據(jù)集上進(jìn)行實驗測試，可以達(dá)到很好的定位精度與魯棒性，但并未解決場景中存在動態(tài)物體的問題。當(dāng)場景中存在動態(tài)物體，如人、車等，會導(dǎo)致V-SLAM 出現(xiàn)定位誤差較大或跟蹤失敗的問題，因此，在動態(tài)環(huán)境中進(jìn)行位姿估計與地圖構(gòu)建依然是一個挑戰(zhàn)。目前用于消除動態(tài)物體影響的方法主要有直接篩選剔除法和語義分割法2 種。

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

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

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

1 算法框架

動態(tài)場景下的RGB-D SLAM 算法結(jié)構(gòu)框圖如圖1 所示，該算法主要包括初始化、位姿跟蹤、局部地圖構(gòu)建、閉環(huán)檢測、深度學(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 特征點保存在系統(tǒng)中，用于初始化、位姿跟蹤和地圖優(yōu)化等。

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

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

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

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

2.1 ORB 特征提取

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

為提高特征匹配點對的篩選速度，將圖像進(jìn)行網(wǎng)格劃分。由網(wǎng)格的可分性度量值可知，P正比于網(wǎng)格數(shù)量K及網(wǎng)格區(qū)域中特征點數(shù)量M，且K與M互補。因此，綜合考慮特征匹配點對的篩選速度和可分精度，采用增大網(wǎng)格數(shù)量K，即縮小網(wǎng)格大小的方式，將網(wǎng)格大小設(shè)置為10×10。然后，直接判斷每個網(wǎng)格中所有特征匹配點對的正確性，如圖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)計左右圖像對中區(qū)域A 的匹配得分sij，其定義如下：其中表示網(wǎng)格對{ik，jk}中對應(yīng)的特征點匹配數(shù)量，根據(jù)運動的平滑性及特征點匹配的一致性，sij值越大，表示網(wǎng)格A 中特征匹配的正確性越高，反之，則正確性越低。此時，設(shè)置閾值t來對網(wǎng)格A 中的特征匹配是否正確進(jìn)行判斷：

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

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

2.3 位姿跟蹤

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

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

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

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

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

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

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

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

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

3.1 圖像語義分割

本文采用Mask R-CNN 對圖像幀進(jìn)行語義分割，使用在COCO［25］數(shù)據(jù)集上訓(xùn)練的深度模型，該模型可以分割常見的運動物體（如人、自行車、汽車、火車、卡車、鳥、馬、羊等）。Mask R-CNN 算法的輸入是原始的RGB 圖像，輸出圖像中包含矩形選擇框、類別標(biāo)簽以及實例的掩膜區(qū)域。然而，在本文系統(tǒng)的應(yīng)用中，僅需用到實例的掩膜信息，因此，在算法中移除了關(guān)于類別預(yù)測的模塊，分割結(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è)計的，因此相對于其他語義分割算法具有較快的預(yù)測速度，在GPU 設(shè)備上預(yù)測COCO 數(shù)據(jù)集上一張圖片的語義信息僅需200 ms。

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

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

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

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

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

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

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

4.1 特征匹配效果分析

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

圖7 匹配效果對比Fig.7 Comparison of matching effect

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

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

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

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

4.2 定位精度及實時性分析

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

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

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

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

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

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

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

圖8 絕對位姿誤差分布Fig.8 APE distribution

圖8 所示為位姿跟蹤過程中OUR_SYSTEM、ORB-SLAM2 及DynaSLAM 在動態(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 可以看出，在低動態(tài)數(shù)據(jù)集（fr3_s_*）的定位過程中，這3 種SLAM 算法均可達(dá)到較小的絕對位姿誤差，并且OUR_SYSTEM 絕對位姿誤差最小。在高動態(tài)場景數(shù)據(jù)集（fr3_w_*）中，原始ORB-SLAM2系統(tǒng)的絕對位姿誤差最大，而OUR_SYSTEM 在最大限度上消除了運動物體的影響，達(dá)到了最高的定位精度。

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

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

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

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

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

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

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

圖11 特征點提取效果對比Fig.11 Effect comparison of feature points extraction

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

圖12 相機預(yù)測軌跡對比Fig.12 Comparison of camera prediction trajectory

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

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

5 結(jié)束語

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