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

【發明內容】

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