一种基于动态阈值的协作机器人碰撞检测方法及系统与流程

1.本发明属于机器人自动化控制技术领域，具体涉及一种基于动态阈值的协作机器人碰撞检测方法及系统。


背景技术：

2.碰撞检测作为人机协作中的核心内容，国内外学者对此提出了多种检测方法，按照是否包含外部传感器可分为两类。kwan等人利用激光雷达和3d扫描仪对工作环境实时建模，与深度相机图像建模的方式相比，避免了图像投影问题但动态建模任务较为耗时。bosch机器人在连杆表面覆盖了电容式电子皮肤，能够以触觉方式将人机接触反馈给控制系统。但考虑到外部传感器通常价格昂贵，并且复杂的布线方式会对机器人构型产生影响。因此，在实际应用中人们更倾向于无外部传感器的技术路线，并且依赖于模型辨识信息建立碰撞观测器，实现经济、高效的碰撞检测。
3.早期的碰撞观测器以机器人的关节运动信息作为输入，得到关节理论驱动力矩并与采样驱动力矩进行比较，判断是否发生碰撞。但该方法需要关节加速度信息，因此会有一定的噪声和延时。陆济民从能量法的角度，提出基于能量偏差观测器的碰撞检测算法。将机器人系统视为整体，当外界碰撞发生时电机做功增加，导致能量变化。监测碰撞时输出力矩与对应位移，从而避免了加速度的计算，控制框图如图1所示。由于检测过程需对能量求导从而产生延时，所以在前向通道增加pd控制，并且通过二阶系统抑制碰撞后的震荡，提高响应速度的同时减小超调量。
4.除了能量分析的角度，deluca等人从动量检测的角度出发，提出了一种基于广义动量的观测器进行碰撞检测。该方法依赖于动力学模型实现一阶低通滤波器搭建，仅需输入关节速度、位移及电流等信息便可估计外部碰撞力的大小。目前已被验证为成熟完整的框架，应用于协作机器人碰撞的在线检测。宋宏祥针对机器人在运行中会存在外部环境扰动，受到通信领域中的“射频干扰对消”技术启发，进而提出了一种基于自干扰对消技术的扩张状态观测器。利用前置观测器与后端扩展状态观测器在检测外扰信号时的观测差异，将建模误差和外部碰撞分别视为内扰和外扰，进而完成外扰分离实现了碰撞检测，工作原理如图2所示。不过该观测器检测扰动时无法估计实际力矩，因此适用于仅需判断碰撞而无需准确估计力矩的应用场合。
5.由于广义动量观测器在碰撞检测中的成功应用，后续的学者基于此做了很多衍生的研究讨论。alexandros利用hann窗对电流信号进行快速傅里叶变换，注意到了与机器人的不同接触会产生不同的频率信号，结合前人在人机协作中对主动拖动和意外碰撞的研究，提出了基于频域分析的人机接触类型的区分方法。李志静在频域分析方法的基础上进行了细化完善，并提出了虚拟传感器对人机碰撞进行了检测和分类。虚拟传感器包含低通、带通两个滤波器，其原理流程如图3所示。利用低通滤波器完成碰撞检测，然后基于电流频域分布特征设计带通滤波器，过滤其中的低频接触信号和高频噪声，仅观测意外碰撞的频率信号。通过对比两个观测器的输出信号准确地预估碰撞力矩，完成了人机碰撞的类型区
分，有着较强的实际应用意义。
6.综上所述，将常见的碰撞检测方法归纳如下表(表1)所示：
[0007][0008]
可以看出动量观测器与关节力矩观测方法相比，无需借助外部传感器即可实现对外部碰撞力矩的观测，但无法对碰撞类型进行有效识别。虚拟传感器基于电流频率信息，通过输出虚拟力矩幅值进行观测，可以有效区分碰撞类型，但存在着无法检测碰撞力矩大小的缺点。因而在实际使用过程中，通常需要对上述观测方法进行改进，从而完成人机碰撞检测与类型识别。


技术实现要素：

[0009]
为了避免固定阈值检测的碰撞漏报，提高扰动观测器检测的灵敏度，在本发明的第一方面提供了一种基于动态阈值的协作机器人碰撞检测方法，包括：根据机器人动力学模型、非线性摩擦模型建立广义动量观测器，并确定其残差的时变方程；根据所述时变方程和摩擦换向误差，确定广义动量观测器残差的时变阈值边界；根据所述时变阈值边界辨识广义动量观测器动态阈值参数；实时获取比较广义动量观测器的残差输出并根据其计算所述动态阈值参数值，根据动态阈值参数值判断协作机器人是否发生碰撞。
[0010]
本发明的第二方面，提供了一种基于动态阈值的协作机器人碰撞检测系统，包括：第一确定模块，用于根据机器人动力学模型、非线性摩擦模型建立广义动量观测器，并确定其残差的时变方程；第二确定模块，用于根据所述时变方程和摩擦换向误差，确定广义动量观测器残差的时变阈值边界；辨识模块，用于根据所述时变阈值边界辨识广义动量观测器动态阈值参数；判断模块，用于实时获取比较广义动量观测器的残差输出并根据其计算所述动态阈值参数值，根据动态阈值参数值判断协作机器人是否发生碰撞。
[0011]
本发明的第三方面，提供了一种电子设备，包括：一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现本发明在第一方面提供的基于动态阈值的协作机器人碰撞检测方法。
[0012]
本发明的第四方面，提供了一种计算机可读介质，其上存储有计算机程序，其中，所述计算机程序被处理器执行时实现本发明在第一方面提供的基于动态阈值的协作机器人碰撞检测方法。
[0013]
本发明的有益效果是：
[0014]
1.由于考虑速度过零点时的摩擦换向误差，并将此因素作为补偿项参与动态阈值的边界确定中；而动态阈值通过实时跟随观测器输出，避免由换向摩擦等局部尖峰导致总体阈值过度放大的现象，从而提高了检测碰撞的可靠性和灵敏度；
[0015]
2.相比于现有基于固定阈值的碰撞检测技术，本发明动态阈值方法的输出阈值能
够跟随观测器误差进行变化，尤其能够避免碰撞检测的漏报，提升了人机协作中的安全保障性能。
附图说明
[0016]
图1为现有技术中基于能量法的碰撞检测控制框图；
[0017]
图2为现有技术中基于自干扰对消技术的扩张状态观测框图；
[0018]
图3为现有技术中基于频域分析的虚拟传感器框图；
[0019]
图4本发明的一些实施例中的基于动态阈值的协作机器人碰撞检测方法的基本流程图；
[0020]
图5本发明的一些实施例中的固定阈值和动态阈值检测示意图；
[0021]
图6本发明的一些实施例中的不同增益系数kr响应对比图；
[0022]
图7本发明的一些实施例中无碰撞时的观测器输出与动态阈值变化关系示意图；
[0023]
图8现有技术中扰动观测器固定阈值检测的波形图；
[0024]
图9本发明的一些实施例中的扰动观测器动态阈值检测的波形图；
[0025]
图10为本发明的一些实施例中的基于动态阈值的协作机器人碰撞检测系统的结构示意图；
[0026]
图11为本发明的一些实施例中的电子设备的结构示意图。
具体实施方式
[0027]
以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
[0028]
参考图4和图5，在本发明的第一方面，提供了一种基于动态阈值的协作机器人碰撞检测方法，包括：s100.根据机器人动力学模型、非线性摩擦模型建立广义动量观测器，并确定其残差的时变方程；s200.根据所述时变方程和摩擦换向误差，确定广义动量观测器残差的时变阈值边界；s300.根据所述时变阈值边界辨识广义动量观测器动态阈值参数；s400.实时获取比较广义动量观测器的残差输出并根据其计算所述动态阈值参数值，根据动态阈值参数值判断协作机器人是否发生碰撞。
[0029]
在本发明的一些实施例中的步骤s100中，根据机器人动力学模型、非线性摩擦模型建立广义动量观测器，并确定其残差的时变方程包括如下步骤：
[0030]
由于建模、辨识和采样数据等误差，实际的广义动量观测器残差可表示为：
[0031][0032]
由拉普拉斯变换可得：
[0033][0034]
其中，r、τ、z、p、f分别表示广义动量观测器的输出、力矩、位置坐标、广义动量、摩擦力；表示r、τ、z、p对应的计算或采样所得实际值，*为卷积运算；q表示关节位置参数，k表示增益系数；ε表示滤波系数，t表示时间；τm表示关节的驱动力矩。
[0035]
进一步，可以定义观测器的理论输出与实际输出误差如下：
[0036][0037]
利用机器人动力学模型，将公式(2)代入式(3)可得：
[0038][0039]
在本发明的一些实施例中的步骤s200中，所述根据所述时变方程和摩擦换向误差，确定广义动量观测器残差的时变阈值边界包括：s201.基于机器人动力学模型、非线性摩擦模型提取的参数集，将所述时变方程分离为第一误差项与第二误差项；其中：第一误差项为动力学模型误差项，第二误差项为非线性摩擦模型项；
[0040]
具体地，根据公式(4)分别定义动力学建模误差、摩擦误差：
[0041][0042]
s202.根据摩擦换向误差修正所述第二误差项；具体地，当无外力输入时，消去公式(5)中的建模误差，可得观测器残差输出为：
[0043][0044]
由于机器人动力学模型、非线性摩擦模型提取的参数集可以直观表示建模时的误差，因此可以公式(5)近似表示为：
[0045][0046]
其中，λh,λk为比例系数对角矩阵，δh(t),δf(t)为补偿项，保持等式两侧成立。考虑到摩擦部分的误差很大程度上是由于模型拟合导致的，尤以速度过零点时的换向误差为最。因此，可将摩擦建模误差调整为如下形式，重点补偿速度零点附近摩擦换向误差：
[0047][0048]
式中，为摩擦换向误差补偿，fc即摩擦模型参数，β为指数阶数，本文取定值为3。此时，摩擦建模误差可表示为：
[0049][0050]
s203.将第一误差项和第二误差项中的补偿量作为约束，确定广义动量观测器残差的时变阈值边界。具体地，针对观测器模型的输出误差，由公式(6)设定输出的时变阈值边界lim：
[0051][0052]
公式左右移项缩放，可以得到观测器输出的阈值上下界如下：
[0053]
[0054]
式中，lr为阈值下界，ur为阈值上界，δ
hk
为δh(t),δk(t)等阈值边界合并后的最大常数。
[0055]
在本发明的一些实施例中的步骤s300中，根据所述时变阈值边界辨识广义动量观测器动态阈值参数。具体地，动态阈值的系数可以通过模型输出的拟合残差中去辨识。由于模型中均为已知，因此可通过最小二乘法辨识得到其系数(参数)λh,λk，构建求解公式如下：
[0056][0057]
至于δ
hk
作为常数项边界，可以由最小二乘辨识中的最大误差所确定，从而完成动态阈值的设定。
[0058]
在本发明的一些实施例中的步骤s400中，实时获取比较广义动量观测器的残差输出并根据其计算所述动态阈值参数值，根据动态阈值参数值判断协作机器人是否发生碰撞。
[0059]
本发明将上述方法得到的动态阈值在关节速度过零点时的误差包容性进行了计算和验证。具体地，基于机器人动力学模型的传递函数，完成基于动量的扰动观测器模型构建，而增益系数kr的设定会影响扰动观测器的输出精度以及响应速度，因此先测试不同kr值对模型影响。首先基于simklink搭建扰动观测器，然后以机器人零重力时的静止模型为基准导入运动学数据，引入幅值为10n
·
m，周期0.17s的方波外力矩，分别设置三组增益系数kr为k1＝100、k2＝500、k3＝1000与原始信号对比，结果如图6所示。
[0060]
由图6分析可知，扰动观测器的不同增益系数对于外部力矩扰动最终都会响应到实际幅值附近，不过k2、k3的信号响应速度远快于系数k1，表明对于一阶传递函数模型，更高的截止频率可以得到更快的响应速度。但考虑到观测器低通滤波的性能，过高的截止频率会带来不必要的噪声，从而使观测结果恶化。考虑到k2、k3有着较为接近的响应速度，为了权衡观测器的快速响应和降噪能力，一般可设定增益系数kr＝500。
[0061]
确定扰动观测器的基础参数后，便可将其部署到协作机器人的实际运动过程。查看无碰撞发生时观测器的模型输出误差，并与提出的动态阈值对比，验证动态阈值方法跟踪输出误差的能力。以泰科机器人j2关节为例，记录观测器输出误差并使用最小二乘法对阈值系数进行辨识，可得出λh＝0.1523，λk＝1.5021，调整常数δ
hk
＝10.0760n
·
m，中的指数系数同摩擦辨识参数。
[0062]
将阈值系数代入动态阈值上下界lr,ur，可得到无碰撞时的观测器输出与动态阈值变化关系如图7所示。可以看出，观测器在无碰撞时其输出值确实存在波动，并且当关节速度换向时由于摩擦模型拟合差异，会出现显著的输出尖峰。通过动态阈值中项的指数形式对误差进行跟踪，可以有效扩大速度零点附近的阈值上限从而实现阈值的动态时变，提升了观测器检测性能。
[0063]
进一步的，对比观测器在有无碰撞时的输出，可以发现实验组在5s、11s附近有两次明显的输出力矩变化，意味着该时刻检测到外部碰撞力矩，表明了扰动观测器的对外力矩的观测能力。同时也观察到，无论是否发生碰撞，观测器都会检测到换向误差，可通过设置阈值方法进行有效规避。
[0064]
广义动量观测器作为碰撞检测中的外力矩感知工具，建模过程中受到辨识、数据采样等环节的影响，其输出会在真值附近波动。通过设定合理的阈值过滤残差扰动，便可以实现碰撞检测。固定阈值的获取方法为，令机器人各关节在无碰撞下运行足够长时间，将各轴扰动最大值增大β
fix
倍得到固定阈值，取β
fix
＝20％，阈值如下表所示：
[0065]
表2碰撞检测固定阈值参数
[0066][0067]
令机器人运行预设工作轨迹，通过扰动观测器的固定阈值方法，查看协作机器人5s、11s左右的碰撞检测情况如图8所示。当观测器输出大于固定阈值时，视为检测到碰撞发生。两次碰撞作用于机器人本体时，j6由于力臂很小的原因，导致碰撞力矩对j6关节作用较小，因此碰撞力矩主要表现在j1-j5关节。
[0068]
从图8可以看出，在使用固定阈值算法时，j2-j4等关节检测出两次碰撞，而j1、j5关节仅检测出了一次碰撞，漏报了t1时刻的碰撞。分析其原因，发现j1、j5关节的换向误差大于t1时碰撞力矩，进而导致固定阈值大于t1碰撞力矩，出现漏报。若为了检测该碰撞将固定阈值缩小，又会导致换向误差大于固定阈值，出现“误报”情况。为了改善碰撞检测时的漏报、误报情况，提升碰撞检测的有效性和灵敏度，有必要引入动态阈值方法。
[0069]
与固定阈值的获取类似，同样使机器人在无外部碰撞的状态下运行并记录观测器残差输出，代入公式(12)中进行最小二乘求解可得动态阈值参数λh,λk。再根据残差拟合的最大误差值，确定边界参数δ
hk
，得到各轴阈值参数如表3：
[0070]
表3碰撞检测动态阈值参数
[0071][0072]
将动态阈值部署到相同的碰撞输出中，查看扰动观测器的动态阈值检测情况如图9所示。可以看出在使用动态阈值算法时，对于j1-j5的两次碰撞均能准确检测，不会出现漏报。将固定阈值和动态阈值检测时间及结果对比，详见表4。
[0073]
表4固定阈值和动态阈值检测时间对比
[0074][0075][0076]
从由表4可以看出，动态阈值相较于固定阈值检测方法，平均检测用时可提前2-3
个检测周期，并且在t1时刻的j1、j5不会出现固定阈值的漏报情况。对比表2和表3可以看出，动态阈值能检测到的最小碰撞力矩均低于固定阈值方法，各轴最小力矩检测灵敏度平均可提升61％左右。综上所述，动态阈值方法的输出阈值能够跟随观测器误差进行变化，在检测碰撞的可靠性、灵敏度上更有优势，尤其能够避免碰撞检测的漏报，提升了人机协作中的安全保障性能。
[0077]
实施例2
[0078]
参考图10，本发明的第二方面，提供了一种基于动态阈值的协作机器人碰撞检测系统1，包括：第一确定模块11，用于根据机器人动力学模型、非线性摩擦模型建立广义动量观测器，并确定其残差的时变方程；第二确定模块12，用于根据所述时变方程和摩擦换向误差，确定广义动量观测器残差的时变阈值边界；辨识模块13，用于根据所述时变阈值边界辨识广义动量观测器动态阈值参数；判断模块14，用于实时获取比较广义动量观测器的残差输出并根据其计算所述动态阈值参数值，根据动态阈值参数值判断协作机器人是否发生碰撞。
[0079]
进一步的，所述第二确定模块包括：分离单元，用于基于机器人动力学模型、非线性摩擦模型提取的参数集，将所述时变方程分离为第一误差项与第二误差项；其中：第一误差项为动力学模型误差项，第二误差项为非线性摩擦模型项；修正单元，用于根据摩擦换向误差修正所述第二误差项；确定单元，用于将第一误差项和第二误差项中的补偿量作为约束，确定广义动量观测器残差的时变阈值边界。
[0080]
实施例3
[0081]
参考图11，本发明的第三方面，提供了一种电子设备，包括：一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现本发明在第一方面的方法。
[0082]
电子设备500可以包括处理装置(例如中央处理器、图形处理器等)501，其可以根据存储在只读存储器(rom)502中的程序或者从存储装置508加载到随机访问存储器(ram)503中的程序而执行各种适当的动作和处理。在ram 503中，还存储有电子设备500操作所需的各种程序和数据。处理装置501、rom 502以及ram 503通过总线504彼此相连。输入/输出(i/o)接口505也连接至总线504。
[0083]
通常以下装置可以连接至i/o接口505：包括例如触摸屏、触摸板、键盘、鼠标、摄像头、麦克风、加速度计、陀螺仪等的输入装置506；包括例如液晶显示器(lcd)、扬声器、振动器等的输出装置507；包括例如硬盘等的存储装置508；以及通信装置509。通信装置509可以允许电子设备500与其他设备进行无线或有线通信以交换数据。虽然图11示出了具有各种装置的电子设备500，但是应理解的是，并不要求实施或具备所有示出的装置。可以替代地实施或具备更多或更少的装置。图11中示出的每个方框可以代表一个装置，也可以根据需要代表多个装置。
[0084]
特别地，根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信装置509从网络上被下载和安装，或者从存储装置508被安装，或者从rom 502被安装。在该计算机程序被处理装置501执行时，执行本公开的实施
例的方法中限定的上述功能。需要说明的是，本公开的实施例所描述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(ram)、只读存储器(rom)、可擦式可编程只读存储器(eprom或闪存)、光纤、便携式紧凑磁盘只读存储器(cd-rom)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开的实施例中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本公开的实施例中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读信号介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：电线、光缆、rf(射频)等等，或者上述的任意合适的组合。
[0085]
上述计算机可读介质可以是上述电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。上述计算机可读介质承载有一个或者多个计算机程序，当上述一个或者多个程序被该电子设备执行时，使得该电子设备：
[0086]
可以以一种或多种程序设计语言或其组合来编写用于执行本公开的实施例的操作的计算机程序代码，程序设计语言包括面向对象的程序设计语言—诸如java、smalltalk、c++、python，还包括常规的过程式程序设计语言—诸如“c”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(lan)或广域网(wan)——连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
[0087]
附图中的流程图和框图，图示了按照本公开各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。需要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
[0088]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。