基于幾何約束的室內(nèi)動態(tài)環(huán)境視覺SLAM

楊世強，范國豪，白樂樂，趙 成，李德信

西安理工大學 機械與精密儀器工程學院，西安710048

同時定位與地圖創(chuàng)建（Simultaneous Localization and Mapping，SLAM）是指機器人在沒有任何先驗環(huán)境信息的情況下，同時完成自身定位以及周圍環(huán)境地圖構(gòu)建[1-2]。現(xiàn)有一些先進的SLAM算法已經(jīng)取得令人滿意的成績，如ORB-SLAM2、VINS-Mono和LSD-SLAM[3-6]等。其中ORB-SLAM2[3-4]因其較好的系統(tǒng)穩(wěn)定性和實時性，得到了研究者們的廣泛關(guān)注。該算法使用ORB特征進行跟蹤匹配，通過基于詞袋方法的閉環(huán)檢測和重定位技術(shù)以及后端優(yōu)化方法使系統(tǒng)具備一致性較高的定位與地圖構(gòu)建精度。然而，ORB-SLAM2和現(xiàn)有大多優(yōu)秀算法一樣，在現(xiàn)實場景的應(yīng)用上依然存在一些問題[7]。現(xiàn)有的算法大多為了計算方便，常將外部環(huán)境作為靜態(tài)假設(shè)，忽略了真實環(huán)境中動態(tài)物體對SLAM算法精度的影響，所使用的靜態(tài)模型無法適應(yīng)復(fù)雜多變的環(huán)境[8]。因此，研究動態(tài)環(huán)境下的SLAM算法顯得尤為重要。

Bescos等[9]基于ORB-SLAM2框架提出了一種DynaSLAM動態(tài)魯棒SLAM算法，融合了深度學習方法和多視圖幾何方法來檢測場景中的動態(tài)特征點，在TUM數(shù)據(jù)集上具有較好的測試精度。同樣地，Yu等[10]基于ORB-SLAM2框架提出DS-SLAM，通過幾何約束方法區(qū)分靜態(tài)和動態(tài)特征，因其在對極約束中所計算的基本矩陣易受外點影響而出現(xiàn)偏差，進而影響了系統(tǒng)精度。張金鳳等[11]結(jié)合深度學習目標檢測方法，將特征點分為動態(tài)特征和靜態(tài)特征，僅基于靜態(tài)特征估計相機位姿，在某些極端環(huán)境中，如當場景中動態(tài)目標占比較大時，由于靜態(tài)特征數(shù)量大量減少，導(dǎo)致系統(tǒng)穩(wěn)定性降低。姚二亮等[12]通過結(jié)合語義和圖像邊緣的距離誤差和光度一致性來劃分動靜區(qū)域，只使用靜態(tài)區(qū)域提取的特征點來估計相機位姿，在高動態(tài)TUM序列有較高的穩(wěn)定性，但對低動態(tài)移動目標不敏感。高成強等[13]使用一種基于單高斯模型（Single Gaussian Model，SGM）來構(gòu)建場景中圖像塊的灰度值變化模型，只提取背景區(qū)域上的特征點來估計相機位姿，在測試序列中表現(xiàn)優(yōu)秀，但其所使用的單高斯模型易受噪聲影響。

為提高ORB-SLAM2在室內(nèi)動態(tài)場景下的穩(wěn)定性和魯棒性，本文提出一種基于幾何約束的視覺SLAM算法，使用一種幾何約束方法來濾除場景中的動態(tài)特征點，只使用場景中的靜態(tài)信息來估計相機位姿。本文首先給出了改進系統(tǒng)框架；然后闡述了基于幾何約束的動態(tài)特征點濾除算法；最后對所提算法進行了實驗測試。

1 改進系統(tǒng)框架

改進系統(tǒng)基于ORB-SLAM2框架搭建，可分為跟蹤、局部地圖構(gòu)建和回環(huán)檢測線程三個主線程，如圖1所示。本文主要改進了ORB-SLAM2的跟蹤線程。在跟蹤線程中，改進系統(tǒng)添加了動態(tài)特征點濾除模塊對特征點進行濾除，只將濾除后的靜態(tài)特征點用于位姿估計。其余兩個線程與ORB-SLAM2基本保持相同，不再贅述。

圖1 改進SLAM系統(tǒng)框架圖Fig.1 Overall-architecture of improved SLAM system

2 基于幾何約束的動態(tài)特征點濾除算法

圖2 是本文動態(tài)特征點濾除算法流程圖。首先，對當前幀圖像提取ORB特征點[14]，依據(jù)匹配點對之間的BRIEF描述符，通過詞袋模型[15]加速特征點對匹配，基本流程與ORB-SLAM2[9]系統(tǒng)中處理方法類似；然后，對各匹配點對進行動態(tài)特征點粗約束，剩余的匹配點對作為改進隨機抽樣一致算法（Random Sample Consensus，RANSAC）的樣本點來估算幀間基本矩陣；最后，使用所估計的穩(wěn)定幀間基本矩陣對幀間圖像進行極線約束，濾除真正的動態(tài)特征點。

圖2 動態(tài)特征點濾除流程圖Fig.2 Flowchart of dynamic feature point filtering

2.1 動態(tài)特征點粗濾除方法

動態(tài)特征點粗約束旨在利用幀間圖像的幾何約束關(guān)系濾除場景中的高動態(tài)和誤匹配特征點，為進一步對動態(tài)特征點精約束提供樣本點。該方法首先采用特征點匹配精度約束來濾除匹配點對之間的誤匹配；然后利用一種特征點幾何約束方法來粗濾除場景中的動態(tài)特征。

2.1.1圖像特征點匹配精度約束方法

由于圖像噪聲以及運動模糊，通過特征描述符匹配的特征點對之間往往存在大量誤匹配，誤匹配信息導(dǎo)致隨后動態(tài)特征點判斷方法的紊亂。鑒于此，在圖像特征點匹配階段，可認為最大漢明距離遠遠大于次近漢明距離的匹配點對為可靠匹配。計算每對特征點與其他特征點之間的最近漢明距離d1和次近漢明距離d2，記它們之間的比值為λ，即λ=d1/d2，λ越小代表這對匹配點對的最近漢明距離遠遠小于次近漢明距離，匹配越可靠。若λ<α，則認為是可靠的匹配，α為精度約束因子。引入匹配質(zhì)量的概念，若一對特征點匹配質(zhì)量越高，表示其正確匹配的概率越大，故可定義一個匹配質(zhì)量得分來量化匹配質(zhì)量。精度約束得分函數(shù)qa定義為：

其中，qa表示精度約束匹配質(zhì)量得分，精度約束分值越小，代表匹配質(zhì)量越高。相對于匹配質(zhì)量低的點對來說，高質(zhì)量匹配點對能為SLAM系統(tǒng)提供更穩(wěn)健的幀間關(guān)系，更有利于提高SLAM系統(tǒng)的追蹤穩(wěn)定性和定位精度。

2.1.2圖像特征點幾何約束方法

通過圖像特征點精度約束后，特征點對得到了相對準確的匹配關(guān)系，但動態(tài)特征點也被保留。為進一步濾除場景中的動態(tài)特征點，引入一種圖像特征點幾何約束模型，如圖3所示。其中，查詢圖像I1和目標圖像I2是兩相鄰幀，它們之間的采樣時間間隔很短，因此，由相機位姿變換引起的相機投影畸變非常小。圖中三角形頂點代表所提取的ORB特征點，三對特征點在I1和I2上構(gòu)成兩個三角形△p1p2p3和△q1q2q3，頂點p1、p2、p3為I1中的三個特征點，其分別與I2上的三個特征點q1、q2、q3相匹配。三角形各邊d為特征點之間的歐式距離。

圖3 幾何約束模型雙向投票示意圖Fig.3 Schematic diagram of two-way voting for geometric constraint model

當場景中沒有動態(tài)目標時，三角形對應(yīng)邊的差應(yīng)該在一個小區(qū)間，可以表示為：

為了更好地描述這一關(guān)系，定義幾何約束得分函數(shù)：

其中，d(a,b)表示特征點a、b之間的歐式距離；A(i,j)代表兩對應(yīng)邊的平均距離，可以表示為：

若場景中出現(xiàn)動態(tài)目標，假設(shè)q1在動態(tài)目標上，在目標圖像上q1移動到了構(gòu)成了新三角形三角形兩兩頂點之間的歐式距離為。由于q1點的運動，由式（3）計算出的幾何約束得分函數(shù)值將會異常大。然而，幾何約束得分涉及到兩對特征點，真正的動態(tài)特征點難以確定，鑒于此，提出一種特征點雙向打分方法來識別場景中真正的特征點。其主要思想為：定義一個特征點異常得分值，當一條邊出現(xiàn)異常，將這條邊上兩個特征點的異常得分值都各加1分。這樣，真正的動態(tài)特征點和靜態(tài)特征點的異常得分值將會出現(xiàn)明顯差距。因此易知，一個特征點的異常分值代表有多少個特征點判斷該點為異常的動態(tài)點。特征點的異常分值的幾何表達為：

其中，qab(i)為第i個特征點的異常分值；s(i,j)表示異常分值增量：

其中，β為幾何約束平均得分比例因子，其控制幾何約束嚴格程度；AS表示圖像上各點對之間幾何得分平均分值：

其中，n表示圖像特征點匹配點對個數(shù)；qg(i,j)表示i、j兩匹配特征點的幾何得分值表示幾何誤差權(quán)重因子，用以降低較大幾何約束得分值對得分均值計算的影響：

其中，θth為設(shè)定的幾何得分閾值，其代表當一個特征點的精度和幾何約束得分的總值大于該閾值時，將不會參與AS的計算。

圖4 為一場景動態(tài)特征點檢測示例，圖4（a）為TUM數(shù)據(jù)集上的一幀圖像，在該圖像上提取了450個ORB特征點。它們的異常分值分布如圖4（b）所示，其中紅色線表示分割閾值，代表當某個特征點的異常分值大于該閾值時，將被判斷為動態(tài)特征點。設(shè)置自適應(yīng)動態(tài)分割閾值為γM，其中M為提取的特征點總數(shù)，γ在圖4中設(shè)置為60%，即當60%的特征點認為一個特征點為異常時，則判定該點為動態(tài)特征點。圖4（a）中紅色點代表識別的動態(tài)特征點，綠色點為檢測場景中的靜態(tài)特征點。

圖4 動態(tài)特征濾除示意圖

整個幾何約束方法對動態(tài)特征點的濾除步驟可歸結(jié)如下：

步驟1對查詢圖像和目標圖像中的匹配點對計算其幾何約束得分以及平均幾何約束得分AS。

步驟2對所計算的各匹配特征點進行雙向打分，依次計算所有特征點的異常分值qab。

步驟3采用自適應(yīng)閾值方法對每個特征點的異常分值進行判斷，若qab(i)>γM，判定第i個圖像特征點為運動的異常點。

2.2 改進RANSAC算法

幀間圖像的位姿變換關(guān)系可以由基本矩陣表示，其通常使用RANSAC算法來估算，而標準RANSAC算法在高動態(tài)場景中估算出的基本矩陣會出現(xiàn)較大誤差，采用一種改進RANSAC算法來改善。

2.2.1標準RANSAC算法理論分析

標準RANSAC算法的基本思想如下[16]：

（1）隨機從有N個數(shù)據(jù)點，內(nèi)點比例為w的樣本集S中抽取4個樣本數(shù)據(jù)點，根據(jù)該樣本數(shù)據(jù)點計算單應(yīng)性矩陣模型H；

（2）計算數(shù)據(jù)集中所有的數(shù)據(jù)與單應(yīng)性矩陣H的投影誤差，將誤差小于閾值Td的數(shù)據(jù)點加入到內(nèi)點集Si中；

（3）如果當前內(nèi)點集Si的大小超過了某個閾值Ti，用Si重新估計模型并結(jié)束，否則，重新選取新的樣本，重復(fù)步驟（2）；

（4）如果迭代次數(shù)大于T，則退出，同時最大的內(nèi)點集Si被選中，用它來重新估計模型，得到最后的估計模型。

從標準RANSAC算法的計算流程可以看出，算法效率取決于選取樣本數(shù)據(jù)點迭代次數(shù)的大小，如采取更優(yōu)的樣本數(shù)據(jù)選點策略，減小樣本點的迭代次數(shù)，就可以提高RANSAC算法的效率。

設(shè)在標準RANSAC算法中，數(shù)據(jù)集中數(shù)據(jù)點的總數(shù)為M，內(nèi)點比例為w，則內(nèi)點總數(shù)為M×w。在一次隨機采樣中，記p0表示所采樣的m個點都為內(nèi)點的概率（一個點可重復(fù)采樣），則p0=wm，則至少有一個點為外點的概率p1=1-p0。重復(fù)進行K次采樣實驗，則每次至少有一個外點的概率p2=pK1,K次采樣至少一次采樣是有效采樣的概率為p=1-p2，一般也稱p為置信度，通常取p為0.95~0.99，則K可以表示為：

上式表述的是內(nèi)點比例為w的樣本集，每次隨機抽取n個點，在K次采樣實驗中至少有一次有效采樣（n個點全為內(nèi)點）。通過上式可以看出，迭代次數(shù)K的大小與內(nèi)點比例w緊密相關(guān)。取置信度p=0.95，通過上式可得到迭代次數(shù)K與內(nèi)點比例w的關(guān)系，如圖5所示。由圖可以看出，隨著內(nèi)點比例的減小，迭代次數(shù)增大，當內(nèi)點比例小于0.5時，迭代次數(shù)幾乎呈指數(shù)增長。若在場景視場中出現(xiàn)較大面積的動態(tài)目標，內(nèi)點比例將大幅下降，進而導(dǎo)致迭代次數(shù)大大增加。為了有效降低迭代次數(shù)，在進行RANSAC算法迭代之前，應(yīng)盡可能最大程度剔除外點。

圖5 內(nèi)點比例與迭代次數(shù)關(guān)系圖Fig.5 Relationship between interior point ratio and iteration number

通過以上分析可以發(fā)現(xiàn)，利用RANSAC算法得到穩(wěn)健的參數(shù)模型實際上是一個不斷尋優(yōu)，尋找一個包含最多內(nèi)點模型的迭代過程。標準RANSAC算法能夠消除大量的錯誤匹配，但仍有許多不足：

（1）標準RANSAC算法在計算估計模型時，采用隨機選取4個點計算單應(yīng)性矩陣。由于每次選擇的樣本點數(shù)目較小，一旦該樣本點存在較大誤差，就會導(dǎo)致計算出的估計模型包含很少的內(nèi)點。這種估計模型隨后被拋棄，導(dǎo)致迭代次數(shù)增加。

（2）當場景中出現(xiàn)較大面積的動態(tài)目標時，輸入到RANSAC算法中的特征點包含大量“異常點”，這些“異常點”隨后參與了估計模型和內(nèi)點個數(shù)的計算，導(dǎo)致RANSAC算法輸出的估計模型出現(xiàn)偏差。

（3）隨機選擇的數(shù)據(jù)點，很可能出現(xiàn)某幾個數(shù)據(jù)點之間歐式距離過小，導(dǎo)致估計模型精度不高以及模型不穩(wěn)定。

（4）標準RANSAC算法在選取樣本點時采用隨機選點法，選點具有盲目性。這也是導(dǎo)致迭代次數(shù)高的重要原因之一。

2.2.2改進的RANSAC算法

為了提高標準RANSAC算法在含有大量外點場景中的計算精度和效率，以樣本之間內(nèi)在約束為導(dǎo)向，引導(dǎo)RANSAC算法擇優(yōu)采樣。首先，通過動態(tài)特征點粗濾除方法濾除場景的動態(tài)特征點；然后，通過一種區(qū)域劃分的RANSAC樣本點采樣策略，按照特征點分值大小依次采樣計算估計模型。

（1）改進RANSAC算法思想

標準RANSAC算法樣本點選點具有盲目性，當場景中出現(xiàn)較大面積動態(tài)目標時，導(dǎo)致其迭代次數(shù)較高，嚴重影響算法效率。鑒于此，提出以下改進思路：

①采用8點法計算基本矩陣替代4對點求出的單應(yīng)性矩陣作為估計模型，增加了樣本點數(shù)目，使模型更加準確。

②在特征匹配點對之間采用一種精度約束和幾何約束雙約束的方法粗濾除場景中的動態(tài)特征點，采用粗約束后的特征點估算估計模型。

③在選取樣本點時采用塊選取法避免樣本點之間過于集中，將圖像劃分成8個塊，在每個塊中選取一個點。這樣既解決了樣本點過于集中的問題，又避免了標準RANSAC算法在選取完樣本點后需要判斷樣本點是否共線的問題。

④優(yōu)先選擇匹配質(zhì)量高的匹配點對進行估計模型的計算，以減少迭代次數(shù)，提高算法效率。

（2）基于區(qū)域劃分的RANSAC樣本點采樣策略

為解決RANSAC隨機抽樣效率不高以及選取樣本點之間歐式距離過小，且每次采樣都要判斷采樣點共線的缺點，充分發(fā)揮精度濾除和幾何濾除對特征點的分值評判，將一幅圖像劃分為2×4的網(wǎng)格，如圖6所示，并將特征點歸屬到所對應(yīng)網(wǎng)格中。

圖6 改進RANSAC算法網(wǎng)格劃分示意圖Fig.6 Meshing diagram of improved RANSAC algorithm

圖像特征點精度約束得分表述圖像特征點匹配情況，幾何約束得分反映了圖像特征點的異常狀態(tài)，使用圖像特征點匹配精度約束和幾何約束聯(lián)合得分函數(shù)來表述一對特征點的優(yōu)劣程度。記Q(i)為第i個特征點的圖像特征點精度和幾何約束聯(lián)合分值，可以由以下得分函數(shù)求得：

式中，c1、c2分別為精度約束得分和幾何約束得分的比例系數(shù)。

在計算估算模型中，改進算法采樣選點按照8個特征點聯(lián)合得分值依次計算估計模型，盡可能地用一組靜態(tài)圖像特征點來估計模型。首先，對每個圖像特征點按照式（10）計算聯(lián)合分值Q；然后，對于每個網(wǎng)格，按照聯(lián)合得分值Q從小到大對特征點進行排序。優(yōu)先選擇網(wǎng)格中最小得分點作為樣本點進行模型估計，若某一網(wǎng)格中沒有提取到特征點，則隨機在其他網(wǎng)格中選取樣本點。改進RANSAC算法流程如圖7所示，首先對輸入的兩幀圖像進行特征點提取和匹配，在各網(wǎng)格區(qū)域中均勻采樣，按照得分值大小順序采樣，依次計算估計模型，直到迭代條件判斷迭代終止。

圖7 改進RANSAC算法流程圖Fig.7 Flowchart of improved RANSAC algorithm

2.3 基于極線幾何的動態(tài)特征點精濾除方法

極線幾何是兩幅圖像之間獨立于外部環(huán)境的攝影幾何，只依賴于攝像機的自身參數(shù)和相對位姿，匹配點對可通過建立極線約束關(guān)系精確地判斷是否為動態(tài)目標上提取的異常點。圖8表示兩幅圖像之間的極線幾何關(guān)系，其中I1和I2為兩幅圖像對同一場景采集的影像，P為空間中一點，在I1與I2上投影分別為x1、x2。O1、O2分別為兩相機光心，連線分別與I1和I2交于e1和e2。PO1O2與I1、I2圖像平面相交的直線l1、l2分別為圖像平面O1、O2上的極線。

圖8 極線幾何示意圖Fig.8 Schematic diagram of epipolar constraint

將查詢圖像I1到目標圖像I2的空間幾何變換記為(R,t)，則有：

其中，p1、p2分別為空間點P在I1和I2相機坐標系下的坐標；k1、k2分別為兩個相機的內(nèi)參矩陣；s1、s2分別是兩個相機坐標系下空間點P的深度值。若記，則可得：

式中，[t]x為向量t的反對稱矩陣。將式（11）代入可得：

式中，F(xiàn)稱為基礎(chǔ)矩陣。式（14）描述了一對正確匹配像素點對的約束關(guān)系，即若某一像素特征點為另一像素特征點的正確匹配，其必在其圖像平面的極線上，從而可估算出相對準確的幀間基本矩陣。然而該矩陣往往存在誤差，導(dǎo)致實際計算中特征點往往不能剛好落在所計算的極線上，因此用每個特征點到各自極線的距離來衡量匹配點對的可靠性。

若記x1、x2點的歸一化平面坐標為，可表示為為像素坐標值。則I1圖像平面的極線可由下式確定：

式中，X、Y、Z代表極線方程向量。則像素點x2到極線l1的距離可以由下式得到：

其中，D代表x2到極線l1的距離。在真實場景中，導(dǎo)致距離D過大，特征點在動態(tài)目標上，一般有兩種情況：特征點對不匹配或者場景中出現(xiàn)動態(tài)目標。因此，對于距離D的約束既可以剔除不匹配點對，又可以剔除動態(tài)目標上的異常點。設(shè)置閾值ε，如果D>ε，則認為所判斷點對為動態(tài)目標上的“異常點”，其中閾值ε可由下式確定：

其中，ORB特征點的總期望數(shù)量表示為N，Di是從第i個特征到其極線的距離。

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

TUM公共數(shù)據(jù)集[17]（簡稱TUM數(shù)據(jù)集）包含具有尺度和旋轉(zhuǎn)變化的彩色圖和深度圖，軌跡通過一個由8臺高速（100 Hz）攝像機組成的運動捕捉系統(tǒng)獲得，該數(shù)據(jù)旨在評估SLAM算法在室內(nèi)含有快速移動動態(tài)物體場景下的精度和魯棒性。TUM數(shù)據(jù)集包含多種典型的動態(tài)環(huán)境方案，其中walking序列中，兩個人在場景中來回走動，場景變化較大，本文選用該序列作為高動態(tài)運動進行測試。該數(shù)據(jù)集包含四種典型相機自我運動：halfsphere、rpy、static、xyz，分別為相機沿半球軌跡運動、相機沿翻滾-俯仰-偏轉(zhuǎn)軸旋轉(zhuǎn)、相機基本固定在適當位置和相機沿xyz軸運動。

為驗證所提算法的有效性和魯棒性，進行三個實驗測試，分別為特征點濾除、改進RANSAC算法和算法在SLAM上的性能測試。所有實驗在一臺處理器為Intel?CoreTMi7-6700 CPU@3.40 GHz，內(nèi)存20 GB，操作系統(tǒng)為ubuntu 14.04的計算機上運行，所有測試均進行5次，取平均值。

3.1 動態(tài)特征點濾除算法測試

以walking_halfsphere序列彩色圖作為實驗輸入，為了充分驗證特征點濾除算法的性能，測試算法對動態(tài)目標在緩慢移動、快速移動和遠處移動三種狀態(tài)下對特征點濾除的效果。

（1）動態(tài)目標緩慢移動

圖9 為算法對測試數(shù)據(jù)第17幀（第1列）和第24幀（第2列）的濾除結(jié)果。在第17幀中，兩個動態(tài)目標剛起身離開座椅，移動相對緩慢，可以看到在幾何濾除后，還有部分特征點在動態(tài)目標上。在第24幀中，右邊目標完全起身，相對第17幀移動速度加快，可以看到幾何濾除后大部分動態(tài)特征點被濾除。從濾除效果可以看到，幾何濾除對于移動速度相對緩慢的動態(tài)目標不是特別敏感。對比極線幾何對兩幀特征點約束可以看出，極線幾何能很好地對幾何濾除后的動態(tài)特征點進行濾除。

圖9 動態(tài)目標緩慢移動時算法對特征點濾除效果Fig.9 Filtering effect of algorithm on feature points when dynamic target is moving slowly

（2）動態(tài)目標快速移動

圖10 為對TUM數(shù)據(jù)第212幀（第1列）和第414幀（第2列）的濾除結(jié)果。兩個場景中，動態(tài)目標正在快速行走。通過分析可以發(fā)現(xiàn)，幾何濾除對快速移動目標敏感，可以很好地對動態(tài)特征點進行過濾。在第414幀中，動態(tài)目標在幾何濾除后仍存在部分動態(tài)特征點，這是因為動態(tài)目標在行走期間重心在右腿，即右腿支撐，左腿在邁進，這些動態(tài)特征點大部分集中在右腿上。兩幀圖像在進行極線幾何濾除后，動態(tài)特征點幾乎全部得到濾除。

圖10 動態(tài)目標快速移動時算法對特征點濾除效果Fig.10 Filtering effect of algorithm on feature points when dynamic target is moving fastly

（3）動態(tài)目標遠處移動

圖11 為對TUM數(shù)據(jù)第356幀（第1列）和第424幀（第2列）的濾除結(jié)果。可以看出極線幾何配合幾何約束能很好地對動態(tài)目標上的動態(tài)特征點進行濾除。

圖11 動態(tài)目標在遠處移動時算法對特征點濾除效果Fig.11 Filtering effect of algorithm on feature points when dynamic target is moving in distance

通過分析算法對動態(tài)目標處于緩慢、快速移動以及在場景遠處移動的結(jié)果可以得出結(jié)論：當動態(tài)目標相對速度較高時，幾何濾除可以很好地對動態(tài)目標上的特征點進行濾除，但對相對速度較慢動態(tài)目標不敏感，極線約束能很好地對幾何濾除后剩余的動態(tài)特征點進行濾除。

3.2 改進RANSAC算法效率測試

為驗證改進RANSAC算法的性能，將其與標準RANSAC算法的迭代次數(shù)和計算耗時進行對比。圖12為標準RANSANC算法與改進RANSAC算法對walking_halfsphere序列計算的迭代次數(shù)對比結(jié)果。設(shè)置最大迭代次數(shù)為2 000，估計模型置信度為0.99。可以看出，標準RANSAC算法的迭代次數(shù)幾乎均勻分布在0到2 000的范圍內(nèi)，有較多的次數(shù)達到最大迭代次數(shù)。改進RANSAC算法迭代次數(shù)大部分在0到500范圍內(nèi)，迭代次數(shù)明顯降低。改進RANSAC算法充分提高了采樣點的質(zhì)量，擇優(yōu)選取高質(zhì)量特征點優(yōu)先計算估計模型，增大了采集到正確樣本點的概率。

圖12 算法迭代次數(shù)對比Fig.12 Comparison chart of algorithm iterations

圖13 所示為兩種算法的計算時間對比，可以直觀看到兩種算法的處理時間分布。標準RANSAC算法處理時間大部分集中在5~30 ms，改進算法大部分在0~5 ms分布，改進算法的平均處理時間僅為標準算法的38.9%，改進算法在平均計算時間上優(yōu)于標準算法。

圖13 兩種算法計算時間箱型圖Fig.13 Time box plot of two algorithms

3.3 改進SLAM系統(tǒng)性能測試

SLAM系統(tǒng)性能可用絕對軌跡誤差（Absolute Trajectory Error，ATE）和相對姿態(tài)誤差（Relative Posture Error，RPE）進行定量評估。絕對軌跡誤差是相機位姿的真實值和SLAM系統(tǒng)估計值之差。首先根據(jù)位姿的時間戳將真實值和估計值對齊，然后計算每對位姿之間的差值。該標準非常適合評估視覺SLAM的位姿準確性。相對位姿誤差用于計算相同兩個時間戳上位姿變化量的差。用時間戳將位姿真實值和預(yù)估值對齊之后，真實位姿和估計位姿均每隔一段時間計算位姿的變化量，然后對該變化量做差，以獲得相機的位姿誤差。該標準適合于估計系統(tǒng)的漂移。

將所提特征點濾除算法加入到ORB-SLAM前端框架中測試算法在SLAM上的性能。采用數(shù)據(jù)集自帶評價工具將兩個系統(tǒng)所估計的相機位姿與數(shù)據(jù)集所給的真實位姿進行對比。圖14為SLAM系統(tǒng)在walking_halfsphere序列上絕對軌跡誤差測試結(jié)果，（a）為標準ORB-SLAM2絕對軌跡誤差曲線，（b）為改進系統(tǒng)絕對軌跡誤差曲線。圖15為對應(yīng)相對軌跡誤差測試結(jié)果，（a）為標準ORB-SLAM2相對軌跡誤差曲線，（b）為改進系統(tǒng)相對軌跡誤差曲線。從絕對軌跡誤差測試結(jié)果可以看出，改進系統(tǒng)定位精度相較于標準ORB-SLAM2系統(tǒng)定位精度明顯提升；從相對估計誤差曲線可以看到，改進系統(tǒng)的相對軌跡誤差波動相對較低，說明改進系統(tǒng)相對于標準系統(tǒng)更穩(wěn)定。在序列第7 s左右時，兩系統(tǒng)相對軌跡誤差均出現(xiàn)明顯波動，分析其原因，測試序列視野中出現(xiàn)了較大面積的移動目標，動態(tài)特征點導(dǎo)致了系統(tǒng)誤差增大。標準ORB-SLAM2系統(tǒng)的最大誤差峰值達到了約1.1 m，改進系統(tǒng)濾除了大部分動態(tài)特征點，最大誤差峰值相對較低，約為0.13 m，因此，改進系統(tǒng)可有效提高SLAM系統(tǒng)視覺里程計的穩(wěn)定性。

圖14 絕對軌跡誤差Fig.14 Absolute trajectory error

圖15 相對軌跡誤差曲線Fig.15 Relative trajectory error curve

表1 ~表3分別為絕對軌跡誤差和相對軌跡誤差的測試結(jié)果，表中的改進值計算方法如下：

表1 絕對軌跡誤差（ATE）實驗測試結(jié)果Table 1 Experimental test results of absolute trajectory error（ATE）

表3 相對位旋轉(zhuǎn)軌跡誤差（RPE）實驗測試結(jié)果Table 3 Experimental test results of relative position rotation trajectory error（RPE）

表2 相對位移軌跡誤差（RPE）實驗測試結(jié)果Talble 2 Experimental test results of relative displacement trajectory error（RPE）

其中，β代表本文測試結(jié)果，α代表原始系統(tǒng)的測試結(jié)果。表中Our_System表示本文方法。

從表1可以看到改進后的系統(tǒng)在精度上得到了很大改善，在絕對軌跡誤差方面，改進系統(tǒng)的平均均方根誤差相較于ORB-SLAM2降低超過了91%。在實際測試中，標準ORB-SLAM系統(tǒng)在walking_rpy序列上跟蹤不穩(wěn)定，多次測試中，會出現(xiàn)跟蹤失敗的情況。改進系統(tǒng)跟蹤相對穩(wěn)定，但仍然誤差較大，分析其原因是walking_rpy測試序列中，相機出現(xiàn)了快速旋轉(zhuǎn)和移動，導(dǎo)致圖像序列出現(xiàn)了大面積的運動模糊，提取到了大量不穩(wěn)定的圖像特征點。而本文的圖像特征點約束算法對特征匹配點對匹配精確度和位置精度敏感，從而破壞了特征約束算法的穩(wěn)定性。

4 總結(jié)

針對室內(nèi)環(huán)境中動態(tài)目標對移動機器人定位精度的影響，提出一種基于動態(tài)檢測的魯棒視覺SLAM系統(tǒng)。首先，對場景中的動態(tài)特征點進行了粗約束；然后，通過改進RANSAC算法以剩余特征點為樣本點估算幀間基本矩陣；最后，通過極線約束來濾除真正的動態(tài)特征點，只使用靜態(tài)特征點來估算相機位姿。結(jié)合實驗分析可以得出：

（1）圖像特征點粗濾除方法可有效濾除場景中的高動態(tài)特征點，但對局部低動態(tài)特征點不敏感。特征點精濾除方法可以有效檢測緩慢移動的動態(tài)特征點。

（2）改進RANSAC算法提高了標準RANSAC算法在動態(tài)場景下的魯棒性，且改進算法的每幀平均處理時間僅為標準算法的38.9%。

（3）在SLAM性能測試中，在絕對估計誤差方面，改進系統(tǒng)相較于ORB-SLAM2的平均均方根誤差降低超過了91%。