杜玉俊， 吳盤龍

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

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

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

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

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

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

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

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

1.1 Franka Panda機(jī)械臂

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

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

1.2 Kinect V1攝像頭

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

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

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

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

2.1 視覺系統(tǒng)

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

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

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

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

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

(1)

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

2.1.2 攝像頭標(biāo)定

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

圖5 攝像頭的標(biāo)定板

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

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

2.1.3 目標(biāo)位姿估計

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

Linemod算法主要步驟如下：

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

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

③ 預(yù)處理響應(yīng)圖。對8方向得到的擴(kuò)散圖逐個像素進(jìn)行匹配(每像素點(diǎn)具有8張預(yù)處理圖)，匹配得到距離最近方向角度的余弦值。

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

2.2 機(jī)械臂控制

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

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

圖6 機(jī)械臂簡化和modified D-H坐標(biāo)系建立

表2 Franka Panda的D-H坐標(biāo)參數(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ī)械臂正運(yùn)動學(xué)

根據(jù)坐標(biāo)系變換的鏈?zhǔn)椒▌t，坐標(biāo)系{i-1}到坐標(biāo)系{i}的變換矩陣可表達(dá)為

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

(2)

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

(3)

因此，末端在世界坐標(biāo)系的位姿矩陣為

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

(4)

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

2.2.2 機(jī)械臂逆運(yùn)動學(xué)

如果一個結(jié)構(gòu)具有3個旋轉(zhuǎn)關(guān)節(jié)，其中關(guān)節(jié)軸線兩兩垂直并相交于一點(diǎn)就可以視為一個球關(guān)節(jié)[7]。Franka Panda的關(guān)節(jié)1～關(guān)節(jié)3具有這樣的結(jié)構(gòu)，所以可以看作球關(guān)節(jié)，相當(dāng)于人的肩部，記作O點(diǎn)(位于關(guān)節(jié)2位置)。關(guān)節(jié)5～關(guān)節(jié)7雖然沒有這樣的結(jié)構(gòu)，但當(dāng)末端位置不變，關(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點(diǎn)(位于關(guān)節(jié)6位置)，關(guān)節(jié)4位置記為B點(diǎn)。Franka Panda機(jī)械臂的簡化圖如圖7所示。

圖7 Franka Panda 的簡化圖

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

(5)

又有：

(6)

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

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

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

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

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

圖8 RRT*算法改進(jìn)效果

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

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

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

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

圖9 正弦速度曲線

對一條軌跡S，設(shè)定最大速度為Vm，最大加速度為A，最大減速度為D，則整個運(yùn)動過程中，加速度、速度和位移的表達(dá)式為

(7)

積分可得速度和位移的表達(dá)式為

(8)

(9)

2.2.5 機(jī)械臂微分運(yùn)動學(xué)

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

(10)

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

(11)

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

(12)

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

(13)

當(dāng)D=I時，

J+=JT(JJT)-1

(14)

為雅可比矩陣的偽逆。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)束語

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

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