舒 奇，黃家才，林 健，施昕昕

（南京工程學(xué)院，江蘇 南京 210000）

1 引言

隨著科技的不斷進(jìn)步，將機(jī)器人應(yīng)用于各行各業(yè)的趨勢已經(jīng)勢不可擋，尤其在工業(yè)生產(chǎn)中，機(jī)器人將改變整個勞動密集型產(chǎn)業(yè)的格局[1-2]。UR 機(jī)器人是丹麥UNIVERSAL ROBOTS 公司生產(chǎn)的通用型機(jī)器人，其具有輕松編程、快速安裝和設(shè)置、重新部署靈活、人機(jī)協(xié)作和安全等特點(diǎn)，機(jī)械臂有反向驅(qū)動功能，可以直接牽引機(jī)械臂編程示教；可以通過設(shè)置安全參數(shù)，并經(jīng)過安全評估，在不增加安全圍欄條件下滿足人機(jī)協(xié)作應(yīng)用需求。

機(jī)器人遠(yuǎn)程控制基本思想就是讓數(shù)據(jù)在客戶機(jī)與服務(wù)器間傳輸，機(jī)器人作為客戶機(jī)，工控機(jī)作為服務(wù)器，常用兩種方式進(jìn)行實(shí)時數(shù)據(jù)交換：①用戶報文協(xié)議（UDP）；②傳輸控制協(xié)議（TCP）。主機(jī)用于負(fù)責(zé)系統(tǒng)的管理、通訊、運(yùn)動學(xué)和動力學(xué)計算，并向客戶機(jī)發(fā)送指令信息；作為客戶機(jī)，各關(guān)節(jié)實(shí)現(xiàn)給定的運(yùn)動，并向主機(jī)反饋信息。針對機(jī)器人自適應(yīng)抓取的需求，文獻(xiàn)[3]通過增加欠驅(qū)動自由度設(shè)計柔性更高的末端執(zhí)行器，文獻(xiàn)[4]則通過柔性材料增強(qiáng)適應(yīng)度；還有的研究致力于在末端執(zhí)器中增加傳感器，如增加觸覺反饋[5]、改進(jìn)末端執(zhí)行器控制[6]等。

通過研究UR 機(jī)器人機(jī)械結(jié)構(gòu)與遠(yuǎn)程通信，實(shí)現(xiàn)對UR 機(jī)器人的TCP/IP 遠(yuǎn)程控制，可以保護(hù)操作人員，在離開危險的操作環(huán)境同時，也避免了UR 機(jī)器人的損壞，極大程度上提高了機(jī)器人的生產(chǎn)穩(wěn)定性和工作精度。在給機(jī)器人能夠在復(fù)雜的環(huán)境中完成多功能任務(wù)提供基礎(chǔ)條件后，再加入自適應(yīng)抓取功能，使得機(jī)器人具備更高的自主性，對于外界環(huán)境的變化，能夠做出智能的反應(yīng)。

2 UR5 機(jī)械結(jié)構(gòu)

UR5 型機(jī)器人是Universal Robots 公司開發(fā)的六關(guān)節(jié)工業(yè)機(jī)器人，如圖1 所示。

圖1 UR5 實(shí)體圖Fig.1 UR5 Entity Map

其擁有6 個旋轉(zhuǎn)關(guān)節(jié)，關(guān)節(jié)從下到上分別命名為：基座，肩部，肘部，腕部1，腕部2 和腕部3?；礊闄C(jī)器人的固定處，確定了機(jī)器人在使用環(huán)境中的安裝位置。腕部3 是機(jī)器人末端，可以連接不同的工具來滿足各種各樣的需求[7]。在這6 個旋轉(zhuǎn)關(guān)節(jié)中，前三個關(guān)節(jié)主要控制了機(jī)器人末端的位置，而后三個關(guān)節(jié)主要控制機(jī)器人末端的姿態(tài)。UR5 型機(jī)器人的具體參數(shù)，如表1 所示。

表1 UR5 機(jī)器人參數(shù)Tab.1 UR5 Robot Parameters

要研究機(jī)器人的運(yùn)動，就必須為機(jī)器人建立運(yùn)動學(xué)模型，然后再通過運(yùn)動學(xué)模型進(jìn)行機(jī)器人正逆解[8]。為UR5 機(jī)器人建立D-H 模型，首先標(biāo)識出機(jī)器人各個旋轉(zhuǎn)關(guān)節(jié)的旋轉(zhuǎn)軸，然后依據(jù)選裝軸找到連桿坐標(biāo)系中各個連桿的原點(diǎn)，坐標(biāo)系的軸與原點(diǎn)，如圖2 所示。得到各個連桿的軸與原點(diǎn)后，推導(dǎo)出各個連桿坐標(biāo)系，UR5 機(jī)器人的連桿坐標(biāo)系，如圖3 所示。

圖2 連桿軸與原點(diǎn)Fig.2 Connecting Rod Shaft and Origin

圖3 連桿坐標(biāo)系Fig.3 Link Coordinate System

根據(jù)已經(jīng)建立的連桿坐標(biāo)系，參考上述的D-H 參數(shù)定義，可以得到UR5 型機(jī)器人的D-H 參數(shù)，如表2 所示。

所謂機(jī)器人運(yùn)動學(xué)逆解，就是在已知機(jī)器人末端執(zhí)行器相對于參考基坐標(biāo)系的位姿以后，逆向求出各個關(guān)節(jié)的運(yùn)動學(xué)參數(shù)。從現(xiàn)實(shí)中的工程應(yīng)用角度來看，人們要控制機(jī)器人完成點(diǎn)到點(diǎn)的準(zhǔn)確運(yùn)動，這時就需要用到對機(jī)器人的運(yùn)動學(xué)逆解來求出各個關(guān)節(jié)如何變換，最終通過每個關(guān)節(jié)的變換協(xié)作實(shí)現(xiàn)機(jī)器人末端執(zhí)行器的位移和旋轉(zhuǎn)，即達(dá)到目標(biāo)位姿[9-11]。

表2 D-H 參數(shù)Tab.2 D-H Parameter

在這里對于UR5 型機(jī)器人的運(yùn)動學(xué)逆解使用的方法使牛頓迭代法。首先需要做的就是為牛頓迭代法構(gòu)建出必要的方程組。對于每一次迭代后的近似解θ 都可以得到一個更接近于最終矩陣的近似矩陣經(jīng)過多次的迭代直到此時的θ 便是要求得到的最終逆解。如此便可建立出關(guān)鍵方程（1）：

將方程中的每一項(xiàng)展開即可獲得一個包含12 個方程的方程組，根據(jù)此方程組可以得到方程的雅可比矩陣，然后運(yùn)用牛頓迭代公式，即可得到，如式（2）所示。

式中：雅可比矩陣J—12×6 的矩陣，只能求其偽逆，所以公式演變?yōu)椋?）：

利用此公式可以對UR5 機(jī)器人進(jìn)行運(yùn)動學(xué)逆解，逆解流程圖，如圖4 所示。程序要求輸入機(jī)器人的各關(guān)節(jié)的初始角θori以及要求到達(dá)的末端位姿矩陣，經(jīng)過運(yùn)算最終輸出到達(dá)要求位姿時的各關(guān)節(jié)旋轉(zhuǎn)角θ。

圖4 逆解程序流程圖Fig.4 Reverse Solution Flow Chart

假設(shè)起始關(guān)節(jié)角θ1，θ2，θ3，θ4，θ5，θ6都為0，逆解結(jié)果為θ=［0，-90，0，-90，0，180］T。但是考慮到機(jī)器人逆解存在多解的情況，該目標(biāo)矩陣應(yīng)該存在多解，于是改變起始關(guān)節(jié)角θ2=-80，θ4=80，其他為0，得出運(yùn)行結(jié)果為θ=［0，-90，0，-90，0，0］T，由此可見由于迭代法雖然運(yùn)算簡單快速，但是最終只能獲得一組解，在多解的情況下可能沒法得到預(yù)期的解，需要調(diào)整不同的起始關(guān)節(jié)角才能獲得其他解。

3 自適應(yīng)抓取

自適應(yīng)抓取的本質(zhì)就是通過改變二夾指的夾持行程來適應(yīng)不同尺寸工件的抓取。自適應(yīng)包括兩個方面：一方面工件處于攝像機(jī)可視范圍的任意位置任意姿勢，都可以進(jìn)行工件的識別定位；另一方面當(dāng)識別工件的大小形狀發(fā)生改變時，通過改變二夾指的夾持參數(shù)可以進(jìn)行自適應(yīng)性抓取[12]。

二指夾持器是為工業(yè)應(yīng)用而設(shè)計的機(jī)器人外部設(shè)備，可以快速挑選、放置和處理不同尺寸和形狀的零件。采用的是加拿大Robotiq 自適應(yīng)二指夾持器（85mm 開口規(guī)格），如圖5 所示。二夾指可進(jìn)行平行抓握或包圍式抓握，主要針對長方體工件實(shí)現(xiàn)平行抓握的控制，2 指85 夾持器的具體參數(shù)，如表3 所示。

圖5 二夾指實(shí)物圖Fig.5 Two Clips Physical Map

表3 二夾指參數(shù)表Tab.3 Two Pin Index Table

由于相機(jī)固定在UR5 機(jī)器人的腕部，根據(jù)機(jī)器人的設(shè)定，機(jī)器人每完成一次抓取任務(wù)都會回到相同的初始位置，因此相機(jī)的高度是固定不變的，在這種情況下，為匹配工件與二夾指開合程度提供了基礎(chǔ)。

硬幣放置圖，如圖6 所示。假設(shè)硬幣實(shí)際大小為x，工件實(shí)際大小為y，硬幣圖像大小為x~，工件圖像大小y~，根據(jù)攝像機(jī)的成像原理，圖像中的工件大小與實(shí)際工件大小會產(chǎn)生一個成像比例k，那么可以得到下式：

因此通過采集已知規(guī)格參數(shù)物體的圖像獲取成像比例，然后通過計算得到未知抓取工件的實(shí)際大小，這樣傳輸?shù)臄?shù)據(jù)就能準(zhǔn)確的控制二夾指的夾持行程，完成自適應(yīng)抓取任務(wù)。

由于選取的參照物要能夠很好的適應(yīng)環(huán)境要求，并且不能影響工件正常的識別和圖像處理，因此選取了日常生活中常見的一元硬幣，標(biāo)準(zhǔn)大小為25mm。硬幣的顏色為銀白色，放置在黃色木塊上較為明顯，很容易識別并通過圖像處理獲取其圖像中硬幣大小。

圖6 工件放置硬幣Fig.6 Workpiece Placement Coin

首先，采集的圖像需要進(jìn)行閾值分割，根據(jù)灰度值的差別分割出較為明顯的硬幣區(qū)域，但是分割出來的圖像，如圖7 所示。不能完全包含硬幣區(qū)域，通過HALCON 中smallest_circle 算子確定硬幣區(qū)域的最小環(huán)繞圓，并計算出其中心點(diǎn)坐標(biāo)和半徑，這樣就可以獲取到硬幣的圖像大小，并通過計算得到此時的成像比例。

圖7 分割出的硬幣區(qū)域Fig.7 Split Coin Area

獲得成像比例之后，在HALCON 圖像處理軟件中實(shí)現(xiàn)對工件的圖像處理，首先通過閾值分割得到工件區(qū)域，然后對分割后的區(qū)域進(jìn)行連通操作獲取感興趣區(qū)域，通過亞像素邊緣提取獲得工件亞像素輪廓，最后通過矩形輪廓擬合獲取工件的中心坐標(biāo)與圖像大小。結(jié)合成像比例，通過計算就可以得到工件的實(shí)際大小。

UR5 機(jī)器人上的二夾指并不是以尺寸大小作為開和程度的指標(biāo)，而是將開合程度換算成百分比，當(dāng)二夾指完全閉合時，參數(shù)顯示為100%；當(dāng)二夾指完全打開時，參數(shù)顯示為0。為了獲取實(shí)際的開合百分比，以參照物一元硬幣為例，通過機(jī)器的夾取操作，獲得了25mm 時的開合百分比為62%。通過實(shí)際操作發(fā)現(xiàn)當(dāng)二夾指完全閉合時參數(shù)顯示為87%。通過這兩組參數(shù)的計算得出每1mm 開合的百分比為1%，這樣當(dāng)工件的大小發(fā)生變化時，通過換算就可以得到二夾指開合百分比，通過改變此數(shù)據(jù)大小控制二夾指開合完成自適應(yīng)抓取任務(wù)。Robotiq 自適應(yīng)二指夾持器設(shè)有壓力傳感器，當(dāng)二夾指之間的壓力超過預(yù)設(shè)值時，將停止動作，這為夾取產(chǎn)生的誤差提供了一定的容錯率。由于二夾指指端的海綿墊的存在，誤差可以在±0.5%內(nèi)進(jìn)行調(diào)試，這樣極大的增加了穩(wěn)定抓取成功的概率。

4 通信控制

控制系統(tǒng)中，以裝有HALCON 軟件的計算機(jī)為服務(wù)器端，UR5 機(jī)器人的控制柜為客戶端，通過兩者之間的Socket 連接與通信，將HALCON 中工件定位和二夾指開合程度相關(guān)數(shù)據(jù)發(fā)送給機(jī)器人以控制機(jī)器人的移動與抓取。

首先，建立Halcon 服務(wù)器端的Socket 半相關(guān)，即確定Halcon服務(wù)器端的協(xié)議、本地地址和本地端口。通過open_socket_connect算子在計算機(jī)上打開一個連接客戶端的套接字，并進(jìn)入監(jiān)聽狀態(tài)。

open_socket_connect（URIP，Port，GenParamName，Socket）

open_flag：=socket_open（"192.168.0.101"，30002）

連接是由UR 機(jī)器人客戶端發(fā)起的，在建立自己的Socket半相關(guān)后，通過上面的程序向服務(wù)器端發(fā)起連接。其中，URIP 為192.168.0.180，端口號為30002，工控機(jī)IP 為192.168.0.101。

open_socket_accept（Port，GenParamName，GenParamValue：AcceptingSocket）

socket_accept_connect（AcceptingSocket，‘a(chǎn)uto’，SockUR）

服務(wù)器端在檢測到連接后，通過open_socket_accept 算子打開一個接受傳入連接請求的套接字。套接字監(jiān)聽由UR 機(jī)器人服務(wù)器端口提供的端口號的傳入連接請求，通過tuple_regexp_match算子判斷協(xié)議，協(xié)議相同無誤后，通過socket_accept_connect 算子自動連接，調(diào)用socket_accept_connect 獲得最終通信的套接字，用來接收傳入的“TCP”連接請求，這樣就建立起一個完整的的Socket 連接。

Socket 連接完成后，通過使用套接字交換數(shù)據(jù)，只能使用send_data 和receive_data 來進(jìn)行數(shù)據(jù)傳輸。通用參數(shù)“timeout”可用于設(shè)置此操作的超時。HALCON 將工件定位和二夾指開合程度相關(guān)數(shù)據(jù)分別轉(zhuǎn)化為數(shù)組，通過Socket 通信傳輸?shù)経R5 機(jī)器人中，UR5 機(jī)器人接收數(shù)據(jù)數(shù)組并讀取，通過對數(shù)據(jù)的判斷，控制機(jī)器人進(jìn)行不同狀態(tài)下的抓取任務(wù)，即完成所設(shè)計的自適應(yīng)抓取。完整的Halcon 與UR5 機(jī)器人Socket 通信過程，如圖8 所示。

圖8 Halcon 與UR5 機(jī)器人Socket 通信Fig.8 Halcon and UR5 Robot Socket Communication

5 綜合控制平臺實(shí)驗(yàn)結(jié)果

綜合測試平臺中的計算機(jī)配置為：Intel（R）Core（TM）i5-4690 cpu@3.5GHz，內(nèi)存12G，在windows 環(huán)境下利用MATLAB 實(shí)現(xiàn)運(yùn)動學(xué)正逆解，利用HALCON 12 實(shí)現(xiàn)圖像處理與通信部分。整個硬件平臺主要由黑色工作臺、工業(yè)攝像機(jī)、6 自由度機(jī)械手臂以及手爪組成。實(shí)驗(yàn)過程中對UR5 共進(jìn)行了10 組位姿測試，結(jié)果表明所建立的模型與實(shí)際物體的模型完全一致，根據(jù)該模型得到的末端位姿與圖形控制界面所展示的位姿數(shù)據(jù)吻合。抽取的兩組位姿仿真結(jié)果，如表4 所示。

表4 仿真位姿對比圖Tab.4 Simulation Pose Comparison Chart

整個實(shí)驗(yàn)過程，提供兩種規(guī)格的長方體木塊用于自適應(yīng)抓取操作，路徑規(guī)劃主要是依靠UR5 自帶的MOVEJ 函數(shù)完成，其功能是從一個設(shè)置路徑點(diǎn)到另一個設(shè)置路徑點(diǎn)。首先通過一個移動指令將機(jī)器人移動到初始位置，也就是等待抓取的地方，此時目標(biāo)物置于相機(jī)視野處，如圖9 所示。

圖9 機(jī)器人初始位置Fig.9 Initial Position of the Robot

第二步是完成圖像處理部分，發(fā)送夾爪開合指令命令機(jī)器人的手抓打開，并控制機(jī)器人從初始位置移動到工件坐標(biāo)處，如圖10 所示。

圖10 機(jī)器人等待抓取Fig.10 The Robot is Waiting to Grab

機(jī)器人在完成自適應(yīng)抓取任務(wù)后可以根據(jù)示教路徑完成搬運(yùn)和放置的動作，如圖11 所示。再最后回到初始位置，等待下一個任務(wù)指令。此時機(jī)器人完成一次工作循環(huán)。

圖11 完成自適應(yīng)抓取Fig.11 Complete Adaptive Crawling

6 結(jié)論

旨在改變傳統(tǒng)機(jī)器人示教編程抓取控制，采用一個較為新穎和智能化的視覺集成的方式來控制機(jī)器人抓取操作，實(shí)現(xiàn)了對UR5 的遠(yuǎn)程控制與抓取。通過視覺標(biāo)定、圖像處理、工件定位和Socket 通信一系列的程序設(shè)計與算子處理，逐步實(shí)現(xiàn)自適應(yīng)抓取的功能。實(shí)驗(yàn)操作簡單、快速，且具有良好跨平臺性。