CN106774181A - 基于阻抗模型的高精度牵引示教机器人的速度控制方法 - Google Patents

Abstract

本发明公开了一种基于阻抗模型的高精度牵引示教机器人的速度控制方法，包括以下步骤：S1：机器人控制器采集六维力传感器的信息，首先对采集到的信息进行滤波处理，然后进行重力补偿，最后得到与期望力或者期望力矩值的偏差量数据；S2：根据阻抗模型将力的偏差量数据或者力矩值的偏差量数据转变为机器人末端在笛卡尔空间中移动的速度和绕轴旋转的角速度；S3：根据变形的S型速度控制曲线对运动进行平滑插补，求得对应的位置函数、速度函数和加速度函数；S4：根据逆运动学进而求得关节空间中的关节角度函数；S5：将关节角度函数进行关节空间的等时同步插补后通过控制器的总线发送至伺服驱动器，进而控制机器人的动作。本发明有效提高了牵引的精度。

Description

基于阻抗模型的高精度牵引示教机器人的速度控制方法

技术领域

本发明涉及工业机器人领域，特别是涉及基于阻抗模型的高精度牵引示教机器人的速度控制方法。

背景技术

手动牵引示教机器人操作是人机协作中不可或缺的一个环节，是指示教人员在笛卡尔空间中或是在关节空间中拖动机器人运动到示教点的操作。牵引示教时，机器人末端或关节需实时并精确地跟踪人拖动的意图方向，并使得机器人能够做出期望的运动状态。传统牵引示教机器人的两种直观的解决方案：基于现有的控制器二次开发接口做导纳控制或是基于力矩模式做无力矩传感器的零力拖动示教。然而，这两种方案存在以下缺点：①由于接口开发程度低，只有部分的接口函数可用，例如点到点的运动指令(MovJ或是MoveL)，无法对机器人运动过程中的速度和加速度进行控制；②基于现有的控制器二次开发接口，只能建立力偏差F与位移量x的关系，因此在每个力控周期内机器人末端都会经历“加速-匀速-减速”或是“加速-减速”的过程，因此会发现在牵引时机器人末端的运动不平滑，牵引效果表现出实时牵引效果差和牵引精度低。③基于零力控制的无力矩传感器的拖动示教而言牵引精度低，需要依赖于精确的动力学模型，况且在低速拖动时，有很多非线性的因素影响了拖动的精度，在无力矩传感器的情况下很难做到精确的牵引示教，因此也很难用在实际的工业现场。

发明内容

发明目的：本发明的目的是提供一种能够解决现有技术中存在的缺陷的基于阻抗模型的高精度牵引示教机器人的速度控制方法。

技术方案：为达到此目的，本发明采用以下技术方案：

本发明所述的基于阻抗模型的高精度牵引示教机器人的速度控制方法，包括以下步骤：

S1：机器人控制器采集六维力传感器的信息，首先对采集到的信息进行滤波处理，然后进行重力补偿，最后得到与期望力或者期望力矩值的偏差量数据；

S2：根据阻抗模型将力的偏差量数据或者力矩值的偏差量数据转变为机器人末端在笛卡尔空间中移动的速度和绕轴旋转的角速度；

S3：根据变形的S型速度控制曲线对运动进行平滑插补，求得对应的位置函数、速度函数和加速度函数；

S4：根据逆运动学进而求得关节空间中的关节角度函数；

S5：将关节角度函数进行关节空间的等时同步插补后通过控制器的总线发送至伺服驱动器，进而控制机器人的动作。

进一步，所述步骤S1中，机器人控制器采集六维力传感器的信息是在指定力控周期通过UDP通讯口读取六维力传感器的力和力矩信息。

进一步，所述步骤S2中，机器人末端在笛卡尔空间中移动的速度和绕轴旋转的角速度组成的矩阵用V_i表示，V_i为6*1的矩阵，矩阵V_i中前三行是机器人末端在笛卡尔空间中移动的速度，后三行是机器人末端在笛卡尔空间中绕轴旋转的角速度，V_i为：

式(1)中，F_zmax表示能够承受的最大六维力，V_max表示允许的机器人末端最大的速度，F_d表示期望跟踪的六维力，F为力的偏差量，f为单调函数，S表示对角线函数为0或1的6*6的对角矩阵。

进一步，所述步骤S3包括以下步骤：

S3.1：根据式(2)求得位置函数θ(t)、速度函数v(t)和加速度函数a(t)：

式(2)中，θ_i为初始位置，v_i为初始速度，v_i+1为期望的终点速度，t为归一化的时间，T为机器人控制器与伺服驱动器的通讯周期；

S3.2：判断期望的终点速度v_i+1与初始速度v_i之差的绝对值是否超过V_max：如果超过V_max，则进行步骤S3.3；否则，结束；

S3.3：判断v_i是否大于v_i+1：如果是，则根据式(3)重新计算位置函数θ(t)、速度函数v(t)和加速度函数a(t)；如果不是，则根据式(4)重新计算位置函数θ(t)、速度函数v(t)和加速度函数a(t)；

进一步，所述步骤S5中，控制器的总线为EtherCAT总线。

进一步，所述机器人控制器从上到下分为六个层次，即用户层、六维力信号采集和处理层、阻抗控制层、速度控制层、关节插补层和EtherCAT总线通讯层；其中，用户层、六维力信号采集和处理层、阻抗控制层对外开放，速度控制层、关节插补层和EtherCAT总线通讯层不对外开发。

进一步，所述用户层提供用户二次开发的用户交互接口，六维力信号采集和处理层提供用户二次开发的六维力采集和处理接口，阻抗控制层提供用户二次开发的阻抗控制接口。

有益效果：与现有技术相比，本发明具有如下的有益效果：

1)改善牵引的实时跟随效果；相比于传统的基于导纳控制的牵引而言，本发明可以实现实时的跟踪效果，且机器人跟随时运动平滑；

2)提供开放式的二次开发接口；为用户开放的接口包括：任务层的用户交互接口、阻抗层的模型接口，用户可根据需求自己修改柔性参数和修改阻抗模型；

3)提高了牵引的精度；相比于传统的基于零力控制的无力矩传感器的拖动示教而言牵引精度更高，无力矩传感器拖动示教的问题是力矩虽然经过滤波后值是相对稳定的，但是从伺服驱动器中读取的力矩值精度不高，因为没有力矩传感器做基准，因此只能达到拖动示教的趋势但是不能达到高精度的牵引示教。

附图说明

图1为本发明具体实施方式的牵引示教机器人的速度控制架构图；

图2为本发明具体实施方式的手动牵引示教下单位力控周期的流程图；

图3为本发明具体实施方式的阻抗控制的框图；

图4为本发明具体实施方式的基于阻抗模型的F与v的对应关系图；

图5为本发明具体实施方式的基于变形S型曲线的速度控制示意图；

图6为本发明具体实施方式的基于速度控制曲线的位置、速度和加速度示意图。

具体实施方式

下面结合具体实施方式对本发明的技术方案作进一步的介绍。

本具体实施方式公开了一种基于阻抗模型的高精度牵引示教机器人的速度控制方法，包括以下步骤：

S1：机器人控制器采集六维力传感器的信息，首先对采集到的信息进行滤波处理，然后进行重力补偿，最后得到与期望力或者期望力矩值的偏差量数据；

S2：根据阻抗模型将力的偏差量数据或者力矩值的偏差量数据转变为机器人末端在笛卡尔空间中移动的速度和绕轴旋转的角速度；

S3：根据变形的S型速度控制曲线对运动进行平滑插补，求得对应的位置函数、速度函数和加速度函数；

S4：根据逆运动学进而求得关节空间中的关节角度函数；

S5：将关节角度函数进行关节空间的等时同步插补后通过控制器的总线发送至伺服驱动器，进而控制机器人的动作。

其中，机器人控制器从上到下分为六个层次，如图1所示，即用户层、六维力信号采集和处理层、阻抗控制层、速度控制层、关节插补层和EtherCAT总线通讯层；其中，1)用户层主要用于与用户接口进行交互，例如与示教盒交互，该层的作用是根据用户需求设定相关的参数，如机器人柔性参数、机器人控制模式等等。2)六维力信号采集和处理层主要用于六维力信号的采集、滤波、重力补偿等。3)阻抗控制层主要建立六维力偏差值与机器人末端运动的关系，本发明中建立了力偏差值F与机器人末端在笛卡尔空间中运动速度v的变换关系，具体的关系式的示意图如图3所示，采用的模型为阻抗控制模型。4)速度控制层主要是根据图3所建立的模型关系在笛卡尔空间中实现基于速度曲线的平滑控制，是机器人末端在笛卡尔空间中基于阻抗模型的最终行为表现：实时跟踪效果好和控制精度高。5)关节插补层主要是在关节空间中对各轴进行等时插补同步规划，机器人最终的行为表现为运动连续且平滑。6)EtherCAT总线通讯层主要完成关节角度值向脉冲量的转换，并通过EtherCAT主站将该脉冲转换量发送给伺服从站，同时接收来自伺服从站的状态信息，包括状态值、当前位置值、速度值、加速度值等。

机器人控制器中最核心的层为：力信号采集和处理层、阻抗层和速度控制层，此三层通称为力控包。1)力信号采集和处理结果影响了牵引的精度，力信号一般通过滤波的方法去除抖动信号，然后对机器人在各种位姿下进行重力补偿。2)阻抗层的核心为阻抗模型，用户可根据需求自己设定的线性函数变换或是二次多项式函数变换，但是需满足单调函数的要求。3)速度控制层是四次多项式的S型变形速度规划，在指定的单位力控周期内对速度进行平滑插补，使机器人末端运动光滑且保证实时性。

在图1中，开放式控制器中的“开放式”体现在：给用户提供二次开发的接口，包括三个开放的接口：用户交互接口(User Interface，UI)、六维力采集和处理接口(Force/Torque Acquire Interface，FTAI)和阻抗控制接口(Impedance Control Interface，ICI)，其它的层(速度控制层、关节插补层和通讯层)涉及具体的控制实现，因此不对外开放。

本具体实施方式基于现有的自主研发的开放式控制器，分析了手动牵引示教的本质特性，包括人牵引机器人时的行为特性和机器人在跟随过程中需满足的特点。图2为手动牵引示教下单位力控周期的流程图。

人的行为具有以下特征：

1)行为的离散性：牵引行为是离散且不连续的，可能随时牵引，也可能随时放开；

2)力的时变性：牵引力是时变的，可能某一时刻力大，某一时刻力小。

结合力牵引时机器人的物理特性和人的行为特征，为达到好的跟随效果，机器人跟随时需满足如下特点：

1)连续性：机器人的运动应该连续和平滑，防止速度突变或是加速度突变；

2)精确性：机器人时刻跟随到人所赋予的牵引力，精确性主要体现在松开的一瞬间，机器人需以最小的距离停止；

3)实时性：能够实时的根据力反馈量做出响应，例如当力大时，以相对较大的速度进行跟随，当力小时，以相对较小的速度进行跟随；

4)安全性：机器人的最大运动速度和加速度应满足人机协作时的国际安全规范。

根据上述的分析结果，分析出如下约束条件：力偏差F变大时，机器人的运动速度v随之变大；力偏差F变小时，机器人的运动速度v随之变小；当力偏差F为0时，机器人应该静止，即速度v等于0，对应的约束表达式如下所示：

式(1)中，F_zmax表示能够承受的最大六维力，V_max表示允许的机器人末端最大的速度，F_d表示期望跟踪的六维力，v_i为初始速度。

根据上述的分析可知，需要建立了力偏差F与机器人运动速度v的变换关系，以单轴为例，具体的关系式如下所示：

此模型根据用户需求进行设定，如说明附图4所示，但是一定要满足单调函数关系。将单轴扩展为六轴，对应的表达式如下所示：

式(3)中，V_i为机器人末端在笛卡尔空间中移动的速度和绕轴旋转的角速度组成的矩阵，V_i为6*1的矩阵，矩阵V_i中前三行是机器人末端在笛卡尔空间中移动的速度，后三行是机器人末端在笛卡尔空间中绕轴旋转的角速度；f为单调函数，S表示对角线函数为0或1的6*6的对角矩阵。V_i、V_max、F、F_d、F_zmax和S均为六维度的矩阵，S为一对角矩阵，指定对角线元素只能为0或是1。元素设置为0表示该方向上的位置偏移量不受力偏差信号的影响，即该方向不被牵引；元素设置为1表示该方向力牵引有效。用户可根据实际需求进行设定，比如设定S的对角线为[0 0 1 0 0 0]，表示只有z轴方向受到力的约束，即只有z方向的牵引，若设定其对角线为[1 1 1 1 1 1]，表示六个维度均受到力的约束，即可以实现六个维度的牵引。

目前控制器与驱动器之间的通讯周期为1ms或是4ms，以1ms或是4ms为基准控制周期定义合适的时间片来实现一个力控周期的控制，定义单位时间片时需考虑以下三点：①与力传感器的传输频率匹配；②机器人加速段或是减速段平滑；③能够在指定的时间和电机允许的加速度范围内加速到指定的速度。综合上述三点，定义单位时间片为20个控制周期，即20ms或是80ms为一个时间片。在控制器底层保证了力控周期足够短，从而可以实现实时的牵引。

已知初始位置θ_i、初始速度v_i、初始加速度(a_i＝0)、期望的终点速度v_i+1和期望的终点加速度(a_i+1＝0)建立如下的四次多项式方程：

其中，w₀,w₁,w₂,w₃,w₄为四次多项式的系数，根据“某一时刻末的速度与下一时刻初的速度连续”的约束条件得到对应如下所示的约束方程。

为实现平滑的牵引效果，需要对速度进行四次多项式的S型插补规划，具体如说明附图5所示，根据运动过程中的速度连续、初始位置(θ_i)、初始速度(v_i)、初始加速度(a_i＝0)、期望的终点速度(v_i+1)和期望的终点加速度(a_i+1＝0)等约束条件，确定四次多项式的系数，并推导出对应的位置、速度和加速度的函数，具体表达式如下所示：

式(6)中，θ_i为初始位置，v_i为初始速度，v_i+1为期望的终点速度，t为归一化的时间，T为机器人控制器与伺服驱动器的通讯周期。

由说明书附图中可知，曲线①～⑤为(i+1)时刻5种典型的速度曲线图，但是⑥和⑦属于两种特殊情况：

对于曲线⑥而言，意味着力F反向，此时需要对初始速度与期望的终点速度进行比较，保证最大的速度变化量不超过Vmax，若超过Vmax，此时变化速度变化量需要按Vmax重新计算，对应的位置函数、速度函数和加速度函数如下所示。

对于曲线⑦而言，在时刻i机器人末端的运动速度v_i达到最大速度v_max，此时人手突然放开，下一个时刻i+1机器人末端的期望运动速度为0，此时机器人以最大的加速度减速，其对应的移动位置量即为手动牵引示教机器人时的最大误差，当人手并非突然放开而是平滑放开时，手动牵引示教机器人时的误差可以达到0。

此外，本具体实施方式还在机器人末端安装六维力传感器，因为在无外界测量仪器的前提下很难找到基准值，导致结果是牵引精度低，因此在无力矩传感器的情况下零力拖动并不适用在实际工业应用中。

在实际的手动牵引过程时，需考虑人机协作安全过程中的最大速度、最大加速度等参数。结合国内外众多人机协作安全和危险测试的研究结果，机器人的运动最大速度应该结合工业机器人的功率和负载而定，建议5Kg负载以下的机器人最大速度为0.25m/s，5Kg-10Kg负载的机器人最大的运行速度为0.2m/s，10Kg-20Kg负载的工业机器人的最大运行速度为0.1m/s，20Kg负载以上的工业机器人不建议采用牵引的方式进行示教。

本发明以ESTUN ER16工业机器人为实验对象，其负载为16Kg，因此限定其最大的运行速度为0.1m/s，下面给出其对应的最大的加速度和单位时间内的最大位移结果，如图6所示。由图6可知，最大的加速度为0.15m/s²，单位时间内的最大位移为0.05m。

Claims (7)

1.基于阻抗模型的高精度牵引示教机器人的速度控制方法，其特征在于：包括以下步骤：

S1：机器人控制器采集六维力传感器的信息，首先对采集到的信息进行滤波处理，然后进行重力补偿，最后得到与期望力或者期望力矩值的偏差量数据；

S2：根据阻抗模型将力的偏差量数据或者力矩值的偏差量数据转变为机器人末端在笛卡尔空间中移动的速度和绕轴旋转的角速度；

S3：根据变形的S型速度控制曲线对运动进行平滑插补，求得对应的位置函数、速度函数和加速度函数；

S4：根据逆运动学进而求得关节空间中的关节角度函数；

S5：将关节角度函数进行关节空间的等时同步插补后通过控制器的总线发送至伺服驱动器，进而控制机器人的动作。

2.根据权利要求1所述的基于阻抗模型的高精度牵引示教机器人的速度控制方法，其特征在于：所述步骤S1中，机器人控制器采集六维力传感器的信息是在指定力控周期通过UDP通讯口读取六维力传感器的力和力矩信息。

3.根据权利要求1所述的基于阻抗模型的高精度牵引示教机器人的速度控制方法，其特征在于：所述步骤S2中，机器人末端在笛卡尔空间中移动的速度和绕轴旋转的角速度组成的矩阵用V_i表示，V_i为6*1的矩阵，矩阵V_i中前三行是机器人末端在笛卡尔空间中移动的速度，后三行是机器人末端在笛卡尔空间中绕轴旋转的角速度，V_i为：

$V_i=S\·f\left(\frac{V_{\max }\·(F-F_d)}{F_{zmax}}\right)---\left(1\right)$

式(1)中，F_zmax表示能够承受的最大六维力，V_max表示允许的机器人末端最大的速度，F_d表示期望跟踪的六维力，F为力的偏差量，f为单调函数，S表示对角线函数为0或1的6*6的对角矩阵。

4.根据权利要求1所述的基于阻抗模型的高精度牵引示教机器人的速度控制方法，其特征在于：所述步骤S3包括以下步骤：

S3.1：根据式(2)求得位置函数θ(t)、速度函数v(t)和加速度函数a(t)：

$\left\{\begin{array}{c}\mathrm{\θ}\left(t\right)={\mathrm{\θ}}_i+v_i\·t+\frac{v_{i+1}-v_i}{T^2}\·t^3-\frac{v_{i+1}-v_i}{2\·T^3}\·t^4\\ v\left(t\right)=v_i+3\·\frac{v_{i+1}-v_i}{T^2}\·t^2-2\·\frac{v_{i+1}-v_i}{T^3}\·t^3\\ a\left(t\right)=6\·\frac{v_{i+1}-v_i}{T^2}\·t-6\·\frac{v_{i+1}-v_i}{T^3}\·t^2\end{array}\right.---\left(2\right)$

式(2)中，θ_i为初始位置，v_i为初始速度，v_i+1为期望的终点速度，t为归一化的时间，T为机器人控制器与伺服驱动器的通讯周期；

S3.2：判断期望的终点速度v_i+1与初始速度v_i之差的绝对值是否超过V_max：如果超过V_max，则进行步骤S3.3；否则，结束；

S3.3：判断v_i是否大于v_i+1：如果是，则根据式(3)重新计算位置函数θ(t)、速度函数v(t)和加速度函数a(t)；如果不是，则根据式(4)重新计算位置函数θ(t)、速度函数v(t)和加速度函数a(t)；

$\left\{\begin{array}{c}\mathrm{\θ}\left(t\right)={\mathrm{\θ}}_i+v_i\·t+\frac{v_i-V_{max}-v_i}{T^2}\·t^3-\frac{v_i-V_{max}-v_i}{2\·T^3}\·t^4\\ v\left(t\right)=v_i+3\·\frac{v_i-V_{max}-v_i}{T^2}\·t^2-2\·\frac{v_i-V_{max}-v_i}{T^3}\·t^3\\ a\left(t\right)=6\·\frac{v_i-V_{\max }-v_i}{T^2}\·t-6\·\frac{v_i-V_{max}-v_i}{T^3}\·t^2\end{array}\right.---\left(3\right)$

$\left\{\begin{array}{c}\mathrm{\θ}\left(t\right)={\mathrm{\θ}}_i+v_i\·t+\frac{v_i-V_{max}-v_i}{T^2}\·t^3-\frac{v_i-V_{max}-v_i}{2\·T^3}\·t^4\\ v\left(t\right)=v_i+3\·\frac{v_i-V_{max}-v_i}{T^2}\·t^2-2\·\frac{v_i-V_{max}-v_i}{T^3}\·t^3\\ a\left(t\right)=6\·\frac{v_i-V_{\max }-v_i}{T^2}\·t-6\·\frac{v_i-V_{max}-v_i}{T^3}\·t^2\end{array}\right.---\left(4\right).$

5.根据权利要求1所述的基于阻抗模型的高精度牵引示教机器人的速度控制方法，其特征在于：所述步骤S5中，控制器的总线为EtherCAT总线。

6.根据权利要求1所述的基于阻抗模型的高精度牵引示教机器人的速度控制方法，其特征在于：所述机器人控制器从上到下分为六个层次，即用户层、六维力信号采集和处理层、阻抗控制层、速度控制层、关节插补层和EtherCAT总线通讯层；其中，用户层、六维力信号采集和处理层、阻抗控制层对外开放，速度控制层、关节插补层和EtherCAT总线通讯层不对外开发。

7.根据权利要求6所述的基于阻抗模型的高精度牵引示教机器人的速度控制方法，其特征在于：所述用户层提供用户二次开发的用户交互接口，六维力信号采集和处理层提供用户二次开发的六维力采集和处理接口，阻抗控制层提供用户二次开发的阻抗控制接口。