楊 永,楊進(jìn)興,黃偉龍,李 俊
(1.中北大學(xué) 電氣與控制工程學(xué)院,山西 太原 030051;2.中國(guó)科學(xué)院海西研究院泉州裝備制造研究中心,福建 泉州 362216)
分揀作業(yè)是工業(yè)機(jī)器人的應(yīng)用之一[1]。在物體的定位方面,基于圖像的物體定位方式[2],易受到環(huán)境光線、相機(jī)拍攝角度等因素的干擾?;诩す饫走_(dá)物體定位方式[3],由于其精度高、響應(yīng)快等優(yōu)點(diǎn)得到了廣泛應(yīng)用。
點(diǎn)云聚類(lèi)是將大量的點(diǎn)云數(shù)據(jù)劃分成若干子類(lèi),從而實(shí)現(xiàn)物體點(diǎn)云數(shù)據(jù)的分割。基于層次劃分的聚類(lèi)算法如K-Means[4,5]、K-Medoids[6]等通過(guò)迭代的方式將類(lèi)中心不斷地向數(shù)據(jù)中心移動(dòng);基于密度的算法如DBSCAN[7]、OPTICS[8]等通過(guò)定義一個(gè)距離半徑,將所有可到達(dá)的點(diǎn)聚為一類(lèi)。Campello R 等人使用HDBSCAN[9]通過(guò)稀疏度變換使噪聲點(diǎn)遠(yuǎn)離核心點(diǎn),引入了層次聚類(lèi)的思想,實(shí)現(xiàn)不同密度點(diǎn)的聚類(lèi)。
上述聚類(lèi)算法,需要獲取整體的點(diǎn)云數(shù)據(jù),再進(jìn)行數(shù)據(jù)處理。在實(shí)際的工程應(yīng)用過(guò)程中,由于空間范圍的限制,需要在掃描完整箱體之后及時(shí)地獲取結(jié)束信號(hào),快速實(shí)時(shí)地對(duì)數(shù)據(jù)進(jìn)行處理,否則會(huì)造成完整箱體被分割,如果加大視覺(jué)區(qū)域,雖然會(huì)避免該現(xiàn)象,但會(huì)壓縮后續(xù)抓取的空間。
本文設(shè)計(jì)了一種基于激光傳感器的機(jī)器人動(dòng)態(tài)抓取系統(tǒng)。在箱體的定位方面,利用LMS511激光傳感器作為視覺(jué)設(shè)備,采用幀內(nèi)和幀間密度聚類(lèi)算法和最小外接矩形實(shí)現(xiàn)箱體實(shí)時(shí)定位與尺寸測(cè)量。在箱體的抓取方面,利用KUKA工業(yè)機(jī)器人作為抓取設(shè)備,采用二分法實(shí)現(xiàn)抓取位置與時(shí)間的近似求解。經(jīng)實(shí)驗(yàn)結(jié)果證明,該系統(tǒng)能夠?qū)崿F(xiàn)大、重箱體的實(shí)時(shí)定位與動(dòng)態(tài)抓取。
系統(tǒng)上位機(jī)控制器為工控機(jī),用于處理點(diǎn)云數(shù)據(jù)、求解近似抓取位置和提供整套系統(tǒng)的人機(jī)交互界面,使用C#設(shè)計(jì)開(kāi)發(fā)程序。下位機(jī)控制器用于實(shí)現(xiàn)工控機(jī)與機(jī)器人控制器之間的數(shù)據(jù)傳輸,抓手開(kāi)合和吸盤(pán)吸氣放氣的指令控制,系統(tǒng)狀態(tài)的實(shí)時(shí)監(jiān)控。工業(yè)機(jī)器人控制器實(shí)現(xiàn)根據(jù)抓取位置自動(dòng)生成機(jī)械臂的運(yùn)動(dòng)軌跡及機(jī)械臂的運(yùn)動(dòng)控制。
工控機(jī)與激光傳感器通過(guò)TCP/IP通信,工控機(jī)與PLC通過(guò)Modbus/TCP通信,PLC 與機(jī)器人控制器通過(guò)Profinet總線連接?;趩尉€激光傳感器的機(jī)器人動(dòng)態(tài)抓取系統(tǒng)架構(gòu)如圖1所示。
圖1 系統(tǒng)架構(gòu)
點(diǎn)云數(shù)據(jù)獲取示意如圖2所示。單線激光雷達(dá)固定于傳送帶上方,并對(duì)傳送帶區(qū)域進(jìn)行掃描。單點(diǎn)位置精度為±12 mm,掃描頻率為100 Hz。箱體放置于傳送帶上,勻速通過(guò)掃描區(qū)域。掃描數(shù)據(jù)為掃描位置點(diǎn)相對(duì)雷達(dá)的距離與角度。
圖2 箱體點(diǎn)云數(shù)據(jù)獲取示意
針對(duì)掃描過(guò)程每一幀數(shù)據(jù),根據(jù)單線激光雷達(dá)與輸送帶之間的坐標(biāo)轉(zhuǎn)換關(guān)系,由式(1)計(jì)算箱體在傳送帶坐標(biāo)系中三維點(diǎn)云數(shù)據(jù),從而建立箱體三維點(diǎn)云模型
式中 (x,y,z)為點(diǎn)云數(shù)據(jù)的三維坐標(biāo);θ1,θ2為單線激光雷達(dá)到掃描點(diǎn)角度;d1,d2為單線激光雷達(dá)到掃描點(diǎn)的長(zhǎng)度;v為傳送帶的運(yùn)行速度;t為掃描時(shí)間。
在箱體的三維掃描過(guò)程中,針對(duì)每一幀的數(shù)據(jù),定義了3種點(diǎn)云數(shù)據(jù)類(lèi)型,如圖3 所示。類(lèi)型A,在多個(gè)箱體同時(shí)到達(dá)時(shí),位于箱體中間噪聲點(diǎn)n 會(huì)造成不同的箱體數(shù)據(jù)連接在一起,從而使系統(tǒng)誤判為一個(gè)箱體。類(lèi)型B,在掃描過(guò)程中,單線激光雷達(dá)對(duì)扇形區(qū)域進(jìn)行掃描,較高的箱體距離單線激光雷達(dá)較近,因而該部分點(diǎn)云數(shù)據(jù)較為密集。反之,點(diǎn)云數(shù)據(jù)較為稀疏。類(lèi)型C,2 個(gè)高度相同的箱體相互接觸,2個(gè)箱體的點(diǎn)云數(shù)據(jù)沒(méi)有明顯的區(qū)分界限。
圖3 幀內(nèi)和幀間密度聚類(lèi)算法
為了實(shí)現(xiàn)點(diǎn)云數(shù)據(jù)的實(shí)時(shí)分割,針對(duì)每一幀的數(shù)據(jù),采取幀內(nèi)和幀間密度聚類(lèi)算法。
1)幀內(nèi)和幀間密度聚類(lèi)算法
a.幀內(nèi)聚類(lèi)
為了防止位于2 個(gè)類(lèi)中間的噪聲點(diǎn)將2 個(gè)類(lèi)連接,在進(jìn)行聚類(lèi)之前,利用稀疏度變換將噪聲點(diǎn)遠(yuǎn)離核心點(diǎn)。設(shè)待聚類(lèi)的數(shù)據(jù)集X ={x1,x2,…,xn},對(duì)于其中每一個(gè)數(shù)據(jù)點(diǎn)計(jì)算計(jì)算其核心距離corek(xi)與互達(dá)距離dmreach-k(xi)。
樣本與第k 個(gè)最近鄰樣本點(diǎn)的距離稱(chēng)為核心距離corek(xi),即
式中 Nk(x)為x的第k個(gè)最近鄰點(diǎn);d(x,N)為2點(diǎn)之間的距離。式(2)可以看出,核心距離corek(xi)取決于點(diǎn)的密度及最近鄰點(diǎn)的個(gè)數(shù)k。
相鄰2個(gè)樣本點(diǎn)間的互達(dá)距離dmreach-k(xi)為
由式(3)可得到數(shù)據(jù)點(diǎn)的稀疏度變換,稀疏點(diǎn)與核心點(diǎn)之間的距離將會(huì)被擴(kuò)大。
由于點(diǎn)云數(shù)據(jù)密度不固定,需要根據(jù)不同的點(diǎn)云密度自動(dòng)的調(diào)整閾值
式中 T(i)為閾值,λ為閾值系數(shù),該系數(shù)由激光傳感器單點(diǎn)精度決定,精度越低,λ 值越大。在點(diǎn)云數(shù)據(jù)密集區(qū)域,點(diǎn)云數(shù)據(jù)的核心距離較小,從而使得T(i)較??;在點(diǎn)云數(shù)據(jù)稀疏區(qū)域,點(diǎn)云數(shù)據(jù)的核心距離較大,從而使得T(i)較大;在噪聲點(diǎn)位置,T(i)達(dá)到一個(gè)極大值。
b.幀間聚類(lèi)
幀間聚類(lèi)主要根據(jù)上一幀數(shù)據(jù)的聚類(lèi)情況對(duì)當(dāng)前幀數(shù)據(jù)的分割,然后按聚類(lèi)中心距離最小進(jìn)行匹配。
2)算法測(cè)試
如圖3所示,根據(jù)實(shí)際情況,選擇6 幀點(diǎn)云數(shù)據(jù)進(jìn)行測(cè)試。圖3(a)中第1~6 幀數(shù)據(jù)類(lèi)型分別為:一類(lèi)數(shù)據(jù)、類(lèi)間含有噪聲點(diǎn)的數(shù)據(jù)、兩類(lèi)密度一樣且相連的數(shù)據(jù)、兩類(lèi)密度不一樣且相連的數(shù)據(jù)、兩類(lèi)不相連的數(shù)據(jù)、一類(lèi)數(shù)據(jù)。圖3(b)為聚類(lèi)結(jié)果。圖3(c)表明閾值隨點(diǎn)云密度而變化。圖中,橫坐標(biāo)為點(diǎn)云數(shù)據(jù)序列,縱坐標(biāo)為與上一點(diǎn)的距離(cm),第2、4、5 幀數(shù)據(jù)點(diǎn)云數(shù)據(jù)密度發(fā)生變化,閾值也隨之變化,在不同密度交界處,閾值小于可達(dá)距離,從而將不同密度的點(diǎn)云數(shù)據(jù)分割。
采用旋轉(zhuǎn)法以實(shí)現(xiàn)箱體最小外接矩形的快速提取。首先,將點(diǎn)云數(shù)據(jù)以固定的角度由0°逐步旋轉(zhuǎn)至90°,分別提取其x方向和y方向最值,組合為矩形的4 個(gè)頂點(diǎn),選擇面積最小的矩形為最小外接矩形;然后,根據(jù)旋轉(zhuǎn)角度和頂點(diǎn)位置計(jì)算箱體的位姿和尺寸;最后,根據(jù)坐標(biāo)標(biāo)定算法[10~12]將計(jì)算得到的位姿轉(zhuǎn)換為機(jī)械人坐標(biāo)系中的位姿。
動(dòng)態(tài)抓取運(yùn)動(dòng)規(guī)劃目的是機(jī)器人在滿(mǎn)足避障要求的前提下,能夠精準(zhǔn)地抓取箱體并平穩(wěn)地將箱體運(yùn)送至指定位置。將箱體從傳送帶上抓取并放置到裝載車(chē)中是典型的點(diǎn)到點(diǎn)(point-to-point,PTP)過(guò)程,關(guān)鍵點(diǎn)如圖4 所示。Home點(diǎn)為抓取起始點(diǎn),P1—P2 為抓取逼近段,P2—P3 為抓取跟隨段,P9為碼垛點(diǎn)。為防止拐角處軌跡突變引起的機(jī)器人振動(dòng),同時(shí)縮短運(yùn)動(dòng)時(shí)間,采用樣條曲線過(guò)渡的方式[13],使得關(guān)節(jié)空間輸出速度、加速度和轉(zhuǎn)矩曲線更加平滑,有效減小對(duì)機(jī)器人沖擊,提高機(jī)器手動(dòng)態(tài)性能[14]。
圖4 機(jī)器人末端運(yùn)動(dòng)路徑規(guī)劃示意
攔截式抓取是根據(jù)目標(biāo)箱體當(dāng)前位置、速度和加速度來(lái)計(jì)算出其將來(lái)任意時(shí)刻的位置,然后規(guī)劃?rùn)C(jī)器人末端的動(dòng)作,使其能與目標(biāo)物體相遇,雖然行李箱的運(yùn)動(dòng)軌跡為勻速直線運(yùn)動(dòng),但是機(jī)械臂末端的運(yùn)動(dòng)軌跡為空間曲線非勻速運(yùn)動(dòng),為了實(shí)現(xiàn)快速的求解抓取位置,采用二分法求近似解。在抓取范圍為1 000 mm的條件下,迭代6次之后,最大誤差范圍小于20 mm。
箱體的線性勻速運(yùn)動(dòng)方程為
機(jī)器人末端的運(yùn)動(dòng)為非線性非勻速運(yùn)動(dòng)方程為
根據(jù)式(5)和式(7),利用二分法求解Ctg=Btg即可得出抓取位置和抓取時(shí)間。
為了測(cè)試系統(tǒng)有效性,搭建了實(shí)驗(yàn)平臺(tái),如圖5所示。
圖5 系統(tǒng)布局
圖6(a)和圖6(b)為2 種隨機(jī)擺放的箱體點(diǎn)云數(shù)據(jù)處理結(jié)果,在掃描完一個(gè)完整的箱體后,自動(dòng)結(jié)束掃描。圖6(c)顯示點(diǎn)云數(shù)據(jù)處理中幾種典型點(diǎn)云數(shù)據(jù)幀的閾值設(shè)定。獲取點(diǎn)云數(shù)據(jù)之后,提取箱體的位姿,計(jì)算抓取位置,最后控制工業(yè)機(jī)器人對(duì)箱體進(jìn)行抓取。在20 次抓取實(shí)驗(yàn)中,尺寸識(shí)別誤差為±14 mm,角度偏差為±5°,揀選位置平均誤差為±16 mm,揀選效率為20 s/件。
圖6 箱體點(diǎn)云數(shù)據(jù)處理測(cè)試
在實(shí)際工程應(yīng)用中,由于設(shè)備空間結(jié)構(gòu)的限制,需要快速提取箱體的尺寸和計(jì)算抓取位置。在箱體定位過(guò)程中,利用幀內(nèi)和幀間密度聚類(lèi)算法,根據(jù)箱體的密度自適應(yīng)選擇閾值,實(shí)現(xiàn)對(duì)點(diǎn)云數(shù)據(jù)的逐幀聚類(lèi)與分割,解決了單一閾值無(wú)法滿(mǎn)足不同點(diǎn)云密度的問(wèn)題。在抓取過(guò)程中,采用PTP的路徑規(guī)劃滿(mǎn)足避障要求,在拐角處采用樣條曲線過(guò)渡滿(mǎn)足運(yùn)動(dòng)過(guò)程中的平穩(wěn)性要求,采用二分法求抓取位置與抓取時(shí)間的近似解。最后搭建了實(shí)驗(yàn)平臺(tái)對(duì)算法的可行性和精確度進(jìn)行了測(cè)試,實(shí)驗(yàn)結(jié)果表明,該系統(tǒng)能夠?qū)崿F(xiàn)箱體在隨輸送帶運(yùn)輸過(guò)程中的實(shí)時(shí)點(diǎn)云數(shù)據(jù)分割和自動(dòng)分揀。