楊世強(qiáng)，范國(guó)豪，白樂(lè)樂(lè)，趙 成，李德信

西安理工大學(xué) 機(jī)械與精密儀器工程學(xué)院，西安710048

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

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

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

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

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

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

2 基于幾何約束的動(dòng)態(tài)特征點(diǎn)濾除算法

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

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

2.1 動(dòng)態(tài)特征點(diǎn)粗濾除方法

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

2.1.1圖像特征點(diǎn)匹配精度約束方法

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

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

2.1.2圖像特征點(diǎn)幾何約束方法

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

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

當(dāng)場(chǎng)景中沒(méi)有動(dòng)態(tài)目標(biāo)時(shí)，三角形對(duì)應(yīng)邊的差應(yīng)該在一個(gè)小區(qū)間，可以表示為：

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

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

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

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

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

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

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

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

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

整個(gè)幾何約束方法對(duì)動(dòng)態(tài)特征點(diǎn)的濾除步驟可歸結(jié)如下：

步驟1對(duì)查詢圖像和目標(biāo)圖像中的匹配點(diǎn)對(duì)計(jì)算其幾何約束得分以及平均幾何約束得分AS。

步驟2對(duì)所計(jì)算的各匹配特征點(diǎn)進(jìn)行雙向打分，依次計(jì)算所有特征點(diǎn)的異常分值qab。

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

2.2 改進(jìn)RANSAC算法

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

2.2.1標(biāo)準(zhǔn)RANSAC算法理論分析

標(biāo)準(zhǔn)RANSAC算法的基本思想如下[16]：

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2.2改進(jìn)的RANSAC算法

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

（1）改進(jìn)RANSAC算法思想

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

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

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

③在選取樣本點(diǎn)時(shí)采用塊選取法避免樣本點(diǎn)之間過(guò)于集中，將圖像劃分成8個(gè)塊，在每個(gè)塊中選取一個(gè)點(diǎn)。這樣既解決了樣本點(diǎn)過(guò)于集中的問(wèn)題，又避免了標(biāo)準(zhǔn)RANSAC算法在選取完樣本點(diǎn)后需要判斷樣本點(diǎn)是否共線的問(wèn)題。

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

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

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

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

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

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

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

圖7 改進(jìn)RANSAC算法流程圖Fig.7 Flowchart of improved RANSAC algorithm

2.3 基于極線幾何的動(dòng)態(tài)特征點(diǎn)精濾除方法

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

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

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

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

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

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

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

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

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

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

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

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

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

3.1 動(dòng)態(tài)特征點(diǎn)濾除算法測(cè)試

以walking_halfsphere序列彩色圖作為實(shí)驗(yàn)輸入，為了充分驗(yàn)證特征點(diǎn)濾除算法的性能，測(cè)試算法對(duì)動(dòng)態(tài)目標(biāo)在緩慢移動(dòng)、快速移動(dòng)和遠(yuǎn)處移動(dòng)三種狀態(tài)下對(duì)特征點(diǎn)濾除的效果。

（1）動(dòng)態(tài)目標(biāo)緩慢移動(dòng)

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

圖9 動(dòng)態(tài)目標(biāo)緩慢移動(dòng)時(shí)算法對(duì)特征點(diǎn)濾除效果Fig.9 Filtering effect of algorithm on feature points when dynamic target is moving slowly

（2）動(dòng)態(tài)目標(biāo)快速移動(dòng)

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

圖10 動(dòng)態(tài)目標(biāo)快速移動(dòng)時(shí)算法對(duì)特征點(diǎn)濾除效果Fig.10 Filtering effect of algorithm on feature points when dynamic target is moving fastly

（3）動(dòng)態(tài)目標(biāo)遠(yuǎn)處移動(dòng)

圖11 為對(duì)TUM數(shù)據(jù)第356幀（第1列）和第424幀（第2列）的濾除結(jié)果?？梢钥闯鰳O線幾何配合幾何約束能很好地對(duì)動(dòng)態(tài)目標(biāo)上的動(dòng)態(tài)特征點(diǎn)進(jìn)行濾除。

圖11 動(dòng)態(tài)目標(biāo)在遠(yuǎn)處移動(dòng)時(shí)算法對(duì)特征點(diǎn)濾除效果Fig.11 Filtering effect of algorithm on feature points when dynamic target is moving in distance

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

3.2 改進(jìn)RANSAC算法效率測(cè)試

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

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

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

圖13 兩種算法計(jì)算時(shí)間箱型圖Fig.13 Time box plot of two algorithms

3.3 改進(jìn)SLAM系統(tǒng)性能測(cè)試

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

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

圖14 絕對(duì)軌跡誤差Fig.14 Absolute trajectory error

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

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

表1 絕對(duì)軌跡誤差（ATE）實(shí)驗(yàn)測(cè)試結(jié)果Table 1 Experimental test results of absolute trajectory error（ATE）

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

表2 相對(duì)位移軌跡誤差（RPE）實(shí)驗(yàn)測(cè)試結(jié)果Talble 2 Experimental test results of relative displacement trajectory error（RPE）

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

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

4 總結(jié)

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

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

（2）改進(jìn)RANSAC算法提高了標(biāo)準(zhǔn)RANSAC算法在動(dòng)態(tài)場(chǎng)景下的魯棒性，且改進(jìn)算法的每幀平均處理時(shí)間僅為標(biāo)準(zhǔn)算法的38.9%。

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