机械臂的拖动补偿方法、拖动补偿装置

技术领域

本发明涉及机械臂技术领域，具体涉及一种机械臂的拖动补偿方法、机械臂的拖动补偿方法。

背景技术

拖动示教又称直接示教，是指通过人工拖动使机械臂（机器人）末端按照需要的轨迹运动，然后机器人记录下运动的点，复原示教运动的过程。拖动示教操作灵活，更加直观，操作者可以直接迅速的使机器人记录下工作点位，大大提高了示教效率，拖动示教在机器人领域具有广泛的应用场景。

目前，拖动示教中机械臂的控制方式主要为两种：基于电机电流和基于末端六维力传感器。基于电机电流为零力模式，依赖动力学和摩擦模型计算出机器人所需要的补偿电流，该方式中，源于模型参数辨识误差和速度、加速度估计误差及信号噪声等，导致补偿电流的计算误差较大，最终导致下发给驱动器的补偿电流出现偏差。

基于末端六维力传感器为力控模式，由传感器力信息及闭环力控架构计算出机器人关节角位置或速度，该方式然需要传感器作支撑，产品成本非常高。

发明内容

本发明为解决上述技术问题，提供了一种机械臂的拖动补偿方法，该方法可以使下发给驱动器的补偿电流更为准确，并增强了安全系数，无需外部设备，在控制成本的同时提升了拖动性能。

本发明还提供了一种机械臂的拖动补偿装置。

本发明采用的技术方案如下：

本发明的第一方面实施例提出了一种机械臂的拖动补偿方法，包括以下步骤：设置机械臂的驱动器模式和参数；对机械臂的运动状态进行滤波处理，得到滤波后的运动状态，所述运动状态包括：机械臂关节的速度、加速度、位置和温度；根据所述运动状态计算机械臂的拖动补偿电流，所述拖动补偿电流包括：重力补偿电流、摩擦补偿电流及阻抗补偿电流；根据机械臂的拖动位置限制、速度限制和力矩限制修正所述拖动补偿电流，，以获取机械臂的拖动驱动电流；根据所述拖动驱动电流驱动所述机械臂。

本发明上述提出的机械臂的拖动补偿方法还具有如下附加技术特征：

根据本发明的一个实施例，根据当前机械臂关节的位置，依赖机械臂的动力学模型计算得重力补偿电流。

根据本发明一个实施例，具体根据以下公式获取摩擦模型（1）-（4）：

（1）；

（2）；

（3）

（4）；

其中，v为关节速度，t为关节温度，τ

根据本发明的一个实施例，在零速区间内，通过以下公式（5）获取所述摩擦模型：

（5）

其中，

根据本发明的一个实施例，根据当前机械臂的各关节位置、速度和加速度，以及机械臂关节所在二阶系统的阻抗参数（M-D-K）计算阻抗补偿电流。

根据本发明的一个实施例，根据机械臂的拖动位置限制、速度限制和力矩限制修正所述拖动补偿电流，具体包括：获取所述机械臂关节所处的位置区间，根据机械臂关节所处的位置区间获取机械臂位置限制阻尼和位置限制刚度，根据位置限制阻尼和位置限制刚度计算拖动位置限制电流；获取所述机械臂关节所处的速度区间，根据机械臂关节所处的速度区间获取机械臂的速度限制阻尼，根据所述速度限制阻尼计算拖动速度限制电流；根据拖动补偿电流、拖动位置限制电流和拖动速度限制电流计算电机驱动电流，根据电机驱动电流获取电机驱动力矩；根据所述机械臂的速度、电机驱动力矩和理论力矩获取电机驱动力矩；根据调整后的电机驱动力矩获取机械臂的拖动驱动电流。

根据本发明的一个实施例，获取所述机械臂关节所处的位置区间，根据机械臂关节所处的位置区间获取机械臂位置限制阻尼和位置限制刚度，根据位置限制阻尼和位置限制刚度计算拖动位置限制电流，具体包括：根据机械臂的关节位置获取机械臂所处的位置区间，所述位置区间包括：正常拖动位置区间、过渡位置区间和极限位置区间；如果所述机械臂所处的位置区间为正常拖动位置区间，则位置限制阻尼为0，所述拖动位置限制电流为0；如果所述机械臂所处的位置区间为过渡位置区间，则位置限制刚度为0，位置限制阻尼与械臂所处的位置的变化率为一次函数，根据所述一次函数和机械臂所处的位置获取位置限制阻尼，根据以下公式（1）获取拖动位置限制电流

（6）

其中，

根据本发明的一个实施例，获取所述机械臂关节所处的速度区间，根据机械臂关节所处的速度区间获取机械臂的速度限制阻尼，根据所述速度限制阻尼计算拖动速度限制电流，具体包括：根据机械臂关节速度获取机械臂关节所处的速度区间，所述速度区间包括：正常拖动速度区间、过渡速度区间和极限速度区间；如果所述机械臂关节所处的速度区间为正常速度位置区间，则速度限制阻尼为0，所述拖动速度限制电流为0；如果所述机械臂关节所处的速度区间为过渡速度区间，则速度限制阻尼与机械臂关节的速度的变化率为一次函数，根据所述一次函数和机械臂关节的速度获取速度限制阻尼，根据以下公式（7）获取拖动速度限制电流；如果所述机械臂关节所处的速度区间为极限速度区间，则速度限制阻尼与机械臂关节的速度的变化率为三次多项式，根据所述三次多项式和机械臂关节的速度获取速度限制阻尼，根据以下公式（7）获取拖动速度限制电流；

（7）

其中，

根据本发明的一个实施例，根据所述机械臂的速度、电机驱动力矩和理论力矩获取电机驱动力矩，具体包括：如果机械臂关节的速度为正且理论力矩为负，当电机驱动力矩>理论力矩时，将电机驱动力矩调整为理论力矩；如果机械臂关节的速度为负且理论力矩为正，当电机驱动力矩<理论力矩时，将电机驱动力矩调整为理论力矩；如果机械臂关节的速度为正且理论力矩为正，当电机驱动力矩>理论力矩，将电机驱动力矩调整为理论力矩；如果机械臂关节的速度为负且理论力矩为负，当电机驱动力矩<理论力矩时，将电机驱动力矩调整为理论力矩；当所述机械臂不满足以上四种条件时，将电机驱动力矩调整为电机驱动力矩。

本发明的第二方面实施例提出了一种机械臂的拖动补偿装置，包括：设置模块，所述设置模块用于设置机械臂的驱动器模式和参数；预处理模块，所述预处理模块用于对机械臂的运动状态进行滤波处理，得到滤波后的运动状态，所述运动状态包括：机械臂关节的速度、加速度、位置和温度；补偿模块，所述补偿模块用于根据所述运动状态计算机械臂的拖动补偿电流，所述拖动补偿电流包括：重力补偿电流、摩擦补偿电流及阻抗补偿电流；修正模块，所述修正模块用于根据机械臂的拖动位置限制、速度限制和力矩限制修正所述拖动补偿电流,，以获取机械臂的拖动驱动电流；驱动模块，所述驱动模块用于根据所述拖动驱动电流驱动所述机械臂。

本发明的有益效果：

1、本发明综合考虑机械臂的重力、摩擦以及阻抗计算机械臂的补偿电流，并根据拖动位置限制、速度限制和力矩限制进一步修正补偿电流，从而可以使下发给驱动器的补偿电流更为准确，并且增强了安全系数，以及无需传感器或使能按钮等外部设备，在控制成本的同时提升了拖动性能；

2、针对0速摩擦力计算时存在速度符号判断不准问题做了特殊处理，进一步提高了补偿电流的准确性。

附图说明

图1是根据本发明一个实施例的机械臂的拖动补偿方法的流程图；

图2是根据本发明一个实施例的拖动补偿电流的修正流程图；

图3是根据本发明一个实施例的位置空间的示意图；

图4是根据本发明一个实施例的速度空间的示意图；

图5是根据本发明一个实施例的机械臂的拖动补偿装置的方框示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1是根据本发明一个实施例的机械臂的拖动补偿方法的流程图，如图1所示，该方法包括以下步骤：

S1，设置机械臂的驱动器模式和参数。

具体地，在进行拖动示教时，驱动器模式设置为电流模式；参数共分为两部分：一是基础参数，例如负载和机械臂的本体动力学参数、摩擦参数、力矩系数等；二是固定参数，如滤波参数、阻抗参数、安全防护参数等，阻抗参数中的阻尼可用来调节拖动手感。

S2，对机械臂的运动状态进行滤波处理，得到滤波后的运动状态，运动状态包括：机械臂关节的速度、加速度、位置和温度。

具体地，本发明中，运动状态的滤波在驱动层（4KHz）实现，具体可以采用滑动平均+低通滤波器的方法。滤波后运动状态可以降低噪声对摩擦力的影响，进而可以提高下述的摩擦补偿电流获取的准确性。

S3，根据运动状态计算机械臂的拖动补偿电流，拖动补偿电流包括：重力补偿电流、摩擦补偿电流及阻抗补偿电流。

具体地，拖动补偿电流=重力补偿电流+摩擦补偿电流-阻抗补偿电流。下面结合具体的实施例描述具体如果获取重力补偿电流、摩擦补偿电流及阻抗补偿电流。

在本发明的一个实施例中，根据当前机械臂关节的位置，依赖机械臂的动力学模型计算得重力补偿电流。

具体地，机械臂的动力学模型是其惯性参数的线性函数，可用于简化模型计算、动力学参数辨识、关节力矩获取，根据当前机械臂关节的位置和动力学模型，可获取关节力矩，根据关节力矩即可得到相应的重力补偿电流。

机械臂的动力学模型的动力学参数采用CAD（Computer Aided Design，计算机辅助设计）参数。由于速度和加速度信号噪声大，动力学参数中惯性张量和哥氏力矩阵参数偏差，导致惯性力和哥氏力计算偏差，很难用于计算惯性力和哥氏力补偿电流，因此本发明中重力补偿电流只取重力补偿电流项，忽略惯性力和哥氏力补偿电流。

根据本发明的采用以下方式获取摩擦补偿电流：根据当前机械臂关节的速度、位置、温度及负载获取摩擦模型；根据摩擦模型计算机械臂的总摩擦力；根据总摩擦力获取摩擦补偿电流。

进一步地，根据本发明的一个实施例，摩擦模型为VTL（速度-温度-负载）模型，具体根据以下公式获取摩擦模型（1）-（4）：

（1）；

（2）；

（3）

（4）；

其中，v为关节速度，t为关节温度，τ

在零速区间（即速度在零速附近，速度接近于零，有一定裕量值）时，一方面速度符号判断不准确，速度测量噪声极大，另一方面零速情况下摩擦力主要为静摩擦，是个不确定的值。因此，在本发明的实施例中，设置速度死区threshold，在零速区间内，通过以下公式（5）获取摩擦模型：

（5）

其中，

由公式可知，摩擦补偿电流符号由速度符号决定，在零速附近具体表现为信号跃迁，造成关节振动，因此可以对总摩擦力

在获取到总摩擦力

根据本发明的一个实施例，根据当前机械臂的各关节位置、速度和加速度，以及机械臂关节所在二阶系统的阻抗参数（M-D-K）计算阻抗补偿电流。

可以理解，机械臂中阻抗的作用有：1）正常拖动时手感的调节；2）非正常拖动时安全防护的处理。本发明中增加阻抗补偿电流的原因：1）动力学和摩擦的计算误差（主要来源于模型参数误差和速度、加速度估计误差），导致根据模型计算的动力学和摩擦电流存在偏差，会出现过补偿（外界微小的干扰力即会导致机器人运动，撤去外力后机器人无法停止）；2）即使动力学和摩擦补偿精确，如果将机器人动力学和摩擦完全补偿掉，使得拖动过程中机器人表现出零惯量-零阻尼的特性，这对于实际拖动是相当危险的。

S4，根据机械臂的拖动位置限制、速度限制和力矩限制修正拖动补偿电流，以获取机械臂的拖动驱动电流。

具体地，机械臂拖动时具有一定为位置限制、速度限制和力矩限制，避免拖动超范围、速度过快或者力度过大导致安全事故，因此，本发明根据机械臂的拖动位置限制、速度限制和力矩限制修正拖动补偿电流，以进行安全防护。

下面结合具体的实施例描述具体如何修正拖动补偿电流。

根据本发明的一个实施例，如图2所示，根据机械臂的拖动位置限制、速度限制和力矩限制修正拖动补偿电流，以获取机械臂的拖动驱动电流，具体包括：

S41，获取机械臂关节所处的位置区间，根据机械臂关节所处的位置区间获取机械臂位置限制阻尼和位置限制刚度，根据位置限制阻尼和位置限制刚度计算拖动位置限制电流。

进一步地，获取机械臂关节所处的位置区间，根据机械臂关节所处的位置区间获取机械臂位置限制阻尼和位置限制刚度，根据位置限制阻尼和位置限制刚度计算拖动位置限制电流，具体包括：

根据机械臂的关节位置获取机械臂所处的位置区间，位置区间包括：正常拖动位置区间、过渡位置区间和极限位置区间，举例而言，正常拖动位置区间为[-330°，+330°]，过渡位置区间为[-355°，-330°)和(+330°，+355°]，极限位置区间为[-360°，-355°)和(+355°，+360°]；如果机械臂所处的位置区间为正常拖动位置区间，则位置限制阻尼为0，拖动位置限制电流

（6）

其中，

总结而言，如图3所示，A区间为正常拖动位置区间，无需位置防护，没有位置限制电流产生，此时

总体表现为关节越接近极限位置阻尼越大，且在C区间产生反弹力，最终停在目标位置。

由此，根据机械臂关节所处的位置区间即可计算拖动位置限制电流

S42，获取机械臂关节所处的速度区间，根据机械臂关节所处的速度区间获取机械臂的速度限制阻尼，根据速度限制阻尼计算拖动速度限制电流

进一步地，根据本发明的一个实施例，获取机械臂关节所处的速度区间，根据机械臂关节所处的速度区间获取机械臂的速度限制阻尼，根据速度限制阻尼计算拖动速度限制电流

根据机械臂关节速度获取机械臂关节所处的速度区间，速度区间包括：正常拖动速度区间、过渡速度区间和极限速度区间，正常拖动速度区间为[-1.8rad/s，+1.8rad/s]，过渡速度区间为[-2rad/s，-1.8rad/s)和(+1.8rad/s，+2 rad/s]，极限速度区间为[-3rad/s，-2 rad/s)和(+2 rad/s，+3 rad/s]；如果机械臂关节所处的速度区间为正常速度位置区间，则速度限制阻尼为0，拖动速度限制电流为0；如果机械臂关节所处的速度区间为过渡速度区间，则速度限制阻尼与机械臂关节的速度的变化率为一次函数，根据一次函数和机械臂关节的速度获取速度限制阻尼，速度限制刚度为0，根据以下公式（7）获取拖动速度限制电流；如果机械臂关节所处的速度区间为极限速度区间，则速度限制阻尼与机械臂关节的速度的变化率为三次多项式，根据三次多项式和机械臂关节的速度获取速度限制阻尼，速度限制刚度为0，根据以下公式（7）获取拖动速度限制电流；

（7）

其中，

总结而言，如图4所示，a区间为正常拖动速度区间，无需速度防护，没有速度限制电流产生，此时

S43，根据拖动补偿电流、拖动位置限制电流和拖动速度限制电流计算电机驱动电流，根据电机驱动电流获取电机驱动力矩。

具体地，电机驱动电流=拖动补偿电流+拖动位置限制电流+拖动速度限制电流，电机驱动力矩即为电机驱动电流对应的力矩。

S44，根据机械臂的速度、电机驱动力矩和理论力矩调整电机驱动力矩。

具体地，机械臂运动由外部拖动力矩及电机驱动力矩组成，即外部拖动力矩+电机驱动力矩=理论力矩，理论力矩为基于动力学模型计算的维持当前运动状态的理论力矩，外部拖动力矩为未知量，机械臂无法根据外部拖动力矩实时调整电机驱动力矩，并且当电机驱动力矩>理论力矩时，即使外部拖动力矩为0（松手状态），机械臂也无法快速停止，容易造成飞车等安全事故。可以理解的是，根据输出力矩与理论力矩的符号即可判断加速度符号，根据当前速度和加速度方向，分析机械臂的运动状态，当机械臂处于以下四种运动状态时，为提高机械臂的安全防护性能，需要对电机驱动力矩进行调整，进而调整力矩限制电流，具体如下：

在本发明的一个具体实施例中，根据所述机械臂的速度、电机驱动力矩和理论力矩获取电机驱动力矩，具体包括：

如果机械臂关节的速度为正且理论力矩为负，机器人为减速状态，当电机驱动力矩>理论力矩时，则加速度较小，需将将电机驱动力矩调整为理论力矩，以获得更大的加速度，从而尽快达到0速；

如果机械臂关节的速度为负且理论力矩为正，机器人为减速状态，当电机驱动力矩<理论力矩时，则加速度较小，需将电机驱动力矩调整为理论力矩，以获得更大的加速度，从而尽快达到0速；

如果机械臂关节的速度为正且理论力矩为正，机器人为加速状态，当电机驱动力矩>理论力矩时，机器人获得了大于可维持自身运动状态的力矩，容易造成飞车危险，需将电机驱动力矩调整为理论力矩。

如果机械臂关节的速度为负且理论力矩为负，机器人为加速状态，电机驱动力矩<理论力矩时，则机器人获得了大于可维持自身运动状态的力矩，容易造成飞车危险，需将电机驱动力矩调整为理论力矩；

当所述机械臂不满足以上四种条件时，将电机驱动力矩无需调整。

S45，根据调整后的电机驱动力矩获取机械臂的拖动驱动电流。

具体地，直接根据电机驱动力矩即可获取对应的机械臂的拖动驱动电流，对机械臂的拖动位置、拖动速度和力矩进行限制，不仅关注拖动体验，更加注重安全防护。

S5，根据拖动驱动电流驱动机械臂。

具体地，对拖动驱动电流进行滤波处理后，下发给驱动器，以使驱动器根据拖动驱动电流驱动机械臂。

综上，根据本发明实施例的机械臂的拖动补偿方法，综合考虑机械臂的重力、摩擦以及阻抗计算机械臂的补偿电流，并根据拖动位置限制、速度限制和力矩限制进一步修正补偿电流，从而可以使下发给驱动器的补偿电流更为准确，并且增强了安全系数，以及无需传感器或使能按钮等外部设备，在控制成本的同时提升了拖动性能；并且针对0速摩擦力计算时存在速度符号判断不准问题做了特殊处理，进一步提高了补偿电流的准确性。

与上述的机械臂的拖动补偿方法相对应，本发明还提出一种机械臂的拖动补偿装置。对于装置实施例中未披露的细节可参照上述的方法实施例，本发明中不再进行赘述。

图5是根据本发明一个实施例的机械臂的拖动补偿装置的方框示意图，如图5所示，该装置包括：设置模块1、预处理模块2、补偿模块3、修正模块4和驱动模块5。

其中，设置模块1用于设置机械臂的驱动器模式和参数；预处理模块2用于对机械臂的运动状态进行滤波处理，得到滤波后的运动状态，运动状态包括：机械臂关节的速度、加速度、位置和温度；补偿模块3用于根据运动状态计算机械臂的拖动补偿电流，拖动补偿电流包括：重力补偿电流、摩擦补偿电流及阻抗补偿电流；修正模块4用于根据机械臂的拖动位置限制、速度限制和力矩限制修正拖动补偿电流，以获取机械臂的拖动驱动电流；驱动模块5用于根据拖动驱动电流驱动机械臂。

根据本发明实施例的机械臂的拖动补偿装置，综合考虑机械臂的重力、摩擦以及阻抗计算机械臂的补偿电流，并根据拖动位置限制、速度限制和力矩限制进一步修正补偿电流，从而可以使下发给驱动器的补偿电流更为准确，并且增强了安全系数，以及无需传感器或使能按钮等外部设备，在控制成本的同时提升了拖动性能。

在本发明的描述中，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。“多个”的含义是两个或两个以上，除非另有明确具体地限定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必针对相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。