一种基于电流波动与动量观测器的工业机器人碰撞检测方法与流程

本发明涉及机器人碰撞检测领域，特别是涉及一种基于电流波动与动量观测器的工业机器人碰撞检测方法。

背景技术：

1、随着制造业对自动化和智能化水平要求的不断提高，工业机器人的应用日益广泛。在高效率、高灵活性的生产线上，要确保机器人能够安全、准确地执行任务，同时在复杂环境中避免碰撞，保护人员与设备安全。此外，国际安全标准，如iso/ts15066的不断更新和完善，对工业机器人的安全功能提出了具体要求，包括碰撞检测和紧急停止机制。这些标准的实施促使制造商在设计时就必须集成碰撞检测技术，以符合法律法规和行业安全规范。因此，工业机器人碰撞检测作为一项关键的安全技术，对于确保自动化工作环境中的人员安全、保护设备免受损害具有重要意义。

2、工业机器人碰撞检测通常采用基于模型的方法，也就是利用机器人的动力学关系估计外部接触力来判断是否与外界发生碰撞。直接估计法和广义动量法是两种广泛应用的技术方法。

3、直接估计法通过比较实际力矩与计算力矩的差值判断是否发生碰撞，具备检测速度快的优点，但由于用到了加速度信息，而加速度是对反馈位置二次差分求得，放大了测量噪声，影响外力的观测精度，易产生碰撞误报。

4、广义动量法利用实际动量与估计动量的差值来估计外力，实际上为一个一阶的龙伯格观测器，其观测结果为外部作用力矩的一阶低通滤波。在碰撞检测中，为使得力矩估计值能较快地跟踪上实际碰撞力矩，观测增益通常选取得较大。然而，较大的观测增益会引入较大噪声，从而拉高碰撞检测的阈值，降低了检测灵敏度。

5、为了兼顾检测灵敏度和检测误报率，本发明基于当前已有的动力学精度边界，分析不同运动阶段机器人的动力学特性，创新性地融合电流波动检测与广义动量法，提出了一种新型碰撞检测机制，在节约研发资源的同时能够在过工程应用中获得相对较好的效果，具备较高的应用价值。

技术实现思路

1、基于此，有必要针对当前机器人碰撞检测技术检测灵敏度低和检测误报率高问题，提供一种基于电流波动与动量观测器的工业机器人碰撞检测方法。

2、一种基于电流波动与动量观测器的工业机器人碰撞检测方法，包括如下步骤：

3、s1、设计软碰撞判定指标，用于检测机器人在执行任务过程中的轻微接触；

4、s2、设计硬碰撞判定指标，用于检测机器人在执行任务过程中的显著冲击；

5、s3、设计碰撞阈值，用于区分正常操作与碰撞事件；

6、s4、设计碰撞检测流程，通过实时监控机器人的状态，检测是否发生碰撞并触发碰撞报警。

7、本技术公开了一种基于电流波动与动量观测器的工业机器人碰撞检测方法，通过设计软碰撞判定指标和硬碰撞判定指标，能够更准确地检测机器人在执行任务过程中的轻微接触和显著冲击，而实现对碰撞事件的精确判断。设计碰撞阈值，能够明确区分机器人在操作过程中正常动作和碰撞事件，避免了误判和漏判的情况。设计碰撞检测流程，以实时监控机器人的状态，能够及时发现碰撞事件并触发碰撞警，从而迅速采取相应的措施，保护机器人及其周围环境的安全。通过该方法及时检测和处理碰撞事件，能够减少机器人因碰撞而产生的故障停时间，提高生产线的运行效率，及时发现并处理在的碰撞风险，从而提高工业自动化的安全性。

8、上述任一技术方案中，设计软碰撞判定指标的方法如下：

9、定义软碰撞判定指标rsoft：

10、rsoft＝α·abs(tfbδ)+(1-α)·abs(fgmo)

11、其中，abs(·)表示取绝对值，tfbδ为机器人反馈力矩的变化量，fgmo为广义动量法估计外力，α为超参数，取0.4；

12、对于第n个插补周期(n>0)，tfbδ(n)计算如下：

13、

14、其中，tfb(·)为机器人反馈力矩，n为超参数，取5；

15、对于第n个插补周期，tfb(n)计算如下：

16、tfb(n)＝k·efb(n)

17、其中，k为机器人电机的转矩常数矩阵，由电机供应商给出；efb(n)为机器人第n个插补周期的反馈电流，通过采集获得；

18、对于第n个插补周期，fgmo(n)计算如下：

19、

20、其中，t为机器人的插补周期，ko为广义动量观测器的观测增益，m(n)、c(n)、g(n)、τf(n)分别为机器人第n个插补周期的惯量矩阵、哥氏力矩阵、重力矢量、摩擦矢量，通过动力学辨识获得，q(n)为机器人第n个插补周期的位置，tm(n)为机器人第n个插补周期的电机驱动力矩，满足tm(n)＝k·ein(n)，ein(n)为机器人第n个插补周期的输入电流；

21、对rsoft指标进行卡尔曼滤波来抑制波动，并将滤波结果作为最终的软碰撞判定指标：

22、rsoft＝kalman_filter(rsoft)

23、对于输入x∈rm×1，将其通过卡尔曼滤波后输出y的具体计算流程如下：

24、1)初始化：

25、

26、

27、2)预测：

28、p＝p′+q

29、3)更新：

30、kg＝p·(p+r)-1

31、p′＝(1-kg)·p

32、y＝y′+kg·(x-y′)

33、y＝y

34、其中，m为机器人的轴数量，p表示估计误差的协方差矩阵，q表示过程噪声的协方差矩阵，r表示观测噪声的协方差矩阵，x表示卡尔曼滤波器的输入，即rsoft；y表示卡尔曼滤波器输出的滤波结果，即rsoft的滤波结果；

35、4)重复步骤2)和步骤3)，对于每一个插补周期的观测数据点进行预测和更新，直到所有数据处理完毕；

36、rsoft按上述计算流程求取。

37、通过设计软碰撞判定指标，能够实现对工业机器人碰撞的精准检测，该指标结合了机器人反馈力矩的变化量、广义动量法估计外力以及超参数，全面反映了机器人动态特性和可能发生的碰撞情况。计算过程中，考虑了每个插补周期的各种参数，包括反馈力矩、电机转矩常数矩阵、反馈电流、插补周期、观测增益以及机器人的动力学参数等，使得判定指标更为精确和全面。针对反馈电流采集过程中可能受到的噪声干扰，采用卡尔曼滤波进行抑制波动。卡尔曼滤波能够有效去除噪声，使得判定指标更为稳定可靠。通过初始化、预测和更新等步骤，卡尔曼滤波能够处理每一个插补周期的观测数据，确保数据实时性和准确性。

38、上述任一技术方案中，设计硬碰撞判定指标的方法如下：

39、定义硬碰撞判定指标rhard：

40、rhard＝β·abs(δerrortfb)+(1-β)·abs(δtfb)

41、其中，δerrortfb为机器人反馈力矩误差的变化量，δtfb为机器人反馈力矩的变化量，β为超参数，取0.7；

42、对于第n个插补周期，δerrortfb(n)计算如下：

43、

44、errortfb(n)＝tfb(n)-tcmd(n)

45、其中，tcmd(n)为机器人第n个插补周期的指令力矩；

46、对于第n个插补周期，δtfb(n)计算如下：

47、

48、同理，对rhard进行卡尔曼滤波，并将滤波结果作为最终的硬碰撞判定指标：

49、rhard＝kalman_filter(rhard)

50、对于输入x∈rm×1，将其通过卡尔曼滤波后输出y的具体计算流程如下：

51、1)初始化：

52、

53、

54、2)预测：

55、p＝p+q

56、3)更新：

57、kg＝p·(p+r)-1

58、p′＝(1-kg)·p

59、y＝y′+kg·(x-y′)

60、y＝y

61、其中，m为机器人的轴数量，p表示估计误差的协方差矩阵，q表示过程噪声的协方差矩阵，r表示观测噪声的协方差矩阵，x表示卡尔曼滤波器的输入，即rhard；y表示卡尔曼滤波器输出的滤波结果，即rhard的滤波结果；

62、4)重复步骤2)和步骤3)，对于每一个插补周期的观测数据点进行预测和更新，直到所有数据处理完毕；

63、rhard按上述计算流程求取。

64、通过基于机器人反馈力矩误差的变化量和变化量，并结合超参数，定义硬碰撞判定指标，从而提高了碰撞检测的准确性。计算每个插补周期的指令力矩和相应的判定指标，使得该方法具有极高的实时性，能够迅速响应机器人可能发生的碰撞，为后续的避障或紧急处理提供足够的时间。采用卡尔曼滤波对判定指标进行处理，能够有效剔除噪声和干扰，得到更为准确的硬碰撞判定指标，大大提高了碰撞检测的可靠性和精度。

65、上述任一技术方案中，设计碰撞阈值的方法如下：

66、按匀速、加减速、零速三个运动段对碰撞阈值进行程序调参，具体如下：

67、1)运动段划分

68、

69、其中，vfb和afb通过对反馈位置pfb做一阶、二阶差分获得；vfb表示反馈速度，对于t时刻的反馈速度，有其中t为机器人系统插补周期；afb表示反馈加速度，对于t时刻的反馈加速度，有其中t为机器人系统插补周期；pfb表示反馈位置；

70、2)计算判定指标，并记录最大值，步骤如下：

71、a)初始化匀速段软碰撞阈值asoft与硬碰撞阈值ahard、加减速段软碰撞阈值bsoft与硬碰撞阈值bhard：

72、

73、

74、其中，m为机器人的轴数量；

75、b)运行机器人出厂跑机程序，运行速度倍率调整为100％；

76、c)对于第n个插补周期，根据1)判定所处运动段，并计算rsoft(n)和rhard(n)；

77、d)如果处在匀速段，对于机器人的第i(i<m)个轴阈值asoft(i)和ahard(i)，有asoft(i)＝max(asoft(i)，rsoft(n,i))，ahard(i)＝max(ahard(i)，rhard(n,i))；如果处在加减速段，对于机器人的第i(i<m)个轴阈值bsoft(i)和bhard(i)，有bsoft(i)＝max(bsoft(i)，rsoft(n,i))，bhard(i)＝max(bhard(i)，rhard(n,i))；如果处在零速段，则不记录；

78、e)循环运行c)、d)，运行10min后停止程序，输出asoft、ahard、bsoft和bhard。

79、通过根据机器人的运动状态将运动段细分为匀速段、加减速段和零速段，使得碰撞阈值设定更为精准。通过这种方式，可以更加准确地识别不同运动状态下可能发生的碰撞，提高了碰撞检测的准确性实时性。通过反馈位置的一阶、二阶差分获得反馈速度和反馈加速度信息，结合机器人的实运动状态，动态计算并调整碰撞阈值，这种方法相较于传统的固定阈值设定，更能适应机器人实际运行中的变化，提高了碰撞检测的适性。根据机器人所处的运动段和运行状态，独立计算和设置阈值，轴级别的阈值设定方式，使得碰撞检测更为精细化，能够应对复杂工况下的多轴协同运动。

80、上述任一技术方案中，所述步骤s1中，设计软碰撞判定指标的步骤如下：

81、s11、基于机器人反馈力矩的变化量、广义动量法估计外力和超参数，定义软碰撞判定指标；

82、s12、对于第n个插补周期，计算机器人的反馈力矩和电机的转矩常数矩阵，同时采集机器人的反馈电流；

83、s13、对反馈电流进行卡尔曼滤波处理，以减少噪声对碰撞检测精度的影响；

84、s14、重复卡尔曼滤波的预测步骤和更新步骤，对每一个插补周期的观测数据点进行预测和更新，将最终输出的滤波结果作为软碰撞判定指标。

85、通过结合机器人的反馈力矩变化量、广义动量法估计外力和超参数，定义了软碰撞判定指标，使得该方法能够更准确地捕捉机器人碰撞时的细微变化，从而提高碰撞检测的精度。在采集机器人的反馈电流过程中，可能存在噪声干扰，为此，采用卡尔曼滤波处理反馈电流，以消除噪声对碰撞检测精度的影响，使得检测结果更加真实可靠。在每个插补周期都进行观测数据的预测和更新，重复卡尔滤波的预测步骤和更新步骤，实现了对机器人碰撞的实时检测，，提高了碰撞检测的准确性，同时，使操作人员可以在第一时间得知机器人的碰撞情况，从而及时采取应对措施。

86、上述任一技术方案中，所述步骤s2中，设计硬碰撞判定指标的步骤如下：

87、s21、基于机器人反馈力矩误差的变化量、反馈力矩的变化量、超参数，定义硬碰撞判定指标；

88、s22、对于第n个插补周期，计算机器人的反馈力矩和指令力矩；

89、s23、对硬碰撞判定指标进行卡尔曼滤波处理，以减少噪声干扰，提高碰撞检测的准确性；

90、s24、重复卡尔曼滤波的预测步骤和更新步骤，对每一个插补周期的观测数据点进行预测和更新，将最终输出的滤波结果作为硬碰撞判定指标。

91、通过结合机器人反馈力矩误差的变化量、反馈力矩的变化量以及超参数，定义硬碰撞判定指标，能够更精确地识别器人是否发生碰撞，避免了误判或漏判的情况。采用卡尔曼滤波处理硬碰撞判定指标，有效地减少了环境中的噪声干扰，提高碰撞检测的抗干扰能力，使得检测结果更加稳定和可靠。对每一个插补周期的观测数据进行预测和更新，能够实时地反映机器人的运动状态和可能发生的碰撞情况，进一步提高了碰撞检测的准确性。

92、上述任一技术方案中，所述步骤s3中，设计碰撞阈值的步骤如下：

93、s31、确定机器人的运动段，运动段分为匀速段、加减速段和零速段三个运动段；

94、s32、分别为匀速段和加减速段的软碰撞阈值和硬碰撞阈值设定初始值；

95、s33、运行机器人出厂跑机程序，运行速度倍率调整为100％；

96、s34、对于每个插补周期，根据运动段的判定结果，计算相应的软碰撞判定指标和硬碰撞判定指标；

97、s35、如果在匀速段，记录当前轴的软碰撞阈值和硬碰撞阈值；如果在加减速段，同样记录当前轴的阈值；如果在零速段，则不记录；

98、s36、重复步骤s34和步骤s35，直到程序运行10分钟后停止；

99、s37、程序停止后，输出匀速段和加减速段的最终软碰撞阈值和硬碰撞阈值。

100、通过区分机器人的运动段，如匀速段、加减速段和零速段，为每个运动段设定软碰撞阈值和硬碰撞阈值的初始值，使得碰撞检测阈值的设定更为精细化和准确。通过运行机器人的出厂跑机程序，并调整运行速度倍率为100％，能够实时地、动态地获取每个插补周期内不同运动段的软碰撞判定指标和硬碰撞判定指标，使得阈值更加贴近实际行状况。通过记录不同运动段的阈值，使得碰撞检测系统的适应性更强，使机器人无论处于匀速运动还是加减速运动，都能够进行有效的碰撞检测。

101、上述任一技术方案中，所述步骤s3中，设计碰撞阈值的步骤还包括：

102、s38、根据机器人的工作历史和维护记录，调整碰撞阈值，以提高碰撞检测的准确性和可靠性；

103、s39、提供用户界面，允许操作人员根据经验手动调整碰撞阈值。

104、通过根据机器人的工作历史和维护记录调整碰撞阈值，可以使得碰撞检测更加符合实际工作环境，从而提高检测的准确性，个性化的调整有助于减少误报和漏报，确保机器人在各种工作条件下都能可靠地检测到碰撞。提供用户界面允许操作人员根据经验手动调整碰撞阈值，增加了系统的灵活性，操作人员可以根据实际情况和个人经验进行调整，使得碰撞检测更加符合特定任务的需求。

105、上述任一技术方案中，所述步骤s4中，设计碰撞检测流程的步骤如下：

106、s41、设定软碰撞判定指标和硬碰撞判定指标的初始阈值；

107、s42、判断运动段；

108、s43、对于每个插补周期，根据当前的运动段，计算软碰撞判定指标和硬碰撞判定指标；

109、s44、实时监控软碰撞判定指标和硬碰撞判定指标，一旦超过设定的阈值，立即触发碰撞报警。

110、通过设定软碰撞判定指标和硬碰撞判定指标的初始阈值，能够实现对工业机器人碰撞的精准判定，通过设定不同的阈值，可以适应不同的工作环境和作业需求，提高碰撞检测的灵活性和实用性。判断运动能够针对机器人的不同运动状态进行碰撞检测。在不同的运动段，机器人面临的风险和碰撞可能性是不同的，因此，对运动段的判断有助于提高撞检测的准确性和实时性。对于每个插补周期，根据当前的运动段计算软碰撞判定指标和硬碰撞判定指标，可以实现对机人运动状态的实时监控和碰撞风险的实时预警。通过实时监控软碰撞判定指标和硬碰撞判定指标，一旦超过设定的阈值，立即触发碰撞报警，以迅速响应并处理潜在的碰撞风险。即时反馈的机制，可以最大程度地减少碰撞对机器人及其工作环境造成的损害，保障生产线安全和稳定。

111、上述任一技术方案中，所述步骤s4中，设计碰撞检测流程的步骤还包括：

112、s45、在检测到碰撞事件时，自动调整机器人的运动参数，如速度或加速度，以减少进一步的碰撞风险；

113、s46、记录碰撞事件的详细信息，包括时间、位置和碰撞力度，用于后续的分析和改进；

114、s47、在必要时，启动紧急停止程序，以确保操作人员和设备的安全；

115、s48、提供远程监控功能，允许操作人员远程查看碰撞检测的状态和记录。

116、通过自动调整机器人的运动参数，可以在检测到碰撞事件时迅速响应，减少进一步的碰撞风险，从而保护机器人及其操作环境免受损害。自动记录碰撞事件的详细信息，为后续的分析和改进提供了数据支持，有助于识别碰撞发生的模式和原因，从而优化机器人的操作参数和环境布局。必要时启动紧急停止程序，可以在检测到潜在危险时立即采取行动，避免或减轻伤害。远程访问能力提高了操作的灵活性和安全性，同时也便于进行实时监控和排除故障。