[发明专利]一种青饲机自动抛送系统机械臂运动控制方法

权利要求书

1.一种青饲机自动抛送系统机械臂运动控制方法，其特征在于，包括有如下的步骤：

利用RGB-D相机识别车斗，并定位车斗在机械臂坐标系下的位置；

根据车斗定位的范围，在车斗范围内设置物料的多个监测点，并获取各个监测点的满度值，其中车斗范围内是指车斗的上中线和下中线、或者是中线，所有的监测点沿车斗的上中线和下中线等步长设置或者所有的监测点沿车斗的中线等步长设置，其中上中线为车斗上边沿和车斗中线的中间线，下中线为车斗下边沿和车斗中线的中间线，中线为车斗的中间线；监测点的物料的满度值检测包括：RGB-D相机获得的物料的点云数据转换到机械臂坐标系下点云数据，在机械臂坐标系下以监测点为中心取一个矩形，求位于矩形内的物料点的Z值的平均值，以该平均值表达监测点的物料的满度值；当设置的预设满度值小于等于100％时，分别在车斗的上中线和下中线设置监测点，当设置的预设满度值大于100％时，在车斗的中线设置监测点；

按一定顺序依次遍历车斗内监测点的物料的满度值，将满度值与预设满度值进行比较，找出满度值小于预设满度值的首个未满监测点；

判断首个未满监测点所位于机械臂的方向，在该方向下的机械臂应转计数累计加一次，同时相反方向的机械臂应转计数清零，其中方向包括左方和右方；

判断机械臂应转计数累计是否达到三次及以上，是则机械臂向该方向转动。

2.根据权利要求1所述的青饲机自动抛送系统机械臂运动控制方法，其特征在于，还包括有如下步骤：

检测物料柱的落点的位置范围；

根据物料柱的落点的定位结果，与首个未满监测点的所在位置比较，控制机械臂的挡板调整物料柱的落点至首个未满监测点的区域。

3.根据权利要求2所述的青饲机自动抛送系统机械臂运动控制方法，其特征在于，检测物料柱的落点的位置具体包括如下骤：

在机械臂坐标系下，在车斗上方Z轴方向上设置一高度L2；

在L2处向上截取L1的高度，计算这个L1高度内物料柱距离Z轴的水平距离d1,即Y轴的值；

计算车斗下中线距离机械臂坐标系Z轴的水平距离d2，车斗上中线距离机械臂坐标系Z轴的水平距离d3，车斗中线距离机械臂坐标系Z轴的水平距离d4，车斗下边沿距离机械臂坐标系Z轴的水平距离d5和车斗上边沿距离机械臂坐标系Z轴的水平距离d6；

比较d1+x、d2、d3、d4、d5及d6的值，判断物料落点的大体位置，其中x在d1位于[d5,d2)、[d2，d4)、[d4,d3)、[d3,d6)范围内分别取不同的阈值，并使得d1+x应控制在[d5,d6]。

4.根据权利要求1所述的青饲机自动抛送系统机械臂运动控制方法，其特征在于，

所述机械臂运动至少包括有三个自由度，第一自由度为机械臂坐标系下机械臂绕Z轴的转动自由度，第二自由度为机械臂坐标系下机械臂绕X轴的转动自由度，第三自由度为机械臂坐标系下机械臂喷头处挡板在YZ平面上摆动自由度，挡板的摆动用于控制物料的喷射远近。

5.根据权利要求1所述的青饲机自动抛送系统机械臂运动控制方法，其特征在于，还包括有：在利用RGB-D相机识别车斗之前，设置填充规则及预设满度值，填充规则包括车斗范围内监测点的设置和物料填充车斗的方向。