一種針對(duì)生活垃圾中的玻璃瓶分揀控制系統(tǒng)的制作方法
【專利摘要】本發(fā)明公開了一種針對(duì)生活垃圾中的玻璃瓶分揀控制系統(tǒng)，該系統(tǒng)包括瓶子位姿接收模塊、多機(jī)械手調(diào)度控制模塊、多個(gè)軌跡控制模塊和帶重力檢測(cè)功能的機(jī)械手，多機(jī)械手調(diào)度控制模塊同時(shí)判斷多個(gè)玻璃瓶是否進(jìn)入空閑機(jī)械手的工作區(qū)間，將該玻璃瓶的坐標(biāo)和姿態(tài)角信息分配給空閑的機(jī)械手對(duì)應(yīng)的機(jī)械手軌跡控制模塊，機(jī)械手軌跡控制模塊控制機(jī)械手對(duì)瓶子進(jìn)行抓取，并對(duì)瓶子重量進(jìn)行判斷，如果具有玻璃瓶的重量特征，則將抓取物放置到物料容器，否則將其放回傳送帶。本發(fā)明將多個(gè)分揀任務(wù)同時(shí)分配給多臺(tái)機(jī)械手進(jìn)行分揀，另外，在機(jī)械手上增加重量傳感器，提取瓶子的重量特征，彌補(bǔ)了機(jī)器視覺識(shí)別系統(tǒng)無法辨識(shí)目標(biāo)對(duì)象材質(zhì)的不足。
【專利說明】
-種針對(duì)生活垃圾中的玻璃瓶分掠控制系統(tǒng)
技術(shù)領(lǐng)域
[0001 ]本發(fā)明設(shè)及一種針對(duì)生活垃圾中的玻璃瓶分煉控制系統(tǒng)，屬于環(huán)保自動(dòng)化裝備領(lǐng) 域。
【背景技術(shù)】
[0002] 在生活垃圾回收生產(chǎn)線的前端，需要對(duì)垃圾中的玻璃瓶進(jìn)行分煉回收，防止破碎 的玻璃對(duì)后續(xù)的生產(chǎn)線設(shè)備造成破壞。現(xiàn)有的生活垃圾中的玻璃瓶回收都是采用人工分煉 的方法，具有一定的危險(xiǎn)性，而且分煉人員工作環(huán)境惡劣、勞動(dòng)強(qiáng)度高和工作效率低下的缺 點(diǎn)。對(duì)玻璃瓶的分煉可W采用機(jī)械手抓取的方法，機(jī)械手抓取具有效率高、成本低和連續(xù)工 作的優(yōu)點(diǎn)，因此被廣泛的運(yùn)用。
[0003] 目前，生活垃圾中玻璃瓶分煉系統(tǒng)架構(gòu)為傳送帶前端安置機(jī)器視覺系統(tǒng)，通過對(duì) 傳送帶上鋪平的垃圾進(jìn)行圖像采集及圖像處理，將識(shí)別的瓶類形狀和位姿信息傳送給機(jī)械 手控制器，由機(jī)械手控制器控制機(jī)械手末端到達(dá)指定的坐標(biāo)點(diǎn)W-定的手抓姿態(tài)進(jìn)行玻璃 瓶的抓取。
[0004] 但是，現(xiàn)有的機(jī)械手控制大部分都是對(duì)單臺(tái)機(jī)械手的運(yùn)動(dòng)控制，即使是多臺(tái)機(jī)械 手的控制也是針對(duì)不同種類的分煉目標(biāo)在不同工序上的分煉任務(wù)。針對(duì)多目標(biāo)在移動(dòng)生產(chǎn) 線上的垃圾分煉的實(shí)際應(yīng)用情況，運(yùn)種分煉方法容易漏檢，效率低。另外，現(xiàn)有的分煉識(shí)別 技術(shù)一般WCCD光學(xué)攝像頭作為識(shí)別對(duì)象的圖像采集元件，通過圖像處理的方法只能辨識(shí) 分煉對(duì)象的形狀，不能辨識(shí)其材質(zhì)。玻璃瓶和塑料瓶是比較容易混淆的瓶類分煉對(duì)象，如果 要實(shí)現(xiàn)對(duì)玻璃瓶的分煉，就要在機(jī)械手抓取分煉對(duì)象的同時(shí)，檢測(cè)其重量，從而辨識(shí)出玻璃 瓶。一般的機(jī)械手無法區(qū)分所抓取的對(duì)象是否為玻璃瓶。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的技術(shù)解決問題是：克服現(xiàn)有技術(shù)的不足，提供一種針對(duì)生活垃圾中的玻 璃瓶分煉控制系統(tǒng)，解決生活垃圾中的玻璃瓶快速自動(dòng)分煉問題，提高垃圾分煉效率，降低 工人的勞動(dòng)強(qiáng)度，提高垃圾分煉生產(chǎn)線的自動(dòng)化程度。
[0006] 本發(fā)明的技術(shù)解決方案：一種針對(duì)生活垃圾中的玻璃瓶的多機(jī)械手分煉控制系 統(tǒng)，該系統(tǒng)包括瓶子位姿接收模塊、多機(jī)械手調(diào)度控制模塊、N個(gè)機(jī)械手、N個(gè)控制機(jī)械手的 軌跡控制模塊和重量傳感器，機(jī)械手與軌跡控制模塊一一對(duì)應(yīng)，重量傳感器用來采集機(jī)械 手手爪與抓取物的共同重量，其中：
[0007] 瓶子位姿接收模塊，接收外部輸入的瓶子位姿信息帖將其存入接收緩沖區(qū)中，所 述瓶子位姿信息帖包括瓶子在成像平面坐標(biāo)系下的坐標(biāo)、姿態(tài)角和拍攝時(shí)間；
[000引多機(jī)械手調(diào)度控制模塊，提取接收緩沖區(qū)中的瓶子位姿信息帖，計(jì)算瓶子位姿信 息帖中瓶子在t2+AT時(shí)刻在傳送帶平面坐標(biāo)系下的預(yù)測(cè)坐標(biāo)，所述t2為當(dāng)前時(shí)刻，AT為機(jī) 械手收到指令到抓取到瓶子所需要的時(shí)間，判斷該預(yù)測(cè)坐標(biāo)是否落入任意一個(gè)機(jī)械手的工 作空間，如果進(jìn)入某個(gè)機(jī)械手的工作空間，則判斷該機(jī)械手的工作狀態(tài)是否"空閑"，如果 "空閑"，則將該瓶子的成像平面坐標(biāo)系下的坐標(biāo)和姿態(tài)角信息分配給空閑的機(jī)械手對(duì)應(yīng)的 機(jī)械手軌跡控制模塊；
[0009] 機(jī)械手軌跡控制模塊，初始化時(shí)，控制機(jī)械手位于預(yù)設(shè)的準(zhǔn)備位置;接收瓶子在成 像平面坐標(biāo)系下的坐標(biāo)信息之后，將工作狀態(tài)置為"忙"，將該瓶子的坐標(biāo)信息和姿態(tài)角進(jìn) 行坐標(biāo)轉(zhuǎn)換，得到該瓶子在本機(jī)械手坐標(biāo)系下的坐標(biāo)，控制機(jī)械手移動(dòng)至瓶子位置，旋轉(zhuǎn)機(jī) 械手的角度與姿態(tài)角一致，抓取瓶子，當(dāng)瓶子離開傳送帶表面時(shí)，讀取重量傳感器值，根據(jù) 重量判斷所抓取的瓶子是否具有玻璃瓶的重量特征，如果是，則將瓶子送到傳送帶W外指 定的物料存儲(chǔ)區(qū)，如果不是玻璃瓶的重量特征，則將瓶子放回到傳送帶上，之后，控制機(jī)械 手回到預(yù)設(shè)的準(zhǔn)備位置，將工作狀態(tài)置為"空閑"。
[0010] 所述機(jī)械手軌跡控制模塊根據(jù)瓶子重量判斷所抓取的瓶子是否為玻璃瓶的方法 為：
[0011] 判斷重量是否在預(yù)設(shè)闊值范圍內(nèi)，如果是，則判定為玻璃瓶，否則判定為其它瓶 類，所述闊值由機(jī)械手手爪與玻璃瓶的統(tǒng)計(jì)重量之和確定。
[0012] 所述N個(gè)機(jī)械手沿傳送帶運(yùn)動(dòng)方向排列，序號(hào)為1~N，多機(jī)械手調(diào)度控制模塊循環(huán) 執(zhí)行如下步驟：
[0013] (1)判斷當(dāng)前時(shí)刻接收緩沖區(qū)是否有新的瓶子位姿信息帖，如果有，將新的瓶子位 姿信息帖存入當(dāng)前抓取隊(duì)列中，進(jìn)入步驟(2)，否則，直接進(jìn)入步驟(2);
[0014] (2)按照先進(jìn)先出的原則，選取抓取隊(duì)列中的第一個(gè)瓶子位姿信息帖；
[0015] (3)解析瓶子位姿信息帖中瓶子在成像平面坐標(biāo)系下的坐標(biāo)信息（xp_o，yp_o)和拍 攝時(shí)間ti;
[0016] (4)將瓶子在成像平面坐標(biāo)系中坐標(biāo)信息(xp_o，yp_o)進(jìn)行坐標(biāo)轉(zhuǎn)換，得到傳送帶平 面坐標(biāo)系下的坐標(biāo)信息(XG_o，yc_o);
[0017] (5)計(jì)算瓶子在t2+ Δ Τ時(shí)刻的在傳送帶平面坐標(biāo)系下的預(yù)測(cè)坐標(biāo)（XC_target, y[-target);
[0018] (6)當(dāng)前時(shí)刻依次判斷該瓶子預(yù)測(cè)坐標(biāo)相_*3柳，則_*3柳)是否進(jìn)入第1個(gè)機(jī)械手 ~第N個(gè)機(jī)械手的工作空間，如果該瓶子進(jìn)入某一個(gè)機(jī)械手的工作空間，則確定該機(jī)械手為 待抓機(jī)械手，進(jìn)入步驟(7)，否則，將該瓶子位姿信息帖保存到暫存隊(duì)列中，進(jìn)入步驟(8);
[0019] (7)判斷待抓機(jī)械手工作狀態(tài)是否"空閑"，如果"空閑"，則將該瓶子位姿信息帖中 的成像平面坐標(biāo)系坐標(biāo)和姿態(tài)角信息發(fā)送給該機(jī)械手對(duì)應(yīng)的軌跡控制模塊，進(jìn)入步驟(8); 如果工作狀態(tài)為"忙"，則將該瓶子位姿信息帖保存到暫存隊(duì)列中，進(jìn)入步驟(8);
[0020] (8)選取下一個(gè)瓶子位姿信息帖，重新執(zhí)行步驟(3)~步驟(7)，直到判斷完抓取隊(duì) 列中的所有瓶子位姿信息帖，進(jìn)入步驟(9);
[0021] (9)清空抓取隊(duì)列，將暫存隊(duì)列中的所有瓶子位姿信息帖存入抓取隊(duì)列。
[0022] 所述步驟(5)通過解算如下方程組計(jì)算瓶子在t2+AT時(shí)刻在傳送帶平面坐標(biāo)系下 的預(yù)測(cè)坐標(biāo)（XC_target，yC_target):
[0026]
[0027] ZR_target = 0
[002引其中，（XR_int，yR_int，ZR_int)為第i個(gè)機(jī)械手的準(zhǔn)備位置在該機(jī)械手坐標(biāo)系下的坐 標(biāo);VR_hand為機(jī)械手沿點(diǎn)對(duì)點(diǎn)直線運(yùn)動(dòng)的預(yù)設(shè)速度，V為傳送帶的速度，Mc_R_i為傳送帶平面坐 標(biāo)系到第i個(gè)機(jī)械手坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣，t2為當(dāng)前時(shí)刻。
[0029] 所述多機(jī)械手調(diào)度控制模塊判斷該瓶子預(yù)測(cè)坐標(biāo)(Xe_target，yC_target)是否進(jìn)入第i 個(gè)機(jī)械手的工作區(qū)間的方法為：
[0030] (6a)將第i機(jī)械手在第i個(gè)機(jī)械手平面坐標(biāo)系XRiORiYRi下的坐標(biāo)原點(diǎn)（0,0)進(jìn)行坐 標(biāo)轉(zhuǎn)換，得到該坐標(biāo)原點(diǎn)在傳送帶平面坐標(biāo)系下的坐標(biāo)(xe_i，yc_i);
[0031] (化）判斷佔(zhàn)_*3棘，心*3巧日0是否滿足條件：
[0032]
時(shí)，機(jī)械手的 工作空間為半徑為R的扇形區(qū)域，滿足，則認(rèn)為(XC_target，yC_target)落入第i個(gè)機(jī)械手的工作 區(qū)間內(nèi)，否則，認(rèn)為(XC_target，yC_target)不在該機(jī)械手的工作區(qū)間內(nèi)。
[0033] 所述步驟（1)中抓取隊(duì)列中瓶子位姿信息帖按照如下規(guī)則排列：不同拍攝時(shí)刻的 瓶子位姿信息帖，按照時(shí)間先后順序排列，同一拍攝時(shí)刻的瓶子位姿信息帖，根據(jù)瓶子的位 置排列，靠近機(jī)械手的瓶子的位姿信息帖排列在前。
[0034] 本發(fā)明與現(xiàn)有技術(shù)相比具有如下有益效果：
[0035] 1、本發(fā)明布置多個(gè)機(jī)械手對(duì)垃圾進(jìn)行分煉，充分利用每臺(tái)機(jī)械手的分煉能力，將 多個(gè)分煉任務(wù)同時(shí)分配給多臺(tái)機(jī)械手進(jìn)行分煉，可實(shí)現(xiàn)整套系統(tǒng)最多目標(biāo)抓取物的分煉能 力，工作效率高。
[0036] 2、本發(fā)明定義了固定的傳送帶平面坐標(biāo)系，坐標(biāo)是否落入機(jī)械手的抓取范圍的計(jì) 算都是在傳送帶平面坐標(biāo)系進(jìn)行計(jì)算，有利于減少計(jì)算量，提高計(jì)算效率。
[0037] 3、本發(fā)明定義了多個(gè)機(jī)械手坐標(biāo)系，機(jī)械手坐標(biāo)系隨著機(jī)械手的安裝位置變化， 降低了機(jī)械手軌跡控制模塊的處理復(fù)雜度，另外，多機(jī)械手控制模塊只需要將瓶子成像平 面坐標(biāo)系的坐標(biāo)發(fā)送給各機(jī)械手軌跡控制模塊，由各機(jī)械手軌跡控制模塊負(fù)責(zé)對(duì)各自需要 抓取的瓶子的坐標(biāo)進(jìn)行坐標(biāo)轉(zhuǎn)換，簡(jiǎn)化了多機(jī)械手控制模塊的復(fù)雜度。
[0038] 4、本發(fā)明解決了一般的分煉識(shí)別環(huán)節(jié)只能識(shí)別分煉對(duì)象外形，無法辨識(shí)分煉對(duì)象 是玻璃瓶或是塑料瓶問題，提出了在機(jī)械手抓取分煉對(duì)象時(shí)檢測(cè)分煉對(duì)象的重量特征，作 為判斷抓取對(duì)象是否為玻璃瓶分煉物的條件，對(duì)玻璃瓶進(jìn)行自動(dòng)分煉，有效地防止破碎的 玻璃對(duì)后續(xù)的生產(chǎn)線設(shè)備造成的破壞。
【附圖說明】
[0039] 圖1為本發(fā)明的系統(tǒng)結(jié)構(gòu)圖；
[0040] 圖2為本發(fā)明的控制方法流程圖；
[0041 ]圖3分煉機(jī)械手分布方式。
【具體實(shí)施方式】
[0042] 下面結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明作進(jìn)一步詳細(xì)的描述：
[0043] 本發(fā)明實(shí)施例中設(shè)及如下幾種坐標(biāo)系，定義如下：
[0044] (1)成像平面坐標(biāo)系
[0045] W攝像頭光軸與傳送帶平面的交點(diǎn)為坐標(biāo)原點(diǎn)，成像平面上沿傳送帶移動(dòng)方向?yàn)?Xp軸，與Xp軸方向垂直的方向?yàn)檠狠S，成像平面為傳送帶平面。
[0046] (2)傳送帶平面坐標(biāo)系
[0047] 傳送帶平面坐標(biāo)系的原點(diǎn)為傳送帶邊緣的一個(gè)固定點(diǎn)化，Xc軸為沿傳送帶的運(yùn)動(dòng) 方向Jc位于傳送帶平面且與姑軸垂直。
[004引（3)N個(gè)機(jī)械手坐標(biāo)系
[0049] 第i個(gè)機(jī)械手的機(jī)械手坐標(biāo)系為Xri化1化1，原點(diǎn)為機(jī)械手的基座與傳送帶平面的交 點(diǎn)，軸Zr垂直與傳送帶平面，軸Xr、軸化分別與傳送帶平面坐標(biāo)系的軸姑和軸Yc方向一致。
[0050] 成像平面坐標(biāo)系隨著成像平面變換，多個(gè)機(jī)械手坐標(biāo)系隨著機(jī)械手的安裝位置變 化，可W降低成像坐標(biāo)獲取的難度和機(jī)械手軌跡控制模塊的處理復(fù)雜度。
[0051] 圖1為一種針對(duì)生活垃圾中的玻璃瓶分煉控制系統(tǒng)結(jié)構(gòu)框圖。如圖所示，該系統(tǒng)包 括瓶子位姿接收模塊、多機(jī)械手調(diào)度控制模塊、N個(gè)機(jī)械手、N個(gè)控制機(jī)械手的軌跡控制模塊 和重量傳感器，機(jī)械手與軌跡控制模塊一一對(duì)應(yīng)，重量傳感器用來采集機(jī)械手手爪與抓取 物的共同重量，安裝在機(jī)械手手爪正上方，且重力傳感器的受力軸線穿過手爪的重屯、，其 中：
[0052] 瓶子位姿信息接收模塊，通過串口通信方式接收外部輸入的瓶子位姿信息帖，將 其存入接收緩沖區(qū)中，傳送標(biāo)準(zhǔn)可W是35232、1?5485、1?5422，所述瓶子位姿信息帖包括瓶子 在成像平面坐標(biāo)系下的坐標(biāo)、姿態(tài)角和拍攝時(shí)間；所述位姿信息中坐標(biāo)為瓶子的最小外接 矩形的幾何中屯、坐標(biāo)，所述姿態(tài)角信息為瓶子的最小外接矩形的長(zhǎng)邊與X軸的夾角;瓶子位 姿信息帖的位姿信息W成像平面坐標(biāo)系為參考，內(nèi)容簡(jiǎn)短且包含了抓取所需要的全部信 息，有利于縮短傳送時(shí)間和傳送的數(shù)據(jù)量。
[0053] 多機(jī)械手調(diào)度控制模塊對(duì)其所控制的多個(gè)機(jī)械手進(jìn)行調(diào)度控制，與機(jī)械手軌跡控 模塊的通信可W采用工業(yè)現(xiàn)場(chǎng)總線的方式來實(shí)現(xiàn)。多機(jī)械手調(diào)度控制模塊，提取接收緩沖 區(qū)中的瓶子位姿信息帖，計(jì)算瓶子位姿信息帖中瓶子在t2+AT時(shí)刻在傳送帶平面坐標(biāo)系下 的預(yù)測(cè)坐標(biāo)，所述t2為當(dāng)前時(shí)刻，ΔΤ為機(jī)械手收到指令到抓取到瓶子所需要的時(shí)間，判斷 該預(yù)測(cè)坐標(biāo)是否進(jìn)入任意一個(gè)機(jī)械手的工作空間，如果進(jìn)入某個(gè)機(jī)械手的工作空間，則判 斷該機(jī)械手的工作狀態(tài)是否"空閑"，如果"空閑"，則將該瓶子的成像平面坐標(biāo)系下的坐標(biāo) 和姿態(tài)角信息分配給空閑的機(jī)械手對(duì)應(yīng)的機(jī)械手軌跡控制模塊。多機(jī)械手調(diào)度控制模塊W 毫秒級(jí)的控制周期輪詢各個(gè)機(jī)械手的運(yùn)行狀態(tài)，及時(shí)對(duì)空閑的機(jī)械手進(jìn)行任務(wù)分配，確保 機(jī)械手對(duì)瓶子進(jìn)行高效的抓取。
[0054] 機(jī)械手軌跡控制模塊，初始化時(shí)，控制機(jī)械手位于預(yù)設(shè)的準(zhǔn)備位置;接收瓶子的成 像平面坐標(biāo)系下的坐標(biāo)信息之后，將工作狀態(tài)置為"忙"，將該瓶子的坐標(biāo)信息和姿態(tài)角進(jìn) 行坐標(biāo)轉(zhuǎn)換，得到該瓶子在本機(jī)械手坐標(biāo)系下的坐標(biāo)，控制機(jī)械手移動(dòng)至瓶子的位置，旋轉(zhuǎn) 機(jī)械手的角度與姿態(tài)角一致，抓取瓶子，在瓶子離開傳送帶平面后，檢測(cè)機(jī)械手手爪與抓取 物的共同重量，判斷其重量是否在一定的闊值范圍內(nèi)，如果是，則判定為玻璃瓶，將其送到 傳送帶W外指定的物料存儲(chǔ)區(qū)，否則，判定為其它材質(zhì)瓶子，松開手爪，將其放回傳送帶，所 述闊值由機(jī)械手手爪與玻璃瓶的統(tǒng)計(jì)重量之和確定。之后，控制機(jī)械手回到預(yù)設(shè)的準(zhǔn)備位 置，將工作狀態(tài)置為"空閑"。所述預(yù)設(shè)準(zhǔn)備位置可W是傳送帶平面的正上方，其在傳送帶平 面上的投影位于傳送帶在押軸方向的中屯、位置。該位置有利于機(jī)械手快速到達(dá)傳送帶表面 進(jìn)行抓取。
[0055] 多機(jī)械手控制模塊只需要將瓶子在成像平面坐標(biāo)系的坐標(biāo)發(fā)送給各機(jī)械手軌跡 控制模塊，由各機(jī)械手軌跡控制模塊負(fù)責(zé)對(duì)各自需要抓取的瓶子的坐標(biāo)進(jìn)行坐標(biāo)轉(zhuǎn)換，簡(jiǎn) 化了多機(jī)械手控制模塊的復(fù)雜度。
[0056] N個(gè)機(jī)械手沿傳送帶運(yùn)動(dòng)方向排列，序號(hào)為1~N，多機(jī)械手調(diào)度控制模塊循環(huán)執(zhí)行 如下步驟：
[0057] (1)判斷接收緩沖區(qū)是否有新的瓶子位姿信息帖，如果有，將新的瓶子位姿信息帖 存入當(dāng)前抓取隊(duì)列中，進(jìn)入步驟(2)，否則，直接進(jìn)入步驟(2)
[0058] (2)按照先進(jìn)先出的原則，選取抓取隊(duì)列中的第一個(gè)瓶子位姿信息帖；
[0059] (3)解析瓶子位姿信息帖在成像平面坐標(biāo)系下的坐標(biāo)信息（xp_o，yp_o)和拍攝時(shí)間 ti;
[0060] (4)將瓶子在成像平面坐標(biāo)系中坐標(biāo)信息(xp_o，yp_o)進(jìn)行坐標(biāo)轉(zhuǎn)換，得到傳送帶平 面坐標(biāo)系下的坐標(biāo)信息(XG_o，yc_o);
[0061]
( 1 )
[0062] 1。_^為成像平面坐標(biāo)系到傳送帶平面坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣；
[0063] 在傳送帶平面坐標(biāo)系的原點(diǎn)化及扣軸和Yg軸上任一點(diǎn)上設(shè)置明顯的物理標(biāo)識(shí)，通 過運(yùn)些明顯物理標(biāo)識(shí)在成像平面坐標(biāo)系所對(duì)應(yīng)的坐標(biāo)值和在傳送帶平面坐標(biāo)系下的坐標(biāo) 值可W解算出成像平面坐標(biāo)系到傳送帶平面坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣Mp_c。
[0064] (5)通過解算如下方程組計(jì)算目標(biāo)抓取物在t2+AT時(shí)刻在傳送帶平面坐標(biāo)系下的 預(yù)測(cè)坐標(biāo)（XG_target，yG_target):
[0070]其中，（XR_int，yR_int，ZR_int)為第i個(gè)機(jī)械手的準(zhǔn)備位置在該機(jī)械手坐標(biāo)系下的坐 標(biāo);VR_hand為機(jī)械手沿點(diǎn)對(duì)點(diǎn)直線運(yùn)動(dòng)的預(yù)設(shè)速度，V為傳送帶的速度，t2為當(dāng)前時(shí)刻，Mc_R_i 為第i個(gè)機(jī)械坐標(biāo)系到傳送帶平面坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣，i = 1~N。
[0071] W機(jī)械手末端安裝的錐形工裝與傳送帶上的Ξ個(gè)特殊點(diǎn)進(jìn)行點(diǎn)接觸，確定機(jī)械手 坐標(biāo)系XriOriYri，平面XriOriYri與傳送帶平面重合。采用精密光學(xué)儀器測(cè)量傳送帶平面坐標(biāo) 系的原點(diǎn)及沿Xe軸和沿Ye軸上的一點(diǎn)的坐標(biāo)值，再W精密光學(xué)儀器測(cè)量機(jī)械手錐形工裝的 尖點(diǎn)在Ξ個(gè)不共線位置的坐標(biāo)值及其在機(jī)械手坐標(biāo)系下的坐標(biāo)值。通過如上關(guān)系可W解算 出轉(zhuǎn)換矩陣Mc_R_i。
[0072] 上述的目標(biāo)坐標(biāo)在機(jī)械手到達(dá)抓取位置時(shí)的預(yù)測(cè)方法，有利于對(duì)目標(biāo)抓取物位置 的準(zhǔn)確判斷，增加抓取的精確度。避免計(jì)算誤差導(dǎo)致目標(biāo)抓取物在機(jī)械手抓取時(shí)已經(jīng)不在 其工作空間內(nèi)，導(dǎo)致抓取失敗。
[0073] (6)當(dāng)前時(shí)刻依次判斷該目標(biāo)抓取物預(yù)測(cè)坐標(biāo)^[_*3巧日*，則_*3巧日0是否進(jìn)入第1個(gè) 機(jī)械手~第N個(gè)機(jī)械手的工作空間，如果該目標(biāo)抓取物進(jìn)入某一個(gè)機(jī)械手的工作空間，則確 定該機(jī)械手為待抓機(jī)械手，進(jìn)入步驟(7)，否則，將該目標(biāo)位姿信息帖保存到暫存隊(duì)列中，進(jìn) 入步驟(8);
[0074] 多機(jī)械手調(diào)度控制模塊判斷該目標(biāo)抓取物預(yù)測(cè)坐標(biāo)(XG_target，yG_target)是否進(jìn)入 第i個(gè)機(jī)械手的工作區(qū)間的方法為：
[0075] (6a)將第i機(jī)械手在第i個(gè)機(jī)械手坐標(biāo)系XRiORi化i下的坐標(biāo)原點(diǎn)（0,0)進(jìn)行坐標(biāo)轉(zhuǎn) 換，得到該坐標(biāo)原點(diǎn)在傳送帶平面坐標(biāo)系下的坐標(biāo)(xe_i，yc_i);
[0076]
( 7 )
[0077] ( 6b )判斷（XG_target，yG_target)是否滿足條件：
[007引
寸，機(jī)械手的 工作空間為半徑為R的扇形區(qū)域，滿足，則認(rèn)為(xc_target，yc_target)落入第i個(gè)機(jī)械手的工作 區(qū)域內(nèi)，否則，認(rèn)為(XC_target，yC_target)不在該機(jī)械手的工作區(qū)域內(nèi)。
[0079] (7)判斷待抓機(jī)械手工作狀態(tài)是否"空閑"，如果"空閑"，則將該目標(biāo)位姿信息帖中 的成像平面坐標(biāo)系坐標(biāo)和姿態(tài)角信息發(fā)送給該機(jī)械手對(duì)應(yīng)的軌跡控制模塊，進(jìn)入步驟(8); 如果工作狀態(tài)為"忙"，則將該目標(biāo)位姿信息帖保存到暫存隊(duì)列中，進(jìn)入步驟(8);
[0080] (8)按照先進(jìn)先出的原則，選取下一個(gè)目標(biāo)位姿信息帖，重新執(zhí)行步驟(3)~步驟 (7)，直到選取完抓取隊(duì)列中的所有目標(biāo)位姿信息帖，進(jìn)入步驟(9);
[0081] (9)清空抓取隊(duì)列，將暫存隊(duì)列中的所有目標(biāo)位姿信息帖存入抓取隊(duì)列。
[0082] 在抓取對(duì)象離開傳送帶時(shí)，機(jī)械手軌跡控制模塊讀取重量傳感器的檢測(cè)值，判斷 其是否處在預(yù)先設(shè)定的闊值范圍內(nèi)，如果是則將抓取物放置物料容器，如果否，則將手爪松 開，抓取對(duì)象落回傳送帶。
[0083] 上述的坐標(biāo)計(jì)算大部分都是在傳送帶平面坐標(biāo)系進(jìn)行計(jì)算，有利于減少計(jì)算量， 提高計(jì)算效率。
[0084] 實(shí)施例：
[0085] -種針對(duì)生活垃圾中的玻璃瓶分煉控制系統(tǒng)中配置3個(gè)機(jī)械手，如圖3所示。機(jī)械 手沿傳送帶傳送方向依次排列，其中，機(jī)械手1和機(jī)械手2位于傳送帶同一側(cè)，機(jī)械手3位于 另一側(cè)。傳送帶平面坐標(biāo)系的原點(diǎn)為傳送帶邊緣的一點(diǎn)化，Xc軸為沿傳送帶的運(yùn)動(dòng)方向，Yc 位于傳送帶平面且與Xc軸垂直。
[0086] 本實(shí)施例中，成像平面坐標(biāo)系與傳送帶平面坐標(biāo)系的重合，則，
[0087]
[0088] 機(jī)械手1的機(jī)械手坐標(biāo)系為Xr化Zr化原點(diǎn)為機(jī)械手的基座且位于傳送帶平面，軸Zr 垂直與傳送帶平面，軸Xr、軸Yr分別與軸姑和軸Yc方向一致。
[0089] 待抓取瓶子①、②、③和④沿傳送帶方向運(yùn)動(dòng)，其中，①為塑料瓶，其余的為玻璃 瓶，在瓶子進(jìn)入機(jī)械手1的工作空間之前，該瓶子的信息帖已經(jīng)發(fā)送給位姿信息接收模塊。 位姿信息接收模塊將所有接收到的信息帖存儲(chǔ)在接收緩沖區(qū)，多機(jī)械手調(diào)度模塊可W是西 口子S7-300系列可編程邏輯控制器(PLC)。在化C的每個(gè)程序執(zhí)行周期（1ms之內(nèi)）都對(duì)循環(huán) 執(zhí)行如下步驟：。
[0090] (1)判斷接收緩沖取是否有新的瓶子位姿信息帖，如果有，將新的瓶子位姿信息帖 存入當(dāng)前抓取隊(duì)列，進(jìn)入步驟(2)，否則，直接進(jìn)入步驟(2)。圖2所示的當(dāng)前抓取隊(duì)列中有4 個(gè)瓶子位姿信息帖。所述抓取隊(duì)列中瓶子位姿信息帖按照如下規(guī)則排列：不同拍攝時(shí)刻的 瓶子位姿信息帖，按照時(shí)間先后順序排列，同一拍攝時(shí)刻的瓶子位姿信息帖，根據(jù)瓶子位姿 信息帖的位置排列，靠近機(jī)械手的瓶子位姿信息帖排列在前。
[0091] (2)選取抓取隊(duì)列中的瓶子①信息帖。
[0092] (3)解析瓶子①信息帖在成像平面坐標(biāo)系下坐標(biāo)信息(祉_日，7。_日）=(0,300)和拍攝 時(shí)間ti=l;
[OOW] (4)將瓶子①在成像平面坐標(biāo)系中坐標(biāo)信息^。_日，7。_日）=(0,300)進(jìn)行坐標(biāo)轉(zhuǎn)換， 得到傳送帶平面坐標(biāo)系下的坐標(biāo)信息(XG_0，K_0);
[0097] Mp_e為成像平面坐標(biāo)系到傳送帶平面坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣，兩坐標(biāo)系重合，即， (xc_o，yc_o) = (0,300);
[0098] (5)設(shè)當(dāng)前時(shí)刻t2 = 3s，第1個(gè)機(jī)械手的準(zhǔn)備位置在該機(jī)械手坐標(biāo)系下的坐標(biāo) (XR_int，yR_int，ZR_int) = (0，600，500); Δ T為機(jī)械手收到指令到抓取到目標(biāo)抓取物所需要的 時(shí)間，VR_hand = 2000,傳送帶的速率v= 1000，Mc_R_i為傳送帶平面坐標(biāo)系到第i個(gè)機(jī)械手坐標(biāo) 系的坐標(biāo)轉(zhuǎn)換矩陣；
[0099] 第1個(gè)機(jī)械手坐標(biāo)系到傳送帶平面坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣為：
[0100]
[0101] 兩坐標(biāo)系為平移關(guān)系。
[0102] 解算方程組（公式(2)~公式(6))得到目標(biāo)抓取物在t2+AT時(shí)刻的在傳送帶平面 坐標(biāo)系下的預(yù)測(cè)坐標(biāo)(XC_target，yC_target):
[0103] 其中，zR_ta 巧 et = 0，xc_o = 0，yc_o = yc_target = 300
[0104] 計(jì)算得到（xc_ta巧et，yc_ta巧et) = (2288.675,300)，ΔT = 0.289。
[0105] (6)當(dāng)前時(shí)刻依次判斷該瓶子預(yù)測(cè)坐標(biāo)(XG_target，K_target)是否進(jìn)入第1個(gè)機(jī)械手 ~第3個(gè)機(jī)械手的工作空間，如果該目標(biāo)抓取物進(jìn)入某一個(gè)機(jī)械手的工作空間，則確定該機(jī) 械手為待抓機(jī)械手，進(jìn)入步驟(7)，否則，將該瓶子位姿信息帖保存到暫存隊(duì)列中，進(jìn)入步驟 (8);
[0106] 判斷^[_*3巧日*，則_*3巧日0是否在機(jī)械手1的工作空間范圍，設(shè)機(jī)械手1在傳送帶的工 作空間半徑R= 1000mm。
關(guān)系，因此，瓶子①為處于機(jī)械手1的工作空間內(nèi)，進(jìn)入步驟(7);
[0111] (7)判斷待抓機(jī)械手1工作狀態(tài)是否"空閑"，如果"空閑"，則將該目標(biāo)位姿信息帖 中的成像平面坐標(biāo)系坐標(biāo)信息發(fā)送給該機(jī)械手對(duì)應(yīng)的軌跡控制模塊，進(jìn)入步驟(8);如果工 作狀態(tài)為"忙"，則將該目標(biāo)位姿信息帖保存到暫存隊(duì)列中，進(jìn)入步驟(9);
[0112] (8)機(jī)械手抓取瓶子，在瓶子①離開傳送帶時(shí)，機(jī)械手軌跡控制模塊檢測(cè)重量傳感 器的輸入量，判斷其是否在預(yù)設(shè)的闊值，本實(shí)施例中，采用超輕材料制成的機(jī)械爪，機(jī)械爪 的重量與一般的玻璃瓶的重量總和一般處在800~850g之間，因此，闊值范圍取[800,850]， 該瓶子為空的塑料瓶，應(yīng)該在420g左右，明顯不在闊值范圍內(nèi)，控制手爪松開，瓶子①落回 傳送帶。
[0113] (9)按照先進(jìn)先出的原則，多機(jī)械手調(diào)度控制模塊選取下一個(gè)瓶子位姿信息帖，重 新執(zhí)行步驟(3)~步驟(7)，直到選取完抓取隊(duì)列中的所有瓶子位姿信息帖，進(jìn)入步驟(10);
[0114] (10)多機(jī)械手調(diào)度控制模塊清空抓取隊(duì)列，將暫存隊(duì)列中的所有瓶子位姿信息帖 存入抓取隊(duì)列。
[0115] 本發(fā)明布置多個(gè)機(jī)械手對(duì)垃圾中的瓶子進(jìn)行分煉，可W充分利用每臺(tái)機(jī)械手的分 煉能力，將多個(gè)分煉任務(wù)同時(shí)分配給多臺(tái)機(jī)械手進(jìn)行分煉，可實(shí)現(xiàn)整套系統(tǒng)最多目標(biāo)抓取 物的分煉能力，工作效率高。由于傳送帶的速度慢，而多機(jī)械手控制模塊的循環(huán)控制速度 快，對(duì)于同一個(gè)目標(biāo)抓取物而言，將有多次機(jī)會(huì)進(jìn)入多個(gè)機(jī)械手的工作空間，可W達(dá)到非常 低的漏檢率。
[0116] 另外，本發(fā)明在機(jī)械手抓取分煉對(duì)象時(shí)檢測(cè)分煉對(duì)象的重量特征，作為判斷抓取 對(duì)象是否為玻璃瓶分煉物的條件，對(duì)玻璃瓶進(jìn)行自動(dòng)分煉，有效地防止破碎的玻璃對(duì)后續(xù) 的生產(chǎn)線設(shè)備造成的破壞。
[0117] W上所述，僅為本發(fā)明最佳的【具體實(shí)施方式】，但本發(fā)明的保護(hù)范圍并不局限于此， 任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明掲露的技術(shù)范圍內(nèi)，可輕易想到的變化或替換， 都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。
[0118] 本發(fā)明說明書中未作詳細(xì)描述的內(nèi)容屬于本領(lǐng)域?qū)I(yè)技術(shù)人員的公知技術(shù)。
【主權(quán)項(xiàng)】
1. 一種針對(duì)生活垃圾中的玻璃瓶分煉控制系統(tǒng)，其特征在于包括瓶子位姿接收模塊、 多機(jī)械手調(diào)度控制模塊、N個(gè)機(jī)械手、N個(gè)控制機(jī)械手的軌跡控制模塊和重量傳感器，機(jī)械手 與軌跡控制模塊一一對(duì)應(yīng)，重量傳感器用來采集機(jī)械手手爪與抓取物的共同重量，其中： 瓶子位姿接收模塊，接收外部輸入的瓶子位姿信息帖將其存入接收緩沖區(qū)中，所述瓶 子位姿信息帖包括瓶子在成像平面坐標(biāo)系下的坐標(biāo)、姿態(tài)角和拍攝時(shí)間； 多機(jī)械手調(diào)度控制模塊，提取接收緩沖區(qū)中的瓶子位姿信息帖，計(jì)算瓶子位姿信息帖 中瓶子在t2+Δ T時(shí)刻在傳送帶平面坐標(biāo)系下的預(yù)測(cè)坐標(biāo)，所述t2為當(dāng)前時(shí)刻，Δ T為機(jī)械手 收到指令到抓取到瓶子所需要的時(shí)間，判斷該預(yù)測(cè)坐標(biāo)是否落入任意一個(gè)機(jī)械手的工作空 間，如果進(jìn)入某個(gè)機(jī)械手的工作空間，則判斷該機(jī)械手的工作狀態(tài)是否"空閑"，如果"空 閑"，則將該瓶子的成像平面坐標(biāo)系下的坐標(biāo)和姿態(tài)角信息分配給空閑的機(jī)械手對(duì)應(yīng)的機(jī) 械手軌跡控制模塊； 機(jī)械手軌跡控制模塊，初始化時(shí)，控制機(jī)械手位于預(yù)設(shè)的準(zhǔn)備位置;接收瓶子在成像平 面坐標(biāo)系下的坐標(biāo)信息之后，將工作狀態(tài)置為"忙"，將該瓶子的坐標(biāo)信息和姿態(tài)角進(jìn)行坐 標(biāo)轉(zhuǎn)換，得到該瓶子在本機(jī)械手坐標(biāo)系下的坐標(biāo)，控制機(jī)械手移動(dòng)至瓶子位置，旋轉(zhuǎn)機(jī)械手 的角度與姿態(tài)角一致，抓取瓶子，當(dāng)瓶子離開傳送帶表面時(shí)，讀取重量傳感器值，根據(jù)重量 判斷所抓取的瓶子是否具有玻璃瓶的重量特征，如果是，則將瓶子送到傳送帶W外指定的 物料存儲(chǔ)區(qū)，如果不是玻璃瓶的重量特征，則將瓶子放回到傳送帶上，之后，控制機(jī)械手回 到預(yù)設(shè)的準(zhǔn)備位置，將工作狀態(tài)置為"空閑"。2. 根據(jù)權(quán)利要求1所述的一種針對(duì)生活垃圾中的玻璃瓶分煉控制系統(tǒng)，其特征在于所 述機(jī)械手軌跡控制模塊根據(jù)瓶子重量判斷所抓取的瓶子是否為玻璃瓶的方法為： 判斷重量是否在預(yù)設(shè)闊值范圍內(nèi)，如果是，則判定為玻璃瓶，否則判定為其它瓶類，所 述闊值由機(jī)械手手爪與玻璃瓶的統(tǒng)計(jì)重量之和確定。3. 根據(jù)權(quán)利要求1所述的一種針對(duì)生活垃圾中的玻璃瓶分煉控制系統(tǒng)，其特征在于所 述N個(gè)機(jī)械手沿傳送帶運(yùn)動(dòng)方向排列，序號(hào)為1~N，多機(jī)械手調(diào)度控制模塊循環(huán)執(zhí)行如下步 驟： (1) 判斷當(dāng)前時(shí)刻接收緩沖區(qū)是否有新的瓶子位姿信息帖，如果有，將新的瓶子位姿信 息帖存入當(dāng)前抓取隊(duì)列中，進(jìn)入步驟(2)，否則，直接進(jìn)入步驟(2); (2) 按照先進(jìn)先出的原則，選取抓取隊(duì)列中的第一個(gè)瓶子位姿信息帖； (3) 解析瓶子位姿信息帖中瓶子在成像平面坐標(biāo)系下的坐標(biāo)信息（xp_o，yp_o)和拍攝時(shí) 間ti; (4) 將瓶子在成像平面坐標(biāo)系中坐標(biāo)信息（祉_o，yp_o)進(jìn)行坐標(biāo)轉(zhuǎn)換，得到傳送帶平面坐 標(biāo)系下的坐標(biāo)信息(XG_o，yc_o); (5) 計(jì)算瓶子在t2+AT時(shí)刻的在傳送帶平面坐標(biāo)系下的預(yù)測(cè)坐標(biāo)(XC_target，yC_target); (6) 當(dāng)前時(shí)刻依次判斷該瓶子預(yù)測(cè)坐標(biāo)(Xe_target，K_target)是否進(jìn)入第1個(gè)機(jī)械手~第N 個(gè)機(jī)械手的工作空間，如果該瓶子進(jìn)入某一個(gè)機(jī)械手的工作空間，則確定該機(jī)械手為待抓 機(jī)械手，進(jìn)入步驟(7)，否則，將該瓶子位姿信息帖保存到暫存隊(duì)列中，進(jìn)入步驟(8); (7) 判斷待抓機(jī)械手工作狀態(tài)是否"空閑"，如果"空閑"，則將該瓶子位姿信息帖中的成 像平面坐標(biāo)系坐標(biāo)和姿態(tài)角信息發(fā)送給該機(jī)械手對(duì)應(yīng)的軌跡控制模塊，進(jìn)入步驟(8);如果 工作狀態(tài)為"忙"，則將該瓶子位姿信息帖保存到暫存隊(duì)列中，進(jìn)入步驟(8); (8) 按照先進(jìn)先出的原則，選取下一個(gè)目標(biāo)位姿信息帖，重新執(zhí)行步驟(3)~步驟(7)， 直到選取完抓取隊(duì)列中的所有目標(biāo)位姿信息帖，進(jìn)入步驟(9); (9) 清空抓取隊(duì)列，將暫存隊(duì)列中的所有目標(biāo)位姿信息帖存入抓取隊(duì)列。4. 根據(jù)權(quán)利要求3所述的一種針對(duì)生活垃圾中的玻璃瓶分煉控制系統(tǒng)，其特征在于所 述步驟(5)通過解算如下方程組計(jì)算瓶子在t2+Δ T時(shí)刻在傳送帶平面坐標(biāo)系下的預(yù)測(cè)坐標(biāo)其中，（XR_int，yR_int，ZR_int)為第i個(gè)機(jī)械手的準(zhǔn)備位置在該機(jī)械手坐標(biāo)系下的坐標(biāo)； VR_hand為機(jī)械手沿點(diǎn)對(duì)點(diǎn)直線運(yùn)動(dòng)的預(yù)設(shè)速度，V為傳送帶的速度，Mc_R_i為傳送帶平面坐標(biāo) 系到第i個(gè)機(jī)械手坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣，t2為當(dāng)前時(shí)刻。5. 根據(jù)權(quán)利要求3所述的一種針對(duì)生活垃圾中的玻璃瓶分煉控制系統(tǒng)，其特征在于所 述多機(jī)械手調(diào)度控制模塊判斷該瓶子預(yù)測(cè)坐標(biāo)(XC_target，yC_target)是否進(jìn)入第i個(gè)機(jī)械手的 工作區(qū)間的方法為： (6a)將第i機(jī)械手在第i個(gè)機(jī)械手平面坐標(biāo)系XriOriYri下的坐標(biāo)原點(diǎn)（0,0)進(jìn)行坐標(biāo)轉(zhuǎn) 換，得到該坐標(biāo)原點(diǎn)在傳送帶平面坐標(biāo)系下的坐標(biāo)(Xe_i，yC_i); (6b)判斷（XG_target, yG_target)是否細(xì)足條件：時(shí)，機(jī)械手的工作 空間為半徑為R的扇形區(qū)域，滿足，則認(rèn)為(XC_target，yC_target)落入第i個(gè)機(jī)械手的工作區(qū)間 內(nèi)，否則，認(rèn)為(XC_target , yC_target)不在該機(jī)械手的工作區(qū)間內(nèi)。6. 根據(jù)權(quán)利要求3所述的一種針對(duì)生活垃圾中的玻璃瓶分煉控制系統(tǒng)，其特征在于所 述步驟（1)中抓取隊(duì)列中瓶子位姿信息帖按照如下規(guī)則排列：不同拍攝時(shí)刻的瓶子位姿信 息帖，按照時(shí)間先后順序排列，同一拍攝時(shí)刻的瓶子位姿信息帖，根據(jù)瓶子的位置排列，靠 近機(jī)械手的瓶子的位姿信息帖排列在前。
【文檔編號(hào)】B07C5/16GK105964567SQ201610362689
【公開日】2016年9月28日
【申請(qǐng)日】2016年5月26日
【發(fā)明人】陳志鴻, 朱成林, 韓志富, 張科, 楊濤, 李常, 梁斌焱, 王燕波
【申請(qǐng)人】北京新長(zhǎng)征天高智機(jī)科技有限公司