基于機器人操作系統的機器人自主追蹤系統設計

張 軍， 劉先祿， 張宇山

(1.安徽理工大學機械工程學院, 淮南 232001； 2.安徽理工大學人工智能學院, 淮南 232001)

目標追蹤是計算機視覺研究領域的熱點之一，其中對人臉的檢測追蹤又是目標追蹤領域的熱門方向，廣泛應用于視頻監控，人機交互等各個領域[1-2]。當下運用較多的目標跟蹤算法如核相關濾波算法(kernelized correlation filters, KCF)[3]，其是對傳統相關濾波算法的改進，減少了參數量，提高了跟蹤效率，然而在面對長時間的跟蹤任務時其性能會有顯著降低，影響正常的跟蹤任務。Zhang等[4]將KCF與CamShift (continuously adaptive mean shift) 算法進行了融合，能實現對人臉目標的準確識別追蹤，但其實時性較差，容易在跟蹤過程中丟失目標。牛穎等[5]利用CamShift 算法融合 TLD (tracking learning detection) 框架實現了對人臉位姿的識別與追蹤，但其場景是在人臉位置相對固定，背景變化不大的情況下進行的。利用移動機器人對運動目標進行實時追蹤能夠獲得更好的人機交互體驗。相較于傳統的目標跟蹤算法跟蹤精度較低，需要手動提取跟蹤目標的特征，基于深度學習的目標跟蹤算法有著天然的優勢，其不需要人工設計特征，只需要利用深度神經網絡來進行自我學習從而達到提取特征的目的[6-7]。使得移動機器人能夠更好地對環境建模，增強其對環境的理解，從而實現其在各種環境下均能對目標進行追蹤[8-9]。

由于基于深度學習的目標跟蹤算法的優越性能，對深度學習算法Siamese[10]進行了改進和優化，針對其對處理器的運算能力有著較高的要求，采用ROS系統的分布式計算框架[11-13]，實現對計算資源分配，加快算法的運行速度并提升訓練效果，使移動機器人能夠更加準確的檢測并追蹤目標[14-15]，實現移動機器人對目標的實時追蹤。

1 追蹤系統總體布局和硬件系統組成

追蹤系統的總體布局如圖1所示。圖1中，以固定在底盤上擁有一定運算性能的樹莓派為系統的主節點，提供運行機器人操作系統(robot operating system，ROS)的Master(節點管理器)的硬件環境；再輔以兩個高性能的筆記本作為從機使用，并且為深度學習算法提供獨立的運行本體，其間以路由器的無線Wifi實現整個系統間的通信，共同構成整個上位機系統。另一筆記本主要用作追蹤過程中的可視化顯示，用以實時觀察系統的追蹤狀況。下位機以STM32為運行本體，充當底盤的控制中心，完成對上位機的速度指令解析。

圖1 系統總體框架Fig.1 The total frame of the system

主要以STM32F429為移動底盤的控制中心。需要運用usart接口實現對上位機的通信；通過IIC實現對IMU(慣性測量單元)慣導模塊傳感器的數據讀取，利用從傳感器中實時獲取的數據來精確地判別底盤自身的位姿信息；通過定時器實現對電機編碼器的數據匯總。

使用樹莓派作為實現復雜算法運行的基礎。對于導航系統，需要借助傳感器來感知移動機器人周圍的環境信息，采用思嵐科技的二維激光雷達，實現對環境的2D平面信息感知。

移動底盤的麥克納姆輪的結構布局如圖2所示，其四輪均采用全向輪，驅動方式采用四輪驅動的方式來實現底盤的運動，以車體的幾何中心為坐標原點，構建平面直角坐標系xoy，以四輪中心構建相對坐標系x1o1y1、x2o2y2、x3o3y3、x4o4y4。其中ω1、ω2、ω3、ω4分別為輪1～輪4的角速度，vg1、vg2、vg3、vg4分別為麥克納姆輪輥子的運動線速度，β為輥子軸線與輪轂軸線的偏置角，L為前后輪的中心距，W為左右輪的中心距，v為移動底盤的速度，vx、vy分別為速度v在水平和豎直方向上的分量，ω為移動底盤的角速度，L1、L2分別為輪子中心到底盤中心的縱向距離和橫向距離，輪子呈現方形對稱分布。此結構布局可以實現0轉彎半徑，只需附以一定的轉速，就能夠實現移動平臺的全方位運動與平面轉動。

圖2 移動底盤示意圖Fig.2 Diagram of moving classis

2 機器人導航系統開發

運動底盤導航系統包括上位機系統與下位機系統，導航系統的總體布局如圖3所示。

圖3 導航系統總體分布Fig.3 General distribution of navigation system

下位機采用ros_serial協議，可以實現對ROS節點編程的移植，實現在STM32內部節點編程，并且能夠在ROS系統中發布與訂閱消息。下位機訂閱位姿話題，實現對上位機的速度解析；同時訂閱比例-積分-微分(proportion integral differential,PID)數據，以方便系統可以在線調整PID的各項參數，實現對電機的精確控制；下位機系統還發布了IMU與編碼器數據，以實現對底盤位姿的實時感知，同時為上位機運行的跟蹤算法實時提供采集的里程計數據。

上位機系統主要運行復雜的定位算法Acml與導航算法Move_base，上位機系統架構如圖4所示。

圖4 上位機系統架構Fig.4 Upper computer system architecture

導航算法通過/map_server加載預先建立的地圖，并且發布給move_base節點；然后，導航系統根據預先建立的地圖以及lidar發布的環境信息，由/acm根據粒子濾波實時估計與定位移動機器人的

全局位姿，并且由/move_base實現避障下的軌跡規劃；與此同時，算法還預留了規劃軌跡目標的位姿接口/move_base_simple/goal，視覺系統將通過該接口實現與導航系統的融合，開發出具有實時追蹤性能的移動機器人追蹤系統框架。

3 機器人視覺系統開發

視覺系統的硬件以Kinect深度相機為基礎，該相機能夠感知環境的三維信息，同時實現RGB(red green blue)圖像與深度點云信息的采集。這樣，在獲取圖像的同時，也能同時感知圖像每個像素的深度信息。追蹤系統通過改進后的Siamese算法在RGB圖像上實時跟蹤目標的像素位置，然后，依據深度信息感知移動機器人的相對位姿。最終，根據相機的內外參變換，實現對目標的全局位姿感知，達到實時追蹤的目的。

3.1 機器人空間狀態變換

依據針孔相機模型進行分析與計算，原理圖如圖5所示。

f為相機的焦距；Z為目標點到相機光心的距離； h′、H′分別為成像物體在成像平面的高度和實際高度圖5 針孔成像原理Fig.5 Principal of pinhole imaging

相機的位姿可由其外參數R和t來描述，則有

ZPuv=K(RPw+t)

(1)

(2)

(3)

則機器人的全局坐標為

(4)

式中：Z為目標點與相機光心之間的距離；Puv為像素坐標的齊次坐標；K為相機內參矩陣，由相機自身決定；Pw世界坐標；[u,v]T為目標點在像素坐標系下的坐標；fx、fy為焦距;[cx,cy]T為像素坐標系到成像平面之間的坐標偏移量；[x,y,z]T機器人在空間中的全局坐標；R與t分別為外參旋轉矩陣與偏移向量，可根據ROS系統的TF(transform)樹進行運算。

所開發的移動機器人跟蹤系統的TF樹如圖6所示。依據跟蹤系統的TF樹，可以得到機器人/map到/kinect_frame_optical的空間變換矩陣R與偏移向量t，即可實現由RGB像素點到全局坐標的空間變換。

圖6 跟蹤系統TF樹Fig.6 TF tree of tracking system

3.2 Siamese算法設計

通過孿生神經網絡結構來設計實時跟蹤目標的算法結構，其中算法的結構如圖7所示。

圖7 注意力機制的Siamese跟蹤算法結構Fig.7 Visualization of tracking results

x為輸入；u為主干網絡卷積層輸出；X為結合了權重之后的輸出；c1、c2為特征通道數；h為輸入圖像的高；w為輸入圖像的寬； H為輸出特征圖的高；W為輸出特征圖的寬；Ftr(·, θ)表示將輸入x轉變成u；Fsq(·)表示對特征圖進行壓縮； Fex(·, w)表示對壓縮后的特征進行excitation操作；Fscale(·)表示channel-wise multiplication(即通道乘法)圖8 注意力機制網絡布局Fig.8 Node distribution of system framework

所使用的目標跟蹤算法是在SiamMask[16]的基礎上，對網絡的backbone進行改進，基于卷積神經網絡Se-net通過對上一層結果進行再次特征提取，再對原先的網絡結果進行權重加成，形成本局部模塊網絡的結果；其中基于Se-net的注意力機制網絡布局原理如圖8所示。利用此方法可以提升backbone對特征的提取能力，同時改善跟蹤算法的跟蹤性能。

改進后的跟蹤算法的輸出結果由3個分支組成，分別代表著對目標屬于圖像網格的分類分支、圖像目標的回歸分支、以及圖像目標的分割分支。最終可以實現對圖像中目標的框取，同時實現對目標的分割，使得對跟蹤目標的定位更加準確。

3.3 追蹤系統架構設計

圖9 追蹤系統框架節點分布Fig.9 Node distribution of tracking system framework

導航系統負責底盤的運動控制和路徑規劃，計算出最優的路徑，然后實時控制機器人底盤的運動速度；同時，導航系統還能夠實時的避障，以保證在追蹤的過程中移動機器人能夠安全高效地運動到預定的目標位置。

/AI_center由Siamese神經網絡算法實現，是視覺系統的核心；其主要是在確定第一幀的模板圖像后，通過實時接收深度相機以后每一幀的RGB圖像，在圖像中識別出跟蹤目標的像素位置；能夠準確形成圖像人臉的分割區域，以計算出人臉區域的中心坐標；能夠使用該節點以話題的形式在ROS系統中進行實時的發布。

/tf_transformer節點主要是實現像素坐標到世界全局坐標的空間變換，并且實時的為move_base提供跟蹤目標的位姿信息；該節點訂閱相機的深度信息與/AI_center的目標像素坐標，根據針孔相機模型準確的計算出目標像素點在相機坐標系下的坐標；然后，根據系統的TF樹，實時地監聽世界坐標到相機坐標的變換矩陣與偏移矩陣數據；最終，找到在map地圖下的目標位置，實現對目標的實時跟蹤。

4 仿真與實驗

對于在固定人臉的情況下，追蹤系統對人臉的追蹤性能做了仿真實驗，仿真環境與目標識別結果分別如圖10所示。

在圖10(a)的rviz可視化顯示的導航系統中，成功地識別了人臉在地圖中的位置，并且以箭頭的形式呈現出來。同時在Siamese算法中，神經網絡成功地識別了人臉的目標像素位置，以方框的形式呈現出來，如圖10(b)所示。

Siamese算法實現了對RGB圖像中人臉的準確定位，同時算法也實現了對人臉的分割，在圖10(b)中以紅色區域來顯示，區域與人臉的重合度反映了算法的追蹤性能；重合度越大表明算法的分割越準確，追蹤精度越高。

在仿真實驗中，追蹤系統成功找到了目標位置，并且最終準確的運動到了目標位置，目標圖像的追蹤結果在rviz中的可視化顯示如圖10(b)所示。

（5）電機結構簡單，抗沖擊能力強：電動機轉子無繞組和永磁體，機械強度高，能夠長期耐受強沖擊與強振動負載；

圖10(b)中，移動機器人位置實現了與箭頭目標位置的重疊，并在導航系統中實時發布目標已經達成的Goal reached信息，表明跟蹤系統對固定目標的跟蹤成功以及該跟蹤系統的可行性。采用移動機器人進行目標追蹤實驗驗證，跟蹤圖11(a)所示的真實人臉特征，實驗結果如圖11(b)所示。

圖11 實驗驗證Fig.11 Experimental verification

為了評價跟蹤系統的跟蹤性能，采集了跟蹤過程中Siamese算法對跟蹤目標的定位數據，如圖12所示。圖12(a)表明跟蹤過程中跟蹤系統的定位誤差，隨著移動機器人與目標點距離的接近而逐漸減小；而在移動機器人和目標點相距較遠時，追蹤系統的定位誤差往往較大，追蹤系統的目標點與真實的目標點之間無法重合。究其原因總結如下。

d表示機器人與追蹤目標之間的距離；x0為距離；x為迭代次數圖12 實驗驗證結果Fig.12 Experimental verification results

(1)相機內參誤差。由于相機的畸變無法準確測出，使得在像素坐標變換時無法準確地計算出像素坐標到相機坐標的變換；使得跟蹤系統帶有系統誤差，其呈現的特點往往是目標越遠離圖像中心像素位置，定位誤差越大，跟蹤系統性能也越差。

(2)Acml算法定位誤差。由于定位算法無法準確給出移動機器人的實時位置，使得外參數據不準確，從而帶來了跟蹤誤差；此時，跟蹤系統的主要問題是導航系統失靈，往往表現為失蹤，無法跟蹤目標，陷于困境與死循環。

(3)深度學習算法訓練。深度學習算法是在訓練好的參數模型下，實現對未知目標的識別與跟蹤；因此，模型參數對算法性能起到決定性的作用，訓練過程中，充分考慮可以給算法帶來誤差的各種影響因素，才能夠提高算法的魯棒性，提升算法的適用范圍，帶來對算法部署性能的提升。

(4)相機與目標間的距離。如圖12(b)所示，相機能夠準確追蹤目標需要對二者之間的距離進行一定的限制。這主要與相機的視角范圍有關，距離過近，將會導致相機采集的圖像超出人臉的顯示輪廓區域，如圖13所示，此時不能將圖像顯示出來，但并不會造成目標的丟失，如圖12(b)所示的d=1 m時的曲線，機器人仍然能夠準確移動到目標區域。而距離過大則會導致圖像中的目標過小，造成跟蹤目標失蹤現象，如圖14所示的d=4 m時的曲線，目標點無法收斂于真實目標，呈現出雜亂無章現象，且跟蹤過程中相機與目標的距離呈現出震蕩不收斂的情況。

圖13 過近距離跟蹤結果Fig.13 Tracking results in too close range

圖14 距離過大實驗結果Fig.14 Results of the experiment with too much distance

5 結論

基于全向輪移動平臺，設計了ROS系統下的人臉追蹤系統框架，實現了Siamese算法在ROS系統中的部署，完成了移動機器人對人臉的實時追蹤(保持二者的間距在一定范圍內)。借助于ROS仿真平臺，經過分析實時記錄的跟蹤數據，得出如下結論。

(1)基于ROS系統，開發了人臉跟蹤系統的基本框架。在上位機中，采用導航與Siamese跟蹤算法結合的思想，設計了人臉追蹤系統。通過Siamese神經網絡算法來實時檢測與識別預定的模板人臉目標，為導航算法提供目標指引，使得追蹤系統能夠準確跟蹤人臉目標。

(2)開發了硬件與仿真平臺，實現了基于深度相機下的Siamese算法在ROS下的部署；使得系統基于感知的深度信息，實現機器人在深度相機下的狀態變換與視覺定位，從而實現了對人臉目標的實時追蹤。