一种基于地缸醅料机器人的高速挖取系统及控制方法

本发明属于酿酒机械设备控制，具体涉及一种基于地缸醅料机器人的高速挖取系统及控制方法。

背景技术：

1、酿酒或酿醋过程中需要将原料醅料放入地缸，经过多天的发酵后，然后通过人工将地缸中发酵后的原料取出蒸馏、配比，最后生产出市场中销售的酒或醋。

2、生产过程中，地缸中的原料需要工人用铁锹逐锹挖出。由于地缸的结构特殊且数量多，人工挖取醅料劳动强度大，行业年轻劳动力缺乏，因此使用机器人来代替。现有的酿酒行业的取料设备不具有力感知的功能，无法保证每一次挖取的挖取量和整个过程的挖取效率。

技术实现思路

1、针对基于机器人力感知挖取醅料及挖取效率的问题，本发明提供了一种基于地缸醅料机器人的高速挖取系统及控制方法。

2、为了达到上述目的，本发明采用了下列技术方案：

3、一种基于地缸醅料机器人的高速挖取系统，包括六轴机器人、取料装置、六维力传感器、视觉传感器和系统控制器；

4、六轴机器人末端安装有取料装置，在取料装置和六轴机器人之间设有六维力传感器，所述六维力传感器用于实时检测取料装置切入醅料时作用力的大小；所述视觉传感器设置在六轴机器人上方，用于检测待取醅料区域的深度；

5、所述系统控制器分别连接六轴机器人、取料装置、六维力传感器和视觉传感器。

6、进一步，所述六轴机器人设置在地面上或自动导向车上(agv)，地缸设置在地面下。

7、进一步，所述系统控制器与六轴机器人、取料装置、六维力传感器和视觉传感器通过有线连接。

8、进一步，所述的视觉传感器为rgb-d相机。

9、进一步，所述视觉传感器通过视觉传感器支架固定安装在六轴机器人上方；或者视觉传感器安装在六轴机器人末端，与六轴机器人组成手眼系统，对待取醅料区域进行深度检测。

10、一种如上述基于地缸醅料机器人的高速挖取系统的控制方法，包括以下步骤：

11、步骤1，通过阻抗控制方法对六轴机器人进行控制，使机器人能够感知作用力，保证机器人能够通过作用力的大小，既避免取料装置切入醅料过浅导致取料量少，又避免取料装置切入醅料过深导致六轴机器人上的六维力传感器达到报警作用力fe报警甚至损坏设备；

12、步骤2，当视觉传感器检测到待取醅料区域的醅料深度后，将信号传输至系统控制器，系统控制器对六轴机器人进行轨迹规划，在醅料区域上方的安全距离内进行高速运动v1，在醅料深度的安全距离进行低速运动，保证取料装置切入醅料时的速度为低速运动v2；

13、步骤3，六轴机器人低速运动带动取料装置切入醅料，给定阻抗控制模型的速度初始值，使取料装置接触到醅料时的低速运动与阻抗控制的控制轨迹的速度连续；

14、步骤4，六维力传感器通过阻抗控制模型输出的结果，防止关节超速或机器人末端超速，进行速度钳位。

15、进一步，所述阻抗控制方法包括设定六维力传感器的报警作用力fe和安全余量fh；

16、阻抗控制的传递函数公式表示为：

17、

18、通过给定输入f(s)得到输出x(s)；其中设置k为0，此时阻抗控制的功能是：当撤掉输入f时，输出保持在上一刻的输出位置；

19、阻抗控制的输入f(s)为：给定期望力fe-fh与实时力检测的值fa的偏差e；阻抗控制参考绝对位置为取料装置接触醅料时刻的实际位置，当输入f(s)＝fe-fs时，得到输出x(s)＝xr；阻抗控制会向偏差e趋于0的方向运动，此时阻抗控制传递函数的输出逐步保持x(s)不变，六轴机器人的绝对位置逐步保持x+xr不变。

20、进一步，所述高速运动为六轴机器人所能达到的最大速度，用于实现六轴机器人的高速作业；所述低速运动是用于避免取料装置与醅料接触时瞬间作用力过大，导致六轴机器人报警或者损坏设备；

21、六轴机器人低速运动带动取料装置切入醅料，给定阻抗控制模型的速度初始值，使取料装置接触到醅料时的低速运动与阻抗控制的控制轨迹的速度连续。

22、进一步，所述步骤3，六轴机器人低速运动带动取料装置切入醅料，给定阻抗控制模型的速度初始值，使取料装置接触到醅料时的低速运动与阻抗控制的控制轨迹的速度连续,具体包括以下步骤：

23、步骤3.1，获取到视觉检测的料位深度后，增加一段安全距离；控制机器人从当前位置从速度为零开始先加速到高速运动，当取料装置到达醅料深度上的安全距离时，运动速度从高速运动转接到低速运动，取料装置以该低速运动切入醅料；

24、步骤3.2，基于视觉检测料位深度的轨迹规划分为5段，0～t1段表示速度从0加速到v1，t1～t2段表示以速度v1匀速运动，t2～t3段表示速度从v1减速到v2，t3～t4表示以速度v2匀速运动，t4～t5表示速度从v2减速到0；其中安全距离不小于t2～t3段的位移；

25、步骤3.3，其中两次速度的变化使用四阶多项式拟合s型曲线进行速度转接，将轨迹规划分成两段处理，即两次“加速-匀速-减速”，其中第二次的加速起始速度与匀速速度相同，即加速位移和加速时间均为0；

26、步骤3.4，根据基于视觉检测料位深度的轨迹规划，每隔固定控制周期选择插补点，并计算其在三维空间的位置；将每个三维空间的插补点通过机器人运动学逆解，求得对应的各关节位置，再以每个控制周期下发到每个伺服电机，控制六轴机器人运动。

27、进一步，所述步骤4，六维力传感器通过阻抗控制模型输出的结果，防止关节超速或机器人末端超速，进行速度钳位包括以下步骤：

28、步骤4.1，通过六维力传感器的返回值，再计算三维力和力矩分别通过阻抗控制传递函数的不同输出s2，分别对应三维空间目标位置px、py、pz、和三维空间目标姿态rx、ry、rz；

29、步骤4.2，通过三维空间目标位置和姿态px、py、pz、rx、ry、rz计算运动学逆解，求得各关节目标位置；

30、步骤4.3，计算各关节当前位置和目标位置以及控制周期，计算是否超过关节最大速度，如果超过关节最大速度，即以关节最大速度通过雅克比矩阵计算机器人末端的目标线速度和目标角速度；

31、步骤4.4，判断当前线速度和角速度是否超过低速v2，如果超过v2，即以该速度作为目标线速度；

32、将该目标线速度和目标角速度，反推理论六维力传感器的接触力，重新代入阻抗控制中，计算目标位置px、py、pz、rx、ry、rz。

33、与现有技术相比本发明具有以下优点：

34、本发明在醅料挖取行业中，使用机器人基于力感知的醅料挖取技术，同时结合视觉对醅料料位深度及高速和低速直线规划保证醅料挖取效率。

技术特征：1.一种基于地缸醅料机器人的高速挖取系统，其特征在于：包括六轴机器人(1)、取料装置(2)、六维力传感器(3)、视觉传感器(4)和系统控制器(5)；

2.根据权利要求1所述的一种基于地缸醅料机器人的高速挖取系统，其特征在于：所述六轴机器人(1)设置在地面(8)上或自动导向车上，地缸(6)设置在地面(8)下。

3.根据权利要求1所述的一种基于地缸醅料机器人的高速挖取系统，其特征在于：所述系统控制器(5)与六轴机器人(1)、取料装置(2)、六维力传感器(3)和视觉传感器(4)通过有线连接。

4.根据权利要求1所述的一种基于地缸醅料机器人的高速挖取系统，其特征在于：所述的视觉传感器(4)为rgb-d相机。

5.根据权利要求1所述的一种基于地缸醅料机器人的高速挖取系统，其特征在于：所述视觉传感器(4)通过视觉传感器支架(7)固定安装在六轴机器人(1)上方；或者视觉传感器(4)安装在六轴机器人(1)末端，与六轴机器人(1)组成手眼系统，对待取醅料区域进行深度检测。

6.一种如权利要求1所述基于地缸醅料机器人的高速挖取系统的控制方法，其特征在于：包括以下步骤：

7.根据权利要求6所述的控制方法，其特征在于：所述阻抗控制方法包括设定六维力传感器(3)的报警作用力fe和安全余量fh；

8.根据权利要求6所述的控制方法，其特征在于：所述高速运动为六轴机器人(1)所能达到的最大速度，用于实现六轴机器人(1)的高速作业；所述低速运动是用于避免取料装置(2)与醅料接触时瞬间作用力过大，导致六轴机器人(1)报警或者损坏设备；

9.根据权利要求6所述的控制方法，其特征在于：所述步骤3，六轴机器人(1)低速运动带动取料装置(2)切入醅料，给定阻抗控制模型的速度初始值，使取料装置(2)接触到醅料时的低速运动与阻抗控制的控制轨迹的速度连续,具体包括以下步骤：

10.根据权利要求6所述的控制方法，其特征在于：所述步骤4，六维力传感器(3)通过阻抗控制模型输出的结果，防止关节超速或机器人末端超速，进行速度钳位包括以下步骤：

技术总结本发明公开了一种基于地缸醅料机器人的高速挖取系统及控制方法，属于酿酒机械设备控制技术领域。针对基于机器人力感知挖取醅料及挖取效率的问题。本申请一种基于地缸醅料机器人的高速挖取系统包括六轴机器人、取料装置、六维力传感器、视觉传感器和系统控制器。通过阻抗控制方法对六轴机器人进行控制，使机器人能够感知作用力，既避免切入醅料过浅，又避免切入醅料过深甚至损坏设备；当视觉传感器检测到待取醅料区域的醅料深度后，将信号传输至系统控制器，系统控制器对六轴机器人进行轨迹规划，在醅料区域上方的安全距离内进行高速运动，在醅料深度的安全距离进行低速运动，保证取料装置切入醅料时的速度为低速运动。技术研发人员：田建艳,王素钢,李济甫,尉小雪,李丽宏受保护的技术使用者：太原理工大学技术研发日：技术公布日：2024/1/15