周 凱，羅 元，張 毅，李晉宏

(1.光電信息傳感與技術重點實驗室(重慶郵電大學)，重慶 400065；2.重慶郵電大學 信息無障礙與服務機器人工程技術研究中心，重慶 400065)

視覺里程計(VO)是一種利用視覺傳感器信息對機器人或其他載體進行準確的位姿估計的方法，相比于使用激光傳感器的方法，視覺信息更加豐富，可用于三維重建、物體識別、語義分割等.同時，因其成本低、體積小、精度高等優(yōu)點，而廣泛地應用于機器人自主導航、自動駕駛、增強現(xiàn)實等領域[1-4].

近年來，許多精度高、效率快的視覺里程計方法被提出來，可以將其大致分為3類.第1類是基于特征點法的視覺里程計[4-6]，這些方法通過提取和匹配相鄰圖像的特征點，建立重投影優(yōu)化函數(shù)來估計幀間運動.第2類是基于直接法的視覺里程計[7-9]，根據(jù)灰度不變假設，建立光度誤差函數(shù)來估計幀間運動.第3類是將直接法與特征點法融合的視覺里程計，F(xiàn)orster等[10]提出一種半直接法視覺里程計(SVO)，通過對圖像中的特征點塊進行匹配來計算位姿，加快了計算精度與速度.這些傳統(tǒng)的算法都基于靜態(tài)環(huán)境假設，忽略了動態(tài)物體對運動估計的影響，從而導致計算結果在動態(tài)環(huán)境下有較大的誤差.

在動態(tài)環(huán)境下，一般可采用隨機采樣一致性(RANSAC)或魯棒Huber函數(shù)等概率方法將動態(tài)部分當作噪聲剔除掉.當動態(tài)物體在圖像中占比較大時，則無法通過概率方法剔除，需要將動、靜態(tài)物體區(qū)分開來，使用靜態(tài)部分估算位姿，避免動態(tài)物體造成的誤差.Besco等[11]提出的DynaSLAM算法采用深度學習結合幾何的方法檢測動態(tài)物體，該方法能準確剔除動態(tài)物體，具有較高的精度，但是該算法需要GPU加速，無法達到實時性與便攜性.Li等[12]使用邊緣點靜態(tài)權重的方法判別動態(tài)點，該方法擁有較大的像素級噪聲，容易導致動、靜態(tài)點判斷錯誤，且僅使用了部分圖像信息，位姿估計精度較差.Jaimez等[13]和Scona等[14]采用聚類的方法，通過構建背景分割優(yōu)化函數(shù)來分離動、靜態(tài)物體，然而該類方法在高動態(tài)環(huán)境下，不能有效地剔除動態(tài)對象，且無法進行準確的位姿估計.

本文提出一種融合邊緣信息的稠密視覺里程計算法，利用邊緣信息對光照變化和運動模糊不敏感的特性，彌補直接法的缺點.同時，在幾何紋理較少的環(huán)境下，加大直接法在計算中的占比，從而減小邊緣信息缺失導致的誤差.本文的主要貢獻如下：1)將邊緣對齊的Distance Transform(DT)誤差引入殘差模型中，增加算法在不同環(huán)境的魯棒性；2)引入平均背景深度，加大動態(tài)部分殘差，以輔助運動分割；3)將帶有邊緣信息的加權聚類殘差加入到位姿估計的非線性優(yōu)化函數(shù)中，提高位姿估算的精度.實驗結果表明，本文算法在動態(tài)環(huán)境下能準確剔除動態(tài)物體，并在位姿估算中具有較高的精度.

1 算法框架

本文算法的全過程見圖1.首先，使用K-means聚類法將圖像分為N個集群.然后，將聚類后的邊緣圖與灰度圖分別坐標變換到關鍵幀及其DT圖上，構建基于聚類的殘差模型.接著，根據(jù)聚類殘差建立基于t分布的非參數(shù)統(tǒng)計模型，并通過分段閾值函數(shù)分離出動態(tài)物體并得到殘差權重.最后，將加權殘差加入到優(yōu)化函數(shù)中進行位姿估算，并將得到的位姿進行跟蹤質(zhì)量檢查，更新下次迭代的變換矩陣或關鍵幀.

圖1 算法框架

本文算法引入了邊緣信息，對當前幀進行邊緣提取與聚類后，計算出融合DT誤差的殘差模型，通過平均背景深度擴大動、靜部分殘差差距來輔助運動分割，并將邊緣對齊作為約束加入到位姿估計中以獲得更準確的估算結果.

2 融合邊緣信息的視覺里程計

使用RGB-D傳感器，每一個時間幀都可以獲得一個灰度圖Ic和一個深度圖Zc.深度信息用于計算場景聚類、參與構建殘差模型并計算平均背景深度.灰度信息用于提取邊緣信息，并結合深度信息構建光度及幾何一致性誤差與DT誤差.

2.1 相機運動模型

使用圖像對應的三維點坐標P=(X,Y,Z)和深度Z=Zc(p)可通過投影函數(shù)π計算出圖像的像素坐標p=(x,y)為

(1)

式中,fx和fy為焦距，cx和cy為標準針孔相機模型定義的光學中心.另外，為了將像素坐標p反投影為三維坐標點P，定義逆投影函數(shù)π-1為

(2)

兩個圖像幀之間的相對運動可以表示為變換矩陣T∈SE(3)，其中包含3×3的旋轉(zhuǎn)矩陣R3×3∈SO(3)和3×1的平移向量t3×1∈R3.通過上一幀的三維坐標點Pc-1以及兩幀間的變換矩陣T，可以計算出當前幀的三維坐標點Pc為

(3)

剛體運動有6個自由度，可使用李代數(shù)se(3)中的ξ∈R6來表示李群SE(3)中的變換矩陣T∈R4×4.定義坐標變換函數(shù)ω為

pc=ω(ξcc-1,pc-1)=π(Tcc-1π-1(pc-1,Zc-1(pc-1)))，

(4)

式中,ξcc-1是Tcc-1的李代數(shù)，pc-1和pc分別是相鄰兩幀對應的像素點坐標.

2.2 場景聚類

為了獲得稠密的運動分割，有的方法直接采用像素點進行權重計算[12]，逐點分割未考慮到像素點之間的空間關系，不可避免地會產(chǎn)生噪聲，導致精度下降.本文參考Jaimez等[13,15]的方法采用K-means聚類法，利用深度信息和逆投影函數(shù)π-1得到三維空間點坐標，通過計算每一個三維空間點距離集群中心點的距離，來確定該空間點屬于哪一個集群，屬于同一個集群的點一般為同一個剛體.使用集群計算能夠避免產(chǎn)生像素級噪聲，簡化運動分割問題.

2.3 殘差模型

2.3.1 光度及幾何誤差

為了識別環(huán)境中的動、靜態(tài)部分，需要計算每個集群的殘差大小，然后進行運動分割.首先，將當前幀F(xiàn)c完成聚類后坐標變換到關鍵幀F(xiàn)k.然后，根據(jù)光度及幾何一致性構建誤差模型：

(5)

(6)

2.3.2 邊緣對齊誤差

Schenk等[16]對比了多種邊緣檢測算法的性能，本文借鑒他們的方法對當前幀F(xiàn)c進行邊緣提取.然后，將邊緣點按照集群分類，并將邊緣坐標變換到關鍵幀的DT圖以計算邊緣對齊誤差.基于邊緣對齊的誤差模型為

(7)

圖2 DT計算

2.3.3 數(shù)據(jù)融合誤差

將邊緣對齊誤差引入到光度及幾何誤差中構建融合邊緣信息的殘差模型，能夠計算出更準確的殘差.但是，背景的遮擋區(qū)域往往會導致殘差模型具有較高的值，類似于Jaimez等[13]的方法，為降低遮擋的影響，定義殘差模型為

(8)

(9)

式中：vc為相機運動速度，N為集群總數(shù)量，η為由αI到αE的尺度系數(shù).在邊緣信息豐富(邊緣點占比高)或相機運動速度較快時(容易產(chǎn)生模糊圖像)，αE有較大值，相反，αE較小時邊緣信息影響較小.

使用原始集群殘差往往準確率不高，運動分割效果較差.因此，參考Jaimez等[13]的方法進行殘差正則化，對連續(xù)的集群偏向于相似的運動分割，建立最小化模型來求取正則化殘差公式為

λT(rn-rn,c-1)2.

(10)

式中，Gmn為聚類連通函數(shù)，為1表示m與n集群連通;rn,c-1表示上一幀中第n個集群的殘差值，因為在時間上，物體都是趨于保持移動或靜止的.通過求解最小問題來得到最佳rn，正則化后的殘差模型與動態(tài)物體有較高的相關性.

2.3.4 平均背景深度輔助運動分割

為了減少場景的錯誤分割與過度分割，提高運動分割的準確性，本文引入平均背景深度來輔助算法進行運動分割.動態(tài)物體一般都為獨立的剛體且遠離背景，通過擴大遠離背景部分的集群殘差，使得靠近背景的集群偏向于分割為靜態(tài)部分，而遠離背景的集群有更大幾率被判定為動態(tài)部分.定義平均背景深度為

(11)

(12)

2.4 相機位姿估計

動態(tài)物體的獨立運動會使得兩幀之間坐標變換后的動態(tài)物體圖像不對齊，而導致屬于該物體的集群有較大的殘差，而靜態(tài)集群的殘差在圖像對齊的情況下往往很小或接近于0.圖3是一個高度動態(tài)場景的統(tǒng)計殘差直方圖，根據(jù)聚類殘差分布的普遍特征，參考文獻[12，19]的方法建立一個基于t分布的非參數(shù)統(tǒng)計模型

(13)

σ=1.482 6Median{|δn-μ|}.

(14)

式中：v0為t分布的自由度，它決定了分布曲線的陡峭程度;平均值μ決定分布中心所處位置，將其設置為0表示殘差小的靜態(tài)集群占比較大;σ為統(tǒng)計方差，通過中位數(shù)絕對偏差估算得到.

圖3 殘差直方圖與t分布曲線

如圖3所示，非參數(shù)統(tǒng)計模型能夠較好地擬合實際實驗殘差的直方圖.由于統(tǒng)計模型的概率代表了聚類的運動似然，因此可以指導場景進行運動分割.在各種復雜的場景下，采用固定閾值會使得分割效果不佳，參考Zhou等[19]的方法使用自適應閾值構建場景分割模型，殘差高于自適應閾值的集群可判定為動態(tài)部分，而低于閾值的集群可作為靜態(tài)部分或賦權部分加入到位姿估算中，其公式為

(15)

式中：φn代表第n個集群的權重，為0時表示該集群為動態(tài)集群，為1時表示為靜態(tài)集群;vc為相機速度;σ為統(tǒng)計方差;β用于調(diào)整因變量到殘差尺度的系數(shù)，通過公式103×min(103,10dc-1)計算得到.

計算出集群權重后，便可得到加權聚類殘差，通過加權聚類殘差來構建非線性優(yōu)化函數(shù)，用以更準確地估計幀間運動.文獻[13,19]的方法表明，采用Cauchy M-estimator模型進行優(yōu)化能獲得較好的結果.構建融合邊緣信息的加權聚類殘差的非線性優(yōu)化函數(shù)

(16)

(17)

2.5 跟蹤質(zhì)量檢測

本文采用幀到關鍵幀的跟蹤方法，只有在更新關鍵幀的時候才會進行復雜的DT計算，相比于傳統(tǒng)的幀對幀計算每一幀DT的方法[20]，大大提高了計算效率.另外，采用幀與關鍵幀的跟蹤方法，可使得非剛性物體的輕微運動更加容易區(qū)分，從而降低輕微運動導致的累計誤差.參考Schenk等[16]的方法，通過將先前跟蹤過的N幀的邊緣投影到當前幀，得到像素點重疊計數(shù)圖，通過對相同重疊數(shù)構建直方圖，來獲取圖像重疊情況，從而評估當前幀跟蹤質(zhì)量，公式為

(18)

式中,H(j)表示重疊數(shù)為j的點的數(shù)量，γj表示重疊數(shù)為j的點在跟蹤質(zhì)量評估中所占的權重.當邊緣重疊的加權和小于非重疊的數(shù)量時，說明跟蹤質(zhì)量較差，此時將上一跟蹤較好的幀更新為關鍵幀.

3 實驗與分析

本實驗所用電腦配置為：CPU為英特爾i5-4210，主頻為1.7 GHz，內(nèi)存為4 G，不使用圖形處理器(GPU)加速，在Ubuntu 16.04下進行實驗.本文算法的運行基于每一時間幀的灰度與深度信息，因此主要適用于RGB-D相機.在慕尼黑工業(yè)大學提供的TUM RGB-D標準數(shù)據(jù)集[21]上進行實驗，每個序列包含RGB圖像、深度圖像以及真實軌跡等，該數(shù)據(jù)集已被廣泛用于測試各種SLAM和VO算法的準確性和性能.為了方便算法評估，將數(shù)據(jù)集分為2類：1)低動態(tài)與靜態(tài)環(huán)境，此類數(shù)據(jù)集下沒有移動物體或僅有少量移動物體；2)高動態(tài)環(huán)境，人或其他移動物體在圖像中占比較大.同時在這2類數(shù)據(jù)集下測試來評估算法的綜合性能.

本文實驗取K-means聚類集群數(shù)N為24，并設置其他參數(shù)如下：1)在殘差模型建立部分，如圖4所示，(a)為原始圖像；僅采用幾何信息進行運動分割時，即αI為0，可見(b)中由于信息不足而導致分割不完全；(c)中融合光度信息并設置較高的αI值導致光度信息占比較高而產(chǎn)生過度分割；(d)為融合光度信息并取合適的αI所產(chǎn)生的較準確的分割結果；(e)中融合邊緣信息并取較高的η值時，同樣會導致過度分割；(f)為最準確的分割結果，此時設置光度誤差權重αI為0.15，計算DT誤差權重的尺度因子η為0.35，來平衡幾何、光度、邊緣信息對殘差模型的影響.2)在殘差正則化部分，參考文獻[13]的方法，將式(10)中的參數(shù)設置為λR=0.5，λT=1.5來設置空間正則化與時間正則化的權重.3)將式(11)中的λD設為0.7來計算穩(wěn)定且緩慢變化的平均背景深度.4)在跟蹤質(zhì)量檢測中，參考文獻[16]的方法，將前3幀的邊緣投影到當前幀進行質(zhì)量評估，設置式(18)中γj=[1,1,1.25,1.5]，其中j∈[0,3].以上參數(shù)為本次實驗中獲取的經(jīng)驗值，并非最優(yōu)參數(shù)，但在實驗中提供了良好的結果.

為了評估本文算法的精度，與現(xiàn)存的3種算法進行了比較：包括結合場景流的VO-SF[13]、基于邊緣對齊的REVO[16]以及文獻[19]的方法.以上算法都是基于RGB-D數(shù)據(jù)進行實驗，其中REVO僅利用邊緣信息進行位姿估計，且未處理動態(tài)目標，與其進行對比的原因在于評估算法在靜態(tài)環(huán)境下的精度以及效率.VO-SF與文獻[19]的方法都是目前動態(tài)環(huán)境下較為先進的視覺里程計算法.

圖4 信息權重

3.1 運動分割

運動分割用于剔除動態(tài)部分，使視覺里程計僅使用靜態(tài)部分計算相機位姿.如若分割錯誤，部分動態(tài)物體未被檢測而加入位姿計算，會導致相機與動態(tài)物體產(chǎn)生錯誤的相對運動而造成位姿估計誤差.若過度分割，將部分靜態(tài)物體剔除，會使得位姿估計中可用像素信息減少而增大位姿估計誤差.因此，正確的運動分割對于動態(tài)環(huán)境下的視覺里程計起到至關重要的作用.

在多種動態(tài)環(huán)境下采用VO-SF算法與本文算法進行運動分割比較實驗，結果見圖5.(a)為原始RGB圖，(b)為有聚類信息的邊緣圖，(c)為VO-SF算法運動分割結果，(d)為本文算法運動分割結果，深色為靜態(tài)部分，淺色為動態(tài)部分.(1)與(2)展示了動態(tài)物體在圖像像素占比較大的情況，因引入了豐富的邊緣信息則相比于VO-SF有更為準確的運動分割.引入的平均背景深度能輔助運動分割，從(3)與(4)可以看出，平均背景深度加大了遠離背景的物體的殘差，從而避免了(3)中動、靜態(tài)部分分割錯誤并在(4)中準確地檢測出了遠離相機的移動物體.(5)中椅子因靠近移動的人而被分割為動態(tài)物體，輕量的過度分割是合理的且不會影響整體的位姿估計.(6)與(7)分別為有效深度過少的圖像以及相機移動過快導致的模糊圖像，這兩種情況會因嚴重的信息丟失而導致較差的分割結果，但是引入邊緣信息使得本文算法在類似情況下仍優(yōu)于VO-SF.

圖5 運動分割

3.2 評估指標

相對位姿誤差(RPE)[21]用于測量每秒的平均漂移量，使用RPE對本文算法以及另外3種算法進行評估，分別在靜態(tài)與低動態(tài)環(huán)境和高動態(tài)環(huán)境下進行了實驗，文獻[19]采用該文獻部分原始數(shù)據(jù).實驗結果見表1，在靜態(tài)與低動態(tài)環(huán)境下，本文算法可能因場景過度分割而造成邊緣信息減少，但光度誤差能為位姿估計提供更多的約束，對比僅采用邊緣信息的REVO算法也擁有較好的性能，而對比VO-SF與文獻[19]的方法，本文算法明顯有更優(yōu)的性能.在高動態(tài)環(huán)境下，本文算法采用幀與關鍵幀的跟蹤方法避免非剛性物體造成的累積誤差，并利用平均背景深度來輔助場景分割.從表1中可看出本文算法在高動態(tài)環(huán)境下?lián)碛懈叩木龋啾萔O-SF平均精度提升了45%，相比文獻[19]平均精度提升了15%.圖6通過對REVO、VO-SF與本文算法在序列fr3/walking_xyz下進行了軌跡漂移比較，可以很直觀地看出本文算法精度高于其他算法.

表1 平移和旋轉(zhuǎn)均方根誤差

絕對軌跡誤差(ATE)[21]用于測量軌跡的整體質(zhì)量，使用ATE圖可以直觀地看出估計軌跡與真實軌跡的區(qū)別，結果見圖7.圖中,(a)為VO-SF的絕對軌跡誤差，(b)為本文算法的絕對軌跡誤差.從圖中可以看出，本文算法具有更小的軌跡誤差，驗證了本文算法改進的有效性.

圖6 fr3/walking_xyz下相對軌跡誤差

圖7 不同算法的絕對軌跡誤差

為了比較運行時間，在同樣的實驗環(huán)境，本文算法采用與其他算法相同的分辨率為320×240的QVGA圖像進行實驗.結果表明，在不采用GPU加速的前提下，REVO因僅采用邊緣信息且未對動態(tài)物體進行處理，因此計算效率較高，平均每幀運行時間為55 ms，VO-SF平均每幀運行時間為80 ms.因本文引入邊緣信息，并在插入關鍵幀的時候進行了更多的處理，因此運行時間比其他算法要較長，平均每幀運行時間為100 ms，基本滿足實時性.

4 結 論

本文提出了一種融合邊緣信息的稠密視覺里程計算法，將邊緣信息加入到運動分割與位姿估計中，并引入平均背景深度輔助運動分割，然后利用邊緣進行關鍵幀選擇.邊緣信息的融合使得算法在多種不同的環(huán)境下?lián)碛懈玫聂敯粜?，同時對于幾何紋理較少的環(huán)境，更多地采用光度結合深度信息來進行位姿估計，降低環(huán)境特征過少而導致的誤差.在TUM公共數(shù)據(jù)集上的實驗結果表明，本文算法在靜態(tài)環(huán)境下以及富有挑戰(zhàn)性的高動態(tài)環(huán)境下都能取得較好的結果，能夠檢測出距離相機較遠的動態(tài)部分，在信息丟失、圖像模糊等情況下也有更好的魯棒性.

視覺里程計算法在各種環(huán)境下不可避免地產(chǎn)生誤差，累積誤差會導致估計位姿偏離真實位姿.因此，下一步工作將融合IMU(慣性測量單元)數(shù)據(jù)，在動態(tài)環(huán)境下為位姿求解添加更多的約束項，用以降低位姿估計誤差，并考慮加入回環(huán)檢測以消除累積誤差，進一步提高算法精度.