(武漢理工大學 物流工程學院，武漢 430063)

0 引言

近年來，視覺同時定位和建圖(VISUAL simultaneous localization and mapping，VSLAM)技術(shù)以其價格低、能耗小、采集信息多等優(yōu)點在可移動機器人定位導航、虛擬現(xiàn)實及自動駕駛等領(lǐng)域得到了應用。視覺里程計(Visual odometry, VO)作為VSLAM系統(tǒng)的前端，負責估計相機位姿，是整個VSLAM系統(tǒng)的重要組成部分。傳統(tǒng)視覺里程計方法假設(shè)系統(tǒng)工作在靜態(tài)環(huán)境下，即特征點的提取與關(guān)聯(lián)匹配不考慮行人、物體移動等因素帶來的影響。然而大多數(shù)實際場景中都存在行人或者運動物體，這給傳統(tǒng)視覺里程計算法的精準定位帶來了嚴峻的挑戰(zhàn)，例如PTAM(parallel tracking and mapping)[1]和ORB-SLAM2[2]的VO、DSO(direct sparse odometry)[3]等視覺里程計算法都不能很好處理動態(tài)場景帶來的影響，在動態(tài)場景中定位精度很差。針對此問題，目前有學者提出了相應的研究方法，并取得了一定效果。Alcantarilla等人將三維場景流應用到視覺里程計(VO)中，通過三維光流矢量的運動方向區(qū)分動靜點[4]，提升了兩幀之間位姿估計的精度。彭真利用靜態(tài)空間點之間距離不變原則及德勞內(nèi)三角剖分法分割動靜態(tài)點集[5]，提升了視覺里程計系統(tǒng)的定位精度。上述兩種方法取得了一定成效，但是當區(qū)分動靜物體的閾值選取不當時，會出現(xiàn)將靜點完全識別為動點，將動點完全識別為靜點的現(xiàn)象。Wang等人通過計算兩幀圖像之間的稠密光流聚類來區(qū)分動靜物體[6]，有效檢測動態(tài)物體，但是稠密光流法耗時較長，不能實時運行。高成強[7]等人針對圖像塊的變化建立了背景實時更新模型，并利用圖像塊方差變化篩選運動物體，但是該方法對環(huán)境適應度差。林志林等人利用三維場景流變換及虛擬場景點的方法識別動態(tài)物體，動態(tài)物體剔除率較高[8]，但是易出現(xiàn)刪除誤匹配點過多而導致估計失敗的情況。

本文受文獻[8]三維場景流變換判斷運動物體的啟發(fā)，采用二維光流計算兩幀間相機運動變換矩陣，并利用求解的變換矩陣對相機進行運動補償，然后利用圖像幀間差分法檢測運動物體。進一步地，將檢測到的運動物體興趣域(Region of Interest, ROI)作為圖像mask，剔除特征匹配中的運動物體區(qū)域特征點。最后利用剔除掉動態(tài)點的點集進行位姿估計，有效提升了VO算法在動態(tài)場景的運動位姿估計精度。

1 基于改進幀間差分的運動物體檢測

當相機固定時，相鄰兩幀圖像間的圖像背景不變。此時如果場景中存在運動物體，相鄰兩幀圖像間像素變化只受行人或者運動物體影響。通過相鄰兩幀圖像間像素差值易判斷出行人或動態(tài)物體的區(qū)域。兩幀圖像間像素差值可由(1)式進行計算：

Idiff=|I(x,y,t)-I(x,y,t-1)|

(1)

其中：I(x,y,t)表示圖像中像素坐標為(x,y)處在t時刻的像素值，而I(x,y,t-1)為像素坐標(x,y)處在t-1時刻的像素值，Idiff表示兩幀圖像間的像素值差值。通過式(2)將兩幀圖像像素值差值結(jié)果二值化，進而構(gòu)建出對應的二值圖像，利用構(gòu)建好的二值圖像中的非零值，即可找到行人或者運動物體在圖像中所對應的區(qū)域，式中的閾值th采用大律法獲得。

(2)

然而上述方法在相機運動時不再適用，因為相機運動時圖像中的像素值變化不僅受行人或運動物體影響，還受相機運動帶來的影響。所以此處需要一種方法對相機運動進行補償，盡量減小乃至消除相機運動造成的影響。

為了盡量減小相機運動帶來的影響，需要求解出相機的位姿變換，進而利用估計出的位姿變換矩陣對相機運動進行補償。

1.1 相機運動估計及運動補償

相機運動估計時，不同于傳統(tǒng)的三維場景流方法，采用透視變換矩陣T表示兩幀圖像間的相機運動[9]。這里選用單應矩陣H作為透視變換矩陣T，兩幀圖像間像素對應關(guān)系見(3)式，簡化記為It=HIt-1，其中：(xt-1,yt-1)和(xt,yt)為前后兩幀圖像匹配點對在兩幀圖像中的像素坐標值。

(3)

求取單應矩陣H時，理論上只需要4對匹配點對就能求得單應矩陣。然而實際中為了求解結(jié)果更精準，往往利用4對以上配對點信息，采用重投影誤差優(yōu)化和隨機采樣一致性(random sample consensus, RANSAC)結(jié)合的方法求取。式(4)中，ε是兩張圖片之間對應像素點的重投影誤差，求解H矩陣時，利用Levenberg-Marquardt方法不斷迭代優(yōu)化ε。當ε最小時，更新得到的H矩陣即為最優(yōu)變換矩陣，接下來可以利用最優(yōu)變換矩陣補償相機運動。相機運動補償?shù)牧鞒虨椋赫蜃儞Q時(此處指將上一時刻圖像像素坐標值變換為當前時刻圖像坐標值，逆向變換則相反)，利用最優(yōu)變換矩陣H將上一時刻中圖像幀中所有像素點坐標值轉(zhuǎn)換到與原圖像相同分辨率大小的新圖像中，作為當前時刻圖像的相機運動補償幀，逆向變換時則反向操作。

(4)

1.2 補償后的運動物體檢測及剔除

對相機的運動進行補償之后，即可進行行人或者運動物體的檢測，具體流程為：

Step1：利用最優(yōu)透視變換矩陣將前一幀圖像中的所有像素點轉(zhuǎn)換到新的圖像中，此圖像即為補償幀，補償幀圖片的尺寸大小和原圖片相同；

Step2：利用后一幀圖像像素值與補償幀圖像求解像素值差I(lǐng)diff，然后利用圖像二值化、圖像形態(tài)學操作、邊緣噪聲處理等方式去除圖像中相應噪聲數(shù)據(jù)，最終通過二值圖中像素值非零值檢測出行人或動態(tài)物體所在區(qū)域。

接下來以rgbd_fr3_walking_xyz數(shù)據(jù)子集中的第416幀圖像與第417幀圖像中的運動物體檢測為例，介紹運動物體或行人檢測檢測過程。具體的檢測效果見圖1，此處的上一幀圖像為第416幀圖像，此處的下一幀圖像為第417幀圖像。圖1(b)是圖1(c)的運動補償幀，圖1(d)是圖1(c)與子圖經(jīng)過下列過程得到的結(jié)果圖：依據(jù)式(1)求解像素值差值Idiff、利用Idiff二值化處理構(gòu)建二值圖、圖像腐蝕操作去除二值圖中小區(qū)域的噪聲、圖像膨脹操作將二值圖中有效區(qū)域范圍擴大。腐蝕操作需要根據(jù)實際需求選擇圖像腐蝕內(nèi)核大小及腐蝕次數(shù)，同理，膨脹操作也需要確定膨脹內(nèi)核大小及膨脹次數(shù)。為了保證后續(xù)在所有數(shù)據(jù)集上取得較好的效果，腐蝕內(nèi)核大小為4，腐蝕操作進行兩次，膨脹內(nèi)核大小為16，膨脹操作進行一次。從圖1(d)中可以看到已經(jīng)基本檢測到真正運動的物體。

通過分析圖1(d)結(jié)果，可以得出雖然采用此方法檢測出了運動物體，但是運動物體上有大約三分之一區(qū)域仍然未檢測到，檢測信息丟失，不符合后續(xù)將檢測區(qū)域作為mask去除動態(tài)點的需求。經(jīng)過對比分析，發(fā)現(xiàn)導致此情況的原因是運動物體表面紋理較為接近，導致圖像二值化時變?yōu)?像素值。為解決此問題，本文采用一種向前向后檢測方式來檢測運動物體。反方向的檢測與正方向的檢測相似，即計算后一時刻圖像相對于前一時刻圖像之間的最優(yōu)相對變換矩陣，像素補償后求解反方向二值化圖像，反向檢測結(jié)果見圖1(e)。將正反方向檢測二值化圖像疊加后，得到運動物體大部分像素點，最終檢測效果圖見圖1(f)。

圖1 TUM RGBD數(shù)據(jù)集[10]測試

圖像特征點建立關(guān)聯(lián)匹配時，將上述二值化圖像中的非零像素值聯(lián)通區(qū)域作為興趣域(Region of Interest, ROI)。刪除掉對應圖像中ROI區(qū)域中的特征點集，即可刪除動態(tài)特征點集，剩下靜態(tài)特征點集。在此基礎(chǔ)上通過求得的靜態(tài)點集優(yōu)化單應矩陣H，重復上述操作，進而求得更優(yōu)的靜態(tài)點集。綜上可以得出采用的靜態(tài)特征點提取流程如圖2所示。

圖2 靜態(tài)特征點提取流程

經(jīng)過上述流程，可以將移動物體或者行人上面的大部分特征點去除，進一步減小了動態(tài)物體及行人對整個系統(tǒng)的影響，具體效果圖見圖3。圖3中動態(tài)點刪除前，行人襯衫、腿部及身體輪廓邊緣有大量特征點。動態(tài)點刪除后，能夠看到，位于行人身上及其輪廓的大量點已經(jīng)刪除，剩下的少部分點通過RANSAC方法也會被刪除掉。綜上，所提算法能夠有效檢測到動態(tài)物體區(qū)域并剔除掉動態(tài)干擾點。

圖3 補償后剔除動態(tài)物體特征點

為了驗證所提方法不僅能對行人進行檢測，還能夠?qū)\動物體進行檢測，此處采用實驗室實際場景中的圖像幀進行測試，具體效果見圖4。圖4中可以看到，算法已經(jīng)將移動的杯子區(qū)域檢測出來。

圖4 實驗室局部場景

2 運動物體特征點去除的視覺里程計

圖5為本文所提系統(tǒng)框架圖，整體包括行人及運動物體檢測、相機位姿估計兩部分。行人及運動物體檢測及特征點去除利用所提方法完成，最終剔除掉動態(tài)點的靜態(tài)點集存儲在局部地圖中。利用單幀圖像與局部地圖點的匹配關(guān)系，采用EPNP[11]位姿估計算法求得相機位姿旋轉(zhuǎn)矩陣R和平移向量t初值，此時求得的R和t不是最優(yōu)解，需要通過重投影誤差公式(5)來進行不斷迭代優(yōu)化得到較優(yōu)值。其中：ui代表圖像I1中點P的像素坐標(即前面用特征點匹配或者跟蹤方法獲得的點像素坐標值)，K(RPi+t)/s為與I1配對的圖像I2上對應點P’通過重投影變換映射到圖像I1的坐標值。

(5)

這里通過式(5)不斷優(yōu)化迭代，使得式中的e最小，最終求得相鄰兩幀之間相機位姿最優(yōu)旋轉(zhuǎn)矩陣R和平移向量t。為了使得定位效果更好，算法還構(gòu)建了局部地圖，在獲得局部關(guān)鍵幀圖像、固定關(guān)鍵幀圖像及局部共視地圖點之后進行局部bundleadjustment優(yōu)化，此處采用G2O框架進行優(yōu)化，利用局部關(guān)鍵幀圖像、固定關(guān)鍵幀圖像及共視地圖點作為G2O的頂點，利用三者之間的關(guān)系作為邊，迭代優(yōu)化使得局部最小二乘誤差最小。

圖5 帶有動態(tài)物體檢測的里程計架構(gòu)

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

3.1 實驗平臺及參數(shù)配置

本文的算法運行環(huán)境為：操作系統(tǒng)Ubuntu16.04，cpu型號為I7-7700HQ，CPU的主頻為2.8 gHz，整機的RAM為8 g。實驗過程中未使用顯卡進行加速，為CPU實時運行。利用TUM的RGBD數(shù)據(jù)集對提出的算法進行了算法精度及耗時評估，并與開源的ORB-SLAM2中的前端里程計算法進行了對比，從而驗證本文所提算法有效性。

表1 視覺里程計輸出RPE對比

實驗驗證使用的TUM RGBD數(shù)據(jù)集圖片大小為640x480，數(shù)據(jù)集分為幾個子集，分別針對FR2、FR3兩個場地，每個數(shù)據(jù)子集都提供了真實移動軌跡groundtruth。為了驗證本文算法能夠有效提升視覺里程計在動態(tài)環(huán)境中的定位精度，這里選取全部低動態(tài)、高動態(tài)場景數(shù)據(jù)子集進行算法評估。算法實驗中使用的圖像畸變校正參數(shù)來自于TUM RGBD數(shù)據(jù)集官方文件。

3.2 實驗結(jié)果分析

算法精度驗證標準為相對位姿誤差(relative pose error,RPE)[10]，相對位姿誤差估計的是一定時間內(nèi)的局部位姿測量誤差，因此其適用于視覺里程計系統(tǒng)的誤差評估。為了更好地評估所提算法提升精度的有效性，實驗分別對比了ORB-SLAM2算法、文獻[7]所提算法和文獻[12]所提BaMVO算法的定位精度實驗結(jié)果。其中ORB-SLAM2算法沒有考慮動靜態(tài)點的區(qū)分，文獻[7]所提算法利用圖像塊及圖像塊方差差異剔除了動態(tài)點，文獻[12]所提BaMVO算法通過對比匹配特征點之間深度值差異情況來去除動態(tài)點影響。由表1可以得出，在低動態(tài)場景和高動態(tài)場景中，本文所提算法相較于ORB-SLAM2算法都有所提升，尤其是在高動態(tài)場景中，整體算法精度提升較大，所有數(shù)據(jù)集定位精度平均提升38%，證明算法有效。fr3_walking_static數(shù)據(jù)子集中定位精度更是提高了87.9%，而fr3_walking_rpy數(shù)據(jù)集中整體運動模糊較多，相機位移較大，對本文算法是較大的挑戰(zhàn)，所以相較于其他高動態(tài)場景精度提升最小。低動態(tài)場景的精度提升較小，主要原因是腐蝕膨脹圖像處理時，為了兼顧所有數(shù)據(jù)集都能平穩(wěn)運行，設(shè)置了固定腐蝕膨脹閾值，導致對輕微的移動現(xiàn)象檢測較弱，此處設(shè)置自適應閾值會取得更佳效果。與文獻[7]所提算法對比，在低動態(tài)場景，本文所提算法更加有效，原因是其背景塊建模算法對低動態(tài)變化不敏感，在高動態(tài)場景，文獻[7]算法在大部分數(shù)據(jù)子集上效果較好，在fr3_walking_rpy數(shù)據(jù)子集上，跟蹤失敗，未獲得最終的運行軌跡，主要原因是其在運動模糊中本身提取角點較少，剔除動態(tài)角點后剩余角點已無法進行位姿估計，導致跟蹤線程失敗。而本文方法雖然在定位精度上稍差于文獻[7]算法，但是魯棒性更強，能夠適應更多環(huán)境。與文獻[12]所提BaMVO算法對比，無論是在低動態(tài)場景還是高動態(tài)場景中，本文算法相對定位精度更高，主要原因是算法對于動態(tài)區(qū)域感知相對較好，而文獻[12]的BaMVO算法動態(tài)點剔除算法相對較弱，對于動態(tài)區(qū)域的感知較弱，大部分動態(tài)點沒有被剔除掉，動態(tài)點會繼續(xù)干擾定位效果。由表1可以得知，只有fr3_walking_rpy數(shù)據(jù)子集實驗BaMVO算法較優(yōu)，原因是對于整體運動模糊過多場景，BaMVO算法受運動模糊干擾較小，特征點提取算法準確度較低，后續(xù)可以結(jié)合稀疏直接法提升本文算法對于場景運動模糊的定位精度。

綜上所述，所提算法對于相機位姿估計精度提升是有效的，與現(xiàn)有基于運動物體刪除的里程計算法對比也有一定優(yōu)勢，此處需要注意的是表中的文獻[7]及文獻[12]算法實驗結(jié)果均來自文獻，但是文獻[7]平臺計算能力更佳，對比是有效的。

為更為直觀地表示所提算法對于位姿估計精度的提升，圖6中將兩種算法在部分數(shù)據(jù)集上的RPE曲線圖展現(xiàn)出來，從圖6中能夠看到，所提算法整體上比ORB-SLAM2精度高。圖6中的實黑線能夠看到，ORB-SLAM2在某些時段誤差突然變大，將曲線圖的時間與數(shù)據(jù)集圖片集一一對應起來，發(fā)現(xiàn)誤差突然變大的地方都有動態(tài)物體存在，所提算法在有動態(tài)物體時表現(xiàn)了良好的穩(wěn)定性。圖6(a)中可以看到，所提算法在剛開始時穩(wěn)定性不如ORB-SLAM2，通過對相應時間段的數(shù)據(jù)集圖片分析得知，不穩(wěn)定的原因在于在對應時間段，運動物體存在但是其運動方向垂直相機平面，此情況是所提算法檢測盲區(qū)，故出現(xiàn)不穩(wěn)定現(xiàn)象。

圖6 所提算法與ORB-SLAM2在兩個數(shù)據(jù)集的RPE曲線圖

除對算法定位精度進行評估外，還對所提算法與ORB-SLAM2整體運行耗時進行了評估，這里不對比文獻[7]及文獻[12]的運行耗時，因為算法運行平臺不同，耗時對比標準會有不同。算法耗時實驗評估發(fā)現(xiàn)ORB-SLAM2跟蹤在測試數(shù)據(jù)集上的平均運行耗時為37.683 ms，而所提算法在測試數(shù)據(jù)集上的運行平均耗時達到了56.746 ms，相較于ORB-SLAM2算法耗時增加了19 ms，雖然較ORB-SLAM2運行耗時有所增加，但是能夠?qū)崟r運行，此處耗時增加需要進一步分析進行優(yōu)化。

4 結(jié)束語

本文提出了一種基于動態(tài)物體檢測與特征點去除的立體視覺里程計算法。算法能夠有效檢測到動態(tài)物體，并能夠利用動態(tài)區(qū)域mask剔除掉對應動態(tài)點，降低了動態(tài)物體對相機位姿估計帶來的影響。通過對TUM RGBD數(shù)據(jù)集的評估測試，發(fā)現(xiàn)所提算法對于定位精度有一定提升，與現(xiàn)有改進算法相比，也具有一定優(yōu)勢。但是所提算法對于垂直相機平面的運行物體檢測較差，后續(xù)可以針對此處做相應優(yōu)化。除此之外，耗時相較于其他算法有所增加，實際使用中需要進行計算機工程上的優(yōu)化。除此之外，下一步工作會嘗試加入其它傳感器，進一步判斷動態(tài)物體，從而更好地去除動態(tài)物體影響。