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

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

A motion control method of the mechanical arm of the automatic throwing system of the green feeding machine

【技术实现步骤摘要】
一种青饲机自动抛送系统机械臂运动控制方法
本专利技术涉及机械臂控制与农业自动化
，尤其是涉及一种青饲机自动抛送系统机械臂运动控制方法。
技术介绍
青饲机是一种常用农业收获机械，主要用来收获青绿牧草、燕麦、甜菜茎叶、玉米等青饲作物。青饲机大体由前方割台、机身喂入装置及切碎抛送装置组成。前方割台为一个装有多把甩刀旋转切碎器用于高速收割作物，机身收集作物并将其切碎，然后由抛送装置抛出，青饲机抛出的物料由后方或侧方同向行驶的运输车承接。具有自动抛送系统的青饲机，机身收集作物并将其切碎，然后由固定在机械臂上的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中为车斗中线的监测点，上下中线的监测点和车斗中线的监测点都是以机械臂下方点为原点分别向左右两方等距离延伸；本文档来自技高网...

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

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


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


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


4.根据权利要求1所述的青饲机自动抛送系统机械臂运动控制方法，其特征在于，获取在车斗范围内的各个监测点的物料的满度值，监测点的物料的满度值检测包括：
RGB-D相机获得的物料的点云数据转换到机械臂坐标系下点云数据；
在机械臂坐标系下以监测点为中心取一个矩形，求位于矩形内的物料点的Z值的平均值，以该平均值表达监测点的物料的满度值。


...

【专利技术属性】
技术研发人员：何创新，班训康，苗中华，李楠，刘成良，
申请(专利权)人：上海大学，
类型：发明
国别省市：上海;31