一种基于二阶广义动量观测器的机器人碰撞检测方法与流程

1.本发明涉及碰撞检测技术领域，特别涉及一种基于二阶广义动量观测器的机器人碰撞检测方法。


背景技术：

2.机器人协作在各个生产领域广泛应用。机器人存在因误操作和机器故障而与人或环境发生碰撞的可能性，为确保人机安全，碰撞检测成为协作机器人不可或缺的功能模块。
3.碰撞检测方法主要可分为基于动力学模型的外力矩监测和无需动力学模型的外力矩监测，而基于动力学模型的外力矩监测是现阶段的主流研究方向。
4.现有技术中，最先提出的是基于能量的观测器，通过系统能量的变化来反映外力矩的变化。但该方法无法检测静止时的外力，也无法检测与机器人运动方向垂直的外力，并且监测信号直接反映的是能量而非外力矩信息。之后又提出了基于速度的外力矩观测器，该方法解决了能量观测器特定外力的无法检测问题，但由于存在惯性矩阵的逆运算，监测信号与外力矩之间是非线性的耦合关系。再之后，提出了基于广义动量的外力矩观测器，该方法成功实现解耦，观测器输出直接反映各关节的外力矩变化，计算简单无需求逆，但是存在着检测上的延迟。


技术实现要素：

5.本发明目的在于提供一种基于二阶广义动量观测器的机器人碰撞检测方法，以解决现有技术中所存在的一个或多个技术问题，至少提供一种有益的选择或创造条件。
6.为解决上述技术问题所采用的技术方案：
7.一种基于二阶广义动量观测器的机器人碰撞检测方法，其特征在于：包括以下步骤：
8.步骤s1：构建机器人动力学模型，并辨识动力学参数；
9.步骤s2：建立广义动量观测器；
10.步骤s3：确定增益系数；
11.步骤s4：针对性建立摩擦模型，并辨识模型参数；
12.步骤s5：以二阶阻尼模型建立二阶广义动量观测器；
13.步骤s6：设定阈值。
14.本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，至少具有如下的有益效果：以二阶传函模型替代一阶惯性环节，在广义动量观测器的基础上重新搭建二阶观测器；相比于一阶模型，二阶模型可调参数增加，更容易获得较短的过渡时间，增加了系统的可控性，降低了系统碰撞检测延时，实现更优的碰撞检测。而且无需使用外部传感器即可实现碰撞力的检测，降低生产成本。本发明的基于二阶广义动量观测器的机器人碰撞检测方法，相比于传统的广义动量观测器，可调参数增加，提高了碰撞检测灵敏度的同时，降低了检测延时。
15.作为上述技术方案的进一步改进，所述步骤s2具体地：设定观测器输出r为关节外力矩τ
ext
的监测值，列出关系式；推导广义动量p与τ
ext
的理论关系式；以p为中间量，结合理论关系式推导r与τ
ext
时域上的关系形式，以此来构建观测器模型。
16.作为上述技术方案的进一步改进，所述步骤s3具体地：设定不同的k1值，采集无外力情况下，机器人沿既定轨迹重复运行时的广义动量观测器的输出值，综合考虑检测延迟和灵敏性，选定合适的k1，同时大致确定高频噪声频率w
n
和低频误差频率w
e
，作为二阶模型增益系数设定依据。k1代表观测器所采用一阶惯性环节的转角频率，值的选取与实验过程中的高频噪声频率范围有关。本发明从时延和误差两方面综合考虑，从而选定增益系数值。
17.作为上述技术方案的进一步改进，所述步骤s4具体地：采集无外力情况下，实际跑机时的观测器输出值、电机驱动力矩以及关节位置，通过动力学模型计算每个点位对应的摩擦力矩或者直接使用观测器输出值；观察摩擦力矩与关节速度的关系曲线，建立适当的摩擦模型，利用最小二乘法辨识摩擦模型参数。通过上述技术方案，由采样值代入动力学模型提取摩擦力矩，根据摩擦力矩与关节角速度关系针对性建立摩擦模型，提高了辨识效果。
18.作为上述技术方案的进一步改进，所述步骤s5具体地：使用二阶阻尼模型替代惯性环节，根据步骤3中的高频噪声频率w
n
设定系统截止频率，设定阻尼比，参考步骤s2来构建二阶观测器模型。通过上述技术方案，以二阶传函模型替代一阶惯性环节，重新搭建观测器，容易获得更低的检测延迟和更高的检测精度。
19.在上述技术方案的基础上进一步地，在二阶阻尼模型基础上串联pd调节器，根据低频误差频率w
e
设定pd调节器转角频率；再参考步骤2构建观测器模型，实现碰撞信号的放大以及高频信号衰减速率的降低。通过上述技术方案，二阶阻尼模型串联pd调节器后，系统的可控性进一步增强，可方便调整超调量，能进一步提高响应快速性，还可降低采用阻尼模型引起的高频信号衰减速率过快的副作用影响，提高了碰撞检测效果。
20.作为上述技术方案的进一步改进，所述步骤s6具体地：采集无外力情况下跑机时观测器在[0,t]时间内的输出值，根据最大值和最小值设定阈值。
[0021]
作为上述技术方案的进一步改进，所述机器人碰撞检测方法，还包括步骤s7：碰撞检测结果对比；将一阶广义动量观测器、二阶阻尼模型观测器、阻尼pd调节观测器的算法配置到同一程序，采集实际跑机时各观测器的输出值，比较施加碰撞力时的检测效果。在对比实验过程中，需要进行检测结果对比，以确定各个观测器算法对于碰撞力的检测效果的优劣。
附图说明
[0022]
下面结合附图和实施例对本发明做进一步的说明；
[0023]
图1是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的碰撞检测流程图；
[0024]
图2是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的广义动量观测器模型示意图；
[0025]
图3是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例中，选取不同增益系数时的监测误差对比图；
[0026]
图4是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实
施例中，选取不同增益系数时的监测误差对比图；
[0027]
图5是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的碰撞过程的监测效果对比图；
[0028]
图6是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的碰撞过程的监测效果对比图；
[0029]
图7是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的摩擦力矩与关节角速度曲线图；
[0030]
图8是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的二阶阻尼观测器模型示意图；
[0031]
图9是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的幅频特性曲线示意图；
[0032]
图10是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的阻尼pd调节观测器模型示意图；
[0033]
图11是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的一阶广义动量观测器对机器人前三轴的监测误差曲线图；
[0034]
图12是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的二阶阻尼模型对机器人前三轴的监测误差曲线图；
[0035]
图13是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的阻尼pd模型对机器人前三轴的监测误差曲线图；
[0036]
图14是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的检测效果对比图；
[0037]
图15是本发明所提供的基于二阶广义动量观测器的机器人碰撞检测方法，其一实施例的检测效果对比图。
具体实施方式
[0038]
本部分将详细描述本发明的具体实施例，本发明之较佳实施例在附图中示出，附图的作用在于用图形补充说明书文字部分的描述，使人能够直观地、形象地理解本发明的每个技术特征和整体技术方案，但其不能理解为对本发明保护范围的限制。
[0039]
在本发明的描述中，需要理解的是，涉及到方位描述，例如上、下、前、后、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0040]
在本发明的描述中，如果具有“若干”之类的词汇描述，其含义是一个或者多个，多个的含义是两个以上，大于、小于、超过等理解为不包括本数，以上、以下、以内等理解为包括本数。
[0041]
本发明的描述中，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。
[0042]
参照图1至图15，本发明的基于二阶广义动量观测器的机器人碰撞检测方法作出
如下实施例：
[0043]
一种基于二阶广义动量观测器的机器人碰撞检测方法，其算法流程图如图1所示，所述机器人碰撞检测方法包括以下步骤：
[0044]
步骤s1：构建机器人动力学模型，并辨识动力学参数。
[0045]
具体地：首先，利用“牛顿拉格朗日法”构建机器人动力学模型；模型线性化，建立回归方程，得到最小参数集。接着，使用傅里叶级数设计激励轨迹，获取轨迹点位，控制器下发轨迹点位，采集编码器以及电机电流参数值。然后，上述数据经转换以及滤波处理得到关节位置、速度、加速度参数和电机驱动力矩。最后，基于加权最小二乘法，将采样数据代入线性回归阵，辨识动力学参数。
[0046]
基于拉格朗日动力学公式，建立机器人动力学模型如下：
[0047][0048]
上式中，q代表关节位置参数，m为惯性矩阵，c为离心力和科里奥利力矩阵，g为重力项，τ
f
为摩擦力矩，τ
m
为关节驱动力矩。
[0049]
步骤s2：建立广义动量观测器。
[0050]
具体地：首先，设定观测器输出r为关节外力矩τ
ext
的监测值，列出关系式：
[0051]
广义动量观测器采用惯性环节作为传递函数模型，观测器输出信号r与外力矩τ
ext
关系如下：
[0052][0053]
上式中，r为观测器输出，k1为增益系数，τ
ext
为关节外力矩。
[0054]
然后，推导广义动量与关节外力矩的关系：
[0055][0056]
令上式改写为：
[0057][0058]
上式表明了与τ
ext
呈线性关系。
[0059]
借助一阶惯性环节的跟随效应，可得出观测器输出信号r的表达式：
[0060][0061]
观测器模型下，考虑动力学和摩擦模型误差，令机器人动力学模型变成：
[0062][0063]
结合机器人动力学模型以及广义动量与关节外力矩的关系式，假设
[0064][0065][0066]
等式两边同时积分，构建观测器形式如下：
[0067][0068]
参照机器人动力学模型，搭建观测器模型如图2所示；由上述推导过程以及观测器模型可知，r与τ
ext
的偏差会引起动量观测值与实际值的偏差，而该偏差又将不断反馈调整监测值r，使得跟随τ
ext
变化，最终满足r≈τ
ext
。
[0069]
步骤s3：确定增益系数k1。
[0070]
增益系数k1代表观测器所采用一阶惯性环节的转角频率，值的选取与实验过程中的高频噪声频率范围有关。目前没有有效手段获取实验过程中高频噪声频率分布，故采取设定不同k1进行多组实验的方式，对比直接法，从时延和误差两方面综合考虑，选定增益系数值。
[0071]
以关节2为对象，选取不同k1值时，监测误差对比如图3和图4所示(图3为k1取0.5、5、10时的监测误差；图4为k1取15或20时的监测误差)，比较可得出：k1取值越小，误差越小；k1取值10、15时，误差基本没有变化；k1取值越大，噪声越大。k1＝20时，误差曲线基本完全覆盖k1＝15的误差曲线，噪声影响更大，检测效果降低。另一方面，这也说明高频噪声下限截止频率w
n
大致位于10～20hz的频率范围内。
[0072]
选定k1值为0.5和15，人为施加碰撞力，与直接法进行比较；对比结果如图5和图6所示；广义动量观测器(gmo)与直接法的具体检测结果见下表：
[0073][0074]
k1＝15的情况下，gmo碰撞检测时间先于直接法，而k1＝0.5的情况下则相反。k1＝15时，gmo输出信号最大幅值与阈值上限比值为3.5，直接法为2，相同环境下对于同一碰撞力，设定增益系数为15时，gmo的检测灵敏度是直接法的1.75倍；k1＝0.5时，gmo输出信号最大幅值与阈值上限比值为1.8，直接法为1.6，gmo的检测灵敏度是直接法的1.125倍。综合考虑检测灵敏度与检测延时，设定k1＝15时的gmo检测性能更优。
[0075]
综合以上因素，设定k1＝15，初步设定低频误差上限频率w
e
为5hz，高频误差下限频率w
n
为15hz。上述实验过程未对误差进行处理，仅用来确定增益系数以及高频误差大致分布，方便后续实验设定参数。
[0076]
步骤s4：针对性建立摩擦模型，并辨识模型参数。
[0077]
机器人沿激励轨迹运行，通过机器人动力学模型提取摩擦力矩，以关节1为对象，建立如下摩擦模型：
[0078][0079]
其中τ
c
为静止状态下的摩擦力，由于位置模式下，维持位姿需要恒定的驱动力矩，使得传动链上的摩擦力矩恒定，与驱动力矩方向相反；τ
s
为机器人启动时需要克服的最大静摩擦力；τ
v
为粘性摩擦项，设定如下：
[0080][0081]
待辨识参数θ＝[τ
c τ
s β
1 β
2 β3]
t
，由上式结合广义动量观测器模型可得到回归方程如下：
[0082][0083]
其中：
[0084][0085][0086][0087]
提取的摩擦力矩以及辨识结果如图7所示。
[0088]
gmo采用的摩擦模型增加了启动时关节需克服的最大静摩擦力，更符合机器人实际运行情况。由于图7中粘性摩擦力并非随着速度严格线性增长，通过增加粘滞摩擦项关节角速度阶次，可使辨识结果更加逼近实际采样数据，提高辨识效果。
[0089]
另外，在无外力作用的情况下，通过机器人动力学模型提取出的摩擦力矩包含有动力学模型误差：
[0090][0091]
将重新辨识的摩擦力矩回代至原动力学模型，可实现部分模型误差的消除。
[0092]
步骤s5：建立二阶广义动量观测器，并在其基础上串联pd调节器。
[0093]
具体地：以二阶阻尼系统作为广义动量观测器传函模型：
[0094][0095]
令k1＝w
n2
,k1k2＝2ξw
n
：
[0096][0097]
等式两边同时积分：
[0098]
r(t)＝k1∫[∫(τ
ext
‑
r)dt
‑
k2r]dt
[0099]
结合上式和步骤s2所得的观测器输出信号r与外力矩τ
ext
表达式以及机器人运动学模型表达式，得到时域关系式：
[0100][0101]
二阶阻尼观测器模型如图8所示，可知二阶阻尼观测器模型的可调参数增加，系统可控性更高。当阻尼ξ为0.707时，幅频特性曲线在低频段近似于直线，系统响应快速性和振荡稳定性能均衡最优。相比于一阶广义动量观测器，该系统容易获得更低的检测延迟和更高的检测精度，但同时也增加了高频衰减速率，导致高频信号检测效果降低，其幅频特性如图9所示。
[0102]
在二阶阻尼模型基础上串联pd调节器，利用pd调节器20db/dec斜率，根据低频误差上限频率设定转角频率，降低高频衰减速率的同时可实现碰撞信号的放大，幅频特性如图9所示，系统传函模型如下：
[0103][0104]
结合上述推导过程，得到时域关系式：
[0105][0106]
阻尼pd调节观测器模型如图10所示。
[0107]
步骤s6：设定观测器阈值：
[0108]
参考低频误差上限频率w
e
为5hz，高频误差下限频率w
n
为15hz，设定观测器系数如下表所示：
[0109]
类型k1k2k3一阶广义动量观测器15
ꢀꢀ
二阶阻尼模型2250.0943 阻尼系统pd调节74.33430.16400.1429
[0110]
观测器对前三轴的检测误差如图11至13所示(图11为一阶广义动量观测器的监测误差；图12为二阶阻尼模型的监测误差；图13为阻尼pd模型的监测误差；其中实线为监测值，虚线为阈值)；
[0111]
各观测器阈值设定下表所示：
[0112][0113][0114]
步骤s7：于轴2、轴3分别施加碰撞力，对比二阶观测器与一阶观测器检测效果。
[0115]
于轴2施加碰撞力时关节2的检测效果对比如图14所示，于轴3施加碰撞力时关节3的检测效果对比如图15所示。两个子图分别代表着阻尼模型、阻尼系统pd调节。图中，虚曲线为一阶广义动量观测器输出，实曲线为二阶观测器输出；点划线代表一阶广义动量观测器阈值，虚直线代表二阶观测器阈值，检测尖峰处局部放大使结果更清晰。
[0116]
一阶、二阶观测器碰撞力检测结果对比如下表所示(其中gmo代表一阶广义动量观测器，damp代表二阶阻尼模型，pd代表阻尼系统pd调节观测器)：
[0117][0118]
由对比结果可知，所设计的观测器算法均能响应外力矩的变化，碰撞发生时，观测器输出均会产生超过阈值的尖峰，说明了算法可用来实现碰撞检测功能。若以观测器输出信号最大幅值与阈值上限的比值作为衡量观测器碰撞检测灵敏度的标准，由上表可知，二阶观测器碰撞力检测灵敏度均优于gmo，其中阻尼模型观测器检测灵敏度最高。若以观测器输出信号初次超过阈值的点位作为碰撞检测点，由上表可知，二阶观测器均先于gmo检测到碰撞。综上所述，二阶算法相较于一阶算法对于碰撞力的检测效果均有所提高。
[0119]
以上对本发明的较佳实施方式进行了具体说明，但本发明并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可作出种种的等同变型或替换，这些等同的变型或替换均包含在本技术权利要求所限定的范围内。