一种机器人碰撞响应方法与流程

1.本发明涉及碰撞检测技术领域，特别涉及一种机器人碰撞响应方法。


背景技术：

2.机器人协作在各个生产领域广泛应用。机器人存在因误操作和机器故障而与人或环境发生碰撞的可能性，为确保人机安全，碰撞检测和碰撞响应成为协作机器人不可或缺的功能模块。
3.检测到碰撞后，最为简单直观的响应策略是停止机器人，但该方法无法解决挤压式碰撞的安全问题。现有技术中的其他碰撞响应策略，如零力响应、反射控制、弹性振荡响应等，都要求将机器人的控制模式切换至力矩模式才能进行碰撞响应，由于机器人在位置模式下沿既定轨迹运行，模式切换存在一定延时。


技术实现要素：

4.本发明目的在于提供一种机器人碰撞响应方法，以解决现有技术中所存在的一个或多个技术问题，至少提供一种有益的选择或创造条件。
5.为解决上述技术问题所采用的技术方案：
6.一种机器人碰撞响应方法，包括以下步骤：
7.步骤s1：构建观测器，根据机器人的外力矩输出监测值r；
8.步骤s2：碰撞发生时，将外力矩的监测值r根据其方向和大小信息映射为关节角速度增量；
9.步骤s3：利用碰撞前一点位的角速度，计算得到碰撞后的撤离点位。
10.本发明所提供的机器人碰撞响应方法，至少具有如下的有益效果：根据外力矩监测值的方向和大小来进行碰撞响应，碰撞响应策略基于位置模式，碰撞响应时无需切换控制模式，不存在模式切换的时间延迟。
11.作为上述技术方案的进一步改进，所述步骤s2具体地：关节角速度增量如下式所示：
[0012][0013]
上式中的k
r
代表阻尼系数；
[0014]
所述步骤s3具体地：撤离点位以下式计算：
[0015][0016]
上式中，q(t)代表当前点位位置参数，q
d
(t)代表下一周期控制器下发点位，t为运行周期。
[0017]
通过上述技术方案，可以根据外力矩监测值映射为关节角速度增量，从而计算得到撤离点位，然后下发给控制器，控制机器人进行碰撞响应。
[0018]
作为上述技术方案的进一步改进，调整阻尼系数k
r
，以设定撤离距离。通过上述技术方案，撤离距离可通过设定阻尼系数k
r
来控制，撤离距离可控使得碰撞响应动作更贴合需求，机器人能够更灵活地适应不同工况。
[0019]
作为上述技术方案的进一步改进，所述步骤s1具体地：首先，构建机器人动力学模型，并辨识动力学参数；然后，建立广义动量观测器；接着，在广义动量观测器基础上串联高通滤波器，建立二阶观测器；最后，设定阈值。通过上述技术方案，在广义动量观测器传递函数模型的基础上串联高通滤波器，构成以带通滤波器为传函模型的二阶观测器，相比于传统的广义动量观测器，提高了碰撞检测的灵敏性，降低了检测延时，并且提高了高频冲击信号的检测效果，使得碰撞响应能够更好地应对冲击力。
[0020]
作为上述技术方案的进一步改进，所述机器人动力学模型的构建，具体地：利用牛顿拉格朗日法构建机器人动力学模型，模型线性化，建立回归方程，得到最小参数集；使用傅里叶级数设计激励轨迹，获取轨迹点位；控制器下发轨迹点位，采集编码器以及电机电流参数值；数据经转换以及滤波处理得到关节位置、速度、加速度参数和电机驱动力矩；基于加权最小二乘法，将采样数据代入线性回归阵，辨识动力学参数。通过上述技术方案，能够构建机器人动力学模型，为构建碰撞检测观测器了提供基础。
[0021]
作为上述技术方案的进一步改进，所述广义动量观测器的建立，具体地：列出观测器输出的监测值r与关节外力矩τ
ext
的理论关系式；推导广义动量p与τ
ext
的理论关系式；结合理论关系式推导输出信号r(t)与τ
ext
时域上的关系式。通过上述技术方案，以广义动量为中间量构建观测器，输出值直接反映外力矩变化，检测直观。
[0022]
作为上述技术方案的进一步改进，所述步骤s1.3具体地：在广义动量观测器的基础上串联高通滤波器，得到带通滤波器传函模型，结合步骤s1.2的推导过程，通过计算变换得到输出信号r(t)新的时域关系式，实现二阶观测器的构建。通过上述技术方案，以带通滤波器为传函模型构建二阶观测器，使得观测器在保持碰撞信号幅值不变的情况下，同时衰减低频段误差和高频段噪声，碰撞检测精度和高频碰撞信号检测灵敏度相较于广义动量观测器会有所提高。
[0023]
作为上述技术方案的进一步改进，所述步骤s1.4具体包括以下步骤：首先，选定增益系数k0：设定不同的k0值，采集无外力情况下，机器人沿既定轨迹重复运行时，二阶观测器的输出值；综合考虑检测延迟和灵敏性，选定合适的k0；
[0024]
然后，采集无外力情况下跑机时观测器在[0,t]时间内的输出值，根据输出值的最大值和最小值设定阈值。
[0025]
通过上述技术方案，采用控制变量法，设定一组增益系数值，采集人为施加碰撞力时的观测器输出，综合考虑检测灵敏性和检测精度，择优选取增益系数。根据输出值的最大值和最小值设定阈值，观测器能响应阻塞形式外力矩的变化，碰撞发生时，观测器输出均会产生超过阈值的尖峰，实现碰撞检测响应。
附图说明
[0026]
下面结合附图和实施例对本发明做进一步的说明；
[0027]
图1是本发明所提供的机器人碰撞响应方法，其一实施例的碰撞响应流程图；
[0028]
图2是本发明所提供的机器人碰撞响应方法，其一实施例的碰撞检测观测器构建
流程图；
[0029]
图3是本发明所提供的机器人碰撞响应方法，其一实施例的广义动量观测器模型示意图；
[0030]
图4是本发明所提供的机器人碰撞响应方法，其一实施例的带通滤波器观测器模型示意图；
[0031]
图5是本发明所提供的机器人碰撞响应方法，其一实施例的广义动量观测器的检测误差曲线图；
[0032]
图6是本发明所提供的机器人碰撞响应方法，其一实施例的带通滤波器模型的检测误差曲线图；
[0033]
图7是本发明所提供的机器人碰撞响应方法，其一实施例的不同观测器对阻塞力的检测效果对比示意图；
[0034]
图8是本发明所提供的机器人碰撞响应方法，其一实施例的不同观测器对阻塞力的检测效果对比示意图；
[0035]
图9是本发明所提供的机器人碰撞响应方法，其一实施例的不同观测器对冲击力的检测效果对比示意图；
[0036]
图10是本发明所提供的机器人碰撞响应方法，其一实施例的不同观测器对冲击力的检测效果对比示意图；
[0037]
图11是本发明所提供的机器人碰撞响应方法，其一实施例的基于位置控制的零力响应原理示意图；
[0038]
图12是本发明所提供的机器人碰撞响应方法，其一实施例的碰撞前后关节力矩监测值与指令速度变化曲线图。
具体实施方式
[0039]
本部分将详细描述本发明的具体实施例，本发明之较佳实施例在附图中示出，附图的作用在于用图形补充说明书文字部分的描述，使人能够直观地、形象地理解本发明的每个技术特征和整体技术方案，但其不能理解为对本发明保护范围的限制。
[0040]
在本发明的描述中，需要理解的是，涉及到方位描述，例如上、下、前、后、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0041]
在本发明的描述中，如果具有“若干”之类的词汇描述，其含义是一个或者多个，多个的含义是两个以上，大于、小于、超过等理解为不包括本数，以上、以下、以内等理解为包括本数。
[0042]
本发明的描述中，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。
[0043]
参照图1至图12，本发明的机器人碰撞响应方法作出如下实施例：
[0044]
一种机器人碰撞响应方法，其算法流程图如图1所示，所述机器人碰撞响应方法包括以下步骤：
[0045]
步骤一：构建观测器对机器人运动进行监测，根据机器人的外力矩输出监测值r。
[0046]
所述步骤一的算法流程图如图2所示，所述步骤一具体包括以下步骤：
[0047]
步骤s1.1：构建机器人动力学模型，并辨识动力学参数。
[0048]
具体地：首先，利用“牛顿拉格朗日法”构建机器人动力学模型；模型线性化，建立回归方程，得到最小参数集。接着，使用傅里叶级数设计激励轨迹，获取轨迹点位，控制器下发轨迹点位，采集编码器以及电机电流参数值。然后，上述数据经转换以及滤波处理得到关节位置、速度、加速度参数和电机驱动力矩。最后，基于加权最小二乘法，将采样数据代入线性回归阵，辨识动力学参数。
[0049]
步骤s1.2：建立广义动量观测器。
[0050]
具体地：首先，设定观测器输出r为关节外力矩τ
ext
的监测值，列出关系式：
[0051]
广义动量观测器采用惯性环节作为传递函数模型，观测器输出信号r与外力矩τ
ext
关系如下：
[0052][0053]
上式中，r为观测器输出，k0为增益系数，τ
ext
为关节外力矩。
[0054]
然后，推导广义动量与关节外力矩的关系：
[0055][0056]
上式中，m为惯性矩阵，c为离心力和科里奥利力矩阵，g为重力项，τ
f
为摩擦力矩，均由动力学参数辨识得到；q代表关节位置参数，τ
m
为关节驱动力矩。
[0057]
令上式改写为：
[0058][0059]
上式表明了与τ
ext
呈线性关系。
[0060]
观测器模型下，考虑动力学和摩擦模型误差，令结合机器人动力学模型可得：
[0061][0062]
上式中，参数带有上标表示该参数在观测器模型下计算得到，与理论值存在偏差。
[0063]
根据上述过程，可得出输出信号r(t)与关节外力矩τ
ext
时域关系式如下：
[0064][0065]
广义动量观测器模型如图3所示。
[0066]
步骤s1.3：在广义动量观测器基础上串联高通滤波器，建立二阶观测器：
[0067]
在广义动量观测器基础上串联高通滤波器，得到带通滤波器传函模型如下：
[0068][0069]
等式两边同时积分，得到如下关系式：
[0070]
r(t)＝∫[k1(τ
ext
‑
r)
‑
k2r
‑
∫k1k2rdt]dt
[0071]
结合上式和步骤s1.2所得的观测器输出信号r与外力矩τ
ext
表达式以及机器人运动学模型表达式，得到时域关系式：
[0072][0073]
带通滤波器观测器模型如图4所示。
[0074]
将广义动量观测器和带通滤波器观测器进行离散化，以q、τ
ext
作为输入，r作为输出，实现gmo算法以带通滤波器为传函模型的二阶观测器的算法构建。
[0075]
步骤s1.4：设定阈值：
[0076]
首先，选定增益系数k0：设定不同的k0值，采集无外力情况下，机器人沿既定轨迹重复运行时，二阶观测器的输出值；综合考虑检测延迟和灵敏性，选定合适的k0。
[0077]
然后，采集无外力情况下跑机时观测器在[0,t]时间内的输出值，根据输出值的最大值和最小值设定阈值。
[0078]
具体地：设定gmo增益系数k0＝15，带通滤波器模型系数k1＝20,k2＝5。观测器前三轴检测误差如图5至图6所示，其中图5为一阶广义动量观测器的检测误差示意图，图6为带通滤波器模型的检测误差示意图。
[0079]
观测器阈值设定如下表所示(其中gmo代表广义动量观测器，bpf代表以带通滤波器为传递函数模型的观测器)：
[0080][0081]
可知bpf模型观测器检测精度较高。
[0082]
于轴2、轴3分别施加阻塞力(阻塞力指缓慢增加且作用时间相对较长的碰撞力)，bpf与gmo检测效果对比如图7至图8所示，图7为轴2施加阻塞力时关节2的检测效果；图8为轴3施加阻塞力时关节3的检测效果(图中，虚曲线为gmo输出，实曲线为bpf输出；点划线代表gmo阈值，虚直线代表bpf阈值)。
[0083]
阻塞力检测结果对比如下表所示：
[0084]
[0085][0086]
于轴2、轴3分别施加冲击力(冲击力指幅值较大且作用时间相对较短的碰撞力)，bpf与gmo检测效果对比如图9至图10所示，图9为轴2施加冲击力时关节2的检测效果；图10为轴3施加冲击力时关节3的检测效果(图中，虚曲线为gmo输出，实曲线为bpf输出；点划线代表gmo阈值，虚直线代表bpf阈值)。
[0087]
冲击力检测结果对比如下表所示：
[0088][0089]
图9中，bpf输出产生超过阈值的尖峰，而gmo输出尖峰未超过阈值，说明bpf冲击力检测性能优于gmo；图10中，bpf的检测点较gmo提前，阈值小且幅值大，说明无论是检测灵敏性还是响应快速性，bpf均优于gmo。
[0090]
步骤二：碰撞发生时，将外力矩监测值r根据其方向和大小信息映射为关节角速度增量
[0091][0092]
上式中的k
r
代表阻尼系数；
[0093]
步骤三：利用碰撞前一点位的角速度，计算得到碰撞后的撤离点位：
[0094][0095]
上式中，q(t)代表当前点位位置参数，q
d
(t)代表下一周期控制器下发点位，t为运行周期。
[0096]
基于位置控制的零力响应原理算法框图如图11所示，调节k
r
的值，可以调整撤离距离。由上式可得，阻尼系数k
r
与撤离距离呈正相关，k
r
越大，撤离距离越大，反之则撤离距离越小。碰撞产生后，采用基于位置控制的碰撞响应策略，按照预期，机器人将沿着外力方向撤离碰撞点，撤离适当距离后停止。
[0097]
碰撞响应具体地包括以下过程：机器人沿预定轨迹运行；碰撞发生；机器人检测到碰撞；机器人响应碰撞，沿外力方向撤离碰撞点；机器人撤离适当距离后停止。
[0098]
以普通的六轴机器人为例，碰撞发生时，关节3为机器人主要撤离关节，以关节3为对象，采集碰撞前后的力矩监测值以及指令角速度，结果如图12所示。
[0099]
观测器的输出值超过设定阈值后，关节指令角速度迅速降为零，随后以梯形速度规划方式反向增加，使得机器人关节朝反方向撤离碰撞点，撤离适当距离后停止运动。检测到碰撞后，进入到碰撞响应阶段，观测器此时不再监视外力矩变化，输出恒定为零。实际上，基于位置模式的碰撞响应策略中，只需通过碰撞发生时的外力矩监测值计算得到撤离点位，然后下发给控制器即可，至于点位之间的运动速度如何规划，则交给相应的控制器来完成。
[0100]
以上对本发明的较佳实施方式进行了具体说明，但本发明并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可作出种种的等同变型或替换，这些等同的变型或替换均包含在本技术权利要求所限定的范围内。