基于ROS系統的行人跟隨機器人設計

金重亮

（徐州工業職業技術學院汽車工程學院 江蘇省徐州市 221000）

目前解決倉儲環境、超市環境短距離運輸問題普遍采用AGV模式，需要對場地進行改造適合大規模運輸，部署靈活性較差。對于需要隨時改變運行軌跡的小規模短距離運輸AGV模式無法實現，本文設計一種基于ROS系統的行人跟隨機器人能夠更好的解決短距離運輸問題。

開源機器人操作系統ROS[1]被廣泛應用于機器人領域，傳統的機器人系統開發使機器人系統之間每個模塊的耦合能力很強，難以做到模塊相互分離，代碼的可復用性和可移植性不高，影響實際應用部署效率。ROS由多個獨立節點構成，每個節點通過發布/訂閱消息機制進行通信。ROS系統支持多種語言協作實現功能，使用標準的TCP/IP協議實現系統內部節點之間的通信，顯著提高開發效率。

目標跟蹤主要分單目標跟蹤（SOT）和多目標跟蹤（MOT），單目標跟蹤基于孿生網絡的目標跟蹤算法逐漸成為主流，主要包括算法SiamFC[2]、SiamRPN[3]、SiamRPN++[4]，但這些算法都需要框選跟蹤目標才能夠實現對單獨目標的跟蹤。為了能夠實現快速選擇目標，使用多目標跟蹤算法Deepsort[5][6]實現單目標跟蹤任務。使用Deepsort多目標跟蹤需要配合目標檢測器，可選擇的檢測器包括主流的目標檢測算法YOLO系列、CenterNet[7]等。本文選擇使用YOLO V3[8]算法并通過TensorRT加速YOLOV3算法實現實時性要求.通過改進DeepSort多目標跟蹤算法流程可實現單目標和多目標跟蹤任務切換。跟蹤目標選擇時，只需輸入被跟蹤目標數字ID就可以實現對單個目標的跟隨。

1 硬件系統設計

為能夠進行深度學習模型實時推理功能部署，使用NvidiaAGXXavier（簡稱AGXXavier）做為控制電腦，Xavier搭載TensorCore的512核VoltaGPU，具有8核ARM 64位CPU可以達到32 TOPS運算性能，模塊功率可配置為10W、15W、30W，能夠針對不同算力要求進行配置，在能耗和效率之間找到平衡點。

AGXXavier控制端口不夠豐富，無法滿足控制器對多執行器、傳感器進行控制和擴展性需求。本文并未使用AGXXavier端口進行控制，而是通過使用ArduinoMega2560（簡稱Mega2560）進行執行器的控制和傳感器數據采集。系統設計如圖1所示。

AGXXavier控制電腦使用USB接口通過USB集線器連接Mega2560、攝像頭進行數據傳輸。Mega2560通過IO端口、PWM端口等連接電機驅動器、編碼器和攝像頭云臺，實現對車輪的控制、編碼器的數據采集和攝像頭云臺伺服電機控制。

目標跟蹤蹤算法主要由AGXXavier處理并得出運動參數，運動參數包括速度、角速度、云臺俯仰角通過ROS節點消息機制進行發布。目標跟隨任務中機器人移動由Mega2560主控制器實現，通過驅動器驅動電機控制機器人的前進、左轉和右轉運動。主控制器根據訂閱的節點消息數據，通過驅動器對電機進行控制，車輪裝有編碼器，其作為里程計反饋機器人的實際運動情況。

2 控制系統設計

控制系統采用ROS系統進行設計，在硬件設計時由于AGXXavier控制端口不夠全面，擴展能力差。需要解決ROS控制器硬件通信問題，對于Mega2560主控制器有兩種方式來實現ROS通信。一種是使用rosserial方法，通過USB通用串口總線實現AGXXavier控制單元與Mega2560控制器的通信。第二種是使用rosbridge方法，自定義通信協議與Mega2560進行通信。由于使用rosserial方法將Mega2560作為一個ROS節點會產生較大的通信延遲，靈活性不夠強等缺點。所以選擇使用ROSArduinobridge方法來進行控制器通信。

圖1：系統設計

圖2：ROS節點圖

圖3：測試1

圖4：測試2

圖5：測試3

圖6：測試4

ROSArduinobridge提供基本控制器可以對機器人基本參數進行設置，讀取模擬和數字量傳感器信號、PWM控制，并通過ROSArduinopython節點發送或訂閱消息。為滿足控制需求對ROSArduinobridge進行改進：

（1）對PID控制器進行修改，使用兩個單獨的PID控制器對機器人左右兩個電機進行控制。

（2）優化編碼器采集方式，使用中斷方式進行數據采集。

（3）對舵機控制進行優化，將服務同步通信修改為消息異步通信，提高控制實時性。

3 跟隨系統設計

跟隨系統分為兩個階段第一階段為目標跟蹤，第二階段為目標跟隨。第一階段通過對多目標檢測算法Yolov3和Deepsort算法的改進實現單目標跟蹤算法得到被跟蹤目標的目標框（BoundingBox），第二階段根據目標框的坐標使用PID控制器驅動電機使機器人實現對行人的跟隨。

3.1 目標跟蹤

通過YOLOv3和Deepsort算法可以對多目標進行檢測、匹配并標示出跟蹤器匹配數字ID。本文將Deepsort多目標跟蹤算法改進為單目標跟蹤算法，并將算法分為兩個階段，第一階段（MOT階段）使用常規Deepsort算法對Yolov3算法的檢測結果進行匹配并得出匹配數字ID，第二階段（SOT階段）輸入跟蹤目標的匹配數字ID，算法會對選定的單一目標進行跟蹤。并可以隨時切換到第一階段對單一目標進行選擇。

Deepsort算法在Sort[9]算法的基礎上增加對外觀深度特征的匹配度量信息，使用深度卷積神經網絡對行人特征進行識別，能夠更好的解決遮擋問題。

3.1.1 運動度量

Deepsort算法使用卡爾曼濾波器對跟蹤目標進行運動軌跡預測，通過計算預測結果與目標檢測結果之間馬氏距離如公式（1）所示。

其中dj表示第j個目標檢測框位置，yi表示第i個跟蹤器對目標的預測位置，Si表示卡爾曼濾波器預測得到的當前時刻觀測空間的協方差矩陣。為排除不可能的關聯，使用逆x2分布計算的95%置信區間作為判斷關聯性的閥值，其閥值t（1）=9.4877。

3.1.2 外觀深度特征度量

每一個由Yolov3得出的檢測目標框圖像，使用深度特征網絡進行特征提取得到128維的特征向量。對于每一個跟蹤器軌跡k，創建一個外觀描述圖庫存儲最近成功關聯的100幀特征向量。

對第j個檢測目標特征向量與第i個跟蹤器軌跡之間特征向量計算最小余弦距離如公式（3）, 如余弦距離結果小于閥值公式（4）認為外觀匹配有關聯性。

使用加權求和方式計算第i個跟蹤器和第j個檢測目標之間關聯度量的總體值?？梢酝ㄟ^參數λ值來調整運動度量和外觀度量的權重值如公式（5）所示,并判斷第i個跟蹤器和第j個檢測目標之間是否關聯如公式（6）。

3.1.3 級聯匹配

物體被長時間遮擋時，卡爾曼濾波器無法更新導致目標狀態不能夠準確預測。使用馬氏距離時，當兩個跟蹤器競爭同一個檢測目標時會出現不確定性。Deepsort使用級聯匹配來解決上述問題，對頻繁出現并被確認的目標賦予優先權。SOT階段算法對Deepsort進行改進在級聯匹配過程中，只對選中目標進行匹配。如表1中Ca,j、ba,j，其中a 表示被選中的跟蹤器（數字ID），算法在接受用戶輸入a后由MOT模式轉變為SOT模式進行目標跟蹤。

表1：SOT階段級聯匹配算法

3.2 目標跟隨系統

目標跟隨系統對目標跟蹤系統返回的目標框進行跟隨，目標跟隨系統通過檢測目標框偏離中心距離值使用轉向PID控制器對機器人轉角進行控制，使用目標框面積值確定目標距離并通過速度PID控制器計算機器人前進速度。通過ROS系統發送消息、控制節點接收信息控制機器人進行轉向和前進操作。

4 系統測試

啟動三個ROS節點分別為sot_bounding_box節點負責目標跟蹤算法，返回并發送sot_bounding_box_msg SOT目標框消息，包含SOT目標框坐標信息。sot_move_controller節點接收SOT目標框坐標信息，根據坐標框信息通過pid控制器得到云臺俯仰角度和機器人運動參數，并通過servo_write_msg和/cmd _vel消息進行發送。Arduino節點接收云臺俯仰角度和機器人運動參數消息進行跟隨控制，具體ROS節點圖如圖2。

測試在圖3狀態下啟動并從MOT切換至SOT模式，行人背對機器人，可以進行跟隨。當行人側向面對時，仍然可以進行跟隨（如圖4）。切換至正向面對時（如圖5）機器人仍然可以進行跟隨，跟隨效果見圖6。Yolov3在TensorRT加速下可以達到30FPS基本保持實時，Deepsport環節經過算法處理可達到21FPS。

測試過程中當機器人移動時（測試中最高速度限制在1.5m/s,此速度為成年人平均行走速度，差速轉向速度最高0.628rad/s），在無遮擋情況下可實現自主跟隨移動。短暫遮擋情況下，目標再次出現后依然可以進行跟隨。但在一些特殊情況如目標消失或長期遮擋情況下會出現跟隨失效現象。

5 總結與改進

通過使用ROS系統進行跟隨機器人的設計，使用Yolov3和Deepsort算法實現目標跟蹤，通過TensorRT進行加速。改進算法將MOT算法應用于SOT進行目標跟蹤，經過目標選擇可實現目標追蹤。

在測試過程中發現長期遮擋失效問題，未來可以嘗試以下3個方面進行改進：

（1）Deepsort算法的外觀深度特征在進入SOT模式后，求解余弦距離后不再使用最小值進行判定。此時根據其屬于正態分布來估算外觀深度特征值。

（2）可以使用效率更高的目標檢測網絡來提高系統的實時性。

（3）在目標遮擋時通過判斷IOU閥值，暫停外觀深度特征的提取。