基于ROS的七自由度機械臂抓取系統研究

杜玉俊， 吳盤龍

(南京理工大學 自動化學院，江蘇 南京 210014)

機械臂是機器人領域中應用最廣泛的自動化裝置，它是機械工程、電氣工程、仿生學、計算機工程、智能控制等多個學科最新成果的集成,是衡量一個國家制造技術水平的重要標志[1]。機械臂控制方法根據是否采用反饋信息可以分成開環控制和閉環控制。在開環控制中，機械臂與控制單元之間沒有反饋，相對簡單，但對機械臂的精度要求很高。相比于開環控制，閉環控制使用反饋信息，在運動過程中不斷更新，計算復雜，但降低了機械臂的精度要求[2]。

隨著工業4.0時代的到來，機械臂的工作環境更加復雜,其性能要求也不斷提高。控制任務不僅要滿足末端位姿，還需要實現避障、避免奇異構型、避免關節突變和關節極限，傳統六自由度機械臂已經無法滿足人們的需求。七自由度機械臂由于增加了冗余量，具有更高的靈活性，能在復雜環境中完成不同的任務，但因為添加了冗余量，所以結構分析與控制也更加復雜。

機械臂的軌跡規劃可分為笛卡爾空間軌跡規劃和關節空間軌跡規劃兩類。笛卡爾空間軌跡規劃是先通過上層找出一條從運動起點到終點并無碰撞的路徑，之后投影到各個關節空間并生成可行軌跡。而關節空間軌跡規劃是通過底層控制來實現軌跡規劃，相比于笛卡爾空間軌跡規劃，該方法的計算量要小得多。

視覺是人類感知世界的重要手段，視覺伺服系統是機械臂獲得環境信息的重要部分。視覺伺服系統根據反饋信息不同，可分為基于位置(Position-Based Visual Servoing,PBVS)和基于圖像(Image-Based Visual Servoing,IBVS)兩種。基于位置的視覺伺服系統利用相機的參數建立圖像信號與機械臂位姿之間的映射關系，借助圖像信號提取機械臂位姿信息，并將它們與目標物位姿進行比較，進行閉環反饋控制。基于圖像的視覺伺服系統則將圖像信號與目標位姿的圖像信號直接進行比較，利用獲得的圖像誤差進行閉環反饋控制。根據相機安裝方式不同，基于圖像的視覺伺服系統可分成兩類：相機安裝在機械臂末端位置(eyes in hand)和相機固定在世界坐標系上(eye to hand)。

本文針對機械臂的自動化、智能化、自主化的性能要求，對七自由度機械臂抓取進行系統設計。該系統基于視覺的機械臂控制，屬于閉環控制，包括機械臂控制、圖像采集設備和上位機三部分，可在復雜多障礙物環境下完成目標檢測與定位、目標跟蹤和抓取任務。本文首先介紹系統硬件設計以及總體控制方案；然后對系統每部分的控制方案進行分析，包括視覺伺服部分和機械臂控制部分；最后介紹實驗結果并進行分析，驗證機械臂運動學結果以及真實環境下機械臂控制的實驗結果。

1 系統硬件設計

系統硬件包括機械臂控制、圖像采集設備和上位機三部分。其中，機械臂控制包括機械臂本體、驅動控制和交互式控制接口；圖像采集設備安裝在機械臂末端獲取目標物和環境的信息；上位機完成圖像處理、坐標轉換、計算目標以及障礙物的位置和尺寸、軌跡規劃等任務，并發送給機械臂控制柜，控制柜控制電機使機械臂按照已規劃好的軌跡運動。

1.1 Franka Panda機械臂

本文研究的機械臂如圖1所示，它是德國Franka Emika Panda公司推出的高精度、輕型、七自由度機械臂。該機械臂提供開源接口(FCI)，允許快速直接的雙向鏈接，可實時提供機械臂當前狀態。

圖1 Franka Panda及其工作空間范圍

1.2 Kinect V1攝像頭

由于機械臂的工作空間范圍小，所以選擇一種較經典的深度攝像機Kinect V1。該攝像頭既能滿足系統的要求，價格又便宜，同時可輸出分辨率為640像素×480像素的彩色圖像和分辨率為320像素×240像素的深度圖像。Kinect V1的結構如圖2所示。

圖2 Kinect V1的結構

2 系統控制方案

系統是由上位機通過ROS(Robot Operating System)集成開發環境來實現控制的。ROS是一個適用于機器人編程的框架，它把原本松散的各類軟件耦合在一起，并提供了通信架構；此外，ROS還提供完善的開發工具，讓開發者更容易上手[3]。上位機通過以太網通信方式連接到機械臂和深度攝像頭，系統控制流程如圖3所示。

圖3 系統控制流程

攝像頭先進行標定，之后采集數據并發送到上位機進行處理，上位機將使用Linemod方法對已經訓練的模板和捕獲的目標圖像進行比較，估計出目標的位置和姿態。

機械臂的控制流程是首先進行笛卡爾空間軌跡規劃獲得末端路徑、插補點和各關節在插補點的角度，之后進行關節空間軌跡規劃控制關節速度、加速度，保證機械臂能夠穩定運動。笛卡爾空間軌跡規劃將根據環境信息采用RRT*算法規劃出一條末端可行路徑。使用正弦速度曲線控制機械臂末端速度，添加路徑插補點，并對機械臂進行逆運動學求解獲得各關節的角度。通過微分運動學的雅可比矩陣求出各關節的角速度和角加速度。關節空間軌跡規劃使用五次多項式軌跡規劃，保證各關節經過各個插補點的銜接過程平滑、無沖擊。最后把規劃信息發送給控制柜進行控制，保證機械臂按照規劃好的軌跡進行運動。

2.1 視覺系統

視覺系統是機械臂抓取系統的重要部分，是機械臂抓取系統獲得外界環境信息的手段。下面將描述視覺系統的搭建、攝像頭標定和目標位姿估計方法。

2.1.1 視覺系統的描述

視覺系統搭建如圖4所示，攝像頭安裝固定在機械臂末端，隨著機械臂的運動，目標在圖像中的位置會發生改變。攝像頭拍攝過程中把物體空間的三維信息變成了圖像的二維信息，所以需要建立坐標轉換關系，從目標的圖像信息中計算出目標在世界坐標系的位置與姿態。

圖4 視覺系統搭建

設空間任意點P在世界坐標系的坐標為(xP,yP,zP),點P在像素坐標系的映射為p(up,vp),則兩坐標系的映射關系可表示為

(1)

式中，MTC為攝像頭內參矩陣，可通過攝像頭標定獲得；CTW為攝像頭外參矩陣，可通過采樣n個已知其在世界坐標系和像素坐標系的坐標計算得出。

2.1.2 攝像頭標定

攝像頭內參矩陣通過攝像頭標定獲得，本文采用張正友于1998年提出的單平面棋盤格標定法，該方法精度相當高，操作簡單，無須高性能設備[4]。攝像頭標定板尺寸為8×6，如圖5所示。

圖5 攝像頭的標定板

Kinect的標定結果如表1所示。

表1 攝像頭標定結果

2.1.3 目標位姿估計

目標檢測與定位使用Linemod方法，由Hinterstoisser等[5]在2011年提出，能解決在復雜環境下3D物體的實時檢測與定位。該方法同時采用彩色圖像和深度圖像信息，可以對應無紋理的情況而不需要增加訓練時間。Linemod特征通過彩色圖像的梯度信息結合物體表面的法向特征作為模板的匹配依據。該方法可以用較短的訓練時間進行訓練RGBD信息來得到物體信息、位置、姿態以及可信度。

Linemod算法主要步驟如下：

① 計算梯度方向。計算每個像素點梯度方向和幅值，去掉低于閾值的梯度，統計在3×3的領域內所有梯度方向，數量超過5才會被采用。提取主成分并放大提取到的特征。

② 在一定鄰域內對所提取的特征進行擴散，利用模板進行滑窗匹配得到容錯度。

③ 預處理響應圖。對8方向得到的擴散圖逐個像素進行匹配(每像素點具有8張預處理圖)，匹配得到距離最近方向角度的余弦值。

④ 線性存儲步驟③得到的結果，并擴展到深度信息圖再次處理。

2.2 機械臂控制

根據機械臂的結構，建立modified D-H坐標系如圖6所示。其中，xi和zi分別為第i(i=1,2,…,7)關節的X軸和Z軸，通過右手定則確定Y軸的方向。建立機械臂D-H坐標系后，可用以下4個參數來描述機械臂關節之間的相對運動變量：連桿長度ai-1、兩連桿距離di、兩連桿夾角θi、連桿扭角αi-1[6]。

D-H坐標參數如表2所示。

圖6 機械臂簡化和modified D-H坐標系建立

表2 Franka Panda的D-H坐標參數

表2中，d1=0.333;d3=0.316;d5=0.384;df= 0.107;a4=0.0825;a5=-0.0825;a7=0.088。

2.2.1 機械臂正運動學

根據坐標系變換的鏈式法則，坐標系{i-1}到坐標系{i}的變換矩陣可表達為

i-1Ti=Rotxi-1(αi-1)Transxi-1(ai-1)Rotzi(θi)Transzi(di)

(2)

將表2的數據代入式(2)可得：

(3)

因此，末端在世界坐標系的位姿矩陣為

0Tf=0T1×1T2×2T3×3T4×4T5×5T6×6Tf

(4)

式中，n，o，a為姿態描述；p為位置描述。式(4)為機械臂正運動方程，給定各關節的角度可唯一確定末端位姿。

2.2.2 機械臂逆運動學

如果一個結構具有3個旋轉關節，其中關節軸線兩兩垂直并相交于一點就可以視為一個球關節[7]。Franka Panda的關節1～關節3具有這樣的結構，所以可以看作球關節，相當于人的肩部，記作O點(位于關節2位置)。關節5～關節7雖然沒有這樣的結構，但當末端位置不變，關節7的姿態改變并不影響關節6的位置，同時，關節5和關節6的軸垂直并相交在關節6位置，所以，可以把關節5～關節7的結構看作球關節，記作A點(位于關節6位置)，關節4位置記為B點。Franka Panda機械臂的簡化圖如圖7所示。

圖7 Franka Panda 的簡化圖

根據幾何關系可得：

(5)

又有：

(6)

當[(OA2+AB2-OB2)/(2OA·AB)]>1時，沒有α2解的可能，這意味著設置的條件無效。如果存在解，B點可以確定，根據幾何關系很容易確認θM和θN的角度。從圖9可以看出，B點可以繞著軸OA做半徑為OBB的圓周(OB;OBB)運動，相當于人的肘部。當B在圓周上旋轉時，M和N也跟著在圓周(OM;OMM)和(ON;ONN)旋轉。所以當已知A點的坐標，在滿足一定的條件下(角度極限、奇異構型、避免障礙物)可找到B點的坐標范圍{B}，并利用幾何關系找出M和N的坐標范圍{M}和{N}。

當知道機械臂末端需要走的路徑時，機械臂某一時刻ti的B點坐標為Bi，在下一時刻ti+1的Bi+1是在范圍{B}內離Bi最近的點，即選擇運動范圍最小的B點。

2.2.3 路徑規劃策略——改進RRT*運動規劃算法

如已知機械臂控制流程，機械臂進行抓取任務之前，需規劃出一條可行的末端路徑，本文選擇RRT*作為機械臂的路徑規劃算法。RRT*算法在快速搜索隨機樹(Rapidly-Exploring Random Trees，RRT)算法的基礎上改進了父節點選擇的方式，采用代價函數來選取擴展節點鄰域內最小代價的節點為父節點，同時，每次迭代后都會重新連接現有樹上的節點，從而保證計算復雜度和漸進最優解[8]。

通過RRT*探索出的路徑往往是曲曲折折有小波浪的，如果末端按照這條路徑運動會造成機械臂關節的抖動和沖擊，影響抓取任務的完成。事實上最佳路徑一般是直線，因此需進行優化路徑讓規劃路徑更光滑并適合實際工程的機械臂運動。筆者對RRT*所規劃出的路徑進行了優化，改進效果如圖8所示。

圖8 RRT*算法改進效果

其原理是：依次遍歷所有節點，判斷當前的節點與后面節點連線是否與障礙物碰撞，若不碰撞則去掉原路徑中兩個節點間的所有節點，若碰撞則原路不變并考慮下一個節點，直到遍歷結束，最后添加插值點并保留最終路徑。

2.2.4 末端速度控制策略——正弦速度曲線

實際運動過程中，起始點和終點的速度一般為0，所以機械臂運動一般分為加速段、勻速段和減速段3個階段，需要進行加減速度控制來避免機械臂因速度突變導致沖擊和驅動器過載等事故。常用的加減速度控制策略有梯形速度曲線、S型速度曲線和正弦速度曲線。

正弦速度曲線過程分為加速、勻速和減速3個階段，其中加速度按正弦規律變化[9]，如圖9所示。

圖9 正弦速度曲線

對一條軌跡S，設定最大速度為Vm，最大加速度為A，最大減速度為D，則整個運動過程中，加速度、速度和位移的表達式為

(7)

積分可得速度和位移的表達式為

(8)

(9)

2.2.5 機械臂微分運動學

速度控制獲得末端執行器的速度、加速度之后，需映射到各關節的角速度和角加速度，引入雅可比矩陣J定義機械臂末端的速度向量ve與各關節角速度向量的映射關系[10]：

(10)

其中，J矩陣中的每一列的計算公式為

(11)

式中,n,o,a,p為矩陣i-1Tf的列向量。

(12)

式中，D為對稱正定加權矩陣。應用Lagrange乘法，式(12)解為

(13)

當D=I時，

J+=JT(JJT)-1

(14)

為雅可比矩陣的偽逆。

2.2.6 關節空間軌跡規劃策略——五次多項式插補

機械臂各連桿運動需要通過關節驅動器來實現，關節空間的常用方法有多項式軌跡規劃、三次樣條插值和B次樣條插值[11]。

多項式軌跡規劃是將關節角位置表示成多項式形式，常用的方法有三次多項式、五次多項式和七次多項式。其中，三次多項式有4個未知參數，只能設置起始點和終點的角位置和角速度，所以當經過多個插補點時，三次多項式軌跡規劃只能保證位置和速度連續，如圖10所示。五次多項式能保證機械臂經過多個插補點時角位置、速度和加速度連續，但沖擊量(角位置的三階導數)無法保證為0，如圖11所示。七次多項式雖然能保證沖擊量為0，但極大地增加了計算量。

圖10 三次多項式軌跡規劃結果

圖11 五次多項式軌跡規劃結果

考慮到機械臂在運動過程中要經過多個插補點，所以選擇五次多項式軌跡規劃作為關節空間軌跡規劃，既能保證角位置、角速度和角加速度連續，又減少了計算量[4]。

3 實驗結果與分析

3.1 機械臂軌跡規劃

在Matlab環境下對Franka Panda機械臂進行運動仿真。為了方便計算，機械臂連桿單位為厘米(cm),設環境中存在4個圓球障礙物，半徑為10 cm，起始點和終點坐標為[-40,40,80]和[53,0,90]。采用RRT*算法和改進RRT*算法規劃出來的路徑如圖12所示。

圖12 路徑規劃結果

使用RRT*算法的總路徑為313.84 cm，改進RRT*算法的總路徑為101.73 cm。從仿真結果可以看出改進RRT*對路徑優化效果明顯，讓路徑更加光滑，且路徑縮短，各關節的運動穩定、連續，如圖13所示。

圖13 各關節在運動過程中的速度

3.2 機械臂抓取系統

實驗系統按照第2節設計進行搭建，通過Kinect V1攝像頭捕獲環境以及目標物的信息，并通過Linemod方法對物體進行6D位姿估計，目標物模板采用SolidWorks設計并生成.stl文件，如圖14(a)所示[12]。根據機械臂當前狀態和機械臂末端的位置要求規劃出一條路徑，添加中間點，并進行軌跡規劃，機械臂抓取系統和末端規劃路徑如圖14(b)所示。

本實驗同時觀察仿真環境和真實環境下的運動，結果如圖15和圖16所示。

圖14 目標模板以及搭建的機械臂抓取系統

圖15 仿真環境中系統運動結果

圖16 真實環境中系統運動結果

從結果來看，系統不管在仿真環境還是在真實環境都按照先前規劃的軌跡運動，機械臂在運動過程中平穩，沒有異音、振動或沖擊。

4 結束語

本文針對移動機器人上的七自由度機械臂進行系統設計，在ROS Kinetic框架下讓Franka Panda機械臂和Kinect V1攝像頭協同工作。先提出系統的硬件設計、控制流程，之后對視覺系統和機械臂控制系統進行分析。系統采用Linemod方法獲取目標和障礙物的位置和姿態，采用RRT*算法搜索機械臂末端可行路徑，并提出改進路徑方法，建立了機械臂的正逆運動學方程和軌跡規劃方案，最后進行了仿真和系統抓取實驗。

研究結果表明，所使用的方法合理，系統具有一定應用價值。