鄒 洵 張 帆 張國勝 馬保平 張召穎

（上海工程技術(shù)大學(xué)機(jī)械與汽車工程學(xué)院 上海 200072）

1 引言

在生產(chǎn)過程中，對于識別出來的殘次零件需要抓取剔除，由于殘次品的識別與迅速抓取難以使用人力進(jìn)行操作并且成本過高［1］。需要使用專業(yè)的攝像頭識別殘次品并用機(jī)器人代替人工抓?。?］。經(jīng)過幾年的發(fā)展，證明了機(jī)器人具有極高的代替人力工作的能力，雖然目前市面上的部分機(jī)器人擁有此功能，如安川機(jī)器人，具有極高的安全性［3］；新松機(jī)器人，是市面上極少數(shù)的具有七自由度的協(xié)作機(jī)器人［4］。但是成本太高，并且入門難，購買的企業(yè)難以實(shí)現(xiàn)二次開發(fā)，最終難以推廣［5］。因此我們需要開發(fā)出一套操作簡單，成本較低，未來有升級潛力，并且對于絕大數(shù)中小企業(yè)都能夠使用的控制系統(tǒng)。

綜上，為實(shí)現(xiàn)以上既定目標(biāo)，通過后期調(diào)研，本研究最終選擇了基于開源機(jī)器人操作系統(tǒng)ROS（Robot Operating System），以生產(chǎn)線上對于目標(biāo)物品的抓取為應(yīng)用背景，搭建了控制系統(tǒng)平臺［6］，進(jìn)行控制系統(tǒng)設(shè)計(jì)并作了相應(yīng)的仿真及實(shí)物控制實(shí)驗(yàn)。

2 機(jī)器人運(yùn)動學(xué)模型的建立

機(jī)器人的運(yùn)動學(xué)正逆解求解是對抓取機(jī)器人進(jìn)行位置和姿態(tài)分析的第一步［7］。由于不同機(jī)器人的正逆解不同，本研究對象的機(jī)器人為三個(gè)相鄰關(guān)節(jié)軸相互平行的六自由度機(jī)器人［8］。其運(yùn)動學(xué)正解是在電機(jī)轉(zhuǎn)動角度和幾何參數(shù)已知的基礎(chǔ)上，推算出機(jī)器人抓取的位置和姿態(tài)等參數(shù)。逆解則與正解恰恰相反［9］。在實(shí)際生產(chǎn)中，我們已知末端抓手的最終位置，通過運(yùn)動學(xué)逆解推算出每個(gè)關(guān)節(jié)對應(yīng)的三維坐標(biāo)以及各個(gè)電機(jī)的絕對位置，使得每個(gè)關(guān)節(jié)運(yùn)動到唯一的指定位置，從而完成每個(gè)末端抓手的定位。如表1所示，本研究采用D-H法建立連桿坐標(biāo)系［10］，在6個(gè)連桿上分別建立各個(gè)連桿的坐標(biāo)系，通過各個(gè)坐標(biāo)系之間的齊次變換矩陣的多次運(yùn)算，最終得到機(jī)器人末端抓手相對于基坐標(biāo)系的位姿矩陣［11］。

表1 機(jī)器人D-H參數(shù)表

其中參數(shù)如下：1）i：連桿；2）di：連桿偏置；3）ai：連桿長度；4）αi：連桿扭曲；5）θi：關(guān)節(jié)角度。

2.1 運(yùn)動學(xué)正解

抓取機(jī)器人可看做是由末端抓手和各個(gè)連桿串聯(lián)而組成的。為此，若根據(jù)機(jī)械手的連桿建立坐標(biāo)系，首先需要為每個(gè)連桿均建立一個(gè)小坐標(biāo)系，則這些不同連桿坐標(biāo)系間的相對關(guān)系可以采用齊次坐標(biāo)變換矩陣表示［12］。本研究中將這類矩陣統(tǒng)稱為矩陣A，定義順序?yàn)閺幕鴺?biāo)到末端抓手的坐標(biāo)系齊次變換矩陣分別為A1，A2……。則二號連桿相對于基坐標(biāo)的位置和位姿可由A1和A2的乘積表示，即：

同上，若A3表示三號連桿相對于二號連桿的位置和位姿，即：

6個(gè)連桿坐標(biāo)系屬于依次相連，它們之間的坐標(biāo)變換關(guān)系可以通過變換矩陣來描述A1A2A3A4A5

A6。這樣機(jī)械手末端位姿可表示為

式中：nx，ny，nz分別表示機(jī)器人抓手的坐標(biāo)軸X6相對于初始坐標(biāo)系在X，Y，Z軸的方向余弦；ox，oy，oz分別表示機(jī)器人抓手的坐標(biāo)軸Y6相對于初始坐標(biāo)系在X，Y，Z軸的方向余弦；ax，ay，az分別表示機(jī)器人抓手的坐標(biāo)軸Z6相對于初始坐標(biāo)系在X，Y，Z軸的方向余弦；px，py，pz表示機(jī)器人抓手的坐標(biāo)軸相對于初始坐標(biāo)系的位置坐標(biāo)。

其中，連桿i相對于連桿i-1的變換矩陣為

式（4）中：θn為關(guān)節(jié)角，由于一個(gè)關(guān)節(jié)中包含兩條公垂線，因而在一個(gè)平面上產(chǎn)生投影，投影間的夾角即為θn；αn為連桿扭轉(zhuǎn)角，即第n+1個(gè)關(guān)節(jié)軸線與第n個(gè)關(guān)節(jié)軸線和公垂線構(gòu)成的平面的夾角；dn為連桿偏移量，即一個(gè)關(guān)節(jié)的兩條公垂線之間的偏移距離；C和S為縮寫的cos和sin；n為關(guān)節(jié)數(shù)，本課題為六自由度機(jī)器人，故而n為6。

將表1中的參數(shù)代入式（1），式（2），即可求出運(yùn)動學(xué)正解。

2.2 運(yùn)動學(xué)逆解

通過機(jī)器人抓手末端的位姿，逆向求解出各個(gè)關(guān)節(jié)的位姿，但是可能存在多解的情況，即部分關(guān)節(jié)可能出現(xiàn)多種符合要求的位置和姿態(tài)，因此計(jì)算量較大。為了方便計(jì)算，我們引入了逆變換法［13］，通過已知矩陣逆推出各個(gè)關(guān)節(jié)的參數(shù)，用此法求解需要注意各個(gè)關(guān)節(jié)的參數(shù)求解順序，并不是按照從后往前依次進(jìn)行，即求逆解的過程中先求θ1，θ5，θ6，后求θ2，θ3，θ4。求θ2，θ3，θ4時(shí)，先求θ2，θ3，θ4這3個(gè)平行關(guān)節(jié)的一個(gè)角度θ2，然后求θ23，通過（θ23-θ2）求得θ3，再求θ234，用（θ234-θ23）求解θ4。其中θ23=θ2+θ3，θ234=θ2+θ3+θ4。通過此法可求出運(yùn)動學(xué)逆解的各個(gè)關(guān)節(jié)參數(shù)。

3 控制系統(tǒng)設(shè)計(jì)

在如圖1所示的工作環(huán)境中，在識別區(qū)域上方安裝攝像頭，當(dāng)裝有物品的收納盒放置到指定位置后，攝像頭進(jìn)行殘次品識別，若識別到殘次品，則將殘次品的位置發(fā)送給工控機(jī)。工控機(jī)控制抓取機(jī)器人運(yùn)動到指定位置，抓取殘次品并放置回收箱。若沒有識別到殘次品，則發(fā)送消息，告訴上位機(jī)，準(zhǔn)備下一個(gè)物品的識別。

圖1 抓取機(jī)器人整體

本控制過程分為三個(gè)部分：圖像識別、機(jī)器人運(yùn)動、抓手控制。對于抓取機(jī)器人的控制，實(shí)質(zhì)上是對于各個(gè)關(guān)節(jié)處電機(jī)轉(zhuǎn)動角度、速度、開關(guān)時(shí)間的控制以及末端抓手位置和位姿的控制。故而本控制系統(tǒng)具體控制步驟如下。

1）調(diào)用攝像頭，進(jìn)行圖像識別，識別出殘次品，發(fā)送數(shù)據(jù)給控制總站，總站分析后發(fā)送信息給機(jī)器人控制系統(tǒng)，準(zhǔn)備抓取目標(biāo)物品。

2）機(jī)器人接收數(shù)據(jù)后，通過攝像頭反饋信息定位目標(biāo)物品，規(guī)劃運(yùn)動路徑，驅(qū)動機(jī)器人運(yùn)動到目標(biāo)位置。

3）當(dāng)機(jī)器人運(yùn)動到指定位置時(shí)，調(diào)整抓手的位置和位姿，并啟動抓手，抓取殘次零件，并放置回收箱中。

4）機(jī)器人歸位，準(zhǔn)備下一次抓取。

總體控制過程如圖2所示。

圖2 控制過程流程圖

4 ROS配置環(huán)境的搭建

4.1 ROS系統(tǒng)主體概要

ROS是一款機(jī)器人控制開發(fā)系統(tǒng)。它包含了三維重建、動態(tài)仿真、實(shí)時(shí)場景仿真、實(shí)物控制等多種功能，能夠最大化地提供機(jī)器人開發(fā)的各種需求［14］。其為建立在Ubuntu［15］系統(tǒng)下的一款內(nèi)置操作系統(tǒng)，但與系統(tǒng)的進(jìn)程管理沒有關(guān)系。它提供各種功能包，然后通過點(diǎn)對點(diǎn)機(jī)制建立功能包之間的通訊，搭建控制平臺，實(shí)現(xiàn)機(jī)器人的仿真和實(shí)物控制。

1）Urdf文件的搭建

編寫urdf文件［16］，包含機(jī)器人模型及其內(nèi)部結(jié)構(gòu)、關(guān)節(jié)、自由度、色彩等參數(shù)。這是ROS可識別的XML文件，為后續(xù)工作做準(zhǔn)備。

2）MoveIt！配置功能包

ROS的MoveIt！是專門為可移動操作平臺而開發(fā)出的ROS軟件包［17］。它集成了路徑規(guī)劃、立體感知、運(yùn)動學(xué)、運(yùn)動控制和導(dǎo)航領(lǐng)域的集成軟件，提供一個(gè)開發(fā)高級機(jī)器人應(yīng)用的易用軟件平臺。圖3展示了ROS MoveIt！的高層框架。其中，Move_Group節(jié)點(diǎn)處于核心地位。其作為樞紐，連接了各個(gè)獨(dú)立的構(gòu)件，提供完成的服務(wù)。整體框架如下。

圖3 ROS MoveIt！高層框架

3）機(jī)器人仿真

將配置好的機(jī)器人導(dǎo)入至ROS內(nèi)置的3D可視化工具（以下簡稱rviz）中，可以進(jìn)行規(guī)劃不同目標(biāo)、在場景中添加和移除對象等操作。對于本研究而言，將抓取機(jī)器人模型導(dǎo)入至rviz中，并在其中添加抓取對象，控制機(jī)器人抓取目標(biāo)對象，進(jìn)行仿真實(shí)驗(yàn)。

4.2 ROS功能包配置

本研究調(diào)用ROS模塊，以其內(nèi)置的算法為核心，結(jié)合本研究目標(biāo)，計(jì)算出各關(guān)節(jié)的必要參數(shù)，最后下發(fā)給各電機(jī)。具體步驟如下。

1）在solidworks里繪制機(jī)器人三維圖，然后調(diào)用SW2URDF插件轉(zhuǎn)換為ROS可識別的文件，最后進(jìn)行ROS下的Setup配置，設(shè)定末端執(zhí)行器的初始位置。

2）進(jìn)行控制系統(tǒng)編程設(shè)計(jì)。Rviz界面打開配置好的仿真機(jī)器人，接收上位機(jī)發(fā)送的末端坐標(biāo)，即可調(diào)用路徑規(guī)劃算法計(jì)算出每個(gè)關(guān)節(jié)運(yùn)動到目標(biāo)位置所需要的轉(zhuǎn)動角度、速度、加速度、位置等參數(shù)，并且依次將每部分的計(jì)算結(jié)果發(fā)送出去。

3）以控制器局域網(wǎng)通訊（Controller Area Network，CAN通訊）的方式建立上下位機(jī)的通訊，將計(jì)算出的每個(gè)關(guān)節(jié)的轉(zhuǎn)動角度及其他必要參數(shù)，通過通信接口發(fā)送給下位機(jī)。

4）下位機(jī)芯片接收數(shù)據(jù)，轉(zhuǎn)換為電機(jī)可識別的參數(shù)發(fā)送給電機(jī)，進(jìn)而控制機(jī)器人運(yùn)動。

5 實(shí)驗(yàn)

實(shí)物抓取機(jī)器人基于UR協(xié)作機(jī)器人，在末端執(zhí)行器上安裝抓手。上位機(jī)接收殘次品位置信息并發(fā)送給機(jī)器人，通過通訊發(fā)送給機(jī)器人內(nèi)置芯片，芯片通過分析接收的數(shù)據(jù)，轉(zhuǎn)換為電機(jī)可識別的參數(shù)，進(jìn)而控制電機(jī)的轉(zhuǎn)速、轉(zhuǎn)動時(shí)間等。其次，抓手運(yùn)動到指定位置，再細(xì)調(diào)抓手的位置和位姿，最終實(shí)現(xiàn)抓手的抓取并放置殘次品至回收箱內(nèi)。機(jī)器人實(shí)驗(yàn)過程如圖4所示。圖4（a）為機(jī)器人移動初試位置，隨后接收指令，移動至圖4（b），并抓取目標(biāo)，隨后通過路徑規(guī)劃，其過程如圖4（c）所示，移動至目標(biāo)位置，如圖4（d）所示。

圖4 實(shí)物機(jī)器人控制實(shí)驗(yàn)

6 結(jié)語

為實(shí)現(xiàn)對于抓取機(jī)器人控制系統(tǒng)設(shè)計(jì)，本研究基于Ubuntu系統(tǒng)下結(jié)合ROS實(shí)現(xiàn)了對于抓取機(jī)器人的控制系統(tǒng)設(shè)計(jì)。開發(fā)了一套基于ROS的控制系統(tǒng)，并改進(jìn)了ROS中上下位機(jī)的通訊方式，達(dá)到了在保證原有實(shí)時(shí)性和魯棒性前提下適用性廣、操作簡單的通訊方式，并搭建了仿真實(shí)驗(yàn)和實(shí)物控制實(shí)驗(yàn)。由實(shí)驗(yàn)可知，控制系統(tǒng)能夠?qū)崿F(xiàn)殘次品的抓取，有實(shí)際的應(yīng)用背景和經(jīng)濟(jì)價(jià)值。未來，可將收納盒放置在流水線上，形成全自動化生產(chǎn)線，大力提高生產(chǎn)效率。