基于激光傳感器的工業(yè)機(jī)器人動(dòng)態(tài)抓取系統(tǒng)設(shè)計(jì)*

楊 永，楊進(jìn)興，黃偉龍，李 俊

（1.中北大學(xué) 電氣與控制工程學(xué)院，山西 太原 030051；2.中國(guó)科學(xué)院海西研究院泉州裝備制造研究中心，福建 泉州 362216）

0 引 言

分揀作業(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)抓取。

1 系統(tǒng)總體設(shè)計(jì)

系統(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)

2 點(diǎn)云數(shù)據(jù)建模

2.1 點(diǎn)云數(shù)據(jù)獲取

點(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í)間。

2.2 點(diǎn)云數(shù)據(jù)實(shí)時(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ù)分割。

2.3 最小外接矩形提取

采用旋轉(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)系中的位姿。

3 動(dòng)態(tài)抓取

3.1 路徑規(guī)劃

動(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ī)劃示意

3.2 抓取位置求解

攔截式抓取是根據(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í)間。

4 實(shí)驗(yàn)驗(yàn)證

為了測(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è)試

5 結(jié) 論

在實(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)分揀。