一种基于多轴联动补偿的机器人瓶口TCP位置恒定倾倒控制方法及系统与流程

摘要: 本发明涉及物联网,尤其涉及一种基于多轴联动补偿的机器人瓶口tcp(toolcenterpoint,工具中心点)位置恒定倾倒控制方法及系统。背景技术:1、在分析化...
一种基于多轴联动补偿的机器人瓶口TCP位置恒定倾倒控制方法及系统与流程

本发明涉及物联网,尤其涉及一种基于多轴联动补偿的机器人瓶口tcp(tool center point,工具中心点)位置恒定倾倒控制方法及系统。

背景技术:

1、在分析化学、化肥品质检测以及材料科学实验室中,样品的自动化转运与精确分装是流程自动化的核心。机器人夹持容器进行倾倒动作看似简单,但在高精度计量要求下,传统方法面临着严峻的物理挑战:

2、变半径旋转导致的tcp轨迹偏移:

3、传统的单轴(倾角轴)旋转倾倒,其瓶口tcp相对于容器底部的旋转半径是不断变化的。这导致瓶口在倾斜过程中会沿圆弧路径发生剧烈的垂直与水平位移。如果落料点随之不断变动,极易造成液体或粉末倒在目标容器边缘或外部,产生严重的物料浪费和交叉污染。

4、重力与动量耦合下的轨迹不可控性:

5、随着容器倾角增大,物料流出的初速度矢量方向不断偏移。如果不进行实时的空间位移补偿,物料落地的抛物线轨迹将随角度变化而产生难以预测的波动。对于开口狭小的比色皿或离心管,这种位置偏差是不可接受的。

6、异构容器引发的物理碰撞与适配难题:

7、实验室中容器规格(如:108mm高烧杯、17mm低培养皿)极其繁杂。单一的下料高度设定无法兼顾不同容器的物理安全性与防溅要求。传统方式依赖人工频繁调整机器人的空间原点,缺乏一套基于物理模型的自适应规避机制。

8、流体惯性与机械振动的干扰:

9、大转矩的单一旋转轴在启动和停止瞬间会产生明显的角加速度脉动,这种物理冲击易传导至容器,导致内部样品大幅波动、挂壁甚至喷溅,严重影响天平实时称重的稳定性。

技术实现思路

1、本发明针对现有技术中存在的技术问题,提供一种基于多轴联动补偿的机器人瓶口tcp位置恒定倾倒控制方法,采用多轴联动tcp空间位置实时补偿算法,利用三角函数抵消旋转圆弧位移的方法,确保在全量程倾倒周期内,瓶口tcp坐标始终、毫米级地锁定在目标容器的中心轴线上,有效解决了窄口容器倒样的洒漏难题。

2、根据本发明的第一方面,提供了一种基于多轴联动补偿的机器人瓶口tcp位置恒定倾倒控制方法,包括:

3、步骤1,构建垂直补偿轴、水平补偿轴和轴向倾倒轴,所述垂直补偿轴为控制执行旋转倾倒操作的执行器移动的垂直于地面方向的轴,所述水平补偿轴为控制所述执行器移动的平行于地面方向的轴;所述倾倒轴为所述执行器的轴;设定所述倾倒轴和所述水平轴的夹角为倾倒角;

4、步骤2,设置所述倾倒角的步长,当所述倾倒角每步进一个步长时,对所述垂直补偿轴和所述水平补偿轴的位置进行同步补偿;

5、对所述垂直补偿轴进行下压或抬升补偿的计算公式为:;对所述水平补偿轴的位置进行补偿的计算公式为:;其中,为垂直补偿轴的位移补偿量,为所述水平补偿轴的位移补偿量,l为执行旋转倾倒操作的旋转中心到瓶口的距离。

6、在上述技术方案的基础上,本发明还可以作出如下改进。

7、可选的,所述倾倒角的步长的大小满足:

8、

9、其中,为目标落点允许横向误差,为目标落点允许垂向误差,为由容器开口尺寸限定的安全上限,为由液体黏度、粉末流动性或防飞溅要求限定的工艺上限。

10、可选的,当容器开口直径,或要求高精度称量、目标误差在范围内时,设置的取值范围为;

11、当,且为常规实验分装时,设置的取值范围为;

12、当,且允许较大落料区域时,设置的取值范围为。

13、可选的,所述步骤2之前还包括:

14、步骤11,建立各种类型的接收容器的形状大小的接收容器参数集;

15、步骤12,建立所述执行器的末端包络范围的执行器参数集;

16、步骤13,根据接收容器的类型确定所述接收容器参数集,基于所述接收容器参数集和所述执行器参数集计算水平逼近距离和垂直段起步位置。

17、可选的,所述水平逼近距离的计算公式为:

18、;

19、其中,为接收容器有效开口直径,为瓶身或执行器的夹持机构在逼近方向上的最大外包络半径,为夹持偏置,为横向安全裕量;

20、所述垂直段起步位置的计算公式为:

21、;

22、其中,为接收容器口沿高度,为推荐落料高度, 为垂向安全裕量, 表示预倾角阶段需要预留的垂向补偿量。

23、可选的,所述恒定倾倒控制方法还包括:

24、构建用于限制所述执行器执行倾倒操作的过程中瓶口tcp的位置的允许落料通道,所述允许落料通道为以接收容器中心轴线为中心轴的圆柱形或棱柱形通道;

25、构建用于限制所有移动物件的移动范围的禁止碰撞包络,所述禁止碰撞包络为对所有实体结构进行外扩形成的膨胀包络,所述禁止碰撞包络;外扩量为;

26、其中,为标定误差,为设备重复定位误差,为当前速度下的停车制动距离,为振动扰动附加量。

27、可选的,所述恒定倾倒控制方法还包括:

28、基于垂直补偿轴、水平补偿轴和轴向倾倒轴当前时刻的姿态对下一时刻候选姿态进行预测计算:

29、

30、

31、

32、其中,和分别为第和第k个控制周期倾倒轴的倾倒角;为第个控制周期垂直补偿轴的高度,为垂直补偿轴的初始高度,为第控制周期的命令角度;为第个控制周期水平补偿轴的水平位移,为水平补偿轴的初始水平位移;

33、基于下一时刻候选姿态的预测值对所述垂直补偿轴、水平补偿轴和轴向倾倒轴进行硬件限位/物理碰撞校验,判断存在硬件限位/物理碰撞触发风险时,禁止下发下一时刻的控制命令。

34、可选的,对所述垂直补偿轴、水平补偿轴和轴向倾倒轴进行硬件限位/物理碰撞校验包括:

35、轴位置校验:校验各个轴的下一时刻位置以及全流程终点位置,是否满足各自软限位和硬限位范围;

36、剩余制动距离校验:校验各个轴下一时刻位置到限位边界的剩余距离是否大于等于设定距离;

37、速度与加速度校验:校验速度和加速度是否不超过设定阈值;

38、瓶口与容器口沿间隙校验:校验瓶口tcp、瓶身外包络和夹爪外包络到容器口沿、容器外壁、称台防风罩的最小距离是否超过设定阈值;

39、中心线保持校验:校验落料位置是否超出允许落料通道;

40、全流程补偿裕量校验:根据最大倾倒角计算各个轴的限定位置,校验各个轴的下一时刻位置是否超过限定位置。

41、可选的,所述恒定倾倒控制方法还包括:构建旋转轴,所述旋转轴用于在倾倒过程中控制所述垂直补偿轴的下端绕垂直于地面方向的轴进行旋转;旋转的频率根据倾倒的样品的材质设置。

42、可选的,所述恒定倾倒控制方法还包括:

43、基于高精度电子天平实时采集接收容器的质量确定倾倒过程的当前执行进度;

44、当前执行进度小于设置的临界点时,设置所述倾倒角的步长为;

45、当前执行进度大于等于所述临界点时,设置所述倾倒角的步长为;。

46、根据本发明的第二方面,提供了一种基于多轴联动补偿的机器人瓶口tcp位置恒定倾倒控制系统,包括:底座、旋转轴j1、倾倒轴j2、垂直补偿轴j3、水平补偿轴j4、执行器、接收容器、高频采样电子天平和主控制器。

47、执行器用于控制瓶子执行旋转倾倒操作。倾倒轴为执行器的轴。

48、垂直补偿轴j3为控制执行器移动的垂直于地面方向的轴。

49、水平补偿轴j4为控制执行器移动的平行于地面方向的轴。

50、垂直补偿轴j3下端基于旋转轴j1固定在底座上,旋转轴j1用于在倾倒过程中控制垂直补偿轴j3的下端绕垂直于地面方向的轴进行旋转;旋转的频率根据倾倒的样品的材质设置。

51、接收容器用于接收瓶子旋转倾倒后的样品。高频采样电子天平用于实时采集接收容器的质量确定倾倒过程的当前执行进度。

52、主控制器接收高频采样电子天平实时采集的数据,下发控制命令控制旋转轴j1、倾倒轴j2、垂直补偿轴j3、水平补偿轴j4和执行器的动作。

53、设定所述倾倒轴和所述水平轴的夹角为倾倒角;设置所述倾倒角的步长,当所述倾倒角每步进一个步长时,对所述垂直补偿轴和所述水平补偿轴的位置进行同步补偿;使得瓶口tcp位于容器出料边缘中心点,在倾倒过程中保持瓶口tcp对准接收容器中心轴线。

54、对所述垂直补偿轴进行下压或抬升补偿的计算公式为:;对所述水平补偿轴的位置进行补偿的计算公式为:;其中,为垂直补偿轴的位移补偿量,为所述水平补偿轴的位移补偿量,l为执行旋转倾倒操作的旋转中心到瓶口的距离。

55、本发明实施例提供的一种基于多轴联动补偿的机器人瓶口tcp位置恒定倾倒控制方法及系统,其有益效果包括:

56、采用多轴联动tcp空间位置实时补偿算法,利用三角函数抵消旋转圆弧位移的方法,确保在全量程倾倒周期内,瓶口tcp坐标始终、毫米级地锁定在目标容器的中心轴线上。

57、针对异构容器(培养皿/烧杯)预设物理模型,采用脉冲零点自适应切换方法,通过构建动态坐标引擎,确保在全量程倾倒周期内,瓶口tcp坐标始终、毫米级地锁定在目标容器的中心轴线上,保证了tcp空间轨迹绝对恒定,瓶口落点误差,有效解决了窄口容器倒样的洒漏难题。

58、通过实时的高频脉冲映射,在j2旋转生成的“圆弧运动”之上叠合了一个反向的“直线位移轨迹”,结合j1轴高频旋流与j2轴动态步进的混合倾倒控制策略,从而在合成运动空间内将瓶口tcp锁定为绝对静止点,实现了颗粒状或黏稠样品的“流体化”精准释放,流量极度平稳。

59、通过容器模型预补偿,机械臂在运动起始阶段即自动规避了与高度敏感容器(如培养皿)的物理干涉,具有高安全性与防碰撞功能。

60、集成天平读数重试机制(高频6hz监测)进行全闭环反馈,基于重量反馈比率()的七级流速梯度自动降档,大幅降低了实验室气流干扰对称量精度的影响。