基于視覺跟蹤與自主導航的移動機器人目標跟隨系統

張 瑞，蔣婉玥

（青島大學 自動化學院未來研究院，山東 青島 266071）

近年來，越來越多的機器人參與到人類的生產和生活中。在許多場景，要求機器人能夠在復雜環境中跟隨目標移動，如跟隨購物車、高爾夫球車等[1-2]，這些都是跟隨技術的應用實例。

目前已有多種基于傳感器的目標跟隨方法[3-4]。通過激光雷達、相機、UWB（ultra wide band，超寬帶）技術等實現對目標位置的感知，并根據目標位置設計機器人導航與控制算法。如：Ferrer等[5]通過激光雷達對目標進行定位，利用SFM（social force mode，社會力模型）量化行人運動意圖，使機器人能夠并排跟隨目標行人；Chung等[6]通過激光點云獲得行人腿部模型，分析了行人腿部運動，得到目標行人的位置，但僅憑點云數據很難區分不同的行人；Ling 等[7]提出了一種通過檢測反射材料來跟隨行人的方法。

在視覺跟隨及視覺目標跟蹤方面，也已有不少的研究成果。視覺跟隨研究方面：Cheng等[8]提出了一種通過檢測目標行人的上身和腳實現跟隨的方法；Sun等[9]開發了一種基于手勢的方法，機器人可以根據手勢完成不同的任務；Koide等[10]提出了一種結合身高和顏色特征來識別目標行人的方法，然而這些身體信息很容易受到目標行人姿勢和步態的干擾；余鐸等[11]通過將快速判別尺度空間的算法與卡爾曼濾波算法進行切換來實現穩定跟隨；萬琴等[12]采用改進的卡爾曼濾波器來預測目標的狀態，并通過數據關聯進行目標匹配，利用多目標跟蹤并通過標定目標人的ID（identity document，身份標識號）來跟隨。視覺目標跟蹤研究方面：Henriques 等[13-14]提出了CSK （circulant structure of tracking-by-detec‐tion with kernels，核檢測跟蹤的循環結構）算法，利用循環矩陣進行圖像密集采樣，提高了跟蹤精度，隨后又拓展了多特征通道，提出了KCF（kernelized correlation filters，核相關濾波）算法；Danelljan等[15]優化了平移檢測和尺度檢測濾波，提出了FDSST（fast discriminative scale space tracking，快速判別尺度空間跟蹤）算法；Bertinetto等[16]將孿生網絡應用于跟蹤，提出了SiamFC（fully-convolutional Sia‐mese network，全卷積孿生網絡）算法；Li等[17-18]在SiamFC 算法的基礎上加入區域候選網絡，提出了SiamRPN（Siamese region proposal network，孿生候選區域生成網絡）算法，后又提出了SiamRPN++算法，采用空間感知采樣策略，在目標跟蹤時采用深層網絡進行特征提取。上述研究考慮了人的外觀特征，而當場景中有外觀相似的行人時則不易區分，在機器人跟隨目標行人時會出現跟蹤失敗的問題。

本文提出了一種基于深度相機的移動機器人目標跟隨方法，通過視覺跟蹤和自主導航實現機器人跟隨社交目標移動。常規的目標跟蹤算法僅能解決目標在短時間內被遮擋的問題；當目標在走廊拐角處轉彎后不再出現在機器人視野內時，就需要機器人主動轉彎，改變相機的視野。本文將機器人跟隨問題分為目標在機器人視野內和目標消失兩種情況。針對前者，將基于卡爾曼濾波的運動信息處理方法與基于特征網絡提取的外觀特征信息處理方法相結合，提高目標識別的精度；針對后者，根據目標消失前與機器人的相對位置，利用自主導航使機器人移動到目標消失位置附近進行搜索，提高對目標跟隨的成功率。本文采用OTB100公開測試集和機器人應用場景下的跟隨測試集進行視覺跟蹤效果評估，并在機器人平臺上進行跟隨實驗，來驗證機器人在室內外行人眾多的環境中對目標行人的跟隨性能。

1 移動機器人結構及跟隨系統框架

移動機器人主要由視覺跟蹤定位模塊和機器人跟隨控制模塊組成，如圖1所示。視覺跟蹤定位模塊通過深度相機獲取機器人跟隨所需的RGB視頻序列和深度視頻序列。外置主機是視覺跟蹤定位信息的處理器，用來處理深度相機獲取的視頻序列，確定目標與機器人的相對位置。移動機器人平臺為由麥克納姆輪驅動的ROS（robot operating system, 機器人操作系統）平臺，搭載Jetson TX1 控制主板，其通過控制算法在該平臺上運行。

圖1 移動機器人結構Fig.1 Structure of mobile robot

移動機器人目標跟隨系統框架如圖2所示。深度相機獲取RGB視頻序列和深度視頻序列；通過目標檢測及目標跟蹤在RGB視頻序列中找出目標，深度視頻序列用來確定目標與相機的相對位置；若跟蹤成功，根據機器人與目標的相對位置，控制機器人進行跟隨；若在視覺跟蹤過程中目標長時間消失，則認定跟蹤失敗，此時啟動自主導航，將目標消失前的相對位置轉化為世界坐標系中的坐標，機器人移動到目標消失的位置去主動尋找。本方法可以有效解決在視覺跟蹤過程中目標長時間消失或被遮擋等問題，較好地處理在機器人跟隨過程中目標在走廊拐角、門口處轉彎而消失的情況。

圖2 移動機器人目標跟隨系統框架Fig.2 Framework of mobile robot target following system

2 視覺跟蹤定位

視覺跟蹤定位包括目標檢測與識別、目標跟蹤和目標定位三部分。首先，由深度相機獲取視頻的幀圖像，利用YOLOv5s 進行行人檢測，結合人臉識別判定目標是否位于機器人視野范圍內；其次，對跟蹤算法進行初始化，并提取目標的運動狀態和外觀特征，在后續幀中對目標進行跟蹤，并對跟蹤成功的目標進行定位；最后，輸出目標與機器人的相對位置信息。

2.1 目標檢測與識別

在實驗中手動框選跟蹤目標是不方便的，因此在運行跟蹤算法前加入人臉識別，且只在第1幀或者前幾幀進行人臉識別，一旦檢測到目標進入機器人視野，則進行目標跟蹤。采用已經訓練好的檢測模型YOLOv5s作為行人檢測器[19]。該模型運行速度快、識別精度高，可以很好地檢測到目標行人。人臉識別算法采用人臉識別庫face_recognition，其在人臉數據集Labeled Faces in the Wild 測試中有99.38%的準確率[20]。

在第1幀中，將人臉識別框和YOLOv5s檢測框進行IOU（intersection over union，交并比）匹配，IOU距離最小的檢測框為目標框。IOU距離可以表示為：

式中：Βface=[x1y1x2y2]，Βi=[xi1yi1xi2yi2]，其元素分別為人臉檢測框和第i個行人檢測框的左上角以及右下角在圖像坐標系中的坐標。

2.2 目標跟蹤

目標跟蹤流程如圖3所示。當目標出現時，基于上述的目標檢測與識別方法可以得到目標框；提取目標框的圖像特征以及位置信息，將該目標框初始化為一個目標軌跡T=[xyrhsF]，其中包含目標邊界框中心位置(x,y)、目標邊界框寬高比r、高度h、當前軌跡經過卡爾曼濾波器預測的次數s以及目標的外觀特征庫F；在第t幀，通過卡爾曼濾波器預測當前幀的目標位置，并將預測次數s加1，將YOLOv5s檢測到的行人框與預測后的軌跡進行關聯匹配；若匹配目標成功，更新目標軌跡的外觀特征庫F、卡爾曼濾波的參數，并將卡爾曼預測次數s置零；若目標匹配不成功，目標軌跡T不進行更新，當預測次數大于消失閾值，認定目標長時間消失，則進行自主導航；當檢測框與目標軌跡再次成功匹配后，關閉自主導航。

圖3 目標跟蹤流程Fig.3 Target tracking process

2.2.1 卡爾曼濾波

本文采用具有恒定速度模型的標準卡爾曼濾波器，假定行人的運動速度是恒定的。跟蹤場景在八維狀態變量上定義，該變量包含目標的4個位置狀態變量及其在各自圖像坐標系中的變化速度，其中將邊界框狀態zt=[xyrh]Τ作為目標狀態的直接觀測值。

狀態預測：

狀態更新：

式中：I為8×8型單位矩陣；分別為t時刻的狀態預測值和t-1時刻的狀態最優估計值，分別為t時刻的先驗估計協方差矩陣和t-1時刻的后驗估計協方差矩陣，A為8×8型狀態轉移矩陣，Q為8×8 型過程噪聲矩陣，R為4×4 型測量噪聲矩陣，H為4×8型狀態觀測矩陣，Κt為卡爾曼增益。

2.2.2 關聯匹配

關聯匹配模塊解決了卡爾曼預測的目標軌跡與新一幀檢測量之間的關聯問題。通過余弦距離和馬氏距離來整合外觀信息和運動信息。

針對外觀信息，用余弦距離來度量。利用訓練好的特征提取網絡提取第i個行人檢測框的外觀特征fi以及目標軌跡的外觀特征庫F=[f1f2…f100]。第i個檢測框與目標軌跡的最小余弦距離為：

式中：fΤi?fj為第i個檢測框特征與目標軌跡第j個特征的余弦相似度。

當有外觀相似的行人時，僅根據余弦距離不能進行準確區分而造成跟蹤漂移，因此須用運動信息進行衡量。為了結合運動信息，采用預測的卡爾曼狀態與新一幀檢測量之間的馬氏距離：

式中：Di為第i個檢測框的狀態向量，為由卡爾曼預測狀態向量映射的4×1 型可觀測的狀態向量，為映射的4×4型協方差矩陣。

這2 種度量可以在不同方面相互補充。一方面，馬氏距離提供了目標基于運動的位置信息，有助于短期預測；另一方面，余弦距離考慮了外觀信息，這些信息對于目標被遮擋或相機抖動后的運動信息不具有辨別力，但有助于對恢復的目標進行跟蹤。設置權重λ加權這2 個度量值，得到最優關聯值d：

為了避免目標被遮擋而導致關聯錯誤，加入一個閾值δ來衡量關聯值d是否可以接受。若d≤δ，則認為關聯成功。

目標軌跡與檢測框匹配成功后，須對目標軌跡的外觀特征庫F進行更新。將匹配成功的檢測框的外觀特征替換掉與其外觀特征相似度最高的目標軌跡的特征fj。

外觀特征的提取采用ResNet殘差網絡，其結構如圖4 所示。輸入模型的尺寸為128×64×3，經過步長為1，64 個3×3 的卷積核卷積并最大池化后得到64×32×64的輸出，然后經過16個普通殘差塊卷積后得到8×4×512 的輸出，經自適應平均池化后進行全連接層分類。在目標跟蹤方法中，采用該特征提取網絡時不需要全連接層分類，在自適應平均池化后對該數據進行歸一化，得到512維的特征，再進行余弦距離計算。對該網絡在行人重識別的Market-1501數據集上進行訓練。該數據集包括由6個攝像頭拍攝到的1 502個行人、32 668個檢測到的行人矩形框。在訓練過程中，學習率衰減采用余弦退火方法。驗證集中分類概率最高類別的準確率（top-1 accuracy）為89.348%。

圖4 外觀特征提取網絡的結構Fig.4 Structure of appearance feature extraction network

2.3 目標定位

為了完成跟隨行人的任務，機器人須知道目標的位置信息。根據跟蹤檢測結果，通過目標矩形框中心點的坐標來計算機器人與目標之間的相對距離。為了減小計算量，根據中心像素的位置確定搜索框，然后從搜索框中選擇40個搜索點進行測距，并對搜索點的有效值進行中位值平均濾波以獲得目標的距離。

根據目標的最優狀態向量，確定搜索框的邊長as為：

則搜索框狀態向量為[xyasas]，搜索點為Ps(x+ε,x+ε)，其中ε為偏差，。

通過測試可知，深度相機的深度視野約為60°，圖像分辨率為（640×480）像素。將機器人的正前方設為0°，左側為負角度，右側為正角度。將相機視野與圖像大小線性映射，可以算出機器人與目標之間的相對角度。設xpi為目標中心的橫坐標，則相對角度α為：

深度相機測得的目標距離和角度如圖5所示。

圖5 深度相機測得的目標距離和角度示意Fig.5 Schematic of distance and angle of target measured by depth camera

3 機器人跟隨控制

機器人跟隨控制系統是在帶有Jetson TX1控制主板的ROS平臺上運行的。通過局域網接收視覺跟蹤定位系統傳輸的機器人與目標的相對距離、相對角度以及目標是否長時間消失的標志位。根據目標是否長時間消失，進行常規跟隨和自主導航兩種處理。

3.1 常規跟隨

在目標長時間消失或被遮擋前，根據機器人與目標的相對距離d(t)、相對角度α(t)，采用PID（proportion-integral-derivative，比例?積分?微分）控制器求解機器人線速度v及角速度ω，令c=[vω]Τ。

式中：kp、ki、kd為PID控制參數；e(t)為t時刻機器人與目標期望位置的誤差；d?為期望的跟隨距離，d?=1 m；α?為期望的跟隨角度，α?=0°。

3.2 自主導航

當目標消失后，將目標消失前的位置保存下來，同時機器人停止運動，等待目標出現。當目標消失30幀后，認定目標長時間消失，則啟動自主導航，機器人主動尋找目標。其路徑規劃采用A*算法。

3.2.1 坐標轉換

2.3 節中定位的目標位置即為機器人與目標的相對位置，在機器人自主導航時須將其轉化為世界坐標系中的坐標。將相機坐標系看作機器人坐標系，則機器人坐標系與世界坐標系的轉換如圖6所示。

圖6 機器人坐標系與世界坐標系的轉換Fig.6 Transformation of robot coordinate system and world coordinate system

世界坐標系為XmOmYm，機器人坐標系為XrOrYr，機器人坐標系與世界坐標系的偏轉角為θ。機器人在世界坐標系中的坐標為Or(xr,yr)，目標在機器人坐標系中的坐標為P(xpr,ypr)，即：

則目標在世界坐標系中的坐標為：

3.2.2 機器人朝向

根據目標在機器人左前方還是右前方消失決定機器人移動到目標位置時的朝向。例如，目標在機器人左前方消失，那么機器人最終朝向是左轉90°。

在實際運行過程中，相機檢測到目標消失前的最后一幀時，因為機器人運動的滯后性，機器人會根據目標消失前檢測到的位置轉動，從而造成目標定位偏差，如圖7所示。目標消失位置為P1，機器人坐標系為XrOrYr。機器人根據目標位置轉動，此時機器人坐標系為。若根據目標消失時的相對距離以及相對角度，目標將會被定位到世界坐標中的P2。如果目標位置被定位到障礙物，會導致路徑規劃失敗，所以需要對目標消失時的世界坐標進行角度補償。

圖7 機器人偏移后目標定位偏差Fig.7 Target positioning deviation after robot offset

根據式(15)，可得：

目標消失時機器人的朝向可以通過機器人發布的Odom的四元數(x,y,z,w)求出：

機器人最終的朝向可表示為：

最終，將得到的歐拉角轉換為四元數(x,y,z,w)發布。因為機器人只有偏航，不涉及俯仰和翻轉，故只求z和w:

4 移動機器人目標跟隨實驗

移動機器人視覺跟蹤定位系統采用搭載了RTX2060 GPU 的主機和Realsense D435 深度相機，其中CUDA （computer unified device architecture，計算機統一設備架構）版本為11.6，深度相機的分辨率為（640×480）像素，目標跟蹤與定位算法運行速度為30幀/s，能夠滿足實時性要求。機器人搭載了Jetson TX1控制主板、四輪麥克納姆輪，通過ROS操作系統進行控制，平均移動速度為0.8 m/s。

4.1 目標跟蹤實驗

首先在OTB100公開測試集上進行測試和評估。由于本文所提出的跟蹤算法適用于行人跟蹤，選取了OTB100測試集上的行人序列。

將本文算法的參數選為表1中的參數組1。當最優關聯值小于0.175 時，認定目標關聯成功，則更新卡爾曼濾波器以及目標的外觀特征庫F。當成功關聯的檢測框的外觀特征余弦距離小于0.15，則將其更新到特征庫F中。由于在測試集上評估，無法實現目標丟失后自主導航，故當目標丟失30幀后，將λ置為0。

表1 本文算法的參數取值Table 1 Parameter values for the algorithm in this paper

采用精確度和成功率兩個性能指標對算法進行評估。精確度是算法運行后輸出的目標邊界框中心點與真實框中心點的距離小于給定閾值的幀數占總幀數的百分比。繪制不同中心誤差閾值下的精確度曲線，并將閾值為20像素所對應的精確度作為代表性的評價指標。成功率是目標邊界框與真實框的區域重疊面積比大于給定閾值的幀數占總幀數的百分比，其取值范圍為0～1，因此可以繪制出重疊率閾值從0到1時的成功率曲線。

采用OTB100 數據集對本文算法與KCF[14]、SiamFC[16]、SiamR-CNN（Siamese re-detection convolu‐tional neural network，孿生重檢測卷積神經網絡）[21]，TLD（tracking-learning-detection，跟蹤學習檢測）[22]、DSST（discriminative scale space tracker，判別尺度空間跟蹤）[23]、SRDCF（spatially regularized discrimina‐tive correlation filters，空間正則判別相關濾波）[24]等其他7 種算法進行比較，結果如圖8 所示。由圖可知，在OTB100數據集上測試時，本文算法的性能稍差于SiamRPN++算法，這是由于結合卡爾曼濾波后目標行人的肢體快速移動不會迅速影響跟蹤框的位置，跟蹤框主要集中在人體的軀干部分，導致一些視頻的幀目標框與準確框的重疊面積比不夠高。因此，當重疊率閾值為0.4～0.8 時，成功率下降較快。同時，本文算法明顯優于其他6種算法，能夠較快速地測到行人位置并識別目標，在目標被遮擋后具有更好的重識別能力。減小λ，可以有效處理相機劇烈晃動的情況。對于目標被遮擋，特別是當目標與其他行人重合時，可以通過減小δ和μ來解決。

圖8 不同算法在OTB100數據集中的跟蹤性能Fig.8 Tracking performance of different algorithms in OTB100 dataset

在OTB100 評估集上，測試序列大多在監控視角下，而缺少相機與行人近距離同時運動的情況。為了進一步評估本文算法的跟蹤性能，作者在室外不同環境中收集了2個數據流。在黑暗環境中，光照有變化，目標有遮擋；在明亮環境中，背景復雜，相似行人多，干擾因素也較多。在收集的數據流中，圖像的分辨率為（1 280×720）像素。采取本文算法對數據流中的目標進行跟蹤，結果如圖9所示。由圖可知，在不同的照明條件下，采用本文算法均可以檢測和定位目標。

圖9 本文算法對數據流中目標的跟蹤效果Fig.9 Tracking effect of the algorithm in this paper on the target in the data stream

從對上述2個數據流的測試結果可以看出，本文算法在行人跟蹤方面有更好的表現。這主要源于以下兩方面：其一，基于YOLOv5s 的檢測器可以準確檢測復雜環境中的人；其二，當目標消失后，得益于算法的目標跟蹤機制，機器人可以準確地重識別目標。將基于卡爾曼濾波預測的運動信息與基于ResNet網絡行人重識別的外觀特征相結合來重識別目標，提高了重識別的準確性和目標跟蹤的穩定性。

此外，為了進一步驗證本文算法的優勢，將本文算法的參數選為表1中的參數組2，采用收集的數據流對本文算法與SiamRPN[17]、SiamRPN++[18]和SiamBAN（Siamese box adaptive network，孿生自適應框網絡）[25]等其他3 種算法進行對比實驗，結果如圖10所示。對于用于比較實驗的數據流，通過Labelimg標注工具手動標記每幀中目標行人的邊界框來獲得真實標簽（ground truth）。從2 個指標來看，本文算法明顯優于其他算法。當場景中有相似行人時，其他方法僅通過外觀特征很難在多個相似行人中跟蹤到目標。本文算法加入了運動信息，可以有效解決出現外觀相似行人后目標切換的問題。從整體上看，本文算法對復雜環境具有更好的適應性，對目標的重識別具有更高的準確性。

圖10 不同算法在收集的數據流中的跟蹤性能Fig.10 Tracking performance of different algorithms in the collected data stream

4.2 機器人目標跟隨實驗

為了在實際的機器人平臺上評估本文算法的性能，在室內環境中進行了目標跟隨實驗。在機器人平臺上安裝了一臺深度相機作為感知傳感器。設定的目標行走路徑如圖11 所示，機器人相機視角的跟隨效果如圖12 所示，移動機器人跟隨效果如13 所示。特別地，當目標消失后，機器人自主導航機制被激活，從而尋找目標并繼續跟隨，如圖14所示。

圖11 設定的目標行走路徑Fig.11 Predefined target walking path

圖12 機器人相機視角的跟隨效果Fig.12 Following effect from robot camera's perspective

圖13 移動機器人跟隨效果Fig.13 Mobile robot following effect

圖14 機器人尋找消失的目標Fig.14 Robot looking for the disappearing target

目標長時間消失后，機器人自主尋找目標，如圖12(c)和圖13(c)所示。當目標再次出現在機器人視野內時，其仍能跟蹤到目標并跟隨。在跟隨過程中，機器人始終可以穩定地跟隨目標，且處理圖片的平均速率在30幀/s以上。實驗表明，采用本文算法可以實現對目標的穩定、實時跟蹤。

由圖14可知，目標消失后，機器人能夠準確地確定目標消失時的位置及方位，可有效解決目標消失后的跟隨問題。

5 結 論

本文提出了一種基于視覺傳感器的目標跟隨方法。采用YoloV5s檢測所有行人，通過人臉識別模塊確定目標的位置。利用特征提取網絡提取目標特征，創建特征庫，采用卡爾曼濾波器對目標進行運動信息的預測，并結合運動信息和外觀特征跟蹤目標。此外，當機器人長時間捕捉不到目標時，通過自主導航移動到目標消失的位置尋找目標。將本文算法在OTB100數據集、收集的測試集中與其他算法進行了比較，并在機器人平臺上進行了實驗。結果表明，采用本文算法可以解決目標長時間消失或被遮擋的問題，在保證實時性的前提下能夠準確、穩定地跟隨目標。

未來，將進一步研究機器人在目標跟隨過程中的避障問題，使機器人能夠應對更加復雜的環境。此外，將多個傳感器與相機結合起來，以進一步提高方法的穩健性。