融合深度信息的移動機器人跟蹤方法研究

潘榮敏，袁 杰，王宏偉，2，米 湯

1.新疆大學 電氣工程學院，烏魯木齊 830047

2.大連理工大學 控制科學與工程學院，遼寧 大連 116024

1 引言

視覺跟蹤技術廣泛應用在人機交互、自動駕駛、導航制導、無人機、工業機器人等領域，高效的跟蹤算法需解決跟蹤過程中由環境干擾引起的模型污染問題[1]，已成為該研究領域的熱點和難點。

在計算機視覺領域，DLT[2]和MDNet方法[3]分別在在OTB50[4]和OTB10[5]中表現出一定準確率，但深度學習模型的復雜性導致其幀率較低，即便是時效性較好的SimFC方法[6]也要依賴高配置GPU，難以滿足機器人實時應用。Henriques等提出的核相關濾波器（KCF）算法[7]繼承了MOOSE算法[8]的高速率特性，還表現出較高的精度，是眾多基于濾波器跟蹤算法的模板。作為該類算法的改進，SAMF[9]和DSST[10]方法通過融合多特征并利用尺度池或比例尺改進了目標尺度自適應的不足，LCT方法[11]借用重檢測的思想實現了較為穩定的跟蹤。但目標及場景動態變化引起的尺度框自適應和目標丟失問題使魯棒實時跟蹤仍然是一個挑戰性問題[12]。在眾多算法中，特征選取是一個跟蹤系統中重要的組成部分，合適的特征可以顯著提高跟蹤效果[13]。

在移動機器人域領，當行人的腿部特征變化時，采用激光測距儀和相機相結合進行行人跟蹤[14-16]的方法容易造成跟蹤失敗[17]?；诔暡y距的行人跟蹤方法[17-18]需要目標體在面向機器人側攜帶超聲波發射器，應用場景受限。受啟發于HOG特征[19]提出的HOD特征[20]和基于RGBD相機的行人檢測跟蹤策略[21]，均證明了利用場景深度信息進行目標跟蹤的高效性。

因為其高性能和高效率及易擴展的特性，KCF方法現已是眾多優良算法[9-11，22-24]的基準，成為跟蹤領域的主流框架。為實現一種實時穩定的機器人視覺跟蹤系統，本文在KCF方法的基礎上通過融合場景深度信息解決目標尺度框自適應問題及目標被遮擋時的漂移問題，并在移動機器人跟蹤應用中具有較好的跟蹤效果。

2 核相關濾波器

KCF算法在線更新一個滿足嶺回歸代價函數的相關濾波器θ，大小為W×H的樣本X及以其作為生成矩陣的循環矩陣Xw,h為濾波器的訓練樣本，其中(w,h)∈所有樣本是由其對應的圖像塊通過特征提取函數映射到特征空間下得到的特征矩陣。期望輸出y不是二分類的結果，是以跟蹤目標所在位置為中心高斯加權后的響應值。

式中φ是通過核相關函數κ映射得到的非線性特征空間，點積 φ(x)?φ(x')=κ( )x,x'。正則化系數λ大于0，可避免模型過擬合。

θ的解和循環矩陣X直接相關，由于循環矩陣可由離散傅里葉變換及其生成向量x對角化得到，所以，θ的解可被看作在非線性特征空間下樣本的線性組合，表示為：

在對偶空間下，最優化目標系數則由θ變為變量α。α的求解公式為：

式中，F和F-1分別表示離散傅里葉變換和離散傅里葉逆變換，Κxx表示κ(X,X)，即樣本X的核自相關。常用的高斯核函數為：

式中，^表示該矩陣的傅里葉變換。

應用中，當獲取圖像后，以上一幀目標位置為中心，裁剪一塊興趣區域作為目標位置的候選塊，并將此候選塊的特征矩陣Z輸入到前一幀訓練的模型，輸出 f(Z)的最高響應點位置即為目標區域的中心位置：

其中，ΚXZ表示X和Z的核相關。

模型參數α隨著興趣區域更新，但是很明顯的問題是當目標尺寸因距離或者形狀的改變而變化時跟蹤窗尺度是不變的，對后續目標位置確定產生影響[25]，又因缺少跟蹤失敗檢查，在跟蹤失敗后并沒有目標找回機制，在實際應用尤其是機器人應用中存在不足。圖1為基于KCF的方法在三維環境跟蹤示例中的過程表示，圖1的A中小框內書籍為n-1幀的識別結果，目標框尺寸為132×183像素。大框內的興趣塊尺寸為330×458像素，將其劃分為82×120個細胞，提取32維HOG特征并圖形化表示為圖1的B，利用此特征的頻域表示進行模型更新。圖1的C為n幀輸入，方框為根據n-1幀興趣區域得到的目標位置候選塊，提取其HOG特征并進行循環位移擴展樣本，其中基礎樣本的特征表示為圖1的D。通過模型匹配得到識別響應結果如圖1的E，目標中心位置像素坐標為（252，246），并將結果表示在n幀圖像候選塊中如圖1的F，尺寸為132×183像素，再將n幀識別結果映射到配準好的深度圖中得到圖1的G，黃色外框為映射得到的初始識別結果。最后通過深度信息對結果進行修正，得到綠色框目標識別結果，尺寸為105×146像素，映射到n幀RGB圖像中表示為圖1的H。

圖1 KCF方法在三維場景的應用

3 跟蹤系統設計

移動機器人在三維場景中跟蹤的系統設計主要在于實時的長時間跟蹤方法設計，為讓模型更好地適應不同特征空間下的目標特征變化，減少場景腐蝕，目標尺度框自適應、跟蹤失敗檢查以及跟蹤丟失后目標找回策略是重要工作。

3.1 目標尺度框估計

深度圖可以從背景中反映出目標的輪廓，且目標體本身的深度信息具有高度的自相似性[26]，跟蹤應用并不要求精準的目標邊緣輪廓，只考慮其邊緣范圍即可，故可以在深度圖中通過邊緣交叉搜索（Cross-Searching Edge，CSE）的方法進行目標尺度框估計。

在像素坐標系下，任意點( )x,y處的梯度求解公式為：

以KCF算法的回歸器得到的最大響應點為起始點，沿X負方向計算每點的X方向梯度值，每點的梯度值和邊緣閾值比較，超過閾值的點記為(x-m,y)，認為其是目標左側邊界點，然后再從起始點沿X正方向計算每點X方向梯度得到目標的右側邊界點(x+n,y)。然后按照式（5）進行目標中點矯正，接著中點開始，分別再沿Y正方向和負方向進行各點Y方向的梯度計算，估計出目標Y方向的尺度邊界點。

為加快計算速度，在按照上述方法搜索邊緣點時可隔點進行梯度計算，這樣即對計算結果影響不大又可節省一半的計算時間。如圖2為本文方法示意，深色像素點為需要計算梯度的點，其中點A為初始起始點，目標中點矯正后得到點B，箭頭所指為梯度取點計算方向，點1、2、3、4分別為該方法依次得到的邊界點，最終確定紅色的目標框。

圖2 邊緣交叉搜索

由于本文方法對目標體輪廓的局部特征敏感，所估計出的目標體尺度可能會偏小，在應用中，會對估計所得目標框的長寬按照一定的系數進行適當放大。此外，由于紅外光源為點狀衍射導致場景深度圖有陰影、系統內外環境不理想，深度圖像受高斯噪聲、椒鹽噪聲等干擾，而本文方法對像素質量依賴性強，需進行圖像降噪處理。

為平滑圖像、消除正態分布噪聲，采用二維零均值離散高斯函數對圖像進行平滑濾波處理，二維高斯函數為：

式中，G(x ,y)為二維高斯函數點(x ,y )處的值，σ1、σ2為X和Y方向的方差。圖像濾波的過程是高斯核和圖像做卷積的過程。高斯核是一個長寬均為正奇數的矩形模板，在此應用中取其為一個方形模板，X和Y的方差相等，均為σ，則該高斯核中每個元素的計算公式為：

其中，Ki,j為( )i,j位置的元素值，高斯核元素位置的確定是以中心處元素位置為原點、i軸水平向右為正方向、j軸水平向上為正方向的坐標系，2t-1為方形高斯核的寬度。為適應目標圖像各像素點的數據類型，提高濾波效率，現將得到的高斯核元素整數化，整數化過程為：首先將首項歸一化，即各元素乘以模板系數和的倒數；然后對各項進行取整；最后在得到的模板前加各整數元素和的倒數即為整數類型高斯核。

圖像中的椒鹽噪聲通常由脈沖響應引起，在深度圖中一般表現為數字化極大或極小值，即純黑或純白，對應場景含義為極近或極遠。而深度相機的有效使用距離為0.6～8 m，故對圖像中異點附近的所有點進行剔除，即可避免椒鹽噪聲的影響。降噪效果如圖3所示，圖（a）為降噪前深度圖，存在許多椒鹽噪點和高斯噪點，圖（b）為降噪后深度圖，減少了噪點影響，平滑了圖像。

圖3 降噪處理

為進一步降低噪點對邊緣檢測的影響，增加梯度值的可信度，可將目標點及臨近點的梯度和作為判斷目標點梯度大小的數值依據，即在計算X方向梯度時，將目標點(x,y)及其左右鄰點的梯度和作為(x,y)處的梯度依據，這即是Prewitt算子。為體現目標點的重要性，將目標點梯度權重加大，即為Sobel算子：

其中，Sx、Sy分別為X方向和Y方向的Sobel算子，I為圖像點z5附近的圖像塊，則在Sobel算子下z5處的梯度可為：

如圖4所示，上方3張為場景的RGB信息圖像，下方3張為對應的場景深度信息圖像，圖4（a）為場景原圖，圖4（b）、（c）分別為對應的X方向梯度圖和Y方向梯度圖，很直觀地可以看出在深度圖下搜索物體邊界要比RGB圖下更簡單。將圖4中行人認為目標點，圖4（a）中藍色點位置為目標中心位置，在深度圖中以目標中心點的X方向邊界搜索為例，得到圖5（a）該點處X方向像素值分布，圖5（b）為圖5（a）各點對應的X方向梯度值，可以看出在物體邊界處存在梯度峰值。應用中，并不需要將Sobel算子與圖像做卷積運算，只需以目標響應點為起點按照CSE方法進行邊界點檢測，減少了計算量。目標框估計效果如圖6所示，黃色框為初始目標框，在深度圖6的A中利用本方法估計得到綠色框所示目標框，并將其映射到RGB圖像中得到圖6的B。本文方法不依賴目標體HOG、紋理、形狀等特征，故理論上對剛體和非剛體在顏色、光線、幾何特征變化時的尺度自適應都是有效的。

圖4 梯度圖像

圖5 目標中心點X方向像素及梯度

圖6 目標尺度框估計

3.2 跟蹤失敗檢查

由于相關濾波器本身就是模板類的方法，不管對樣本采用何種特征提取方法，只要在該特征空間下的樣本表示發生較大的改變都會對跟蹤效果造成嚴重影響，比如，相關濾波器類方法對采用HOG特征模型下的目標要求不能有快速變形、采用CN顏色特征模型下的目標不能有強烈的光線和顏色變化。在3D環境中短時間內物體深度信息的變化不會特別劇烈，而當目標被遮擋時，遮擋位置處的深度信息會發生突變，因此提出通過軸向相對動能波動（Fluctuations in Axial Relative Kinetic Energy，FARKE）進行跟蹤失敗檢查，FARKE定義式如下：

其中，fe表示軸向相對動能波動的值，D表示物體到相機的相對距離，n+1為當前相關濾波器響應出的目標中心位置，n為前一幀圖像中目標中心位置。tn代表n-1到n之間目標丟失的圖像幀數，若跟蹤正常時tn為1。當目標被遮擋時，fe值會發生明顯波動，設定閾值ξ，當fe>ξ時認為目標丟失或被遮擋，ξ與目標體的縱向尺寸及移動機器人相對目標體的速度有關。如果令圖像相鄰兩幀間的更新時間為1，則式（9）涵義為相鄰幀間目標在軸向方向相對相機的動能比。因為考慮到了目標早先的深度，相比于僅檢測目標位置深度變化量的方法[27]采用動能比的方法更適合快速運動的目標。

fe響應值超出閾值時，為保護模型不被當前幀的遮擋物或環境中其他干擾污染，模型停止更新。

3.3 歷史模板池

將通過FARKE檢查的圖像對應的目標模型保存到歷史模板池，在目標跟蹤模型被污染后，利用目標面積變化率估計的方法估計目標位移，然后計算取樣間隔并從歷史模板池抽取模板作為新的目標跟蹤模型。目標面積變化率估計方法是一種基于目標框像素數的方法，假設模板池中各模板對應的目標模型準確，且目標體在相鄰幀間的位移和形變是連續的即目標被遮擋的過程至少反應在連續的兩幀圖像中。

式中，ra是目標面積變換率估計值，Sn為第n幀中目標框的面積，a為取樣幀數，可參照相機拍照幀率取值，為第n幀目標框的左邊緣像素點的X軸坐標，代表右邊緣像素點X軸坐標，t代表最終取樣間隔幀數，Smax=max{ }Sn-a,Sn-a+1,…,Sn-1,Sn為取樣內目標面積的最大值。在目標被遮擋或其他環境干擾引起目標漂移時，FARKE識別出目標丟失，丟棄當前幀對應的跟蹤模型，歷史模板池作為待取模型庫，根據目標面積變化率方法計算所得間隔t，從中選取第n-t幀對應的模型來更新第n+1幀模型參數。

3.4 搜索策略

當目標被大面積物體遮擋再出現后，由于位移發生較大變化，在當前搜索區域可能不會檢測到目標，因此根據目標被遮擋時間在一定范圍內擴大搜索區域可確保目標跟蹤的準確性。搜索策略為：

式中，pad是搜索區域相對目標框尺寸的倍率，初始值為1.5；α是調控系數，本文取0.8。當丟失目標找回后，pad恢復初始值。

如圖7所示為應用相對動能波動進行目標跟蹤失敗檢查的過程，上面為遮擋過程中的RGB圖像，下面為對應的深度圖，目標跟蹤結果用綠色框標識。第一組的第657幀圖像，目標尺寸面積63×241，距離2.86 m，第二組為目標被遮擋前一幀的第658幀圖像，目標尺寸31×192，距離2.96 m，之后目標被遮擋，遮擋物距離為1.73 m，如第三組圖所示。設置閾值ξ=16.5，計算出fe=151.29大于閾值，目標丟失，停止模型更新。在目標丟失過程中，令取樣幀數a=3，得Smax=15 183，取樣間隔t=5，即取目標丟失時的前第5幀模型更新跟蹤器。隨著丟失幀數增加不斷擴大搜索區域，在第四組圖即第676幀目標重新出現時，目標已丟失17幀，搜索區域擴大到丟失前目標框區域的20倍范圍，跟蹤器檢測到目標，目標距離3.12 m，計算 fe=2.56小于閾值，通過跟蹤失敗檢查，目標被重新跟蹤。

圖7 跟蹤失敗檢查

3.5 跟蹤系統流程

綜上所述，由識別算法或人工框選初始化跟蹤目標后，本文跟蹤機器人系統的流程如圖8所示，圖中綠色箭頭線代表KCF方法處理進程，其余橘黃色為本文方法實現。

4 實驗

為驗證所提方法的有效性，在對RGBD相機進行標定和配準后首先完成場景數據采集，然后在數據集上進行規則和不規則幾何特征物體形變下的跟蹤以及目標體在運動和不運動時被遮擋條件下的跟蹤；之后，完成了移動機器人行人跟蹤實驗。

圖8 跟蹤機器人系統流程

4.1 尺度框自適應

在圖9圖像序列中，目標體為綠色書籍，由于相對相機軸向方向運動故其在圖像不同幀中尺度差異較大。實驗表明，KCF方法不能準確適應由距離變化引起的目標尺度變化，而本文方法則有效地解決了這類問題，識別結果能夠很好地適應目標尺度的變化，避免了跟蹤窗口包含過多環境噪聲，有效調整了目標搜索區域。

圖9 距離自適應

在圖10圖像序列中，目標體為黃色書籍，由于旋轉其在圖像中的幾何特征發生變化。KCF方法識別結果不能很好適應目標體變化的幾何特征，本文方法的識別結果在這種變化中具有穩定的適應性，能隨目標變化自適應尺度框大小和位置。在圖序后兩幀圖中，目標體局部被白色障礙物遮擋干擾，本文方法仍表現出魯棒的跟蹤效果。

為進一步說明，在圖11圖像序列中，不規則外形的紅色玩偶為目標體，其在不同距離和姿態下具有差別較大的外觀特征。KCF識別效果沒有準確地適應目標體的變化，甚至在一段時間后目標識別框漂移到了大部分圖像空間，失去了跟蹤結果的有效性，本文方法則表現出較強的可靠性，面對這種變化能夠準確穩定地做出目標框調整。

4.2 目標遮擋

在圖12圖像序列中，在靜止目標黑衣少年被路人完全遮擋并短暫消失的情況下，KCF算法受到路人干擾，模型更新錯誤，目標漂移到路人上面，跟蹤失敗，本文方法能夠在被遮擋時停止模型更新，在遮擋物離開目標又出現后，仍能繼續搜索到目標體，繼續跟蹤。

在圖13所示視頻序列中，目標體遠處少年在運動中被近處靜止的黑衣路人遮擋并消失。KCF方法不能適應目標遮擋的情況，本文方法能夠在移動目標被遮擋再出現后仍可繼續準確檢測到目標位置，完成長間跟蹤。

圖10 形變自適應

圖11 不規則目標運動自適應

圖12 靜止目標被遮擋

圖13 運動目標被遮擋

4.3 目標跟蹤系統在移動機器人上的應用

4.3.1 機器人系統簡介

圖14為本文實驗采用的移動機器人平臺，搭載處理器為Core i5-3230M，主頻2.6 GHz，內存8.0 GB的計算機，依賴Ubuntu14.4下的Indigo版本機器人操作系統（ROS），相機采用奧比中光Astra Pro。本文跟蹤算法處理速度能夠保持在30 FPS以上。圖15（a）為Gazebo仿真實驗環境界面，圖15（b）為真實實驗環境。

圖14 移動機器人

圖15 機器人跟蹤實驗

4.3.2 實驗效果

IR鏡頭與物體需保持至少0.6 m的距離獲取的信息才可靠，并且實際應用中近距離條件下獲取的圖像中不能完整包括目標體完整特征，且目標在相機徑向方向運動時由于相對角速度過大容易引起機器人姿態調整不及時從而丟失目標。為保證在使用過程中穩定可靠，仿真實驗和實物驗證實驗都使跟蹤機器人和目標體保持最少1.5 m的距離，因此在曲線運動時二者轉彎半徑不同，兩者軌跡重合度降低，但移動機器人的運動軌跡重合度并不等價于跟蹤方法的精度和魯棒性。圖16為傳統KCF方法跟蹤軌跡，在跟蹤過程中即使目標簡單運動也很快發生目標漂移，跟蹤失敗，效果不穩定。圖17（a）和（b）分別為本文方法在仿真和真實環境下的跟蹤軌跡，移動機器人能夠長時間保持對目標的有效跟蹤。為進一步驗證本方法在機器人跟蹤應用的穩定性，分別在無行人遮擋和有行人遮擋以及目標體直線運動和做轉彎動作條件下對本文方法和KCF方法進行實驗統計，實驗結果如表1。從整體效果來看，本文跟蹤方法表現穩定，機器人能夠保持實時準確的動作響應，說明本跟蹤方法對運動目標體具有可靠的跟蹤能力。

表1 機器人跟蹤實驗結果

5 結論

圖16 KCF方法跟蹤軌跡圖

圖17 本文方法在仿真和真實環境下的跟蹤軌跡圖

本文基于核相關濾波器提出了融合場景深度信息的機器人跟蹤系統。為解決目標框自適應問題，實現了融合場景深度信息的邊緣交叉搜索（CSE）方法，在保證實時性的同時避免了跟蹤窗口包含過多的環境信息。針對目標在復雜環境下容易被遮擋發生目標漂移的現象，提出了基于場景深度信息的軸向相對動能波動（FARKE）跟蹤失敗檢測方法，能夠快速檢測出目標是否被遮擋或發生目標漂移；同時根據目標尺度變化構建了含有歷史模板池的模型自更新策略，為適當擴大搜索范圍建立了目標丟失恢復機制，提高了復雜環境下跟蹤的魯棒性。實驗結果證明，本文方法可以有效可靠地應用在3D場景及移動機器人目標跟蹤中。