樊虹岐,陳海軍,2,3,呂丞干,胡曉兵,3

(1.四川大學(xué)機(jī)械工程學(xué)院,成都 610065;2.四川德恩精工科技股份有限公司,眉山 620460;3.宜賓四川大學(xué)產(chǎn)業(yè)技術(shù)研究院,宜賓 644000)

0 引言

由于全球市場(chǎng)競爭日益激烈,制造行業(yè)面臨著更高的產(chǎn)品質(zhì)量和生產(chǎn)效益、更低的生產(chǎn)成本和資源消耗等嚴(yán)苛要求[1]。為了應(yīng)對(duì)上述挑戰(zhàn),制造業(yè)借助于信息技術(shù)的快速發(fā)展,不斷革新制造技術(shù),提高生產(chǎn)系統(tǒng)的自治性和協(xié)同性,逐步向“智慧工廠”轉(zhuǎn)變[2]。作為智能工廠柔性制造單元的核心組成,機(jī)械臂、自動(dòng)引導(dǎo)車(automated guided vehicle,AGV)等工業(yè)機(jī)器人構(gòu)成的自動(dòng)化上下料系統(tǒng)已成為推動(dòng)智能工廠實(shí)現(xiàn)智能制造的關(guān)鍵技術(shù)[3]。但是該系統(tǒng)在開發(fā)難度、多機(jī)器人協(xié)同管理等問題上需要的較大成本投入,這導(dǎo)致多數(shù)制造企業(yè)的上下料系統(tǒng)仍處于半無人化水平,高度智能化上下料系統(tǒng)難以推廣應(yīng)用,相關(guān)技術(shù)還需要進(jìn)一步研究和完善。

當(dāng)前,自動(dòng)化上下料技術(shù)受到了國內(nèi)外學(xué)者的廣泛研究,其實(shí)現(xiàn)方式可以分為兩類。第一種方式借助激光雷達(dá)與反光標(biāo)志物[4-5]、二維碼標(biāo)志物[6]、全局雙目視覺[7]或多傳感器融合[8]等方式,實(shí)現(xiàn)AGV的高精度定位,將工件準(zhǔn)確運(yùn)輸至機(jī)械臂的示教點(diǎn)處實(shí)現(xiàn)自動(dòng)化上下料;第二種自動(dòng)上下料技術(shù)則是借助力或視覺等傳感器[9-10],實(shí)現(xiàn)機(jī)械臂的柔性化,動(dòng)態(tài)更改上下料點(diǎn)位補(bǔ)償AGV的定位誤差。上述兩種自動(dòng)上下料技術(shù)僅考慮了單一機(jī)器人的控制,但機(jī)器人之間缺乏配合,智能化程度仍然受限。針對(duì)該問題,呂開旺等[11]設(shè)計(jì)了AGV與機(jī)械臂結(jié)合的上下料系統(tǒng),AGV與機(jī)械臂之間通過以太網(wǎng)連接,實(shí)現(xiàn)了兩機(jī)器人簡單的協(xié)作配合。然而,機(jī)械臂與AGV仍然由各自獨(dú)立的控制系統(tǒng)驅(qū)動(dòng),需要在此基礎(chǔ)上建立額外的協(xié)同系統(tǒng),成本高,維護(hù)困難,因此需要一個(gè)統(tǒng)一的系統(tǒng)控制并協(xié)調(diào)多機(jī)器人實(shí)現(xiàn)自動(dòng)化智能化的上下料。

針對(duì)上述問題,本文以被廣泛用于機(jī)器人開發(fā)的ROS系統(tǒng)為平臺(tái)[12],降低開發(fā)難度,提高系統(tǒng)擴(kuò)展性[13];采用單目全局視覺[14]作為傳感器并使用AR Tag作為標(biāo)志物,研究低成本高穩(wěn)定性的機(jī)械臂和AGV同一視場(chǎng)視覺識(shí)別[7];并基于ROS開發(fā)了多機(jī)器人上下料視覺協(xié)作系統(tǒng),實(shí)現(xiàn)了工作區(qū)間任意指定位置的上下料接駁,避免了傳統(tǒng)固定式上下料托盤與AGV精確定位的難題,為自動(dòng)化上下料技術(shù)提供了一種低成本、易維護(hù)、高擴(kuò)展性的解決方案。

1 平臺(tái)方案

本文設(shè)計(jì)的上下料視覺協(xié)作平臺(tái)如圖1所示,包括六自由度工業(yè)機(jī)械臂,差速AGV,標(biāo)志物,桁架以及2D相機(jī)。機(jī)械臂由控制柜控制,通過網(wǎng)線與系統(tǒng)連接;AGV搭載ROS系統(tǒng),通過無線網(wǎng)絡(luò)與系統(tǒng)連接;二者均可以在工作空間內(nèi)自由移動(dòng),并固定了ArUco Tag標(biāo)志物用于視覺識(shí)別,其中為了確保在遮擋情況下也能正確識(shí)別AGV的位姿,在AGV的四角均貼有相同的標(biāo)志物。相機(jī)分辨率為1920×1080,固定在3 m高的桁架上形成全局視覺,確保視場(chǎng)覆蓋整個(gè)工作空間,能夠同時(shí)觀測(cè)到機(jī)械臂與AGV。

圖1 上下料視覺協(xié)作平臺(tái)

基于該平臺(tái)搭建的控制系統(tǒng)結(jié)構(gòu)如圖2所示,主要由圖像處理單元、協(xié)同節(jié)點(diǎn)、機(jī)械臂控制節(jié)點(diǎn)和AGV控制節(jié)點(diǎn)四部分組成。該系統(tǒng)基于ROS開發(fā),系統(tǒng)中每個(gè)功能由一個(gè)或多個(gè)節(jié)點(diǎn)程序?qū)崿F(xiàn),添加與維護(hù)功能節(jié)點(diǎn)只需要添加或修改一個(gè)或幾個(gè)節(jié)點(diǎn),因此系統(tǒng)維護(hù)簡便,擴(kuò)展性能好。圖像處理采用優(yōu)化后的ArUco Tag識(shí)別算法,將識(shí)別到的機(jī)器人位姿發(fā)送給協(xié)同、機(jī)械臂控制與AGV控制節(jié)點(diǎn)。協(xié)同節(jié)點(diǎn)根據(jù)機(jī)器人當(dāng)前位姿和協(xié)同算法計(jì)算出機(jī)械臂與AGV的目標(biāo)位姿,并分別發(fā)送給機(jī)械臂與AGV控制節(jié)點(diǎn)。AGV控制節(jié)點(diǎn)收到AGV的當(dāng)前位姿與目標(biāo)位姿后,計(jì)算出左右輪的運(yùn)行速度并通過主從機(jī)通信發(fā)送給AGV,以控制AGV運(yùn)動(dòng)。機(jī)械臂控制節(jié)點(diǎn)收到機(jī)械臂的當(dāng)前與目標(biāo)位姿后將運(yùn)動(dòng)指令發(fā)送至move_group節(jié)點(diǎn)進(jìn)行運(yùn)動(dòng)學(xué)解算,解算結(jié)果通過TCP/IP通信發(fā)送至機(jī)械臂控制柜,控制機(jī)械臂運(yùn)動(dòng)。

圖2 上下料視覺協(xié)作系統(tǒng)結(jié)構(gòu)

2 平臺(tái)設(shè)計(jì)與實(shí)現(xiàn)

2.1 基于ArUco Tag的視覺識(shí)別

2.1.1 視覺識(shí)別

自動(dòng)化上下料需要機(jī)械臂與AGV多機(jī)器人之間協(xié)同工作,為了能夠正確引導(dǎo)工作空間內(nèi)的機(jī)器人執(zhí)行各自工作,系統(tǒng)的視覺識(shí)別節(jié)點(diǎn)需要同時(shí)識(shí)別多個(gè)不同的目標(biāo),并且獲得多個(gè)目標(biāo)各自的位姿信息。因此,本文采用ArUco Tag作為輔助視覺識(shí)別的標(biāo)志物。ArUco Tag[15]是一種用于機(jī)器人定位的方形標(biāo)志物,可以計(jì)算出標(biāo)志物的6自由度信息,而且每一個(gè)標(biāo)志物都對(duì)應(yīng)唯一的ID號(hào),因此可以使用ID區(qū)分不同機(jī)器人。適用于本文系統(tǒng)中工作空間內(nèi)多機(jī)器人識(shí)別與定位。由于噪聲等因素的影響,單個(gè)標(biāo)志物的方形輪廓可能被重復(fù)檢測(cè)兩次,得到兩個(gè)ID相同的識(shí)別結(jié)果。為了避免上述現(xiàn)象,在識(shí)別到兩個(gè)相同的ID結(jié)果時(shí),會(huì)去除掉其中一個(gè)。但由于本文中AGV存在多個(gè)ID相同的標(biāo)志物,因此該方法并不適用于本文中的AGV識(shí)別,其識(shí)別效果如圖3a所示,并不能準(zhǔn)確識(shí)別出所有標(biāo)志物。因此本文對(duì)重復(fù)檢測(cè)的判斷邏輯進(jìn)行了優(yōu)化,改進(jìn)后的ArUco Tag識(shí)別算法流程如圖4所示。

(a) 去重算法改進(jìn)之前,只能識(shí)別到左側(cè)兩個(gè)標(biāo)志物,右側(cè)的標(biāo)志物被視為重復(fù)檢測(cè)而被去除 (b) 去重算法改進(jìn)之后,可以同時(shí)識(shí)別4個(gè)ID相同的標(biāo)志物

圖4 改進(jìn)后的ArUco Tag 識(shí)別流程

該算法核心在于添加了兩結(jié)果是否相交的檢測(cè),只有當(dāng)兩結(jié)果中心點(diǎn)距離大于兩結(jié)果矩形輪廓的任意邊長時(shí),表明這兩個(gè)相鄰結(jié)果并不相交,是識(shí)別兩個(gè)ID相同的標(biāo)志物得到的,兩個(gè)結(jié)果均保留?；谏鲜龈倪M(jìn)的ArUco Tag識(shí)別算法,在不重復(fù)檢測(cè)到單一標(biāo)志物的同時(shí),能夠正確識(shí)別出工作空間中多個(gè)相同ID的標(biāo)志物。效果如圖3b所示。

2.1.2 位姿計(jì)算

通過上述識(shí)別算法可以得到標(biāo)志物4個(gè)角點(diǎn)在圖像中的像素坐標(biāo),像素坐標(biāo)與世界坐標(biāo)系的轉(zhuǎn)換關(guān)系如圖5所示。

圖5 圖像坐標(biāo)轉(zhuǎn)換關(guān)系

圖5中對(duì)應(yīng)轉(zhuǎn)換關(guān)系式為:

(1)

式(1)中內(nèi)參矩陣為:

(2)

式中:fx和fy分別為x軸、y軸方向上的尺度因子,(cx,cy)、(u0,v0)均為光軸對(duì)應(yīng)的像素坐標(biāo),dx、dy為像元尺寸,f為相機(jī)焦距。內(nèi)參矩陣可以通過內(nèi)參標(biāo)定方式獲得,本文采用張正友標(biāo)定法[16],標(biāo)定流程簡單,精度高,標(biāo)定過程如圖6所示。標(biāo)定完成后,獲得內(nèi)參矩陣為:

圖6 相機(jī)內(nèi)參標(biāo)定

(3)

(4)

式中:R3×3為旋轉(zhuǎn)矩陣,T3×1為平移矩陣,二者表示標(biāo)志物在相機(jī)坐標(biāo)系下的六自由度位姿信息。外參矩陣基于Levenberg-Marquardt迭代估計(jì)標(biāo)記姿勢(shì)[17]得出,使得外參矩陣的重投影誤差最小。

2.2 機(jī)械臂識(shí)別與控制

機(jī)械臂控制節(jié)點(diǎn)如圖7所示中虛線框內(nèi)所示。每個(gè)控制周期中,首先由驅(qū)動(dòng)節(jié)點(diǎn)接收來自機(jī)械臂識(shí)別節(jié)點(diǎn)識(shí)別得到的機(jī)械臂當(dāng)前六自由度信息,以及協(xié)同節(jié)點(diǎn)發(fā)送的目標(biāo)位置信息,取差值作為控制信號(hào)發(fā)送給MoveIt,并由MoveIt通過逆運(yùn)動(dòng)學(xué)求解出目標(biāo)關(guān)節(jié)角度。通信節(jié)點(diǎn)接收到目標(biāo)關(guān)節(jié)角度后,將其轉(zhuǎn)化為動(dòng)態(tài)指令,通過TCP/IP通信發(fā)送給機(jī)械臂,以實(shí)現(xiàn)機(jī)械臂的運(yùn)動(dòng)控制。同時(shí)通信節(jié)點(diǎn)通過動(dòng)態(tài)指令查詢機(jī)械臂當(dāng)前六關(guān)節(jié)角度,并將關(guān)節(jié)狀態(tài)同步給MoveIt。直到機(jī)械臂運(yùn)動(dòng)到目標(biāo)位置允許誤差范圍內(nèi),或者目標(biāo)點(diǎn)發(fā)生更改時(shí)結(jié)束伺服,開始新的控制周期。

圖7 機(jī)械臂控制流程圖

2.2.1 機(jī)械臂識(shí)別與手眼標(biāo)定

本文采用全局視覺的方式識(shí)別機(jī)械臂上特定ID的ArUco Tag標(biāo)志物,實(shí)現(xiàn)對(duì)機(jī)械臂的識(shí)別,并在此基礎(chǔ)上指導(dǎo)機(jī)械臂運(yùn)動(dòng)到指定位置。為了確保位置控制的精確性,首先需要進(jìn)行手眼標(biāo)定,獲得相機(jī)坐標(biāo)系到世界坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換關(guān)系,如圖8所示。

圖8 機(jī)器人坐標(biāo)系

圖8中坐標(biāo)系存在如下轉(zhuǎn)換關(guān)系:

(5)

在標(biāo)定過程中,機(jī)械臂不斷改變位姿,同時(shí)相機(jī)記錄不同的機(jī)器人位姿下,ArUco Tag的六自由度位姿信息,有:

(6)

由式(5)和式(6)可得:

(7)

整理可得:

(8)

通過上式即可求出相機(jī)坐標(biāo)系到世界坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換關(guān)系,為了確保轉(zhuǎn)換關(guān)系唯一,機(jī)械臂至少需要改變3次位姿,且位姿改變次數(shù)越多,得到結(jié)果越精確穩(wěn)定。本文經(jīng)過圖9所示手眼標(biāo)定得到的轉(zhuǎn)換關(guān)系為:

圖9 手眼標(biāo)定過程圖

(9)

2.2.2 通信節(jié)點(diǎn)

MoveIt是ROS系統(tǒng)中包含的機(jī)器人相關(guān)工具集,可以有效地實(shí)現(xiàn)機(jī)械臂的路徑規(guī)劃與逆運(yùn)動(dòng)學(xué)求解。由于機(jī)械臂本身為獨(dú)立的系統(tǒng),無法直接與ROS系統(tǒng)連接以獲取MoveIt的計(jì)算結(jié)果,因此為了實(shí)現(xiàn)ROS系統(tǒng)對(duì)機(jī)械臂的有效控制,本文搭建了機(jī)械臂與ROS系統(tǒng)之間的通信節(jié)點(diǎn)。

通信節(jié)點(diǎn)作為連接控制系統(tǒng)與機(jī)械臂橋梁,負(fù)責(zé)將得到的機(jī)械臂目標(biāo)關(guān)節(jié)角發(fā)送至機(jī)械臂,同時(shí)將機(jī)械臂當(dāng)前關(guān)節(jié)角反饋回控制系統(tǒng),從而在不更改機(jī)械臂原有功能的前提下,實(shí)現(xiàn)控制系統(tǒng)對(duì)機(jī)械臂的有效控制。

(1)通信方式。通信節(jié)點(diǎn)與機(jī)械臂之間采用TCP/IP協(xié)議,使用socket套接字連接,實(shí)現(xiàn)了機(jī)械臂與系統(tǒng)之間可靠高效的信息交互。通信中使用的動(dòng)態(tài)指令采用JSON格式,結(jié)構(gòu)如下:

{"token":"googoltech123","post":"Get1":"1"}

動(dòng)態(tài)指令主要分為兩個(gè)部分,前半部分為“token”口令作為校驗(yàn)位,校驗(yàn)通過了才會(huì)執(zhí)行“post”指令;后半部分為“post”指令,用于查詢機(jī)器人狀態(tài)或控制機(jī)器人執(zhí)行相關(guān)功能。

(2)發(fā)送控制指令。通信節(jié)點(diǎn)采用TCP/IP通信,將設(shè)定好的動(dòng)態(tài)運(yùn)動(dòng)指令發(fā)送至機(jī)械臂,具體流程如圖10所示。

圖10 動(dòng)態(tài)指令發(fā)送流程

2.3 AGV識(shí)別與控制

AGV作為重要的運(yùn)輸設(shè)備,在上下料工作中不可獲缺。為了發(fā)揮全局視覺的優(yōu)勢(shì),確保AGV在工作空間中正確運(yùn)行,系統(tǒng)在AGV進(jìn)入工作空間后對(duì)其進(jìn)行識(shí)別,并控制其運(yùn)行。整個(gè)AGV控制系統(tǒng)的流程圖如圖11所示。

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

2.3.1 AGV識(shí)別

AGV作為物料搬運(yùn)的重要組成部分,在自動(dòng)化上下料中不可或缺。為了提高工作效率,同一工作空間內(nèi)可能會(huì)存在多臺(tái)AGV,此外,由于全局相機(jī)視角固定,在工作過程中可能會(huì)出現(xiàn)機(jī)械臂遮擋部分AGV的情況。上述情況給AGV的識(shí)別與控制帶來了困難。為此,本文將相同ID的4張標(biāo)志物按照同樣的位姿固定在AGV四角,不同的AGV之間使用不同ID的標(biāo)志物區(qū)分,提高了識(shí)別的準(zhǔn)確性及穩(wěn)定性,同時(shí)可以得到AGV托盤的準(zhǔn)確范圍。

通過識(shí)別標(biāo)志物,可以得到每個(gè)標(biāo)志物的物體坐標(biāo)系,該坐標(biāo)系與標(biāo)志物相對(duì)固定(如圖4中坐標(biāo)系),因此可以根據(jù)坐標(biāo)系判斷AGV的朝向。AGV的識(shí)別算法流程圖如圖12所示。

圖12 AGV識(shí)別算法流程圖

為了區(qū)分不同AGV之間的識(shí)別信息與位姿信息,兩種信息的格式如圖13所示。

圖13 AGV識(shí)別信息與AGV位姿信息格式

2.3.2 AGV控制

(1)AGV運(yùn)動(dòng)模型。差速AGV的運(yùn)動(dòng)模型如圖14所示。

根據(jù)上圖運(yùn)動(dòng)模型可以得到AGV的運(yùn)動(dòng)方程[18]:

(10)

式中:vr、vL分別為AGV的右輪和左輪轉(zhuǎn)速,(xt,yt,θt)為目標(biāo)位姿,(xc,yc,θc)為當(dāng)前位姿,ΔT為從當(dāng)前位姿移動(dòng)到目標(biāo)位姿需要的時(shí)間。上式中左右輪速度會(huì)隨當(dāng)前位姿與目標(biāo)位姿間的距離變化,不利于AGV的穩(wěn)定運(yùn)行,因此可以將式(10)中的AGV前進(jìn)速度設(shè)為固定值u,則左右輪速度為:

(11)

(2)AGV運(yùn)動(dòng)路徑。相比于AGV系統(tǒng)內(nèi)的傳感器及路徑規(guī)劃算法,系統(tǒng)依靠全局視覺可以準(zhǔn)確快速識(shí)別出AGV在當(dāng)前工作空間中的位置,并規(guī)劃出在當(dāng)前工作空間中更加合理的運(yùn)動(dòng)路徑。因此,當(dāng)AGV進(jìn)入工作空間后,其控制權(quán)由系統(tǒng)接管,并根據(jù)系統(tǒng)規(guī)劃的路徑運(yùn)動(dòng)。為了能夠讓AGV平穩(wěn)地從當(dāng)前位姿移動(dòng)到目標(biāo)位姿,考慮到工作空間中不存在障礙物,本系統(tǒng)采用光滑的貝塞爾曲線算法對(duì)AGV的運(yùn)動(dòng)路徑進(jìn)行規(guī)劃,如圖15所示。

圖15 AGV的運(yùn)動(dòng)軌跡中間點(diǎn)

圖16 本文中協(xié)同節(jié)點(diǎn)的工作流程

AGV的當(dāng)前位姿與目標(biāo)位姿確定了兩個(gè)位置點(diǎn)P0=(x0,y0)、P2=(x2,y2)以及朝向θ0、θ2從兩點(diǎn)沿朝向做延長線交于一點(diǎn),得到P1(x1,y1),有:

(12)

以P0、P1、P2三點(diǎn)作為控制點(diǎn)做二次方貝塞爾曲線,可以得到一條平滑的運(yùn)動(dòng)軌跡,使AGV從當(dāng)前位姿移動(dòng)到目標(biāo)位姿。該曲線根據(jù)下式得出:

B(t)=(1-t)2P0+2t(1-t)P1+t2P2,t∈[0,1]

(13)

式中:t=kh,k∈{0,1,…,n},h為步長。曲線上任一點(diǎn)切線的朝向角度為:

θb=artan(B′(t))=arctan((2t-2)P0+
(2-4t)P1+2tP2)t∈[0,1]

(14)

通過上式計(jì)算得到中間點(diǎn){Pb1,Pb2,…,Pbn}及其對(duì)應(yīng)的切線方向{θb1,θb2,…,θbn},將中間點(diǎn)的位姿作為目標(biāo)點(diǎn)帶入式(14)即可得出左右輪的速度。

(3)AGV控制規(guī)則。多個(gè)ROS系統(tǒng)之間可以通過主從機(jī)的方式通信,AGV作為從機(jī)接入多機(jī)器人控制系統(tǒng),可以直接獲取主機(jī)及控制系統(tǒng)發(fā)布的信息(圖13)。AGV在運(yùn)行過程中接收識(shí)別節(jié)點(diǎn)發(fā)送的識(shí)別信息,并與自身ID核對(duì),若與自身ID相同,則進(jìn)一步核對(duì)識(shí)別標(biāo)志位,若標(biāo)志位為真,則判斷自身被系統(tǒng)成功識(shí)別,并開始接收系統(tǒng)發(fā)布的控制信息,按照控制信息中的左右輪速度運(yùn)行。若發(fā)布ID與自身ID不同或標(biāo)志為假,則按照預(yù)定規(guī)則運(yùn)行。

2.4 工作區(qū)域識(shí)別與協(xié)同控制

協(xié)同節(jié)點(diǎn)的主要作用根據(jù)當(dāng)前的工作內(nèi)容設(shè)定協(xié)同方式,并按照協(xié)同方式將目標(biāo)位姿發(fā)送給機(jī)械臂與AGV的控制節(jié)點(diǎn),控制機(jī)器人按照預(yù)定方式協(xié)同工作。本文中,在AGV進(jìn)入動(dòng)作空間后,系統(tǒng)根據(jù)AGV的ID判斷當(dāng)前任務(wù),并根據(jù)任務(wù)計(jì)算機(jī)械臂與AGV的目標(biāo)位姿,然后控制機(jī)械臂與AGV同時(shí)向目標(biāo)區(qū)域移動(dòng),以實(shí)現(xiàn)機(jī)械臂與AGV的初步定位,便于后續(xù)分揀工作的開展。

2.4.1 工作區(qū)域識(shí)別與工作任務(wù)判斷

通過矩形識(shí)別得到對(duì)應(yīng)的工作區(qū)域的4個(gè)角點(diǎn),取角點(diǎn)坐標(biāo)的均值作為中心點(diǎn)(xa,ya),并取相鄰兩點(diǎn)中距離最大的邊作為長邊,計(jì)算長邊與水平線夾角θa,將中心點(diǎn)坐標(biāo)與夾角發(fā)布至協(xié)同節(jié)點(diǎn)。

在實(shí)際生產(chǎn)過程中,不同的AGV進(jìn)入工作空間執(zhí)行的任務(wù)不同,其目標(biāo)位置與協(xié)同方式也不同。在AGV進(jìn)入工作空間后,系統(tǒng)通過AGV識(shí)別節(jié)點(diǎn)獲取其ID,根據(jù)ID查詢其預(yù)先設(shè)定好的工作任務(wù),根據(jù)工作任務(wù)獲取機(jī)械臂與AGV的工作區(qū)域、機(jī)械臂與AGV的啟動(dòng)時(shí)間與運(yùn)行速度等協(xié)同配合信息,并開始計(jì)算AGV與機(jī)械臂的目標(biāo)位姿。

2.4.2 機(jī)械臂與AGV目標(biāo)位姿算法

(1)AGV目標(biāo)位姿計(jì)算。在實(shí)際控制中,若將工作區(qū)域信息直接作為AGV目標(biāo)位姿,則AGV無法正確運(yùn)行至工作區(qū)域內(nèi),這是由于相機(jī)的小孔成像原理導(dǎo)致現(xiàn)實(shí)中同一點(diǎn)在不同高度下會(huì)投影在圖像的不同位置,如圖17所示。

圖17 相機(jī)成像原理與目標(biāo)點(diǎn)偏移

工作區(qū)域中心點(diǎn)A投影在圖像中a處,若以該點(diǎn)作為AGV的目標(biāo)位姿則AGV運(yùn)動(dòng)之B′點(diǎn),與預(yù)期不符。AGV的真正目標(biāo)位姿位于B點(diǎn),其投影在圖像b處。因此為了使AGV能夠正確的運(yùn)行到工作區(qū)域中,需要將b點(diǎn)位置計(jì)算出來,得到AGV的目標(biāo)位姿(xb,yb,θa),計(jì)算公式如下:

(15)

式中,(cx,cy)、(u0,v0)均為光軸對(duì)應(yīng)的像素坐標(biāo),H為相機(jī)鏡頭至地面的高度,h為AGV的高度。

(2)機(jī)械臂目標(biāo)位姿計(jì)算。通過工作區(qū)域的識(shí)別與前文中目標(biāo)點(diǎn)的計(jì)算得到了目標(biāo)位姿的像素坐標(biāo)(xb,yb,θa),由于機(jī)械臂控制需要目標(biāo)位姿的六自由度信息,因此需要對(duì)上述像素坐標(biāo)進(jìn)行轉(zhuǎn)換,進(jìn)而得到機(jī)械臂的目標(biāo)位姿(Xc,Yc,Zc,Qc):

(16)

式中:(Xc,Yc,Zc)為相機(jī)坐標(biāo)系下目標(biāo)點(diǎn)的XYZ坐標(biāo),Zc為預(yù)先設(shè)定值,fx、fy為尺度因子,(xb,yb)為目標(biāo)點(diǎn)像素坐標(biāo),可以根據(jù)協(xié)同方式更改,此處協(xié)同方式為機(jī)械臂末端與AGV中心位于圖像中相同位置。

(17)

式中:Qc為相機(jī)坐標(biāo)系下目標(biāo)點(diǎn)四元數(shù),其等式右側(cè)乘法順序根據(jù)實(shí)際繞軸旋轉(zhuǎn)的順序而定,在本文中旋轉(zhuǎn)是沿著XYZ軸依次旋轉(zhuǎn),因此Qx,Qy,Qz的以此右乘;α、β、γ分別為XYZ軸旋轉(zhuǎn)角度。

3 實(shí)驗(yàn)結(jié)果及分析

為了驗(yàn)證系統(tǒng)是否能夠?qū)崿F(xiàn)多機(jī)器人之間的有效協(xié)同,本文設(shè)計(jì)了AGV與機(jī)械臂的模擬上下料實(shí)驗(yàn)。工作空間中劃分出了3個(gè)區(qū)域,分別對(duì)應(yīng)AGV的啟動(dòng)區(qū)域、上料區(qū)域和下料區(qū)域。啟動(dòng)區(qū)域表示AGV進(jìn)入工作空間,由系統(tǒng)接管控制,上料區(qū)域?yàn)锳GV1(ID:103)的目標(biāo)點(diǎn),下料區(qū)域?yàn)锳GV2(ID:104)的目標(biāo)點(diǎn)。

在實(shí)驗(yàn)過程中,系統(tǒng)將自動(dòng)識(shí)別工作區(qū)域的中心位置以及AGV的標(biāo)志ID,將機(jī)械臂與AGV引導(dǎo)至ID對(duì)應(yīng)的工作區(qū)域。為了便于結(jié)果的直觀展示,多機(jī)器人的引導(dǎo)配合以圖像中的配合為基準(zhǔn),即機(jī)械臂末端與AGV中心位于圖像中相同位置。

3.1 實(shí)驗(yàn)流程

AGV ID為103,位于初始區(qū)域,機(jī)械臂處于初始姿態(tài)。系統(tǒng)控制AGV與機(jī)械臂移動(dòng)至與AGV ID對(duì)應(yīng)的工作區(qū)域,記錄從開始到二者均移動(dòng)到位所花費(fèi)的時(shí)間。如圖18所示。

(a) 兩機(jī)器人同時(shí)啟動(dòng) (b) 兩機(jī)器人運(yùn)動(dòng)過程中,系統(tǒng)在遮擋情況下仍能正常識(shí)別AGV

(c) 兩機(jī)器人均運(yùn)動(dòng)到目標(biāo)點(diǎn)

更換AGV的標(biāo)志物ID為104,并更改AGV判斷程序,模擬另外一臺(tái)AGV進(jìn)入工作空間。將AGV和機(jī)械臂移動(dòng)至初始區(qū)域,并由系統(tǒng)控制二者同時(shí)移動(dòng)至對(duì)應(yīng)的工作區(qū)域,記錄多機(jī)器人協(xié)作系統(tǒng)完成初始定位花費(fèi)的時(shí)間。如圖19所示。

(a) 兩機(jī)器人同時(shí)啟動(dòng) (b) 兩機(jī)器人運(yùn)動(dòng)過程中,機(jī)械臂遮擋了AGV的部分標(biāo)志物,系統(tǒng)仍能成功識(shí)別出AGV的ID、中心點(diǎn)和朝向

(c) 兩機(jī)器人均運(yùn)動(dòng)到目標(biāo)點(diǎn)

3.2 實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)結(jié)果如表1所示。多次上下料時(shí)間數(shù)據(jù)表明,控制系統(tǒng)工作穩(wěn)定可靠,可以有效地識(shí)別工作區(qū)域。機(jī)械臂與AGV標(biāo)志物,并且能根據(jù)標(biāo)志物ID正確地引導(dǎo)機(jī)械臂與AGV移動(dòng)到目標(biāo)區(qū)域,實(shí)現(xiàn)任意指定位置上下料的協(xié)同。在運(yùn)行過程中,即使兩機(jī)器人之間存在遮擋關(guān)系,系統(tǒng)仍然能夠根據(jù)剩余標(biāo)志物正確的計(jì)算出當(dāng)前AGV的位姿并引導(dǎo)其繼續(xù)運(yùn)動(dòng),并實(shí)現(xiàn)與機(jī)械臂的配合。這說明系統(tǒng)在控制多機(jī)器人協(xié)同上下料上有較好的抗干擾性。因此基于該系統(tǒng),工件托盤可以通過AGV在任意指定位置靈活放置,提高了整個(gè)工作空間的柔性,避免了傳統(tǒng)上下料中,AGV需要精確定位的問題。證明了該工業(yè)機(jī)器人與AGV的視覺協(xié)同系統(tǒng)能夠在上下料的場(chǎng)景中正確的實(shí)現(xiàn)多機(jī)器人的穩(wěn)定識(shí)別、控制以及協(xié)同。

表1 協(xié)同上下料實(shí)驗(yàn)所用的時(shí)間

4 結(jié)論

為了提高上下料系統(tǒng)的智能程度,提高上下料的柔性,本文基于開源的ROS開發(fā)平臺(tái),提出了一套工業(yè)機(jī)器人及AGV的視覺協(xié)同控制系統(tǒng),完成了以下工作:

(1)針對(duì)工業(yè)機(jī)械臂的控制,提出了一種基于ArUco Tag以及動(dòng)態(tài)指令的機(jī)械臂視覺識(shí)別與控制方法,實(shí)驗(yàn)證明,基于動(dòng)態(tài)指令的通信節(jié)點(diǎn)設(shè)計(jì)方法可以有效實(shí)現(xiàn)上層ROS系統(tǒng)與底層工業(yè)機(jī)器人控制系統(tǒng)的穩(wěn)定通訊,并準(zhǔn)確控制機(jī)械臂的運(yùn)動(dòng)狀態(tài)。

(2)針對(duì)AGV控制,結(jié)合差速AGV運(yùn)動(dòng)模型和貝塞爾曲線路徑規(guī)劃算法,提出了一種基于全局視覺的AGV識(shí)別及控制方法。實(shí)驗(yàn)證明,上述方法可以有效解決不同AGV的識(shí)別與控制問題,確保不同AGV均能準(zhǔn)確地到達(dá)指定工作區(qū)域。

(3)結(jié)合上述機(jī)器人識(shí)別與控制模塊,根據(jù)不同機(jī)器人的協(xié)作規(guī)則和目標(biāo)位姿計(jì)算方法,提出了一種機(jī)械臂與AGV的視覺協(xié)同控制方法。實(shí)驗(yàn)證明,通過對(duì)相機(jī)透視模型的位姿轉(zhuǎn)換,機(jī)械臂和AGV均能在全局視覺下,實(shí)現(xiàn)統(tǒng)一的目標(biāo)位姿計(jì)算并根據(jù)任務(wù)要求完成協(xié)同。

上述研究表明,本文提出的基于ROS的多機(jī)器人視覺協(xié)同控制系統(tǒng)具有較強(qiáng)的穩(wěn)定性與柔性,能夠根據(jù)當(dāng)前任務(wù)靈活控制工作空間內(nèi)的多個(gè)機(jī)器人協(xié)同作業(yè),提高了整個(gè)加工單元的智能化程度,對(duì)智能工廠的建設(shè)有一定的參考意義。