CN105964567A - 一种针对生活垃圾中的玻璃瓶分拣控制*** - Google Patents

Abstract

本发明公开了一种针对生活垃圾中的玻璃瓶分拣控制***，该***包括瓶子位姿接收模块、多机械手调度控制模块、多个轨迹控制模块和带重力检测功能的机械手，多机械手调度控制模块同时判断多个玻璃瓶是否进入空闲机械手的工作区间，将该玻璃瓶的坐标和姿态角信息分配给空闲的机械手对应的机械手轨迹控制模块，机械手轨迹控制模块控制机械手对瓶子进行抓取，并对瓶子重量进行判断，如果具有玻璃瓶的重量特征，则将抓取物放置到物料容器，否则将其放回传送带。本发明将多个分拣任务同时分配给多台机械手进行分拣，另外，在机械手上增加重量传感器，提取瓶子的重量特征，弥补了机器视觉识别***无法辨识目标对象材质的不足。

Description

一种针对生活垃圾中的玻璃瓶分拣控制***

技术领域

本发明涉及一种针对生活垃圾中的玻璃瓶分拣控制***，属于环保自动化装备领域。

背景技术

在生活垃圾回收生产线的前端，需要对垃圾中的玻璃瓶进行分拣回收，防止破碎的玻璃对后续的生产线设备造成破坏。现有的生活垃圾中的玻璃瓶回收都是采用人工分拣的方法，具有一定的危险性，而且分拣人员工作环境恶劣、劳动强度高和工作效率低下的缺点。对玻璃瓶的分拣可以采用机械手抓取的方法，机械手抓取具有效率高、成本低和连续工作的优点，因此被广泛的运用。

目前，生活垃圾中玻璃瓶分拣***架构为传送带前端安置机器视觉***，通过对传送带上铺平的垃圾进行图像采集及图像处理，将识别的瓶类形状和位姿信息传送给机械手控制器，由机械手控制器控制机械手末端到达指定的坐标点以一定的手抓姿态进行玻璃瓶的抓取。

但是，现有的机械手控制大部分都是对单台机械手的运动控制，即使是多台机械手的控制也是针对不同种类的分拣目标在不同工序上的分拣任务。针对多目标在移动生产线上的垃圾分拣的实际应用情况，这种分拣方法容易漏检，效率低。另外，现有的分拣识别技术一般以CCD光学摄像头作为识别对象的图像采集元件，通过图像处理的方法只能辨识分拣对象的形状，不能辨识其材质。玻璃瓶和塑料瓶是比较容易混淆的瓶类分拣对象，如果要实现对玻璃瓶的分拣，就要在机械手抓取分拣对象的同时，检测其重量，从而辨识出玻璃瓶。一般的机械手无法区分所抓取的对象是否为玻璃瓶。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种针对生活垃圾中的玻璃瓶分拣控制***，解决生活垃圾中的玻璃瓶快速自动分拣问题，提高垃圾分拣效率，降低工人的劳动强度，提高垃圾分拣生产线的自动化程度。

本发明的技术解决方案：一种针对生活垃圾中的玻璃瓶的多机械手分拣控制***，该***包括瓶子位姿接收模块、多机械手调度控制模块、N个机械手、N个控制机械手的轨迹控制模块和重量传感器，机械手与轨迹控制模块一一对应，重量传感器用来采集机械手手爪与抓取物的共同重量，其中：

瓶子位姿接收模块，接收外部输入的瓶子位姿信息帧将其存入接收缓冲区中，所述瓶子位姿信息帧包括瓶子在成像平面坐标系下的坐标、姿态角和拍摄时间；

多机械手调度控制模块，提取接收缓冲区中的瓶子位姿信息帧，计算瓶子位姿信息帧中瓶子在t₂+ΔT时刻在传送带平面坐标系下的预测坐标，所述t₂为当前时刻，ΔT为机械手收到指令到抓取到瓶子所需要的时间，判断该预测坐标是否落入任意一个机械手的工作空间，如果进入某个机械手的工作空间，则判断该机械手的工作状态是否“空闲”，如果“空闲”，则将该瓶子的成像平面坐标系下的坐标和姿态角信息分配给空闲的机械手对应的机械手轨迹控制模块；

机械手轨迹控制模块，初始化时，控制机械手位于预设的准备位置；接收瓶子在成像平面坐标系下的坐标信息之后，将工作状态置为“忙”，将该瓶子的坐标信息和姿态角进行坐标转换，得到该瓶子在本机械手坐标系下的坐标，控制机械手移动至瓶子位置，旋转机械手的角度与姿态角一致，抓取瓶子，当瓶子离开传送带表面时，读取重量传感器值，根据重量判断所抓取的瓶子是否具有玻璃瓶的重量特征，如果是，则将瓶子送到传送带以外指定的物料存储区，如果不是玻璃瓶的重量特征，则将瓶子放回到传送带上，之后，控制机械手回到预设的准备位置，将工作状态置为“空闲”。

所述机械手轨迹控制模块根据瓶子重量判断所抓取的瓶子是否为玻璃瓶的方法为：

判断重量是否在预设阈值范围内，如果是，则判定为玻璃瓶，否则判定为其它瓶类，所述阈值由机械手手爪与玻璃瓶的统计重量之和确定。

所述N个机械手沿传送带运动方向排列，序号为1～N，多机械手调度控制模块循环执行如下步骤：

(1)判断当前时刻接收缓冲区是否有新的瓶子位姿信息帧，如果有，将新的瓶子位姿信息帧存入当前抓取队列中，进入步骤(2)，否则，直接进入步骤(2)；

(2)按照先进先出的原则，选取抓取队列中的第一个瓶子位姿信息帧；

(3)解析瓶子位姿信息帧中瓶子在成像平面坐标系下的坐标信息(x_{P_0},y_{P_0})和拍摄时间t₁；

(4)将瓶子在成像平面坐标系中坐标信息(x_{P_0},y_{P_0})进行坐标转换，得到传送带平面坐标系下的坐标信息(x_{C_0},y_{C_0})；

(5)计算瓶子在t₂+ΔT时刻的在传送带平面坐标系下的预测坐标(x_{C_target},y_{C_target})；

(6)当前时刻依次判断该瓶子预测坐标(x_{C_target},y_{C_target})是否进入第1个机械手～第N个机械手的工作空间，如果该瓶子进入某一个机械手的工作空间，则确定该机械手为待抓机械手，进入步骤(7)，否则，将该瓶子位姿信息帧保存到暂存队列中，进入步骤(8)；

(7)判断待抓机械手工作状态是否“空闲”，如果“空闲”，则将该瓶子位姿信息帧中的成像平面坐标系坐标和姿态角信息发送给该机械手对应的轨迹控制模块，进入步骤(8)；如果工作状态为“忙”，则将该瓶子位姿信息帧保存到暂存队列中，进入步骤(8)；

(8)选取下一个瓶子位姿信息帧，重新执行步骤(3)～步骤(7)，直到判断完抓取队列中的所有瓶子位姿信息帧，进入步骤(9)；

(9)清空抓取队列，将暂存队列中的所有瓶子位姿信息帧存入抓取队列。

所述步骤(5)通过解算如下方程组计算瓶子在t₂+ΔT时刻在传送带平面坐标系下的预测坐标(x_{C_target},y_{C_target})：

x_{C_target}＝x_{C_0}+v×(t₂+ΔT-t₁)

y_{C_target}＝y_{C_0}

$\mathrm{\Δ}T=\sqrt{{(x_{R\_t\arg et}-x_{R\_\mathrm{int}})}^2+{(y_{R\_t\arg et}-y_{R\_\mathrm{int}})}^2+{(z_{R\_t\arg et}-z_{R\_\mathrm{int}})}^2}/{V_{R\_}}_{hand}$

$\left[\begin{array}{c}{x}_{R\_t\arg et}\\ y_{R\_t\arg et}\\ 1\end{array}\right]=M_{C\_R\_i}\·\left[\begin{array}{c}{x}_{C\_t\arg et}\\ y_{C\_t\arg et}\\ 1\end{array}\right]$

z_{R_target}＝0

其中，(x_{R_int}，y_{R_int}，z_{R_int})为第i个机械手的准备位置在该机械手坐标系下的坐标；V_{R_hand}为机械手沿点对点直线运动的预设速度，v为传送带的速度，M_{C_R_i}为传送带平面坐标系到第i个机械手坐标系的坐标转换矩阵，t₂为当前时刻。

所述多机械手调度控制模块判断该瓶子预测坐标(x_{C_target},y_{C_target})是否进入第i个机械手的工作区间的方法为：

(6a)将第i机械手在第i个机械手平面坐标系X_RiO_RiY_Ri下的坐标原点(0,0)进行坐标转换，得到该坐标原点在传送带平面坐标系下的坐标(x_{C_i},y_{C_i})；

(6b)判断(x_{C_target},y_{C_target})是否满足条件：

时，机械手的工作空间为半径为R的扇形区域，满足，则认为(x_{C_target},y_{C_target})落入第i个机械手的工作区间内，否则，认为(x_{C_target},y_{C_target})不在该机械手的工作区间内。

所述步骤(1)中抓取队列中瓶子位姿信息帧按照如下规则排列：不同拍摄时刻的瓶子位姿信息帧，按照时间先后顺序排列，同一拍摄时刻的瓶子位姿信息帧，根据瓶子的位置排列，靠近机械手的瓶子的位姿信息帧排列在前。

本发明与现有技术相比具有如下有益效果：

1、本发明布置多个机械手对垃圾进行分拣，充分利用每台机械手的分拣能力，将多个分拣任务同时分配给多台机械手进行分拣，可实现整套***最多目标抓取物的分拣能力，工作效率高。

2、本发明定义了固定的传送带平面坐标系，坐标是否落入机械手的抓取范围的计算都是在传送带平面坐标系进行计算，有利于减少计算量，提高计算效率。

3、本发明定义了多个机械手坐标系，机械手坐标系随着机械手的安装位置变化，降低了机械手轨迹控制模块的处理复杂度，另外，多机械手控制模块只需要将瓶子成像平面坐标系的坐标发送给各机械手轨迹控制模块，由各机械手轨迹控制模块负责对各自需要抓取的瓶子的坐标进行坐标转换，简化了多机械手控制模块的复杂度。

4、本发明解决了一般的分拣识别环节只能识别分拣对象外形，无法辨识分拣对象是玻璃瓶或是塑料瓶问题，提出了在机械手抓取分拣对象时检测分拣对象的重量特征，作为判断抓取对象是否为玻璃瓶分拣物的条件，对玻璃瓶进行自动分拣，有效地防止破碎的玻璃对后续的生产线设备造成的破坏。

附图说明

图1为本发明的***结构图；

图2为本发明的控制方法流程图；

图3分拣机械手分布方式。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细的描述：

本发明实施例中涉及如下几种坐标系，定义如下：

(1)成像平面坐标系

以摄像头光轴与传送带平面的交点为坐标原点，成像平面上沿传送带移动方向为X_P轴，与X_P轴方向垂直的方向为Y_P轴，成像平面为传送带平面。

(2)传送带平面坐标系

传送带平面坐标系的原点为传送带边缘的一个固定点O_C，X_C轴为沿传送带的运动方向，Y_C位于传送带平面且与X_C轴垂直。

(3)N个机械手坐标系

第i个机械手的机械手坐标系为X_RiO_RiY_Ri，原点为机械手的基座与传送带平面的交点，轴Z_R垂直与传送带平面，轴X_R、轴Y_R分别与传送带平面坐标系的轴X_C和轴Y_C方向一致。

成像平面坐标系随着成像平面变换，多个机械手坐标系随着机械手的安装位置变化，可以降低成像坐标获取的难度和机械手轨迹控制模块的处理复杂度。

图1为一种针对生活垃圾中的玻璃瓶分拣控制***结构框图。如图所示，该***包括瓶子位姿接收模块、多机械手调度控制模块、N个机械手、N个控制机械手的轨迹控制模块和重量传感器，机械手与轨迹控制模块一一对应，重量传感器用来采集机械手手爪与抓取物的共同重量，安装在机械手手爪正上方，且重力传感器的受力轴线穿过手爪的重心，其中：

瓶子位姿信息接收模块，通过串口通信方式接收外部输入的瓶子位姿信息帧，将其存入接收缓冲区中，传送标准可以是RS232、RS485、RS422，所述瓶子位姿信息帧包括瓶子在成像平面坐标系下的坐标、姿态角和拍摄时间；所述位姿信息中坐标为瓶子的最小外接矩形的几何中心坐标，所述姿态角信息为瓶子的最小外接矩形的长边与X轴的夹角；瓶子位姿信息帧的位姿信息以成像平面坐标系为参考，内容简短且包含了抓取所需要的全部信息，有利于缩短传送时间和传送的数据量。

多机械手调度控制模块对其所控制的多个机械手进行调度控制，与机械手轨迹控模块的通信可以采用工业现场总线的方式来实现。多机械手调度控制模块，提取接收缓冲区中的瓶子位姿信息帧，计算瓶子位姿信息帧中瓶子在t₂+ΔT时刻在传送带平面坐标系下的预测坐标，所述t₂为当前时刻，ΔT为机械手收到指令到抓取到瓶子所需要的时间，判断该预测坐标是否进入任意一个机械手的工作空间，如果进入某个机械手的工作空间，则判断该机械手的工作状态是否“空闲”，如果“空闲”，则将该瓶子的成像平面坐标系下的坐标和姿态角信息分配给空闲的机械手对应的机械手轨迹控制模块。多机械手调度控制模块以毫秒级的控制周期轮询各个机械手的运行状态，及时对空闲的机械手进行任务分配，确保机械手对瓶子进行高效的抓取。

机械手轨迹控制模块，初始化时，控制机械手位于预设的准备位置；接收瓶子的成像平面坐标系下的坐标信息之后，将工作状态置为“忙”，将该瓶子的坐标信息和姿态角进行坐标转换，得到该瓶子在本机械手坐标系下的坐标，控制机械手移动至瓶子的位置，旋转机械手的角度与姿态角一致，抓取瓶子，在瓶子离开传送带平面后，检测机械手手爪与抓取物的共同重量，判断其重量是否在一定的阈值范围内，如果是，则判定为玻璃瓶，将其送到传送带以外指定的物料存储区，否则，判定为其它材质瓶子，松开手爪，将其放回传送带，所述阈值由机械手手爪与玻璃瓶的统计重量之和确定。之后，控制机械手回到预设的准备位置，将工作状态置为“空闲”。所述预设准备位置可以是传送带平面的正上方，其在传送带平面上的投影位于传送带在Y_P轴方向的中心位置。该位置有利于机械手快速到达传送带表面进行抓取。

多机械手控制模块只需要将瓶子在成像平面坐标系的坐标发送给各机械手轨迹控制模块，由各机械手轨迹控制模块负责对各自需要抓取的瓶子的坐标进行坐标转换，简化了多机械手控制模块的复杂度。

N个机械手沿传送带运动方向排列，序号为1～N，多机械手调度控制模块循环执行如下步骤：

(1)判断接收缓冲区是否有新的瓶子位姿信息帧，如果有，将新的瓶子位姿信息帧存入当前抓取队列中，进入步骤(2)，否则，直接进入步骤(2)

(2)按照先进先出的原则，选取抓取队列中的第一个瓶子位姿信息帧；

(3)解析瓶子位姿信息帧在成像平面坐标系下的坐标信息(x_{P_0},y_{P_0})和拍摄时间t₁；

(4)将瓶子在成像平面坐标系中坐标信息(x_{P_0},y_{P_0})进行坐标转换，得到传送带平面坐标系下的坐标信息(x_{C_0},y_{C_0})；

$\left[\begin{array}{c}{x}_{C\_0}\\ y_{C\_0}\\ 1\end{array}\right]=M_{P\_C}\·\left[\begin{array}{c}{x}_{P\_0}\\ y_{P\_0}\\ 1\end{array}\right]---\left(1\right)$

M_{P_C}为成像平面坐标系到传送带平面坐标系的坐标转换矩阵；

在传送带平面坐标系的原点O_C及X_C轴和Y_C轴上任一点上设置明显的物理标识，通过这些明显物理标识在成像平面坐标系所对应的坐标值和在传送带平面坐标系下的坐标值可以解算出成像平面坐标系到传送带平面坐标系的坐标转换矩阵M_{P_C}。

(5)通过解算如下方程组计算目标抓取物在t₂+ΔT时刻在传送带平面坐标系下的预测坐标(x_{C_target},y_{C_target})：

x_{C_target}＝x_{C_0}+v×(t₂+ΔT-t₁) (2)

y_{C_target}＝y_{C_0} (3)

$\mathrm{\Δ}T=\sqrt{{(x_{R\_t\arg et}-x_{R\_\mathrm{int}})}^2+{(y_{R\_t\arg et}-y_{R\_\mathrm{int}})}^2+{(z_{R\_t\arg et}-z_{R\_\mathrm{int}})}^2}/{V_{R\_}}_{hand}---\left(4\right)$

$\left[\begin{array}{c}{x}_{R\_t\arg et}\\ y_{R\_t\arg et}\\ 1\end{array}\right]=M_{C\_R\_i}\·\left[\begin{array}{c}{x}_{C\_i\arg et}\\ y_{C\_t\arg et}\\ 1\end{array}\right]---\left(5\right)$

z_{R_target}＝0 (6)

其中，(x_{R_int}，y_{R_int}，z_{R_int})为第i个机械手的准备位置在该机械手坐标系下的坐标；V_{R_hand}为机械手沿点对点直线运动的预设速度，v为传送带的速度，t₂为当前时刻，M_{C_R_i}为第i个机械坐标系到传送带平面坐标系的坐标转换矩阵，i＝1～N。

以机械手末端安装的锥形工装与传送带上的三个特殊点进行点接触，确定机械手坐标系X_RiO_RiY_Ri，平面X_RiO_RiY_Ri与传送带平面重合。采用精密光学仪器测量传送带平面坐标系的原点及沿X_C轴和沿Y_C轴上的一点的坐标值，再以精密光学仪器测量机械手锥形工装的尖点在三个不共线位置的坐标值及其在机械手坐标系下的坐标值。通过如上关系可以解算出转换矩阵M_{C_R_i}。

上述的目标坐标在机械手到达抓取位置时的预测方法，有利于对目标抓取物位置的准确判断，增加抓取的精确度。避免计算误差导致目标抓取物在机械手抓取时已经不在其工作空间内，导致抓取失败。

(6)当前时刻依次判断该目标抓取物预测坐标(x_{C_target},y_{C_target})是否进入第1个机械手～第N个机械手的工作空间，如果该目标抓取物进入某一个机械手的工作空间，则确定该机械手为待抓机械手，进入步骤(7)，否则，将该目标位姿信息帧保存到暂存队列中，进入步骤(8)；

多机械手调度控制模块判断该目标抓取物预测坐标(x_{C_target},y_{C_target})是否进入第i个机械手的工作区间的方法为：

(6a)将第i机械手在第i个机械手坐标系X_RiO_RiY_Ri下的坐标原点(0,0)进行坐标转换，得到该坐标原点在传送带平面坐标系下的坐标(x_{C_i},y_{C_i})；

$\left[\begin{array}{c}{x}_{C\_i}\\ y_{C\_i}\\ 1\end{array}\right]=M_{P\_R\_i}\·\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]---\left(7\right)$

(6b)判断(x_{C_target},y_{C_target})是否满足条件：

时，机械手的工作空间为半径为R的扇形区域，满足，则认为(x_{C_target},y_{C_target})落入第i个机械手的工作区域内，否则，认为(x_{C_target},y_{C_target})不在该机械手的工作区域内。

(7)判断待抓机械手工作状态是否“空闲”，如果“空闲”，则将该目标位姿信息帧中的成像平面坐标系坐标和姿态角信息发送给该机械手对应的轨迹控制模块，进入步骤(8)；如果工作状态为“忙”，则将该目标位姿信息帧保存到暂存队列中，进入步骤(8)；

(8)按照先进先出的原则，选取下一个目标位姿信息帧，重新执行步骤(3)～步骤(7)，直到选取完抓取队列中的所有目标位姿信息帧，进入步骤(9)；

(9)清空抓取队列，将暂存队列中的所有目标位姿信息帧存入抓取队列。

在抓取对象离开传送带时，机械手轨迹控制模块读取重量传感器的检测值，判断其是否处在预先设定的阈值范围内，如果是则将抓取物放置物料容器，如果否，则将手爪松开，抓取对象落回传送带。

上述的坐标计算大部分都是在传送带平面坐标系进行计算，有利于减少计算量，提高计算效率。

实施例：

一种针对生活垃圾中的玻璃瓶分拣控制***中配置3个机械手，如图3所示。机械手沿传送带传送方向依次排列，其中，机械手1和机械手2位于传送带同一侧，机械手3位于另一侧。传送带平面坐标系的原点为传送带边缘的一点O_C，X_C轴为沿传送带的运动方向，Y_C位于传送带平面且与X_C轴垂直。

本实施例中，成像平面坐标系与传送带平面坐标系的重合，则，

$M_{P\_C}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

机械手1的机械手坐标系为X_RY_RZ_RO_R原点为机械手的基座且位于传送带平面，轴Z_R垂直与传送带平面，轴X_R、轴Y_R分别与轴X_C和轴Y_C方向一致。

待抓取瓶子①、②、③和④沿传送带方向运动，其中，①为塑料瓶，其余的为玻璃瓶，在瓶子进入机械手1的工作空间之前，该瓶子的信息帧已经发送给位姿信息接收模块。位姿信息接收模块将所有接收到的信息帧存储在接收缓冲区，多机械手调度模块可以是西门子S7-300系列可编程逻辑控制器(PLC)。在PLC的每个程序执行周期(1ms之内)都对循环执行如下步骤：。

(1)判断接收缓冲取是否有新的瓶子位姿信息帧，如果有，将新的瓶子位姿信息帧存入当前抓取队列，进入步骤(2)，否则，直接进入步骤(2)。图2所示的当前抓取队列中有4个瓶子位姿信息帧。所述抓取队列中瓶子位姿信息帧按照如下规则排列：不同拍摄时刻的瓶子位姿信息帧，按照时间先后顺序排列，同一拍摄时刻的瓶子位姿信息帧，根据瓶子位姿信息帧的位置排列，靠近机械手的瓶子位姿信息帧排列在前。

(2)选取抓取队列中的瓶子①信息帧。

(3)解析瓶子①信息帧在成像平面坐标系下坐标信息(x_{P_0},y_{P_0})＝(0,300)和拍摄时间t₁＝1；

(4)将瓶子①在成像平面坐标系中坐标信息(x_{P_0},y_{P_0})＝(0,300)进行坐标转换，得到传送带平面坐标系下的坐标信息(x_{C_0},y_{C_0})；

$\left[\begin{array}{c}{x}_{C\_0}\\ y_{C\_0}\\ 1\end{array}\right]=M_{P\_C}\·\left[\begin{array}{c}{x}_{P\_0}\\ y_{P\_0}\\ 1\end{array}\right]$

其中，

$M_{P\_C}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

M_{P_C}为成像平面坐标系到传送带平面坐标系的坐标转换矩阵，两坐标系重合，即，(x_{C_0},y_{C_0})＝(0,300)；

(5)设当前时刻t₂＝3s，第1个机械手的准备位置在该机械手坐标系下的坐标(x_{R_int},y_{R_int},z_{R_int})＝(0,600,500)；ΔT为机械手收到指令到抓取到目标抓取物所需要的时间，V_{R_hand}＝2000，传送带的速率v＝1000，M_{C_R_i}为传送带平面坐标系到第i个机械手坐标系的坐标转换矩阵；

第1个机械手坐标系到传送带平面坐标系的坐标转换矩阵为：

$M_{C\_R\_1}=\left[\begin{array}{ccc}1& 0& 2000\\ 0& 1& -300\\ 0& 0& 1\end{array}\right]$

两坐标系为平移关系。

解算方程组(公式(2)～公式(6))得到目标抓取物在t₂+ΔT时刻的在传送带平面坐标系下的预测坐标(x_{C_target},y_{C_target})：

其中，z_{R_target}＝0，x_{C_0}＝0，y_{C_0}＝y_{C_target}＝300

计算得到(x_{C_target},y_{C_target})＝(2288.675,300)，ΔT＝0.289。

(6)当前时刻依次判断该瓶子预测坐标(x_{C_target},y_{C_target})是否进入第1个机械手～第3个机械手的工作空间，如果该目标抓取物进入某一个机械手的工作空间，则确定该机械手为待抓机械手，进入步骤(7)，否则，将该瓶子位姿信息帧保存到暂存队列中，进入步骤(8)；

判断(x_{C_target},y_{C_target})是否在机械手1的工作空间范围，设机械手1在传送带的工作空间半径R＝1000mm。

(x_{C_1},y_{C_1})＝(2000,-300)

$x_{C\_1}+\sqrt{R^2-{(y_{C\_t\arg et}-y_{C\_1})}^2}=2800$

$x_{C\_1}-\sqrt{R^2-{(y_{C\_t\arg et}-y_{C\_1})}^2}=1200$

x_{C_target}满足关系，因此，瓶子①为处于机械手1的工作空间内，进入步骤(7)；

(7)判断待抓机械手1工作状态是否“空闲”，如果“空闲”，则将该目标位姿信息帧中的成像平面坐标系坐标信息发送给该机械手对应的轨迹控制模块，进入步骤(8)；如果工作状态为“忙”，则将该目标位姿信息帧保存到暂存队列中，进入步骤(9)；

(8)机械手抓取瓶子，在瓶子①离开传送带时，机械手轨迹控制模块检测重量传感器的输入量，判断其是否在预设的阈值，本实施例中，采用超轻材料制成的机械爪，机械爪的重量与一般的玻璃瓶的重量总和一般处在800～850g之间,因此，阈值范围取[800,850]，该瓶子为空的塑料瓶，应该在420g左右，明显不在阈值范围内，控制手爪松开，瓶子①落回传送带。

(9)按照先进先出的原则，多机械手调度控制模块选取下一个瓶子位姿信息帧，重新执行步骤(3)～步骤(7)，直到选取完抓取队列中的所有瓶子位姿信息帧，进入步骤(10)；

(10)多机械手调度控制模块清空抓取队列，将暂存队列中的所有瓶子位姿信息帧存入抓取队列。

本发明布置多个机械手对垃圾中的瓶子进行分拣，可以充分利用每台机械手的分拣能力，将多个分拣任务同时分配给多台机械手进行分拣，可实现整套***最多目标抓取物的分拣能力，工作效率高。由于传送带的速度慢，而多机械手控制模块的循环控制速度快，对于同一个目标抓取物而言，将有多次机会进入多个机械手的工作空间，可以达到非常低的漏检率。

另外，本发明在机械手抓取分拣对象时检测分拣对象的重量特征，作为判断抓取对象是否为玻璃瓶分拣物的条件，对玻璃瓶进行自动分拣，有效地防止破碎的玻璃对后续的生产线设备造成的破坏。

以上所述，仅为本发明最佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。

Claims (6)

1.一种针对生活垃圾中的玻璃瓶分拣控制***，其特征在于包括瓶子位姿接收模块、多机械手调度控制模块、N个机械手、N个控制机械手的轨迹控制模块和重量传感器，机械手与轨迹控制模块一一对应，重量传感器用来采集机械手手爪与抓取物的共同重量，其中：

瓶子位姿接收模块，接收外部输入的瓶子位姿信息帧将其存入接收缓冲区中，所述瓶子位姿信息帧包括瓶子在成像平面坐标系下的坐标、姿态角和拍摄时间；

多机械手调度控制模块，提取接收缓冲区中的瓶子位姿信息帧，计算瓶子位姿信息帧中瓶子在t₂+ΔT时刻在传送带平面坐标系下的预测坐标，所述t₂为当前时刻，ΔT为机械手收到指令到抓取到瓶子所需要的时间，判断该预测坐标是否落入任意一个机械手的工作空间，如果进入某个机械手的工作空间，则判断该机械手的工作状态是否“空闲”，如果“空闲”，则将该瓶子的成像平面坐标系下的坐标和姿态角信息分配给空闲的机械手对应的机械手轨迹控制模块；

机械手轨迹控制模块，初始化时，控制机械手位于预设的准备位置；接收瓶子在成像平面坐标系下的坐标信息之后，将工作状态置为“忙”，将该瓶子的坐标信息和姿态角进行坐标转换，得到该瓶子在本机械手坐标系下的坐标，控制机械手移动至瓶子位置，旋转机械手的角度与姿态角一致，抓取瓶子，当瓶子离开传送带表面时，读取重量传感器值，根据重量判断所抓取的瓶子是否具有玻璃瓶的重量特征，如果是，则将瓶子送到传送带以外指定的物料存储区，如果不是玻璃瓶的重量特征，则将瓶子放回到传送带上，之后，控制机械手回到预设的准备位置，将工作状态置为“空闲”。

2.根据权利要求1所述的一种针对生活垃圾中的玻璃瓶分拣控制***，其特征在于所述机械手轨迹控制模块根据瓶子重量判断所抓取的瓶子是否为玻璃瓶的方法为：

判断重量是否在预设阈值范围内，如果是，则判定为玻璃瓶，否则判定为其它瓶类，所述阈值由机械手手爪与玻璃瓶的统计重量之和确定。

3.根据权利要求1所述的一种针对生活垃圾中的玻璃瓶分拣控制***，其特征在于所述N个机械手沿传送带运动方向排列，序号为1～N，多机械手调度控制模块循环执行如下步骤：

(1)判断当前时刻接收缓冲区是否有新的瓶子位姿信息帧，如果有，将新的瓶子位姿信息帧存入当前抓取队列中，进入步骤(2)，否则，直接进入步骤(2)；

(2)按照先进先出的原则，选取抓取队列中的第一个瓶子位姿信息帧；

(3)解析瓶子位姿信息帧中瓶子在成像平面坐标系下的坐标信息(x_{P_0},y_{P_0})和拍摄时间t₁；

(4)将瓶子在成像平面坐标系中坐标信息(x_{P_0},y_{P_0})进行坐标转换，得到传送带平面坐标系下的坐标信息(x_{C_0},y_{C_0})；

(5)计算瓶子在t₂+ΔT时刻的在传送带平面坐标系下的预测坐标(x_{C_target},y_{C_target})；

(6)当前时刻依次判断该瓶子预测坐标(x_{C_target},y_{C_target})是否进入第1个机械手～第N个机械手的工作空间，如果该瓶子进入某一个机械手的工作空间，则确定该机械手为待抓机械手，进入步骤(7)，否则，将该瓶子位姿信息帧保存到暂存队列中，进入步骤(8)；

(7)判断待抓机械手工作状态是否“空闲”，如果“空闲”，则将该瓶子位姿信息帧中的成像平面坐标系坐标和姿态角信息发送给该机械手对应的轨迹控制模块，进入步骤(8)；如果工作状态为“忙”，则将该瓶子位姿信息帧保存到暂存队列中，进入步骤(8)；

(8)按照先进先出的原则，选取下一个目标位姿信息帧，重新执行步骤(3)～步骤(7)，直到选取完抓取队列中的所有目标位姿信息帧，进入步骤(9)；

(9)清空抓取队列，将暂存队列中的所有目标位姿信息帧存入抓取队列。

4.根据权利要求3所述的一种针对生活垃圾中的玻璃瓶分拣控制***，其特征在于所述步骤(5)通过解算如下方程组计算瓶子在t₂+ΔT时刻在传送带平面坐标系下的预测坐标(x_{C_target},y_{C_target})：

x_{C_target}＝x_{C_0}+v×(t₂+ΔT-t₁)

y_{C_target}＝y_{C_0}

$\mathrm{\Δ}T=\sqrt{{(x_{R\_t\arg et}-x_{R\_\mathrm{int}})}^2+{(y_{R\_t\arg et}-y_{R\_\mathrm{int}})}^2+{(z_{R\_t\arg et}-z_{R\_\mathrm{int}})}^2}/V_{R\_hand}$

$\left[\begin{array}{c}{x}_{R\_t\arg et}\\ y_{R\_t\arg et}\\ 1\end{array}\right]=M_{C\_R\_i}\·\left[\begin{array}{c}{x}_{C\_t\arg et}\\ y_{C\_t\arg et}\\ 1\end{array}\right]$

z_{R_target}＝0

其中，(x_{R_int}，y_{R_int}，z_{R_int})为第i个机械手的准备位置在该机械手坐标系下的坐标；V_{R_hand}为机械手沿点对点直线运动的预设速度，v为传送带的速度，M_{C_R_i}为传送带平面坐标系到第i个机械手坐标系的坐标转换矩阵，t₂为当前时刻。

5.根据权利要求3所述的一种针对生活垃圾中的玻璃瓶分拣控制***，其特征在于所述多机械手调度控制模块判断该瓶子预测坐标(x_{C_target},y_{C_target})是否进入第i个机械手的工作区间的方法为：

(6a)将第i机械手在第i个机械手平面坐标系X_RiO_RiY_Ri下的坐标原点(0,0)进行坐标转换，得到该坐标原点在传送带平面坐标系下的坐标(x_{C_i},y_{C_i})；

(6b)判断(x_{C_target},y_{C_target})是否满足条件：

时，机械手的工作空间为半径为R的扇形区域，满足，则认为(x_{C_target},y_{C_target})落入第i个机械手的工作区间内，否则，认为(x_{C_target},y_{C_target})不在该机械手的工作区间内。

6.根据权利要求3所述的一种针对生活垃圾中的玻璃瓶分拣控制***，其特征在于所述步骤(1)中抓取队列中瓶子位姿信息帧按照如下规则排列：不同拍摄时刻的瓶子位姿信息帧，按照时间先后顺序排列，同一拍摄时刻的瓶子位姿信息帧，根据瓶子的位置排列，靠近机械手的瓶子的位姿信息帧排列在前。