我現在研二，發表這篇部落格的時候，我剛開始做畢業論文。
畢業論文的方向是機械臂，具體做機械臂的什麼領域還不知道。
為了對機械臂領域瞭解更多，明確自己要做什麼，這個系列，我將閱讀一些機械臂的論文（時間有限，沒辦法大量看，可能就10多篇吧），我會將比較好的論文做成筆記分享出來，供後來人借鑑。

論文下載

基於卷積神經網路的二指機械手抓取姿態生成研究_李耿磊

摘要

本課題立足機械手自主抓取，研究利用卷積神經網路實現物體表面抓取點生成的解決方案。本課題以場景的深度影像作為輸入資訊，採取
“先取樣，後預測”的兩步走抓取生成方案。首先利用 Laplace 方法在深度影像中提取物體邊緣畫素點，並利用對跖法生成抓取空間；然後基於重要性取樣方法從抓取空間中取樣獲得候選抓取集，最後利用訓練好的抓取預測卷積神經網路模型預測候選集每個抓取的成功置信度，取其中極大者作為結果指導機器人完成抓取。

為將卷積神經網路應用於抓取預測，本課題建立了基於卷積神經網路的抓取預測模型。將抓取預測問題抽象為深度學習中的分類問題，網路輸出為某抓取點屬於成功抓取的置信度。本課題定義一組抓取為抓取中心座標和二指機械手開合方向的組合，以抓取中心畫素座標為中心取 32×32 畫素大小的深度影像作為網路的輸入資訊，將網路的輸出與標籤資料的交叉熵距離和作為控制網路訓練的成本函式，使網路能夠提取深度影像中的高階特徵來學習深度影像、抓取類別之間的關係，實現抓取預測。

為生成候選抓取預測集合，本文研究了基於Laplace運算元的物體邊界提取方法，構建了對跖抓取點空間，確定了基於重要性取樣的候選抓取取樣策略，並利用交叉熵方法優化得到了高質量抓取點分佈的概率密度函式，使得候選抓取集能夠最大程度的包含對跖抓取點空間中抓取成功置信度較高的抓取點。

關鍵詞：深度影像；二指機械手；卷積神經網路；抓取生成；候選抓取

第1章 緒論

1.1 抓取生成國內外研究現狀

1.1.1已知物體抓取生成

這類抓取規劃問題假設待抓取物體的外形是已知的。研究人員通常會離線製作一個抓取資料庫，其中包含待抓取物體的 3D 模型及對應的若干組穩定抓取配置。因此，此類抓取規劃問題簡化為識別物體形狀並估計物體位姿，然後從抓取資料庫中檢索出合適的抓取。整個方案可以由圖 1-1 抽象表示。

美國哥倫比亞大學Andrew T. Miller等人將物體簡化為一系列形狀單元組合，這些單元包括球、圓柱體、圓錐或者長方體等規則形狀，如圖 1-2 a)所示。通過預先定義形狀單元的抓取配置實現對複雜物體的抓取生成，然後將生成的抓取依次輸入到模擬環境 GraspIt！[8]進行抓取排序，從而建立物體-抓取資料庫。之後，Andrew T. Miller 等人又對上述方法提出改進，通過超二次曲面分解細化物體的分解，如圖 1-2 b)所示。瑞典皇家理工學院 Kai Huebner 等人[10]更是直接將物體簡化為長方體，如圖 1-2 c)所示。通過考慮抓取任務、方向以及物體形狀等條件採用啟發式方法在長方體表面法向向量中選擇完成抓取生成，然後利用離線訓練的神經網路模型獲得最優抓取。

基於模擬環境進行抓取規劃的方法，其衡量指標大多是目前廣泛採用的$ϵ$-metric ，但該指標對於誤差的敏感性較高。比如當物體實際位姿與理論位姿出現偏差時，容易造成抓取失敗，如圖 1-3 所示。Jonathan Weisz 和 Peter K. Allen 提出了具有魯棒性的 p(f) 指標，將物體的位姿視為滿足一定概率分佈的隨機變數，通過隨機取樣的方式獲得若干物體位姿，分別計算抓取在不同位姿上的抓取質量
$ϵ$-metric ，以其均值作為魯棒抓取指標 p( f) 。通過實物實驗發現， p(f) 的效果遠超前者，尤其是在抓取較大的物體方面，如圖 1-4 所示。

抓取生成和排序不僅可以來源於模擬環境分析和啟發式方法，還可以從人類直接示教或者間接學習獲得。俄勒岡州立大學 Ravi 等人提出一種物理人類互動指導的方法完成抓取生成，由人類緩慢拖動機械手到達抓取位姿（如圖 1-5 所示），記錄手爪姿態；Ekvall 等人提出一種基於形狀單元和人類示教生成抓取的方法，通過綜合感知資訊和經驗選擇合適的抓取靠近方向。整個系統會首先識別人類的抓取型別和抓取物體的類別，記錄資料手套的感測器資料（如圖 1-5 所示），然後對映到機械手空間中，完成抓取生成和抓取規劃。

1.1.2相似物體抓取生成

相似物體的抓取規劃遵循一個樸素的經驗想法：同一類物體的抓取可行區域大多數情況下是相同的，或者具有相似形狀的區域性區域的抓取質量是相似的。因此可以通過提取物體的低階或者高階特徵表徵物體之間的相似程度，進而基於已知物體的抓取去規劃對相似物體的抓取。基於資料驅動的相似物體抓取規劃整體方案可以由抽象表示。離線部分，人工定義低階或者高階特徵，學習這些特徵與抓取生成及抓取質量之間的關係，作為相似物體抓取預測的模型；線上部分幾乎與已知物體的抓取規劃方案相同。

加州大學伯克利分校 Jeffrey Mahler 等人同樣建立了一個擁有 10,000 個 3D 模型、總共 25 萬抓取點的大型抓取資料庫，並提出一種具有相關性獎勵的多臂老虎機模型學習特徵和抓取之間的關係。作者提出了一種 MV-CNN 多視角卷積神經網路學習其形狀相似指標，如圖 1-7 a)所示。同時考慮了物體的區域性表面高度圖（heightmaps，如圖 1-7 b）所示），提高了對相似物體的抓取成功率。

1.1.3 未知物體抓取生成

對於未知物體的抓取規劃，建立在資料庫中沒有該物體甚至完全沒有資料庫的前提下，利用啟發式方法建立感知資料和抓取點之間的直接對映關係。根據處理方式的不同，可以將這類問題的解決方法分為三類：1）利用感知資料重構物體模型，通過分析抓取的力封閉性規劃抓取點；2）通過感知資料（例如點雲）提取低階特徵，使用一系列啟發式方法作為判斷；3）高度依賴被感知物體的整體形狀資訊進行判斷規劃。這類問題的解決方案可以抽象總結為圖 1-8。

德國慕尼黑工業大學 Michael Beetz 等人提出一種基於單一角度方向感知資料建立物體模型的方法。他們首先根據跳邊檢測和統計分析去除錯誤的測量值，然後估算每個表面體素的法線。接下來，通過使用 RANSAC 演算法的有效變種檢測水平感興趣區域（例如桌子、平臺等）來限制搜尋範圍，因為通過這些平面可以找到其支撐的物體，然後對感興趣區域內的點進行聚類分割物體點雲。在對場景中的點雲進行分割後，再次對每一個物體的點雲進行重取樣，並估計每個點的表面法線和最小曲線半徑。最後將擬合出的物體模型和剩餘點雲的三角化表面進行合併，重建三維模型，利用分析式抓取規劃方法生成抓取點，選擇最滿足力封閉性的抓取。

1.2 深度學習在抓取領域的研究現狀

美國康奈爾大學 Lenz 等人提出了一種基於 RGBD 影像的級聯神經網路模型對目標物體進行抓取規劃。整個系統分為候選抓取集取樣和抓取評估選擇兩部分，每一部分分別由一個級聯神經網路完成。候選抓取集取樣是從圖片中所有可能的抓取區域進行評分篩選分數相對較高的少部分，然後再進入第二個級聯神經網路進行更加精細的評分和篩選，如圖 1-10 所示。

谷歌、劍橋大學以及加州大學伯克利分校成立研究團隊提出一種基於 Q 函式的強化學習演算法訓練真實環境中的機器人進行抓取，共同探討如何將深度強化學習應用在機器人操作中。美國華盛頓大學 Joseph Redmon 等人提出一種基於卷積神經網路的實時、準確抓取檢測演算法，可以在不使用標準滑動視窗或者區域建議技術的情況下對可抓取的邊界框執行單階段迴歸。同時，該網路可以同時執行分類，以便在一個步驟中識別物件並找到一個良好的抓取矩形，對該模型的修改通過使用區域性約束的預測機制來預測每個物件的多個抓取，如圖 1-12 所示。區域性約束模型的表現明顯更好，特別是對於可以通過各種方式掌握的物體。

1.3 主要研究內容

本文主要針對二指機械手抓取姿態生成問題進行研究，採取“先取樣，後預測”的兩步走抓取方法。第一步，以場景的深度影像作為先驗資訊，基於 Laplace 方法提取物體邊界畫素點以構建對跖抓取點空間，利用重要性取樣方法生成候選抓取集；第二步，利用訓練的抓取預測卷積神經網路模型對各抓取進行抓取成功置信度預測，從中選擇極大者作為結果指導機器人自主抓取物體。具體研究內容如下：

（1）第二章主要研究建立卷積神經網路解決抓取預測問題的數學模型，確定卷積神經網路模型的輸入輸出形式，推導控制網路訓練的目標函式。同時，研究並建立抓取系統的概率擾動模型，引入並確立力封閉概率 E(g)作為抓取質量的衡量指標計算資料標籤，以生成網路訓練需要的資料集。
（2）第三章主要研究並搭建搭建抓取預測卷積神經網路架構，研究卷積神經網路訓練中涉及的優化演算法選擇、學習率指數衰減以及模型過擬合等超引數調整問題，保證訓練得到的模型具有較高的預測準確率和較好的預測泛化能力。
（3）第四章主要研究候選抓取集的取樣策略，保證最大程度的獲得高成功率抓取點。區別於以往研究者常採用的隨機取樣法，本文采用重要性取樣方法進行抓取取樣，利用交叉熵方法優化確定指導取樣的概率密度函式。
（4）第五章主要工作是進行綜合抓取驗證實驗，包括模擬實驗和實物實驗。模擬主要是藉助 ROS、MoveIt!和 gazebo 軟體，在 gazebo 中搭建一個簡單的生活場景，將得到的抓取點通過 ROS 話題釋出給 MoveIt!進行機械臂的軌跡規劃控制完成抓取嘗試；實物實驗部分首先研究實驗準備階段的相機標定和手眼標定等工作，然後搭建實驗場景進行多組多次不同實驗評估抓取姿態生成演算法的實用性。

第2章 抓取預測模型建立與資料集製作

2.1 引言

基於深度學習或者卷積神經網路的研究方案成為近幾年機器人自主抓取領域的研究熱點。但來自真實環境的抓取資料集獲取是一項費時費力的工作，且很難達到網路訓練所需要的資料集規模，造成訓練得到的抓取模型非常容易過擬合，泛化能力較差。本章旨在建立二指機械手抓取模型和基於卷積神經網路的抓取預測模型，研究通過力封閉分析方法從模擬環境中獲得大規模抓取資料集及抓取質量標籤的可行性，為第三章抓取預測網路模型的訓練提供資料基礎。

2.2 基於卷積神經網路的抓取預測建模

本課題將抓取預測視為機器學習中的分類問題，使訓練好的網路模型能夠給出當前抓取姿態下成功和失敗抓取物體的概率 p 和1-p ，從候選抓取集合中選擇成功概率最大的一組抓取點執行抓取動作。為獲得足夠規模的抓取資料集，避免訓練過擬合，提高模型的泛化預測能力，本課題從模擬環境而非真實場景中獲取抓取資料集，並通過力封閉方法分析得到資料標籤。

上述過程涉及兩個重要的數學模型，它們構成了機器人自主抓取系統的理論基礎。

（1） 基於卷積神經網路方法的抓取預測模型：建立基於卷積神經網路的抓取預測模型，確定卷積神經網路的輸入輸出物件和訓練集格式，定義抓取預測問題的成本函式，指導網路權重的更新方向。

（2） 機械手抓取模型：分析機械手接觸模型，使用力封閉方法計算機械手抓取質量，為從而得到訓練集資料標籤。

總結人類對於各種物品的抓取不難發現，當物體以某一特定的穩定位姿擺放於平面上時，人眼只要獲取有關物體的 2.5D 影像即可對某抓取的可行性做出正確率極高的判斷。假設人眼獲得的 2.5D 影像（深度影像）定義為 y ，某一抓取點定義為u ，抓取質量定義為 S，則人類可以依靠自身獲得的經驗在已知（u,y） 的情況下給出 S $\in$ {0,1} 。因此，u, y,S 之間必然存在某種必然的聯絡。本課題擬採用卷積神經網路模型學習三者之間的聯絡，生成抓取質量預測函式。

首先，本文假設待抓取的物體具有足夠的剛度，保證在抓取的過程中物體不會因為受力而發生形變；其次，假設物體是以穩定位姿放置在平面上，排除比較極端的擺放姿態；最後，要求相機的安裝位置始終位於物體的上方，簡化後續渲染資料集的過程。

2.4 建立抓取資料庫和訓練集

卷積神經網路的訓練需要相當規模的資料集，本課題採取利用模擬環境生成抓取訓練集的方法首先建立一個包含 1500 個家庭常見生活物品的 3D 模型庫，然後在每個模型表面生成至多 250 個抓取點，並計算、儲存對應的魯棒抓取質量 E（u） ；與此同時，記錄每個物體至多 10 組穩定位姿，並針對每組穩定位姿生成並儲存若干深度影像，構成抓取資料庫。最後按照$(S_i,u_i,x_i,y_i)()$ 取樣大量抓取訓
練集。

2.4.1 建立抓取資料庫

本課題構建的 3D 模型庫由 3DNet 和 KIT 組成。3DNet[32]是奧地利 Vienna University of Technology 製作的用於物體識別和姿態估計的模型庫，共包含 20 大類 1371 個三維模型；KIT 是專門面向家用服務機器人物體識別和姿態估計任務而建立的模型庫，包含 129 個家庭常見物品的三維模型，圖 2-6 中展示了家庭中經常能夠見到幾種物品，同時也是抓取任務中非常有代表性的物件。

2.5 本章小結

本章針對二指機械手的抓取質量分析進行了數學建模，並確定抓取力螺旋空間作為抓取質量的衡量指標。考慮到實際抓取中視覺感知、機械臂標定和摩擦係數的不確定性引入的誤差，本章確定引入力封閉概率 E(u)保證誤差存在的情況下抓取質量的魯棒性。同時，本章還提出了基於卷積神經網路訓練的方法尋找抓取點、深度影像和抓取質量之間的關係，完成對抓取質量的快速預測，同時，利用卷積神經網路的泛化能力，期望能解決對未知物體的抓取質量預測，這一部分能力將在第四章抓取實驗中評估。最後，利用已有的 3D 模型庫建立了抓取資料庫，並通過取樣的方式獲得了規模可觀的訓練集供卷積神經網路學習使用。

第3章 抓取預測網路模型框架搭建與訓練

3.1 引言

卷積神經網路在計算機視覺領域諸多方面擁有非常優異的表現，相比於傳統的深度學習網路將一幅 24x24畫素的圖片調整為 576 維向量作為輸入，卷積神經網路能夠保留影像中各畫素之間的位置結構資訊，更好的學習影像的深層特徵。本章將介紹本課題抓取預測卷積神經網路結構以及訓練方法，並在測試集上測試模型對深度影像、抓取點和抓取質量預測之間關係的學習效果。最後，針對隨機取樣候選抓取集出現的問題提出一種新的取樣方法，並驗證新方法的實際效果。

3.2 抓取預測網路架構搭建

3.2.1 卷積層

卷積層是卷積網路的核心構建模組，其功能類似於普通神經網路的隱含層。卷積層的核心思想有兩點，其一是允許每個神經元只與前一層區域性的若干神經元連線，其二是同一隱含層的神經元共享權重引數，如圖 3-1 所示，同一隱含層共享的權重引數稱為“卷積核”。依然以 200x200x3尺寸的影像為例，使用卷積的第一隱含層的權重引數總共只有3x3 =9個，而同樣神經元數目下全連線所需要的權重引數是5x5x9 =225個，相差達 25 倍，當影像尺寸更大時效果更明顯。

3.2.2 池化層

通常，在卷積網路架構中連續卷積層之間會定期插入池化層。其功能是逐步減小表示的空間大小，以減小網路的計算量，控制過度擬合。池化層作用於輸入的每個深度切片，如圖 3-3 所示，因此池化層並不改變輸入的深度，只是減小每個深度切片的尺寸。同卷積層一樣，池化層也存在超引數：過濾器的尺寸 f 和步長 S 。目前有兩種常用的池化方法：

（1）平均池化法：取過濾器範圍內的平均值，能夠減小領域大小受限造成的估計值方差增大，但是目前已經不常使用。

（2）最大池化法：取過濾器中最大的輸入值，能夠減小卷積層引數誤差造成估計均值的偏移，更多的保留紋理資訊。圖 3-3 採用的就是最大池化方法。

3.2.3 整體網路架構建立

正如第一章綜述中提到的，本課題按照抓取生成問題兩步走的思路，首先生成候選抓取，然後進行抓取預測評估抓取可行性最高的抓取點。同時，本課題將抓取預測視為二分類問題：抓取成功 S = 1和抓取失敗 S = 0 。按照 3.2.3 節中的內容，卷積網路最終的輸出層是一個 softmax 層，輸出在給定深度影像 y 下抓取點u 成功的概率 p（S =1| y,u） 和失敗的概率 p（S =0| y,u）。

該網路採用雙通道結構，如圖 3-4 所示。上面通道獲取深度影像，經四個卷積層、一個池化層以及一個全連線層輸出 1024 維向量的抽象特徵；下面通道單獨處理抓取點的深度值 z ，經過包含 16 個神經元的全連線層分解成 16 維向量；然後將這兩個通道的向量融合組成 1040 維向量，再次進入一個包含 1024 個神經元節點全連線層處理，最後由 softmax 層輸出得到 $[p(S=1|y,u)p(S=0|y,u){]}^T$。該網路架構各層引數見表 3-1。

3.5 本章小結

本章詳細介紹了抓取預測卷積神經網路的框架和訓練過程，針對本網路中涉及的優化演算法、成本函式正則化和輸入特徵歸一化等訓練技巧展開詳細的敘述，對網路超引數的調整給出了訓練技巧。最後在驗證集上進行驗證保證訓練模型沒有出現過擬合現象，資料表明本網路具有不錯的預測泛化能力。

第4章 取樣候選抓取預測集合

4.1 引言

取樣候選抓取預測集合是抓取生成的最後一個步驟，利用第三章的抓取預測網路對候選抓取預測集合中的每一組抓取進行抓取預測和抓取成功置信度排序，從中挑選置信度最高的一組抓取完成抓取生成。本章的工作有兩點：1）如何構建物體的抓取全集空間；2）如何保證候選抓取子集空間擁有儘可能多的高置信度抓取，最大可能的得到近似全域性最優解。

4.6 本章小結

本章工作聚焦在如何更好的取樣得到候選抓取預測集合。首先利用拉普拉斯運算元提取了物體邊緣，在其邊緣畫素中尋找所有滿足對跖性的畫素點對，構成抓取空間全集。然後利用交叉熵方法更新計算重要性函式 $f(X;w)$，本章最後進行了實物驗證，證明了重要性取樣方法獲得的候選抓取預測集合優於隨機取樣方法，並基於$argmax{Q}_{\theta }(u,y),u\in U$ 確定了最終的抓取位姿。

第5章 綜合實驗平臺搭建與演算法驗證

5.1 引言

本章的工作內容是搭建模擬和實物實驗平臺，整合第三、四章演算法，基於 ROS（機器人作業系統）分散式框架優勢搭建抓取姿態生成軟體系統，設計實驗驗證本論文所提出的抓取演算法的可行性。本章實驗設計緊密貼合研究目標，一要測試系統在視覺感知和機械臂運動有誤差情況下的效能，二要測試系統在有噪聲真實環境下的抓取泛化預測能力。

5.2 抓取系統軟體框架

圖 5-1 是抓取系統的軟體框架。硬體層包括感測器和直流（或者虛擬）電機，系統感測器件使用能夠獲取深度影像的 TSP-V3 特種相機，直流電機用於控制機械臂各關節運動，模擬平臺使用到 Gazebo 軟體，可以視為本系統中的虛擬硬體；演算法層是本系統的核心層，依靠 ROS 系統的分散式框架優勢，本課題將候選取樣、 抓取預測和策略擇優模組分散設計在不同的節點，模組之間的執行互不依賴，提高
程式的可讀和可寫性。為驗證生成的一組抓取的真實抓取可行性，首先將抓取位置從相機座標系轉換到機器人座標系，然後藉助 MoveIt!開源運動規劃庫完成機械臂的軌跡規劃。為保證使用者能夠視覺化抓取規劃整個過程，本系統還提供使用者互動介面。一方面，依靠 ROS 提供的 Rviz 軟體和 Gazebo 使用者介面使使用者視覺化整個抓取過程；另一方面，在策略擇優模組適時地顯示最優抓取的深度影像，使使用者能夠直觀的判斷生成的抓取姿態的好壞。

5.3 系統效能評價指標的建立

本課題旨在解決系統誤差對機器人抓取的影響，並希望卷積神經網路能夠提取影像高階特徵泛化抓取預測的能力。基於此目標，本文在第二章資料集標籤製作中引入力封閉概率 E（g）作為抓取質量的衡量指標，其發明者指出該指標相較於傳統的力封閉指標能夠明顯提高生成的抓取的魯棒性。本章實驗目的有三點：

（1）驗證模擬環境下抓取系統在有誤差情況下對已知物體的抓取效果，抓取實驗物件使用抓取資料庫中的物體模型；
（2）驗證模擬環境下抓取系統在有誤差情況下對未知物體的抓取效果，抓取實驗物件使用 YCB 資料集中的物體模型；
（3）驗證真實環境中抓取系統的抓取效果，抓取實驗物件使用生活中常見的幾種物品。考查系統在獲取的深度影像有噪聲、系統誤差不確定情況下對未知物體的抓取泛化能力。

在抓取規劃系統程式的邏輯設計上，如果最優抓取預測的抓取成功置信度低於 50%，則輸出“無法抓取”且不執行任何動作；反之，才輸出到 MoveIt!軌跡規劃模組控制機械臂運動。本課題將機械臂執行一次抓取動作視為一次“抓取”實驗，將機械臂抓取物體並且在機械臂移動過程中不掉落視為一次“抓取成功”實驗。統計實驗總次數$N_1$ ，實際抓取成功的實驗次數$N_2$ ，將$N2/N_1\times 100\%$ 作為本課題實驗系統評價指標。

5.4 模擬環境下抓取生成演算法測試

5.4.1 模擬實驗平臺搭建

ROS（Robot Operating System）是目前非常先進的跨平臺機器人作業系統，擁有靈活、通用的機器人軟體編寫框架，並且提供強大的 Rviz 視覺化介面。自 2007年史丹佛大學人工智慧實驗室開發 ROS 至今，已經有越來越多的研究團隊通過開放原始碼的方式加入到 ROS 開發中，使得越來越多的優秀機器人演算法得以封裝到ROS 中；另一方面，ROS 社群更是以極快的增長速度支援更多的機器人硬體，避免了後來人重複的進行機器人底層開發工作，縮短了機器人設計開發的週期，圖5-2 給出 ROS 支援的部分機器人硬體平臺。基於上述優點，本課題選擇 ROS 作為機器人抓取生成演算法開發平臺，使用 MoveIt!作為機械臂軌跡規劃的工具，同時使用與 ROS 相容性較好的 gazebo 作為物理模擬平臺。

5.4.1.1 編寫 URDF

URDF（全稱 Unified Robot Description Format）是一種統一機器人描述檔案，使用 XML 格式來描述機器人模型，使用關鍵字 link、joint 分別定義機器人連桿和關節屬性。其中 link 標籤下會定義連桿的質量 mass、慣性 inertia、形狀等性質，joint 標籤會定義關節的連線形式 type、關節轉動限制 limit 以及相鄰連桿座標系的位姿關係等屬性。通過上述內容就可以建立最基本的機器人模型描述檔案。圖 5-3 a)是本課題使用的機器人在 SolidWorks 中三維模型展示圖，圖 5-3 b)是該機器人URDF 檔案的部分內容，圖 5-3 c)是通過 Rviz 進行視覺化的結果。

5.4.1.2 配置 MoveIt!

MoveIt!是目前針對移動操作最先進的開源軟體庫，結合了運動規劃、操作、三維感知、運動學、控制和導航的最新進展，廣泛用於工業、商業領域。MoveIt!核心功能是由 move_group 節點提供的，可實現的功能如圖 5-4 所示。該節點一方面會獲取 ROS 引數伺服器中的機器人模型資訊 URDF 和關節約束、正運動學及運動規劃等配置資訊；另一方面還提供 action 和 service 通訊機制，支援實現抓（pick）、放（place）和運動規劃的 action 請求，提供機器人正逆運動學查詢、路徑合法性等查詢服務；同時，還接收感測器傳輸的資料進行三維環境感知，從 joint_state 話題接收機器人各關節的狀態，通過 JointTrajectoryAction 與機器人控制器建立聯絡，控制機器人運動。

在使用 move_group 節點進行運動規劃之前，需要根據自己的機器人配置MoveIt!，包括設定機器人的免碰撞檢測矩陣、機器人運動規劃群組、確定末端執行器。圖 5-5 a)給出 MoveIt!配置中最重要的機器人運動規劃群組的設定，圖 5-5 b)在 Rviz 中視覺化 move_group 節點完成的一次機械臂運動規劃。

5.4.1.3 搭建 ROS 和 Gazebo 聯合模擬平臺

Gazebo 是一款機器人動力學模擬軟體，能夠提供高精度的物理模擬。同時，它還提供一個豐富的機器人環境庫和一系列感測器、控制器外掛。相比 V-REP 和OpenRAVE，Gazebo 開發者通過維護 gazebo_ros 軟體包更好使用 ROS 與 Gazebo進行節點間的通訊，從而成為 ROS 系統中使用的預設機器人模擬軟體。

ROS 作為機器人頂層演算法開發平臺，如何與底層硬體進行通訊是一大難題。為解決這一問題，ROS 維護 ros_control 軟體包，圖 5-6 能夠跟蹤資料流的運動。每個機器人會有若干個控制器，每個控制器可以完成對關節的控制，請求下層的硬體資源，同時每個控制器還可以進行 PID 調節。ros_control 提供了多種控制器，包括力矩控制器、位置控制器、速度控制器以及關節軌跡控制器，可以滿足對不同控制的需求，關節狀態控制器可以實時讀取硬體資源介面中各關節的旋轉角度反給上層演算法。同時，為了能夠方便、有效的管理各種控制器，ros_control 提供了controller_manager 節點。硬體資源介面層（Hardware Resource Interface Layer）用來傳送命令給硬體，並且從硬體中接收反饋資料；硬體抽象層（RobotHWSim）直接和 Gazebo 中的硬體打交道，通過 write 和 read 的方式來完成硬體的操作。

因此，要完成 ROS 和 Gazebo 的聯合模擬，一方面要向機器人描述檔案中新增更多關於 Gazebo 的資訊，此時不再使用 URDF 檔案描述機器人模型，而是使用另一種 xacro 檔案，在保留原 URDF 內容的基礎上，新增 gazebo 標籤，增加針對深度影像獲取的感測器外掛，確定硬體介面型別；另一方面還要做好各控制器的配置工作，本課題需要規劃機械臂運動軌跡完成抓取任務，同時實時獲得各關節的旋轉角度資訊，因此選擇啟用 joint_state_controller 和 joint_trajectory_controller 兩個控制器，為每個控制器編寫相關配置檔案。圖 5-7 a)和 b)表示的是 MoveIt!中運動規劃的起始和終止狀態，c)圖和 d)圖展示的是通過 ros_control 控制模擬機器人的運動狀態，可以發現兩者能夠達到同步，因此說明 ROS 和 Gazebo 的聯合模擬平臺已經搭建成功，可以通過在 ROS 中編寫演算法程式實現控制模擬環境中的機器人運動。

圖 5-8 是 Gazebo 下搭建的模擬實驗環境，由帶有機械手的機器人、深度相機、待抓取物體和儲物盒組成。圖中機械臂的位置為抓取生成前的觀察位置，每次實驗前機械臂都將運動到該位置獲取物體的深度影像，實驗過程中將物體平穩的抓起放到儲物盒中視為一次成功抓取。

5.4.2 模擬實驗方案

由於抓取預測網路模型的訓練集來自於模擬環境，基於控制變數的思想，首先在模擬環境中驗證抓取系統對已知和未知物體進行抓取測試。為在模擬環境中模擬系統誤差，將其定義為物體在放置平面上的移動和轉動，用$(x,y,\theta )$ 表示，如圖5-9 所示，其中 x, y 表示物體在平面上的移動距離，$\theta$ 表示繞垂直平面方向的旋轉角度。物體的移動和旋轉是以規劃得到的抓取中心為原點。

根據 Peter K. Allen 等人的研究，誤差模型引數的變動範圍見表 5-1。

模擬實驗中的抓取物體模型來自於已有的資料集，利用 Gazebo 中 add_model命令新增物體.stl 格式三維模型，建立 Gazebo 模擬軟體標準的物體描述檔案（sdf檔案）。圖 5-10 給出 Gazebo 中抓取模擬實驗所要抓取的物體模型。

其中對已知物體抓取效果測試中的抓取物件來自於第二章提到的KIT資料集，從中選擇 6 件物品：沐浴露瓶、黃鹽方盒、牙膏、噴霧瓶、水杯和玻璃碗，見圖圖5-10 a)所示。對未知物體抓取效果測試的抓取物件來自於 YCB 資料集，YCB 資料集是由耶魯大學、卡耐基梅隆大學和加州伯克利分校共同建立的物體模型資料集，是目前世界上公認的機器人操作基準測試集之一，從中挑選 8 件生活中常用的不規則物體：杯子、香蕉、可樂罐、汽水瓶、罐頭、電鑽、榔頭和活動扳手，如圖 5-10 b)所示。

實驗設定 6 組誤差引數，每組誤差引數包含平面平移資訊 x y 和繞垂直平面方向旋轉資訊$\theta$ ： $(0mm,0mm,0^{\circ })$,$(2mm,2mm,4^{\circ })$，$(4mm,4mm,8^{\circ })$，$(6mm,6mm,12^{\circ })$，$(8mm,8mm,16^{\circ })$，$(10mm,10mm,20^{\circ })$，每組誤差引數均要抓滿所有14 件物品，共 840次實驗。其中$(0mm,0mm,0^{\circ })$代表無系統誤差。

對已知物體和未知物體的抓取實驗按照統一的實驗步驟有序進行：

（1）預定義機械臂觀察位置並固定，如圖 5-11 a)所示。每次抓取實驗前機械臂都將運動到此狀態觀察物體，獲取場景的深度影像，如圖 5-11 b)所示；
（2）選擇一件物品以任意的穩定位姿放置在相機可視範圍之內；
（3）啟動抓取系統生成最優抓取位姿，將表徵規劃抓取在機械臂基座標系下位姿的四元數和平移向量傳送到話題等待 move_group 節點訂閱完成軌跡規劃；
（4）在 Gazebo 模擬環境中按照 6 組誤差引數以抓取點為原點、增量式更改物體位置和姿態，如圖 5-11 所示。分別執行抓取軌跡，記錄抓取效果；

（5）重複（2）（3）（4）步驟直到抓完所有 14 件物品，統計抓取次數$N_1$ 和抓取成功次數$N_2$ 。

5.4.3 模擬實驗驗證

按照上一小節介紹的實驗步驟開展實驗，模擬實驗執行在筆者的膝上型電腦上，搭載 UBUNTU 16.04 作業系統，CPU 為 Intel Core i5-4200M @ 2.50GHz x 4，GPU 為 AMD R5（此 GPU 僅僅用於抓取實驗，卷積神經網路模型的訓練是使用實驗室工作站 GPU NVIDIA GeForce GTX 1080Ti）。附表 1 和附表 2 給出對已知物體的抓取模擬實驗資料，表中各項資料自左向右依次是：實驗次數$N_0$ 、抓取次數$N_1$ 和真實抓取成功次數$N_2$ 。

實驗發現，本課題提出的抓取規劃系統的抓取次數與實驗次數幾乎相當，說明其總“認為”物體能夠被抓取，即總能夠輸出一個抓取姿態。在所有的抓取實驗次數中，系統對已知物體的抓取成功率能夠達到 90%左右，特別是在沒有系統誤差的情況下成功率能夠達到 93.33%。另一方面，隨著系統誤差的增加，抓取成功率受到影響，但即使在最大誤差 $(10mm,10mm,20^{\circ })$ 的情況下抓取成功率仍能達到86.67%，與無誤差情況相比波動在 7 個百分點左右。說明對系統誤差具備一定的魯棒性。

附表 3 和附表 4 給出對未知物體的抓取模擬實驗資料。雖然 YCB 資料集中的物體模型並不在訓練集中，但是抓取系統對其抓取的成功率還是能夠達到 85%左右。這是因為像杯子、方形罐頭、可樂罐這些物體的形狀與 KIT 資料集中的杯子、方形盒以及圓柱罐頭外形相似，說明抓取預測網路模型在訓練中能夠提取深度影像中的抽象特徵，將其泛化在對未知或相似物體的抓取質量預測上。另一方面，由實驗資料可以看出，在對未知物體的抓取成功率也維持在 7 個百分點左右，進一步說明引入力封閉概率作為抓取質量的評價指標是一個正確的選擇。

實驗中發現，當規劃得到的抓取姿態在一個狹小的近封閉空間中，這時得到的抓取點雖然直觀上觀察能夠成功抓取，但是其對系統誤差會更加敏感，在超過$(6mm,6mm,12^{\circ })$ 的誤差後很容易導致機械手在自上向下抓取的過程中懟到物體上，或者直接運動到近封閉空間之外，根本無法抓取抓取，如圖 5-12 右側案例所示。左側給出模擬實驗中一部分抓取成功的實驗示例。

5.5 實物實驗平臺搭建

5.5.1 相機內參標定

視覺感知非常重要的一步是根據某點在畫素座標系中的位置計算三維環境空間中該點在相機座標系中的座標，進而確定其在世界座標系中的位置。這一步的準確程度直接影響視覺感知的精度，進一步會影響生成的最優抓取結果。因此準確的標定相機的內外參和畸變是實驗準備環節的重要一環。

本課題使用的 TSP-V3 特種相機是由深圳繁維科技有限公司生產的基於 TOF技術的 CCD 小型深度相機模組產品，可通過 micro USB 介面輸出 640*480 畫素的深度影像、灰度影像和點雲資料。其技術引數見表 5-2。

目前使用比較廣泛的內外參標定方法是張正友標定法，通過記錄 n 組點的世界座標系及其對應的畫素座標系，利用最小二乘法進行內引數$k_x,k_y,t_0,t_0$ 的計算，已知相機內參之後，再求畸變係數$k_1,k_2,k_3,t_1,t_2$ 。目前 matlab 和 ROS 都有計算相機內外引數和畸變的高階封裝，因為本課題對機械臂的控制以及抓取演算法都是在ROS 下實現的，故使用 ROS 的 camera_calibration 軟體包進行內引數的計算。首先將 TSP-V3 相機的驅動用 ROS 節點封裝，將相機獲得的紅外影像視訊流釋出到話題“/TSP-V3/image_ir”，然後啟動“/camera_calibrator”節點捕獲並記錄棋盤格黑白角點在世界座標系和畫素座標系的座標，如圖 5-13 b)所示，本課題總共採集 46組資料，最後按照張正友內參標定方法完成相機內參和畸變係數的計算。整個計算過程中影像和資料傳輸的路線如圖 5-13 c)所示。

本課題所使用的深度相機的相機內引數為$\left[\begin{array}{ccc}397.03& 0& 314.38\\ 0& 402.36& 113.29\\ 0& 0& 1\end{array}\right]$，徑向畸變係數$k_1=0.1179$ , $k_2=-0.1643$ ，切向畸變係數$t_1=0.011$, $t_2=-0.00274$, $t_3=-0.4378 $ 。

5.5.2 機械臂 DH 引數標定

模擬實驗中，建立的 URDF 模型一方面會作為引數輸入 MoveIt!中進行運動學建模和軌跡規劃，另一方面經 Gazebo 外掛處理轉換為 sdf 檔案載入到模擬環境中，這樣可以使得軌跡規劃的機械臂模型同虛擬實驗平臺完全同步。但是在真實實驗中，必須完全依據真實機械臂的 DH 引數建立 URDF 模型，才能儘可能減小規劃模型和實際機械臂不同步所帶來的“災難性”誤差。

本課題所用的實驗平臺為實驗室自行研製的六自由度機械臂，末端搭載二指欠驅動操作手。其依託於環保部專項課題“失控放射源探測與處置智慧機器人技術研究”，是一款用於危險環境中尋找和處置危險源的靈活機械臂。二指機械手的張合通過特殊的機械結構由一個電機控制，保證二指張合的同步性。圖 5-14 a)給出六自由度機械臂的結構簡圖和前置 DH 引數法座標系的建立，b)給出機械臂和相機之間的位置關係。

機械臂 DH 引數的標定使用的是 API 公司生產的鐳射跟蹤儀和 Spatial Analysis軟體，如圖 5-15 所示。以 2 軸和 3 軸之間的杆長2a 標定為例，首先將鐳射跟蹤儀放置在一空曠位置，軟體將以跟蹤儀所在位置建立世界座標系，保持其他軸不動只轉動第 2 軸，記錄 30 組靶球位置，在 Spatial Analysis 軟體中擬合點為右圖中綠色小圓，然後同樣保持其他軸不動只轉動第 3 軸，同樣記錄 30 組靶球位置，擬合為右圖中紫色大圓。查詢兩圓圓心距即能夠確定杆長2a 。

本課題採用前置法建立機械臂關節座標系，整個 DH 引數如表 5-3 所示。

5.5.3 手眼標定

通過視覺演算法獲得的抓取點座標是相對於相機座標系而言的，而要控制機械臂完成真正的抓取，就必須建立相機座標系和機械臂基座座標系的位姿關係${}^{base}{T}_{cam}$而這正是手眼標定所要實現的目標。圖 5-16 顯示的是“眼在手上”安裝方案的標定原理。保持基座標系固定不動，通過不斷改變末端工具座標系{E}的位置，獲取圖中多組 A 、B 、C 的關係，最終通過求解$A_{1}B{C}_1^{-1}=A_{2}B{C}_2^{-1}$ 計算相機和末端工具座標系之間的位姿關係矩陣。

本課題在深入研究手眼標定的原理之後，使用 ROS 中封裝的軟體包visp_hand2eye_calibration 完成具體的手眼標定。該軟體包提供了兩套手眼標定的服務：compute_effector_camera 和compute_effector_camera_quick，前者是通過 ROS話題機制接收基座-末端座標系關係 A 和相機-標定板座標系關係C ，而後者是利用ROS 服務機制在向 visp_hand2eye_calibration 節點申請服務時傳入申請服務的資料。在計算收集相機-物體座標關係iC 的過程中，本文使用一種目前比較常用的能夠簡單有效的估計物體位姿的方法——Aruco Marker（以下簡稱靶碼），如圖 5-17 左圖所示。其位姿估計的原理是通過檢測角點的畫素距離和實際距離之間的相似關係，計算相機和靶碼之間的距離資訊 z ，最後利用相機內參計算兩者的座標變換矩陣，圖 5-17 右圖給出的是機械臂基座標系、末端工具座標系、相機座標系以及靶碼座標系在 Rviz 中的顯示效果。

整個標定過程如圖 5-18 所示。一方面，編寫 TSP-V3 相機驅動節點，以 30Hz的頻率不斷拾取視訊幀傳送到話題/TSP-V3/image_ir，啟動 aruco_ros 節點訂閱該話題，利用 OpenCV 中角點檢測函式獲取靶碼的角點畫素座標，利用靶碼的實際長度與畫素長度計算相機和靶碼之間的距離資訊，從而獲得相機和靶碼的位姿矩陣${}^{base}{T}_{maker}$ 釋出到 tf 中；另一方面，獲取機械臂關節角度狀態值釋出到 joint_state 話題，/Robot_state_publisher 會根據機器人的 urdf 資訊建立正運動學模型，計算機械臂基座標系和末端座標系之間的關係${}^{base}{T}_{eff}$ ，同樣釋出到 tf 中。easy_handeye 節點從 tf 中獲得 ${}^{base}{T}_{eff}$和${}^{base}{T}_{maker}$，申請 Visp_eye2hand_calibration 節點的服務。

最終得到手眼矩陣：

5.6 真實環境下抓取演算法測試

5.6.1 實物實驗方案

真實環境中獲得的深度影像通常會引入自然噪聲，這對於使用源自模擬環境的資料集訓練的卷積神經網路模型的預測效果是一個很大的考驗；另一方面，模擬環節中引入的誤差引數模型難以準確描述實際存在的系統誤差。因此，本課題擬利用真實環境中的抓取效果檢測抓取系統的實際能力。與模擬實驗不同，實物實驗開始之前需要標定相機內參和畸變係數，確定畫素座標系和相機座標系之間的位姿矩陣；然後進行手眼標定確定相機和機器人基座標系之間的關係。規劃出的抓取點與機器人座標系之間的座標轉換如圖 5-19 所示。

實物實驗所抓取的物體選自生活中 5 種常見物品：礦泉水瓶、香蕉、洗衣液、膠帶和訂書機，如圖 5-20 所示。按照物體外形特徵不同可以將這 5 種物品分為三類，香蕉、礦泉水瓶和訂書機代表擁有明顯長軸的物品，膠帶代表環形物品，藍月亮洗衣液代表外形比較複雜的一類抓取物件。抓取步驟同模擬實驗相似，只是減少了物體按照誤差引數移動的實驗環節，因為本身提出誤差引數模型也是為了能夠在模擬環境中模擬系統誤差的存在。上述 5 種物體沒有任何一種同 KIT 資料集物體模型完全一致，使得實驗結果能夠驗證本課題提出的抓取方案在真實環境中對相似或未知物體的規劃抓取效果。

5.6.2 實物實驗驗證

相機內參和手眼標定完成代表實驗前的準備工作已經就緒，按照 5.3.2.2 小節提到的場景佈置抓取實驗，整個流程如圖 5-21 所示。每次實驗機械臂會首先從初始位置 a)到達觀察位置 b)，獲取物體的深度影像，然後使用重要性取樣方法生成候選抓取預測集合 c)，將每組抓取輸入到抓取預測卷積神經網路模型中預測每組抓取的成功置信度，從中選擇最優抓取 d)。然後利用座標轉換模組計算抓取點在機械臂基座標系中的位姿，依靠 MoveIt!進行軌跡規劃完成一次抓取。

按照上述步驟分別對五種實驗物件進行抓取測試，每種物體抓 10 次，每次擺放的位置和角度不同，但保證在機械臂的可達空間之內，實驗結果統計見表 5-4（實驗資料一欄自左向右表示：實驗次數、抓取次數、實際抓取成功次數）。實驗發現，該系統對實際物體的規劃成功率很高，幾乎總能發現物體可抓取的位置，即系統總是“認為”物體是可抓取的。但實際抓取成功率相較於模擬環境稍低，對礦泉水瓶的抓取遠遠低於模擬環境。這是由於礦泉水瓶的透光性，導致相機中的紅外接收器難以準確識別物體的深度和邊緣，一方面影響了邊緣提取演算法的效果，另一方面噪聲的混入使得預測網路模型的預測能力變弱。最終的抓取成功率仍能達到 76%以上，表明訓練得到的網路模型有著較好的泛化能力。

5.7 本章小結

本章基於 ROS 搭建了抓取規劃系統軟體框架，並利用 ROS-MoveIt!-Gazebo 聯合搭建了模擬實驗平臺，為驗證自主抓取系統對物體位置姿態誤差的魯棒性，本課題提出一種簡化的誤差模型$(x,y,\theta )$ ，實驗證明在誤差高達$(10mm,10mm,20^{\circ })$的情況下抓取規劃的成功率仍能達到 85%以上；與此同時，本課題搭建了實物實驗平臺驗證抓取預測模型在有噪聲的真實環境中的模型泛化能力，實驗統計的抓取成功率在 76%左右，取得了良好的泛化效果。

結論

智慧機器人概念的興起使得人們不再滿足於計算機擁有智慧的大腦，更需要機器人能夠靈活精確的同物理世界進行互動，自主抓取就是其中一項非常重要的能力。本課題旨在將卷積神經網路方法同抓取規劃問題聯絡在一起，建立基於卷積神經網路的抓取預測模型，將抓取姿態生成問題分解為“先取樣，後預測”兩個動作來實現。本論文的具體研究成果總結如下：

（1）建立了基於卷積神經網路的抓取預測模型，用抓取中心畫素位置和抓取角度定義一組抓取點。研究了二指機械手接觸抓取模型，針對傳統力封閉方法對系統誤差的敏感性，引入力封閉概率作為抓取質量的衡量指標，指導資料集標籤的結果；

（2）搭建了抓取預測卷積神經網路框架，針對權重引數訓練中的優化演算法、成本函式正則等引入的超引數進行了不斷的調整，直到模型的成本函式值能夠收斂穩定在一個較小的範圍。為了驗證訓練好的模型的泛化能力，又測試了模型在測試集上的預測效果，結果表明模型沒有出現過擬合現象；

（3）解決了如何取樣抓取成功率高的抓取點到候選抓取預測集合的問題。首先通過 Laplace 運算元提取物體邊緣及其法向量，使用對跖法確定合適的抓取集合C ；然後使用交叉熵優化方法得到重要性取樣的概率密度函式$f(X;w)$，以此進行抓取取樣就可以對抓取集合C 進行取樣生成候選抓取預測集合；

（4）基於 ROS 搭建了抓取姿態生成系統框架。使用 C++和 python 程式設計，利用 Gazebo 搭建了機器人模擬環境，對抓取規劃方法的誤差魯棒性進行評估；另一方面，整合實物平臺，進行了相機內參標定和手眼標定，驗證系統對帶有噪聲的真實環境的抓取規劃泛化能力。實驗資料顯示，本方法在模擬環境中的抓取成功率達到 85%以上，在真實環境中的抓取成功率也能達到 76%，具有較強的實用性。

本課題的抓取方案在模擬和真實實驗中取得了較好的抓取效果，但在以下兩個方面後續還可以進行更加深入的研究。

（1）目前的抓取姿態生成僅停留在單物體場景中，無法抓取多物體場景中的指定物體。後續可以在抓取規劃之前新增目標識別步驟，利用 bounding box 將待抓取物體框選出來，裁剪得到只包含單個物體的深度影像；然後再利用本課題演算法進行規劃最終的抓取姿態，實現多物體場景中指定物體抓取任務；

（2）在模擬和實際實驗中發現每次抓取規劃演算法的總時長達 2s，使得該方案難以滿足工業生產等緊湊環節的實時性需求。分析發現，其中取樣候選抓取預測集合所用時間為 1.7s 左右，佔抓取總時長的 85%。改進候選抓取預測集合的生成策略或者取樣方法可以作為後續的研究和改進方向。