基于目標檢測的移動抓取機器人系統設計與實現

鄭若璇

（西南交通大學計算機與人工智能學院，成都 611756）

0 引言

近些年，人工智能的發(fā)展使得越來越多的智能化產品投入到人類的生活、生產中替代人工，提升生活水平、生產效能。目前公共場所清潔很大程度上仍然依賴于人工，但是垃圾回收類工作完全可被機器人系統替代，極具發(fā)展前景。日常常見的機器人大致分為兩類：工業(yè)型［1］和服務型［2］。工業(yè)型機器人廣泛應用于工農業(yè)生產制造環(huán)節(jié)；服務型機器人更為公眾熟知，例如送餐機器人、迎賓機器人等?，F階段很多服務型機器人應用了深度學習技術實現機器人的智能化，比如迎賓機器人的對話功能、人臉識別功能等。

基于深度學習的目標檢測算法主要分為兩種：兩階段和單階段算法。兩階段算法將目標檢測分為候選區(qū)域生成和卷積神經網絡分類兩步，代表算法有RCNN 系列［3-5］。其在目標框的定位精度和目標類別的檢測準確度兩個性能上表現優(yōu)異，然而檢測速度較慢，很難應用于實時場景。單階段算法是一種端到端算法，不再生成候選區(qū)域，而是直接對圖像進行目標的定位和類別檢測，以一部分檢測精度為代價換取了檢測速度的大幅度提升，代表算法有YOLO系列［6-9］、SSD［10］等，而且隨著YOLO系列算法的逐漸優(yōu)化，算法的檢測精度也得到提升，因而被普遍應用在實時檢測場景。

本文基于ROS［11］系統，將可移動機器人、機械臂控制與目標檢測算法相結合，設計并實現了基于目標檢測的移動抓取機器人系統，完成機器人定位導航抓取物品并分類回收的功能，并針對此系統的應用場景構造了Grasp 數據集，使用YOLOv5在數據集上訓練、測試；最終進行了不同場景下的實景測試，抓取率達到92%，分類回收放置正確率達到100%。

1 系統架構

本系統（圖1）基于ROS進行開發(fā)，主要分為三個模塊，建圖導航、目標檢測、抓取及分類放置。

建圖導航模塊采用激光SLAM［12］算法進行全局地圖構建，采用Amcl 算法實現導航過程中實時定位，采用Move_base 進行全局路徑規(guī)劃和局部避障，通過ROS 的話題通信向底盤運動控制話題發(fā)布運動命令，以實現系統架構的“移動”功能。該模塊話題通信發(fā)布機器人是否到達指定位置的消息，待目標檢測模塊訂閱。

目標檢測模塊只有接收到機器人到達指定位置的消息才會運行，包含目標檢測和坐標系轉換兩部分。目標檢測部分通過話題通信，訂閱REALSENSE D435 深度攝像機的RGB 圖像話題和深度圖像話題，使用YOLOv5 模型對實時RGB 圖像進行目標檢測，確定當前位置可抓取目標的分類和在圖像像素坐標系中的坐標，再結合深度圖像得到目標的深度信息。坐標系轉換部分通過整合當前圖像所有待抓取目標的圖像中定位和深度信息，確定本次抓取目標的深度及其在圖像像素坐標系的定位，再經過坐標系轉換的矩陣計算，獲得本次抓取目標在相機坐標系的定位，最終將該定位維護到機器人系統的坐標系轉換樹中，待抓取及分類放置模塊獲取。

抓取及分類放置模塊采用MoveIt!［13］控制機械臂，主要包含機械臂抓取和分類回收兩個階段。機械臂抓取階段，該模塊獲取本次抓取目標的定位并調用MoveIt!，以該位置為目標點進行機械臂路徑規(guī)劃以及機械臂移動控制以到達目標所在位置抓取目標。目標物抓取完成后進入分類回收階段，該模塊訂閱目標檢測模塊發(fā)布的本次抓取目標的分類消息，根據分類結果，控制機械臂到達該類別的回收位置，并松開夾爪以回收目標物。

圖1 系統框架

2 建圖導航

2.1 建圖導航TF坐標系轉換

ROS 通過坐標系標定各關節(jié)或物體位置，三維世界中任何兩個坐標系的關系可以用平移量和旋轉量兩個量表示。TF（坐標系轉換）維護了機器人系統中全部坐標系的關系，形成了一棵遵循“單繼承”原則的樹，因此TF 樹中任兩個已存在的非父子坐標系的轉換關系可以由樹形結構中記錄的父子坐標系的轉換關系獲得。

建圖導航中的TF 除了機器人本身各個部件之間的TF 之外，主要維護了地圖坐標系、機器人底盤坐標系以及里程計坐標系之間的關系，以便機器人在移動建圖和導航過程中確定行進的方位和距離以及在地圖中的方位，一般設置如圖2所示。

圖2 建圖導航坐標系轉換關系

2.2 SLAM建圖

在實際場景中，實現導航功能通常是基于已經獲取全局地圖的條件，之后每當機器人得到目標地點信息時，可以結合全局地圖、自身位置經過一系列算法規(guī)劃一條全局最優(yōu)路徑。SLAM 是一項二維地圖建模技術，用于機器人在陌生環(huán)境中通過移動實時獲取自身定位及周圍環(huán)境信息，并構建增量式地圖，最終得到靜態(tài)全局地圖。

圖3 建圖示例

本系統采用基于激光雷達和里程計的SLAM算法。建圖節(jié)點訂閱激光雷達發(fā)布的環(huán)境信息以及激光雷達、里程計和機器人底盤的TF 信息，在機器人探索建圖的過程中，發(fā)布機器人姿態(tài)、以固定頻率更新地圖的元數據（長、寬、分辨率等），同時發(fā)布地圖坐標系和里程計坐標系的轉換，將通過里程計記錄的移動信息和地圖原點信息相結合，從而使機器人在探索未知領域時能夠確定當前激光雷達掃描到的信息更新在二維地圖中的方位。

2.3 導航

導航模塊（圖4）采用ROS_Navigation，其核心部分是Move_base節(jié)點。

圖4 導航模塊框架

Move_base 基于靜態(tài)全局地圖，通過在移動過程中實時獲取的外界環(huán)境信息和自身定位信息等維護了全局代價地圖和局部代價地圖，用于全局和局部路徑規(guī)劃，其中全局路徑規(guī)劃用于規(guī)劃完整行駛路線，而局部路徑規(guī)劃用于躲避移動過程中激光雷達掃描到的建圖時不存在的障礙物，最終實現控制底盤移動到指定目標點。導航模塊會將導航結果發(fā)布在設定的相應話題中。

由于里程計只是通過里程計數據實現里程計坐標系和機器人底盤坐標系的坐標系轉換，存在累計誤差并且打滑等特殊情況會導致定位偏差，不適用于長期定位或長距離定位，所以采用ROS_Navigation 的Amcl 功能包實現機器人導航過程中的定位。Amcl 訂閱激光雷達數據、地圖數據等估計機器人在地圖坐標系中的位姿同時結合里程計信息降低定位偏差。

3 目標檢測

3.1 數據集

本文針對機器人抓取系統的應用場景及目標抓取物類別，制作了Grasp 數據集，圖像共2200 余張，分為瓶子（金屬、塑料、玻璃）、水果及果皮（香蕉、蘋果、橘子等）、紙盒3 個類別，全部為人工標注，其數據格式采用VOC 數據集的格式。

3.2 目標檢測

目標檢測模塊以深度攝像機的RGB 圖像作為輸入，使用YOLOv5s模型進行可抓取物檢測，檢測結果是圖中每個可抓取物的類別及其檢測框在二維圖像像素坐標系的坐標。結合深度攝像機的深度信息，可以計算得每個目標檢測框的平均深度值，以此作為該目標的深度距離d。由于單次機械臂抓取只抓取單個目標，所以設定深度距離最小的為當前抓取目標，同時目標檢測模塊以自定義的話題消息格式，向指定話題發(fā)布本次抓取目標的分類待機械臂抓取模塊訂閱。

YOLO 系列是典型的端到端目標檢測算法。YOLOv5 主要包含四個模塊：Input 負責數據輸入和數據增強，其創(chuàng)新點在于將初始錨框的計算嵌入到訓練中，使模型在不同訓練集的訓練過程中找到相應的最佳初始錨框；Backbone 主要包含切片操作的Focus 結構、特征提取網絡CSP 以及激活函數，用于提取圖像特征；Neck由FPN+PAN 結構構成，融合不同特征層參數；Head 利用Neck 傳遞的特征進行預測，輸出目標的分類和定位，而在訓練中，網絡將預測框和真實框求差，反向更新迭代網絡參數。

YOLOv5 官方提供了5 種不同深度和寬度的結構。深度加深和寬度加寬能夠提升網絡的特征提取、特征融合能力，同時也會增加網絡的計算量。由于本系統是部署在移動端，對網絡的輕量化有較高的要求，而YOLOv5s 的準確度和輕量化能夠滿足本系統使用場景的要求，所以系統采用YOLOv5s作為檢測模型之一。

3.3 TF坐標系轉換

在實際抓取時，機械臂需要獲取目標相對機械臂的三維物理坐標，而機械臂坐標系與相機坐標系均已維護在機器人系統TF中，所以目標檢測模塊首先依據圖像像素坐標系（二維）、圖像物理坐標系（二維）、相機坐標系（三維）的轉換關系（圖5）以及深度攝像機的參數矩陣經矩陣運算得到目標在相機坐標系中的三維物理坐標。

圖5 坐標系轉換示意圖

圖像像素坐標系的原點為圖像左上角，單位為像素（pixel），原點為op(0,0)；圖像物理坐標系的原點是圖像的中心點，在圖像像素坐標系中坐標為o(u0,v0)，坐標系單位為毫米（mm）。以目標物檢測框的中心點為目標點，設目標點在圖像像素坐標系中坐標為(u,v)，在x方向1 pixel =dxmm，在y方向1 pixel =dymm，則目標點在圖像物理坐標系中坐標(x,y)滿足：

oOc為相機焦距f，依據三角形相似，目標點在相機坐標系中坐標(X,Y,Z)滿足：

其中Z即目標點的深度距離d。

聯立公式（1）和（2），則目標物的相機坐標系坐標(X,Y,Z)與圖像像素坐標系坐標(u,v)滿足：

目標檢測模塊以相機坐標系為父坐標系，目標物坐標系為子坐標系，通過TF 的broadcaster 發(fā)布目標物在相機坐標系中的位置，將目標物維護至TF樹，從而使機器人TF樹中任一坐標系均可與目標物坐標系進行坐標系轉換，其中也包括機械臂坐標系。

4 抓取及分類放置

4.1 機械臂控制

MoveIt!是ROS 提供的控制機械臂的平臺，其中結合了運動規(guī)劃、運動學求解、仿真配置、控制等內容。抓取及分類放置模塊中機械臂主要包含四個靜止狀態(tài)：歸位狀態(tài)、探出狀態(tài)、抓取狀態(tài)、放置狀態(tài)。模塊通過調用MoveIt!的不同接口，完成實時讀取機械臂各關節(jié)的位姿、規(guī)劃末端執(zhí)行器姿態(tài)移動方案、發(fā)布指定目標姿態(tài)、控制機械臂移動等功能。

4.2 抓取及分類放置

機械臂在機器人未到達指定目標地點時，處于歸位狀態(tài)。抓取及分類放置模塊首先訂閱導航模塊發(fā)布的導航結果信息，若機器人沒有到達指定目標點，則循環(huán)監(jiān)測導航結果；若機器人完成導航到達指定目標，則機械臂移動至探出狀態(tài)。之后，該模塊觸發(fā)目標檢測模塊發(fā)布的目標檢測結果話題的回調函數，獲取待抓取目標的分類信息，若分類信息不屬于具體類別，表示機器人所處位置不存在可抓取物，機械臂返回歸位狀態(tài)，等待指定下一個地點。若分類信息指向具體某個類別，表示當前位置存在可抓取物，則機械臂訂閱TF 樹，獲取目標檢測模塊維護的目標物與機械臂的位置關系，并移動到目標物上方，收緊夾爪（即抓取狀態(tài)）。抓取完畢后，該模塊根據分類信息的分類結果，控制機械臂到相應的放置點并松開夾爪（即放置狀態(tài)），完成一個目標地點的一個可抓取物的抓取。之后，機械臂回到探出狀態(tài)，抓取當前目標地點的下一個可抓取物，直到當前地點不存在可抓取物，機械臂回到歸位狀態(tài)，完成了一個目標點的抓取及分類放置任務，等待下一個地點。流程如圖6所示。

圖6 抓取及分類放置流程

5 系統運行效果

5.1 算法評估

5.1.1 實驗環(huán)境

YOLOv5實驗設置最大迭代次數為100 epochs，初始學習率為0.01，學習率變化因子為0.1；實驗使用3 塊NVIDIA GeForce RTX 2080 Ti，環(huán)境為Python3.7、PyTorch1.6.0、CUDA10.2。

5.1.2 結果

在Grasp 數據集的測試集上對訓練完成的YOLOv5s模型進行測試，結果如表1所示。

表1 YOLOv5s-Grasp數據集測試結果

5.2 系統評估

本系統的實現基于上海一坤電氣的輪式機器人Bulldog、遨博智能機械臂manipulator_i5、Intel 雙目攝像頭D435、搭載第八代智能Intel 酷睿處理器的INUC 套件NUC8i7BEH 等硬件設備，以移動抓取室內雜物并將抓取目標分為瓶子和其他（非瓶子）兩類進行分類回收為測試要求進行測試，測試物品共50 件，測試次數100 次，對正確抓取、漏抓、分類放置正確及放置錯誤次數進行統計，其中正確抓取92 次，漏抓8 次，抓取準確率92%；分類回收準確92 次，分類回收正確率100%。

測試過程中部分導航軌跡如圖7所示，實景效果如圖8、圖9所示。

圖7 建圖導航模塊展示

圖8 目標檢測模塊展示

圖9 抓取及分類放置模塊展示

6 結語

本文設計的基于目標檢測的移動抓取機器人系統，將深度學習算法與機器人相關技術結合，實現了機器人自主導航至指定目標地點并抓取及分類回收等功能，可應用于日常生活、服務中，具有一定的實踐意義。在實驗演示中，系統運行效果良好，為移動機器人實際應用提供了新的思路。增加機器人自主循環(huán)導航，實現分類多樣化等，是本系統進一步優(yōu)化的方向。