楊 永，楊進興，黃偉龍，李 俊

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

0 引 言

分揀作業(yè)是工業(yè)機器人的應用之一［1］。在物體的定位方面，基于圖像的物體定位方式［2］，易受到環(huán)境光線、相機拍攝角度等因素的干擾?；诩す饫走_物體定位方式［3］，由于其精度高、響應快等優(yōu)點得到了廣泛應用。

點云聚類是將大量的點云數(shù)據(jù)劃分成若干子類，從而實現(xiàn)物體點云數(shù)據(jù)的分割?；趯哟蝿澐值木垲愃惴ㄈ鏚-Means［4，5］、K-Medoids［6］等通過迭代的方式將類中心不斷地向數(shù)據(jù)中心移動；基于密度的算法如DBSCAN［7］、OPTICS［8］等通過定義一個距離半徑，將所有可到達的點聚為一類。Campello R 等人使用HDBSCAN［9］通過稀疏度變換使噪聲點遠離核心點，引入了層次聚類的思想，實現(xiàn)不同密度點的聚類。

上述聚類算法，需要獲取整體的點云數(shù)據(jù)，再進行數(shù)據(jù)處理。在實際的工程應用過程中，由于空間范圍的限制，需要在掃描完整箱體之后及時地獲取結(jié)束信號，快速實時地對數(shù)據(jù)進行處理，否則會造成完整箱體被分割，如果加大視覺區(qū)域，雖然會避免該現(xiàn)象，但會壓縮后續(xù)抓取的空間。

本文設計了一種基于激光傳感器的機器人動態(tài)抓取系統(tǒng)。在箱體的定位方面，利用LMS511激光傳感器作為視覺設備，采用幀內(nèi)和幀間密度聚類算法和最小外接矩形實現(xiàn)箱體實時定位與尺寸測量。在箱體的抓取方面，利用KUKA工業(yè)機器人作為抓取設備，采用二分法實現(xiàn)抓取位置與時間的近似求解。經(jīng)實驗結(jié)果證明，該系統(tǒng)能夠?qū)崿F(xiàn)大、重箱體的實時定位與動態(tài)抓取。

1 系統(tǒng)總體設計

系統(tǒng)上位機控制器為工控機，用于處理點云數(shù)據(jù)、求解近似抓取位置和提供整套系統(tǒng)的人機交互界面，使用C#設計開發(fā)程序。下位機控制器用于實現(xiàn)工控機與機器人控制器之間的數(shù)據(jù)傳輸，抓手開合和吸盤吸氣放氣的指令控制，系統(tǒng)狀態(tài)的實時監(jiān)控。工業(yè)機器人控制器實現(xiàn)根據(jù)抓取位置自動生成機械臂的運動軌跡及機械臂的運動控制。

工控機與激光傳感器通過TCP／IP通信，工控機與PLC通過Modbus／TCP通信，PLC 與機器人控制器通過Profinet總線連接。基于單線激光傳感器的機器人動態(tài)抓取系統(tǒng)架構(gòu)如圖1所示。

圖1 系統(tǒng)架構(gòu)

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

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

點云數(shù)據(jù)獲取示意如圖2所示。單線激光雷達固定于傳送帶上方，并對傳送帶區(qū)域進行掃描。單點位置精度為±12 mm，掃描頻率為100 Hz。箱體放置于傳送帶上，勻速通過掃描區(qū)域。掃描數(shù)據(jù)為掃描位置點相對雷達的距離與角度。

圖2 箱體點云數(shù)據(jù)獲取示意

針對掃描過程每一幀數(shù)據(jù)，根據(jù)單線激光雷達與輸送帶之間的坐標轉(zhuǎn)換關系，由式（1）計算箱體在傳送帶坐標系中三維點云數(shù)據(jù)，從而建立箱體三維點云模型

式中 （x，y，z）為點云數(shù)據(jù)的三維坐標；θ1，θ2為單線激光雷達到掃描點角度；d1，d2為單線激光雷達到掃描點的長度；v為傳送帶的運行速度；t為掃描時間。

2.2 點云數(shù)據(jù)實時分割

在箱體的三維掃描過程中，針對每一幀的數(shù)據(jù)，定義了3種點云數(shù)據(jù)類型，如圖3 所示。類型A，在多個箱體同時到達時，位于箱體中間噪聲點n 會造成不同的箱體數(shù)據(jù)連接在一起，從而使系統(tǒng)誤判為一個箱體。類型B，在掃描過程中，單線激光雷達對扇形區(qū)域進行掃描，較高的箱體距離單線激光雷達較近，因而該部分點云數(shù)據(jù)較為密集。反之，點云數(shù)據(jù)較為稀疏。類型C，2 個高度相同的箱體相互接觸，2個箱體的點云數(shù)據(jù)沒有明顯的區(qū)分界限。

圖3 幀內(nèi)和幀間密度聚類算法

為了實現(xiàn)點云數(shù)據(jù)的實時分割，針對每一幀的數(shù)據(jù)，采取幀內(nèi)和幀間密度聚類算法。

1）幀內(nèi)和幀間密度聚類算法

a.幀內(nèi)聚類

為了防止位于2 個類中間的噪聲點將2 個類連接，在進行聚類之前，利用稀疏度變換將噪聲點遠離核心點。設待聚類的數(shù)據(jù)集X ＝｛x1，x2，…，xn｝，對于其中每一個數(shù)據(jù)點計算計算其核心距離corek（xi）與互達距離dmreach-k（xi）。

樣本與第k 個最近鄰樣本點的距離稱為核心距離corek（xi），即

式中 Nk（x）為x的第k個最近鄰點；d（x，N）為2點之間的距離。式（2）可以看出，核心距離corek（xi）取決于點的密度及最近鄰點的個數(shù)k。

相鄰2個樣本點間的互達距離dmreach-k（xi）為

由式（3）可得到數(shù)據(jù)點的稀疏度變換，稀疏點與核心點之間的距離將會被擴大。

由于點云數(shù)據(jù)密度不固定，需要根據(jù)不同的點云密度自動的調(diào)整閾值

式中 T（i）為閾值，λ為閾值系數(shù)，該系數(shù)由激光傳感器單點精度決定，精度越低，λ 值越大。在點云數(shù)據(jù)密集區(qū)域，點云數(shù)據(jù)的核心距離較小，從而使得T（i）較?。辉邳c云數(shù)據(jù)稀疏區(qū)域，點云數(shù)據(jù)的核心距離較大，從而使得T（i）較大；在噪聲點位置，T（i）達到一個極大值。

b.幀間聚類

幀間聚類主要根據(jù)上一幀數(shù)據(jù)的聚類情況對當前幀數(shù)據(jù)的分割，然后按聚類中心距離最小進行匹配。

2）算法測試

如圖3所示，根據(jù)實際情況，選擇6 幀點云數(shù)據(jù)進行測試。圖3（a）中第1～6 幀數(shù)據(jù)類型分別為：一類數(shù)據(jù)、類間含有噪聲點的數(shù)據(jù)、兩類密度一樣且相連的數(shù)據(jù)、兩類密度不一樣且相連的數(shù)據(jù)、兩類不相連的數(shù)據(jù)、一類數(shù)據(jù)。圖3（b）為聚類結(jié)果。圖3（c）表明閾值隨點云密度而變化。圖中，橫坐標為點云數(shù)據(jù)序列，縱坐標為與上一點的距離（cm），第2、4、5 幀數(shù)據(jù)點云數(shù)據(jù)密度發(fā)生變化，閾值也隨之變化，在不同密度交界處，閾值小于可達距離，從而將不同密度的點云數(shù)據(jù)分割。

2.3 最小外接矩形提取

采用旋轉(zhuǎn)法以實現(xiàn)箱體最小外接矩形的快速提取。首先，將點云數(shù)據(jù)以固定的角度由0°逐步旋轉(zhuǎn)至90°，分別提取其x方向和y方向最值，組合為矩形的4 個頂點，選擇面積最小的矩形為最小外接矩形；然后，根據(jù)旋轉(zhuǎn)角度和頂點位置計算箱體的位姿和尺寸；最后，根據(jù)坐標標定算法［10～12］將計算得到的位姿轉(zhuǎn)換為機械人坐標系中的位姿。

3 動態(tài)抓取

3.1 路徑規(guī)劃

動態(tài)抓取運動規(guī)劃目的是機器人在滿足避障要求的前提下，能夠精準地抓取箱體并平穩(wěn)地將箱體運送至指定位置。將箱體從傳送帶上抓取并放置到裝載車中是典型的點到點（point-to-point，PTP）過程，關鍵點如圖4 所示。Home點為抓取起始點，P1—P2 為抓取逼近段，P2—P3 為抓取跟隨段，P9為碼垛點。為防止拐角處軌跡突變引起的機器人振動，同時縮短運動時間，采用樣條曲線過渡的方式［13］，使得關節(jié)空間輸出速度、加速度和轉(zhuǎn)矩曲線更加平滑，有效減小對機器人沖擊，提高機器手動態(tài)性能［14］。

圖4 機器人末端運動路徑規(guī)劃示意

3.2 抓取位置求解

攔截式抓取是根據(jù)目標箱體當前位置、速度和加速度來計算出其將來任意時刻的位置，然后規(guī)劃機器人末端的動作，使其能與目標物體相遇，雖然行李箱的運動軌跡為勻速直線運動，但是機械臂末端的運動軌跡為空間曲線非勻速運動，為了實現(xiàn)快速的求解抓取位置，采用二分法求近似解。在抓取范圍為1 000 mm的條件下，迭代6次之后，最大誤差范圍小于20 mm。

箱體的線性勻速運動方程為

機器人末端的運動為非線性非勻速運動方程為

根據(jù)式（5）和式（7），利用二分法求解Ctg＝Btg即可得出抓取位置和抓取時間。

4 實驗驗證

為了測試系統(tǒng)有效性，搭建了實驗平臺，如圖5所示。

圖5 系統(tǒng)布局

圖6（a）和圖6（b）為2 種隨機擺放的箱體點云數(shù)據(jù)處理結(jié)果，在掃描完一個完整的箱體后，自動結(jié)束掃描。圖6（c）顯示點云數(shù)據(jù)處理中幾種典型點云數(shù)據(jù)幀的閾值設定。獲取點云數(shù)據(jù)之后，提取箱體的位姿，計算抓取位置，最后控制工業(yè)機器人對箱體進行抓取。在20 次抓取實驗中，尺寸識別誤差為±14 mm，角度偏差為±5°，揀選位置平均誤差為±16 mm，揀選效率為20 s／件。

圖6 箱體點云數(shù)據(jù)處理測試

5 結(jié) 論

在實際工程應用中，由于設備空間結(jié)構(gòu)的限制，需要快速提取箱體的尺寸和計算抓取位置。在箱體定位過程中，利用幀內(nèi)和幀間密度聚類算法，根據(jù)箱體的密度自適應選擇閾值，實現(xiàn)對點云數(shù)據(jù)的逐幀聚類與分割，解決了單一閾值無法滿足不同點云密度的問題。在抓取過程中，采用PTP的路徑規(guī)劃滿足避障要求，在拐角處采用樣條曲線過渡滿足運動過程中的平穩(wěn)性要求，采用二分法求抓取位置與抓取時間的近似解。最后搭建了實驗平臺對算法的可行性和精確度進行了測試，實驗結(jié)果表明，該系統(tǒng)能夠?qū)崿F(xiàn)箱體在隨輸送帶運輸過程中的實時點云數(shù)據(jù)分割和自動分揀。