CN111090258A - 一种青饲机自动抛送系统机械臂运动控制方法 - Google Patents

Abstract

一种青饲机自动抛送系统机械臂运动控制方法，包括有如下的步骤：利用RGB‑D相机识别车斗，并定位车斗在机械臂坐标系下的位置；根据车斗定位的范围，在车斗范围内设置物料的多个监测点，并获取各个监测点的满度值；按一定顺序依次遍历车斗内监测点的物料的满度值，将满度值与预设满度值进行比较，找出满度值小于预设满度值的首个未满监测点；判断首个未满监测点所位于机械臂的方向，在该方向下的机械臂应转计数累计加一次，同时相反方向的机械臂应转计数清零，其中方向包括左方和右方；判断机械臂应转计数累计是否达到三次及以上，是则机械臂向该方向转动。控制械臂运动对车斗进行自动填充物料。

Description

一种青饲机自动抛送系统机械臂运动控制方法

技术领域

本发明涉及机械臂控制与农业自动化技术领域，尤其是涉及一种青饲机自动抛送系统机械臂运动控制方法。

背景技术

青饲机是一种常用农业收获机械，主要用来收获青绿牧草、燕麦、甜菜茎叶、玉米等青饲作物。青饲机大体由前方割台、机身喂入装置及切碎抛送装置组成。前方割台为一个装有多把甩刀旋转切碎器用于高速收割作物，机身收集作物并将其切碎，然后由抛送装置抛出，青饲机抛出的物料由后方或侧方同向行驶的运输车承接。

具有自动抛送系统的青饲机，机身收集作物并将其切碎，然后由固定在机械臂上的RGB-D相机识别和定位车斗位姿，然后基于RGB-D相机和机械臂坐标变换矩阵，得出车斗在机械臂坐标系下的位姿，自动调整机械臂实现物料的精准抛投。

如公开号为CN109920006A的中国发明专利公开了一种青饲机自动抛送系统位姿变换矩阵的标定方法，得到的位姿变换矩阵就是用于将车斗在相机坐标系下RGB-D相机获得到的坐标数据转换为机械臂坐标系统下的坐标数据。

如公开号为CN108550141A的中国发明专利公开了一种基于深度视觉信息的运动车斗自动识别与定位方法，利用RGB-D对运动车斗自动识别与定位。

青饲机自动抛送系统利用深度相机实现对运输车车斗进行自动识别与定位以及对车斗内物料满度进行检测，基于车斗的识别与定位和物料满度检测，对机械臂进行运动控制实现物料精准的抛洒在车斗未满区域。

目前物料的喷洒主要通过操作员人眼观察车斗内物料的情况，然后通过手柄控制机械臂进行物料的喷洒。通过操作人员手动控制的方式，需要增加一名专业的操作人员增加了收割成本；由于运输车和收获车相对行驶速度会有所不同，导致操作人员要时刻观察车斗状况频繁操作手柄，造成人力劳顿和物料的浪费。

发明内容

本发明的目的是针对现有技术存在的不足，提出了一种青饲机自动抛送系统机械臂运动控制方法，控制械臂运动对车斗进行自动填充物料。

为达到上述目的，本发明采用的技术方案如下：

建立机械臂坐标系，机械臂坐标系的Z轴与水平面垂直向上，X轴与Y轴呈水平，Y轴随着机械臂运动，X轴为满足笛卡尔坐标系右手定则确得。

机械臂运动有三个自由度，第一自由度为机械臂坐标系下机械臂绕Z轴的转动自由度，第二自由度为机械臂坐标系下机械臂绕X轴的转动自由度，第三自由度为机械臂坐标系下机械臂抛送口处挡板在YZ平面上摆动自由度。

第一自由度可以控制机械臂进行水平面的左右转动，第二自由度可以控制机械臂上下的升降，第三自由度可以控制机械臂的挡板进行上下摆动控制物料的喷射远近。

对于车斗的识别与定位可以参考公开号为CN108550141A的中国发明专利公开。

安装RGB-D相机，并在安装位置进行相机坐标系与机械臂坐标系的准确标定，其标定方法可以参考公开号为CN109920006A的中国发明专利，RGB-D相机实时获取的点云数据转换为机械臂坐标系下的点云数据，这些点可能是车斗的点，也可能是车斗中的物料点，还可能是抛送在空中的物料柱的点。

本技术方案中RGB-D相机获得的点云数据都转换到机械臂坐标系下，对转换后的这些车斗、物料和物料柱的点云数据进行分析及比较，实现对机械臂运动的控制。

青饲机自动抛送系统机械臂运动控制方法包含步骤如下：

步骤1：控制机械臂，按第二自由度使机械臂抬升到作业高度，使车斗出现在 RGB-D相机的视野范围内。

步骤2：设置填充规则及预设满度值，填充规则包括车斗范围内监测点的设置和物料填充车斗的方向。

根据现场作业实际情况选择车斗内喷洒填充的路径及物料填充的预设满度值，可选一次横向喷洒及喷洒最终预设满度值，两次喷洒及分别喷洒的预设满度值；横向喷洒的方向等。

步骤3：利用RBG-D相机进行识别车斗，定位车斗在机械臂坐标系下的位置；

在机械臂坐标系，转换后的车斗的点云数据，这些点云数据基本包括了可以在相机中出现的车斗的点云数据，其中就有车斗上端的开口边框的。

步骤4：设置车斗内监测点，监测点的设置是在机械臂坐标系下，在车斗范围内设置多个监测点。

监测点的位置选择规则为：当设置的物料满度值小于等于100％时，分别在上中线和下中线设置监测点，当设置满度值大于100％时，在车斗中线设置监测点。

其中，上中线为车斗上边沿和车斗中线的中间线，下中线为车斗下边沿和车斗中线的中间线，中线为车斗的中间线，上中线、下中线和中线都是由三维点构成的。

监测点的设置方法为在机械臂坐标系下，以机械臂坐标系的Y轴为基线，分别向在机械臂的左右方向等步长设置检测点，步长值可设置。

步骤5：计算监测点的物料的满度值，根据选定的填充规则，依次遍历车斗内物料的监测点的满度值，找出最先一个满度值小于预设满度值的监测点，称之为首个未满监测点；

监测点的满度值检测过程如下：RGB-D相机获得的点云数据转换到机械臂坐标系下点云数据；在机械臂坐标系下的XY平面或平行面上以监测点为中心取一个矩形，求位于矩形内的物料点的Z值的平均值，以该平均值表达监测点的满度值。

步骤6：判断首个未满监测点位于机械臂的左方、下方或右方，即该监测点在机械臂坐标系下X轴的值为负、零或正，当监测点位于机械臂的左方时，机械臂向左的应转计数累计加一次，同时对应的向右的应转计数清零，反之同理，当监测点位于机械臂的下方时，机械臂向左和向右的应转计数都清零。

步骤7：判断机械臂向右转或向左转的应转计数累计是否达到三次及以上，是则控制机械臂使机械臂向该方向转动一定角度，否则不转，实现机械臂的横向控制。

步骤8：检测在车斗内的物料落点的位置范围；

步骤9：判断物料落点和监测点相对位置，控制机械臂调整其挡板使物料落点至物料首个未满监测点的区域内，实现机械臂的纵向控制；

RGB-D相机得到的每一帧的图像数据转换后，进行步骤3-9，实现对青饲机自动抛送系统的实时控制。若需要重新设置填充规则及预设满度值，可以按填充规则及满度设置按钮操作。

本发明的技术方案的技术效果在于，依据车斗的识别与定位、车斗内物料满度的检测以及落点检测，通过对机械臂横向和纵向控制，实现物料准确无误的喷洒到车斗未满区域。与人工控制方法相比，实现了自动控制机械臂，反应速度快，实现成本低。

附图说明

图1示出了机械臂的位置及工作状态示意图；

图2a示出了机械臂坐标系的XY平面坐标系图，图2b示出了机械坐标系的YZ平面坐标系图，图中3是机械臂；

图3a至3d示出了物料的填充规则图，图3a和3b中(1)为喷洒路径，(2)为机械臂横向走向，图3c和3d为多次横向喷洒填充；

图4a至4c示出了填充车斗的满度值示例，图4a为满度值100％的填充车斗，图 4b为满度值80％的填充车斗，图4c为满度值110％的填充车斗；

图5a和5b示出了监测点的设置原理图，图5a中监测点分为上中线的监测点和下中线的监测点，图5b中为车斗中线的监测点，上下中线的监测点和车斗中线的监测点都是以机械臂下方点为原点分别向左右两方等距离延伸；

图6a至6c示出了监测点满度值检测方法的原理图；

图7示出了为检测物料落点及机械臂的纵向控制原理图；

图8示出了机械臂运动控制的过程流程图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1中所示，在作业时，青饲机1收割青饲料，通过RGB-D相机4识别定位运输车的车斗2以及检测车斗内物料满度，控制机械臂3使物料准确喷洒到车斗未满区域。

机械臂的运动有三个自由度，第一自由度11为机械臂坐标系下机械臂绕Z轴的转动自由度，可以控制机械臂进行水平面的左右转动；第二自由度12为机械臂坐标系下机械臂绕X轴的转动自由度，第二自由度可以控制机械臂上下的升降；第三自由度13 为机械臂坐标系下机械臂抛送口处挡板在YZ平面上摆动自由度，挡板的摆动用于控制物料的喷射远近。

如图2a和2b所示，机械臂坐标系的Y轴随着机械臂运动，Z轴竖直向上，X轴为满足笛卡尔坐标系右手定则可得，其原点为第二自由度的转动位置。

基于该机械臂坐标系下，识别的车斗主要是识别车斗的开口边框点云数据和出现在相机下的车斗上边沿与下边沿的；车斗内物料点主要是车斗内的物料表面的点云；物料柱轨迹线上的点主要是根据物料柱表面的点经过估算得到的；所有这些点云数据都是根据RGB-D相机获取的点云数据后通过转换后得到。

青饲机自动抛送系统机械臂运动控制方法包含步骤如下：

步骤1：控制机械臂第二自由度，使机械臂抬升到作业高度，使车斗位于RGB-D相机的视野范围内；

步骤2：设置填充规则及满度。本实施例的填充规则选择从右到左一次填充，如图3b中所示，选定填充的预设满度值为100％，如图4a中所示；在其它的实施例中，也可以选定其它的设置，如图4b,4c中所示。

步骤3：识别车斗，定位车斗在机械臂坐标系下的位置，由于机械臂在车斗的侧面和RGB-D相机位于车斗的上方，在机械臂坐标系下，识别及定位的车斗是横向排列的长方体，在机械臂坐标系下的XY平面上为长方形，车斗靠近机械臂的一侧称为下方，其边线为车斗的下边沿，远离机械臂的一侧称为上方，其边线为上边沿，车斗的前进方向称为左方，车斗的前进的反方向称为右方。

步骤4：根据车斗定位的范围，设置车斗范围内用于检测物料的满度值的监测点；

监测点的位置选择规则为：当设置的物料预设满度值小于等于100％时，分别在上中线和下中线设置监测点，当设置的预设满度值大于100％时，在车斗中线设置监测点。其中上中线为车斗上边沿和车斗中线的中间线，下中线为车斗下边沿和车斗中线的中间线，中线为车斗的中间线。

这些监测点以机械臂坐标系的Y轴为基准线，分别向机械臂的左右方向等步长设置，步长值可以设置，本实施例的步长值为500mm，设置原理图如图5a或图5b中所示。

步骤5：根据步骤2中选定的填充规则从右向左，依次对车斗内物料的监测点处的满度值与设置的预设满度值进行比较，找出最先出现的一个首个未满监测点，其满度值小于预设满度值的监测点。如图6a中所示，比较监测点的顺序为监测点1，监测点 2，监测点3，当发现监测点3为首个未满监测点时停止，就不进行比较了；

各个的监测点的满度的检测方式为：RGB-D相机下的车斗中物料表面的点云数据经过矩阵转换到机械臂坐标系下点云数据，例如机械臂坐标系下，点i是物料的点，其坐标数据表示为(x_i,y_i,z_i)。

已知监测点3在机械臂坐标系下的(X₃,Y₃)，以该二维点(X₃,Y₃)为中心在XY平面上选取一个矩形，求矩形内所有点i的平均高度值，即z_i的平均值，这个平均值作为该监测点的满度值，如图6b所示。

如图6c中所示，矩形内所有点i是指其到XY平面上二维点(x_i,y_i)位于上述的距形内的点。

步骤6：判断监测点3位于机械臂的方向，如图6b中所示监测点3位于机械臂的右方，即监测点3在机械臂坐标系下的X轴的值为正，机械臂向右转的应转计数累计加一次，同时机械臂向左转的应转计数清零；

步骤7：判断机械臂应转计数累计是否达到三次及以上，是则控制机械臂的第一自由度11使机械臂向右转动，否则不转；

步骤8：检测物料落点的位置；

机械臂位置如图7中所示，物料落点的检测原理图，在车斗上方的高度L2处开始截取L1的高度，L1和L2的值是预先设置好的，L2的设置值应大于设置物料预设满度值且高出车斗的值，计算这个高度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]。

关于x的取值，因为x值的设置和喷头挡板与水平面得夹角有关，关系方程为

其中θ为喷头挡板与水平面的夹角，v为物料喷出的速度，g为重力加速度，L1、 L2为图7所示。对于很多青饲机机械臂无角度传感器，无角度传感器的青饲机的x可设置为可调阈值，本实施例中选用了，当d1位于[d5,d2)区间时选用x＝0,当d1位于 [d2,d4)区间时选用x＝0.25(d4-d2)，d1位于[d4,d3)区间时选用x＝0.5(d3-d4),d1位于[d3,d6)区间时选用x＝0.75(d3-d4).

步骤9：根据物料落点的定位结果，与首个未满监测点的所在位置比较，控制机械臂第三自由度13调整挡板使物料落在车斗内的首个未满监测点的区域。这个区域是以首个未满监测点为中心的一定区域范围内，容许物料落点与首个未满监测点在一定的距离内。

对RGB-D获取的每一帧的图像数据在经过转换后，进行步骤3-9，如此重复进行，实现对青饲机自动抛送系统的实时控制。若需要重新设置填充规则及满度，可以按填充规则及满度设置按钮。

在本说明书的描述中，参考术语“一个实施方式”、“一些实施方式”、“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

以上内容是结合具体的实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换。

Claims (8)

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

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

根据车斗定位的范围，在车斗范围内设置物料的多个监测点，并获取各个监测点的满度值；

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

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

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

2.根据权利要求1所述的青饲机自动抛送系统机械臂运动控制方法，其特征在于，在车斗定位之后，设置车斗范围内多个监测点，监测点的设置是在机械臂坐标系下，所有的监测点沿车斗的上中线和下中线等步长设置；

或者，所有的监测点沿车斗的中线等步长设置，其中上中线为车斗上边沿和车斗中线的中间线，下中线为车斗下边沿和车斗中线的中间线，中线为车斗的中间线。

3.根据权利要求2所述的青饲机自动抛送系统机械臂运动控制方法，其特征在于，当设置的预设满度值小于等于100％时，分别在车斗的上中线和下中线设置监测点，当设置的预设满度值大于100％时，在车斗的中线设置监测点。

4.根据权利要求1所述的青饲机自动抛送系统机械臂运动控制方法，其特征在于，获取在车斗范围内的各个监测点的物料的满度值，监测点的物料的满度值检测包括：

RGB-D相机获得的物料的点云数据转换到机械臂坐标系下点云数据；

在机械臂坐标系下以监测点为中心取一个矩形，求位于矩形内的物料点的Z值的平均值，以该平均值表达监测点的物料的满度值。

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

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

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

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

在机械臂坐标系下，在车斗上方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]。

7.根据权利要求1所述的青饲机自动抛送系统机械臂运动控制方法，其特征在于，所述机械臂运动至少包括有三个自由度，第一自由度为机械臂坐标系下机械臂绕Z轴的转动自由度，第二自由度为机械臂坐标系下机械臂绕X轴的转动自由度，第三自由度为机械臂坐标系下机械臂喷头处挡板在YZ平面上摆动自由度，挡板的摆动用于控制物料的喷射远近。

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

Images