基于ROS的七自由度機械臂抓取系統(tǒng)研究

杜玉俊， 吳盤龍

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

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

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

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

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

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

1 系統(tǒng)硬件設計

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

1.1 Franka Panda機械臂

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

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

1.2 Kinect V1攝像頭

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

圖2 Kinect V1的結(jié)構(gòu)

2 系統(tǒng)控制方案

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

圖3 系統(tǒng)控制流程

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

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

2.1 視覺系統(tǒng)

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

2.1.1 視覺系統(tǒng)的描述

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

圖4 視覺系統(tǒng)搭建

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

(1)

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

2.1.2 攝像頭標定

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

圖5 攝像頭的標定板

Kinect的標定結(jié)果如表1所示。

表1 攝像頭標定結(jié)果

2.1.3 目標位姿估計

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

Linemod算法主要步驟如下：

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

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

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

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

2.2 機械臂控制

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

D-H坐標參數(shù)如表2所示。

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

表2 Franka Panda的D-H坐標參數(shù)

表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 機械臂正運動學

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

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

(2)

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

(3)

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

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

(4)

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

2.2.2 機械臂逆運動學

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

圖7 Franka Panda 的簡化圖

根據(jù)幾何關(guān)系可得：

(5)

又有：

(6)

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

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

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

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

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

圖8 RRT*算法改進效果

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

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

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

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

圖9 正弦速度曲線

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

(7)

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

(8)

(9)

2.2.5 機械臂微分運動學

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

(10)

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

(11)

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

(12)

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

(13)

當D=I時，

J+=JT(JJT)-1

(14)

為雅可比矩陣的偽逆。

2.2.6 關(guān)節(jié)空間軌跡規(guī)劃策略——五次多項式插補

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

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

圖10 三次多項式軌跡規(guī)劃結(jié)果

圖11 五次多項式軌跡規(guī)劃結(jié)果

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

3 實驗結(jié)果與分析

3.1 機械臂軌跡規(guī)劃

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

圖12 路徑規(guī)劃結(jié)果

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

圖13 各關(guān)節(jié)在運動過程中的速度

3.2 機械臂抓取系統(tǒng)

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

本實驗同時觀察仿真環(huán)境和真實環(huán)境下的運動，結(jié)果如圖15和圖16所示。

圖14 目標模板以及搭建的機械臂抓取系統(tǒng)

圖15 仿真環(huán)境中系統(tǒng)運動結(jié)果

圖16 真實環(huán)境中系統(tǒng)運動結(jié)果

從結(jié)果來看，系統(tǒng)不管在仿真環(huán)境還是在真實環(huán)境都按照先前規(guī)劃的軌跡運動，機械臂在運動過程中平穩(wěn)，沒有異音、振動或沖擊。

4 結(jié)束語

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

研究結(jié)果表明，所使用的方法合理，系統(tǒng)具有一定應用價值。