CN112894821B - 基于电流法的协作机器人拖动示教控制方法、装置及设备 - Google Patents

Abstract

本发明涉及一种基于电流法的协作机器人拖动示教控制方法、装置及设备，所述方法包括以下步骤：获取机器人在有外力施加时的实时状态参数，所述实时状态参数包括各关节的关节位置、关节速度和关节电机电流；构建机器人动力学模型，基于所述关节位置和关节速度获得对应关节的理论力矩，基于所述关节电机电流获得对应关节的实际输出力矩；基于所述理论力矩和实际输出力矩确定各节点受到的外力矩，并基于所述外力矩试算获得机器人需要的驱动速度；将所述驱动速度作为机器人各关节电机的输入，实现机器人示教时关节空间的拖动控制。与现有技术相比，本发明能够有效提高机器人拖动控制的准确性和快速性。

Description

基于电流法的协作机器人拖动示教控制方法、装置及设备

技术领域

本发明涉及工业机器人技术领域，尤其是涉及一种基于电流法的协作机器人拖动示教控制方法、装置及设备。

背景技术

与传统的工业机器人相比，协作机器人具有与人类协同工作的理念，且具备明显的人机友好的特点，而示教技术是协作机器人实现人机协同工作的关键，其中拖动控制则是实现示教的基础。实现不同应用下的、简单、快速、友好的示教技术，一直以来都是世界各国机器人领域学者的研究热点。

示教，即在执行工作前对机器人进行“示范教导”。根据示教过程具体的实现方式可以对示教进行分类，具体如图1所示，机器人示教方法包括离线示教和在线示教，其中在线示教包括遥控示教和直接示教。

国内外学者对于直接示教的研究总的来说可以分为三类：第一类为基于力/力矩传感器的直接示教方案，第二类为基于力矩补偿控制的直接示教方案，第三类为设计专门的示教装置或者传感器的直接示教方案。

对于第一类基于力/力矩传感器的示教方案，已有较多相关的研究和应用。其基本思路是在机器人末端安装多维力矩传感器，通过将传感器感知到的力信号转变成电机的控制信号，当示教者或操作员牵引机器人末端时，传感器便可检测到外力，机器人就会沿着外力作用的方向进行运动。相应的，当撤去外力后，机器人停止运动，保持姿态。

对于第二类基于力矩控制的示教方案，国内外也有一些研究成果。一般是基于力矩控制模式对机器人进行完整的动力学力矩补偿，使机器人达到无摩擦力、无重力的状态，实现机器人的直接示教。有的基于机器人速度与外力之间的导纳关系，进而实现直接示教的效果。有的对摩擦力以及重力等主要扰动进行补偿，构建一个无摩擦力、无重力的状态，并通过构建位置伺服的近似方程，基于位置指令实现机器人的直接示教。

对于第三类通过设计专门的示教装置或传感器实现直接示教，大多是设计一种多维的力/位移传感器的辅助装置，或者是设计一种专门的示教策略，或者设计专用的示教臂，通过采集示教臂的关节数据，然后基于机器人运动学得到任务空间下示教轨迹的数据，最后基于实际机器人的逆运动学解得到关节的示教轨迹。

这些方法在不同的角度，存在一些优势，都可以实现协作机器人的示教。但它们在应用上仍存在一些不足。

(1)基于力/力矩传感器的方法的优势是不需要建立机器人的动力学模型，但是力/力矩传感器的价格一般比较昂贵，此方案增加了示教的实现成本，同时也不利于机器人关节的集成化。

(2)基于力矩控制的方法优势是免去了力/力矩传感器的使用，降低了技术实现的成本，但是无法适用于未开放力矩控制接口的机器人上，普遍性差，在某些类型机器人的实际使用中不容易实现。

(3)第三类方法是针对具体的示教需求设计额外的示教硬件或装置，或者基于特殊机器人的特点设计专用的示教方案，优势是面向具体需求所设计装置或方案能实现较好的预期效果，但同时增加了示教技术实现的成本，以及难以拓展到一般的机器人应用中。

总的来说，国内外对于直接示教技术的研究，主要集中在直接力矩控制的方法上，或者是基于力/力矩传感器反馈对机器人做简单的位置规划，以及面向直接示教设计专用的传感器或者示教装置。然而，基于速度指令的无力/力矩传感器的直接示教研究则较少。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种有效提高机器人拖动控制的准确性和快速性的基于电流法的协作机器人拖动示教控制方法、装置及设备。

本发明的目的可以通过以下技术方案来实现：

第一方面，本发明提供一种基于电流法的协作机器人拖动示教控制方法，包括以下步骤：

获取机器人在有外力施加时的实时状态参数，所述实时状态参数包括各关节的关节位置、关节速度和关节电机电流；

构建机器人动力学模型，基于所述关节位置和关节速度获得对应关节的理论力矩，基于所述关节电机电流获得对应关节的实际输出力矩；

基于所述理论力矩和实际输出力矩确定各节点受到的外力矩，并基于所述外力矩试算获得机器人需要的驱动速度；

将所述驱动速度作为机器人各关节电机的输入，实现机器人示教时关节空间的拖动控制。

进一步地，所述机器人动力学模型基于拉格朗日法建立。

进一步地，所述理论力矩的计算公式为：

T(i)＝G(i)+C(i)

其中，T(i)、G(i)和C(i)分别表示关节i的理论力矩、重力矩和非线性力矩，所述重力矩和非线性力矩基于所述机器人动力学模型解析获得。

进一步地，所述实际输出力矩的计算公式为：

torque＝n·τ_m

其中，torque表示实际输出力矩，n为减速器的减速比，τ_m为与关节电机电流成线性关系的电机输出力矩。

进一步地，所述驱动速度通过以下方式获得：

根据所述外力矩计算得到各关节实时的加速度，基于所述加速度试算获得机器人需要的驱动速度。

各关节实时的所述加速度基于各关节所述理论力矩与实际输出力矩的比较结果确定，具体地：

其中，Acc(i)、T(i)、torque(i)、τ(i)和dq(i)分别表示关节i的加速度、理论力矩、实际输出力矩、外力矩和关节速度，k为比例系数。

第二方面，本发明提供一种基于电流法的协作机器人拖动示教控制装置，包括：

获取模块，用于获取机器人在有外力施加时的实时状态参数，所述实时状态参数包括各关节的关节位置、关节速度和关节电机电流；

第一计算模块，用于构建机器人动力学模型，基于所述关节位置和关节速度获得对应关节的理论力矩，基于所述关节电机电流获得对应关节的实际输出力矩；

第二计算模块，基于所述理论力矩和实际输出力矩确定各节点受到的外力矩，并基于所述外力矩试算获得机器人需要的驱动速度；

控制模块，将所述驱动速度作为机器人各关节电机的输入，实现机器人示教时关节空间的拖动控制。

进一步地，所述第二计算模块包括：

加速度计算单元，用于根据所述外力矩计算得到各关节实时的加速度；

驱动速度计算单元，用于基于所述加速度试算获得机器人需要的驱动速度。

进一步地，各关节实时的所述加速度基于各关节所述理论力矩与实际输出力矩的比较结果确定，具体地：

其中，Acc(i)、T(i)、torque(i)、τ(i)和dq(i)分别表示关节i的加速度、理论力矩、实际输出力矩、外力矩和关节速度，k为比例系数。

第三方面，本发明提供一种电子设备，包括：

一个或多个处理器；

存储器；和

被存储在存储器中的一个或多个程序，所述一个或多个程序包括用于执行如上所述基于电流法的协作机器人拖动示教控制方法的指令。

本发明针对现有技术的问题和不足，在考虑简单实用的情况下，提出了一种主要针对直接示教的基于电流法的协作机器人拖动示教控制方法，其中机器人运动控制采用速度模式。本发明具有以下有益效果：

1)相比于传统的基于外部安装力/力矩传感器的方法，本发明的实现无需外部传感器，而是考虑了机器人的实际运动中的关节电机的电流值，通过机器人的电流值估计力矩，进而得到机器人的驱动速度，使得机器人的拖动控制更加准确和迅速的同时，也降低了机器人拖动示教的成本。

2)由于力反馈信号变化较快，因而希望实际中用于实现拖动控制算法的指令周期越小越好，相比于大多数基于位置的控制模式，本发明采用基于速度模式的控制方法，因为速度环的调节周期要比位置环快，相对来说，速度指令的允许带宽要大，因此，速度指令更有利于拖动控制的实现。

3)本发明通过速度约束对速度指令的调节，在机器人被拖拽而启动或撤去外力而静止时，不会突然变大或变为零，避免了机器人的抖动，加强了对驱动电机的保护。

本发明方法的提出，实现了机器人各关节的拖动控制，使得机器人可以由操作员随意拖拽，到达实际工况中想要达到的构型和目标点，提高了机器人拖动控制的准确性和快速性。

附图说明

图1为机器人示教分类图；

图2为协作机器人拖动示教控制的系统流程图；

图3为Rozum机器人连杆结构图，其中给出的对应尺寸图是Rozum机器人尺寸图；

图4为协作机器人坐标系变换示意图；

图5为Rozum机器人拖动时一个关节的给定速度和电流曲线图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

本发明方法是利用协作机器人关节电流估计力矩的拖动示教控制方法，以实现协作机器人示教时的拖动控制，解决机器人力控制问题。如图2所示，该方法的步骤如下：

Step1：通过修改机器人控制程序的标志位的布尔值，设置机器人进入拖动控制模式，在该模式下，操作人员可以用手对机器人施加外力，随意拖拽机器人，使得机器人随着外力作用而运动，从而达到实际工况期望的关节构型和末端位姿。

Step2：在Step1的情况下，获取机器人的实时状态参数，包括机器人各关节位置、关节电机电流、关节速度等。

Step3：由动力学模型计算补偿力矩，得到力矩T。其中，机器人动力学模型基于拉格朗日法建立。通过将Step2中实时获得的关节位置和关节速度等状态参数，作为动力学模型的输入参数，从而计算出机器人各个关节的理论力矩T。其中，理论力矩由重力矩和非线性力矩的代数和组成。

Step4：基于实时的关节电流值估计出各个关节的实际输出力矩。

Step5：计算出机器人各关节的驱动速度。根据各关节所受的外力矩，得到各关节实时的加速度，根据加速度值，计算得到实际需要的驱动速度。

Step6：将Step5计算得到的驱动速度值，作为接口参数，传递给伺服电机，实现机器人关节空间的拖动控制。

经过上述设计步骤，本发明通过机器人实时的关节电流值估计关节力矩值，基于速度控制模式，实现了对协作机器人的拖动控制。

上述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

上述存储介质可供一电子设备调用，所述电子设备包括一个或多个处理器、存储器和被存储在存储器中的一个或多个程序，所述一个或多个程序包括用于执行如上所述基于电流法的协作机器人拖动示教控制方法的指令。

在另一实施例中，提供一种基于电流法的协作机器人拖动示教控制装置，包括：获取模块，用于获取机器人在有外力施加时的实时状态参数，所述实时状态参数包括各关节的关节位置、关节速度和关节电机电流；第一计算模块，用于构建机器人动力学模型，基于所述关节位置和关节速度获得对应关节的理论力矩，基于所述关节电机电流获得对应关节的实际输出力矩；第二计算模块，基于所述理论力矩和实际输出力矩确定各节点受到的外力矩，并基于所述外力矩试算获得机器人需要的驱动速度；控制模块，将所述驱动速度作为机器人各关节电机的输入，实现机器人示教时关节空间的拖动控制。

以Rozum Robotics公司的七轴协作机器人为例，来说明本发明提出的基于电流法的机器人拖动示教控制方法在协作机器人拖动示教功能上的应用。图3是Rozum协作机器人的实际连杆结构示意图，其在某一常用构型下的结构简图及坐标变换示意图，如图4所示。

(1)建立机器人动力学模型

机器人动力学建模的常用方法主要包括牛顿欧拉法和拉格朗日法。本发明基于拉格朗日法建立机器人动力学模型。拉格朗日法主要是利用拉格朗日公式建立机器人的动力学方程。这种方法通过系统整体的能量和功来描述动态系统的特性，而不是单独利用某个部件的力和力矩来描述。

对于任意机械系统而言，拉格朗日函数L定义为系统总动能E_k与总势能E_p之差，即：

L＝E_k-E_p

由拉格朗日函数L所描述的系统动力学状态的拉格朗日方程为：

其中，q_i为系统选定的广义坐标，

为广义速度，F_i为作用在第i个坐标系上的广义力。对于一个机器人系统而言，q_i即为各关节的角度，

为角速度，n为连杆数目，F_i则是各关节的力矩τ_i。上述两式联立可得各关节力矩的表达式：

其中，系统的势能E_p仅是关节角q_i的函数，而动能E_k是关节角q_i、关节速度

以及时间t的函数。首先，我们需要知道整个系统的动能E_k，对于某个关节i，其动能为：

其中，m_i为连杆的质量，I_i为该连杆在基坐标系中表示的关于质心的惯性张量。因此，上式的第一项为连杆进行平移运动所具备的动能，第二项为连杆绕质心旋转所具备的动能。同时，

与ω_i表示为关节角度与角速度的函数为：

其中，

和

表示关节i的线速度与角速度的雅各比矩阵，

和

则对应着这两个矩阵的第j列。

将上述关节角度与角速度的函数关系代入动能表达式中，可得整个系统的动能为：

其中：

同时将基坐标系中的惯性张量I_i用第i坐标系中连杆关于质心的惯性张量表示，为：

接下来需要计算出系统的势能，可以表示为：

其中，g为重力加速度向量，

为第i各连杆的质心在基坐标系中的质心坐标。

将动能表达式与势能表达式代入上述关节力矩表达式，可以得到：

通过以上的推导，我们得到了基于拉格朗日法的动力学方程。上面的推导过程清晰，意义明确，能够得到每项系数即H,C,G的解析式，其G为重力矩，C为非线性力矩。根据这三项系数的解析式就能有效地进行动力学的相关运算。

(2)获取机器人实时状态参数

首先设置Rozum协作机器人处于拖动状态，示教者用手牵引机器人，此时外力作用于机器人上。通过各关节电机自身的编码器获取机器人的运动状态参数，包括关节位置q(i)(即q)、关节速度dq(i)(即

)和关节电机的电流值current(i)。将关节的运动状态参数q(i)、dq(i)，带入动力学模型中，计算出重力矩G(i)和非线性力C(i)。根据取到的电机电流值current(i)，计算出电机作用于关节的驱动力矩torque(i)。其中，电机的输出力矩τ_m，其与电机电流i近似看成线性关系：

τ_m＝K_t·i

式中，K_t为电机力矩常数。这样，基于采集到的电机电流信号可以计算得到电机的输出力矩，电机的输出力矩通过自身减速器得到放大，在忽略电机转子和减速器的转动惯量的前提下，作用于关节的驱动力矩为：

torque＝n·τ_m

式中，n为减速器的减速比。

(3)计算各关节的速度指令

关节空间拖动示教的系统框图如图2所示。首先根据(2)中由读取到的当前的关节位置q(i)和关节速度dq(i)计算出当前的重力矩G(i)和非线性力C(i)，将这两项力矩代数和作为理论力矩，即

T(i)＝G(i)+C(i)

将理论力矩值T(i)与当前各关节电流值得到的实际力矩torque(i)进行比较，得到机器人在各关节上所受外力的力矩τ(i)，即

τ(i)＝torque(i)-T(i)

最后根据外力矩计算得到各关节的加速度Acc(i)，具体计算方法如下式所示：

其中k为比例系数，表示拖动的惯性，在合理范围内，k值越大，则机器人越容易被拖动。k值的大小根据机器人实际动力学参数而人为设定，本Rozum协作机器人在实际应用中，各关节的k值设为0.5-2不等。由加速度值Acc(i)进而得到各电机的期望速度值shouldspeed(i)，这里各电机的期望速度值shouldspeed(i)通过调整其比例系数进行速度约束的规整，得到最终的速度指令。最后，将速度指令发送给机器人电机，便可使机器人随着操作人员的牵引而运动。相应的，当操作人员停止牵引、撤去外力时，机器人保持姿态。

该基于电流法的协作机器人拖动示教控制方法在Rozum七轴协作机器人上进行了验证，并展示了良好的效果。图5展示了拖动过程中协作机器人第4关节运动速度的曲线，图中，当开始拖动时，随着示教者对机器人施加外力，关节速度随之增大，进行相应的运动；当关节速度增大到一定程度后，随着示教者不断施加外力，机器人的关节速度保持相对平缓；当示教者施加的外力开始减小时，相应的，机器人关节的运动速度也随之下降；最后，当示教者撤去外力后，机器人关节的运动速度不断减小，最后降至零，保持拖动后的姿态。图中曲线可以看出，机器人的启动速度不会突然增大，运动过程中不会一直加速，撤去外力后也不会突然降至零，这是速度约束后呈现的效果，有效避免了机器人抖动，加强了对驱动电机的保护。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (6)

1.一种基于电流法的协作机器人拖动示教控制方法，其特征在于，包括以下步骤：

获取机器人在有外力施加时的实时状态参数，所述实时状态参数包括各关节的关节位置、关节速度和关节电机电流；

构建机器人动力学模型，基于所述关节位置和关节速度获得对应关节的理论力矩，基于所述关节电机电流获得对应关节的实际输出力矩；

基于所述理论力矩和实际输出力矩确定各节点受到的外力矩，并基于所述外力矩试算获得机器人需要的驱动速度；

将所述驱动速度作为机器人各关节电机的输入，实现机器人示教时关节空间的拖动控制；

所述驱动速度通过以下方式获得：

根据所述外力矩计算得到各关节实时的加速度，基于所述加速度试算获得机器人需要的驱动速度，各关节实时的所述加速度基于各关节所述理论力矩与实际输出力矩的比较结果确定，具体地：

其中，Acc(i)、T(i)、torque(i)、τ(i)和dq(i)分别表示关节i的加速度、理论力矩、实际输出力矩、外力矩和关节速度，k为比例系数。

2.根据权利要求1所述的基于电流法的协作机器人拖动示教控制方法，其特征在于，所述机器人动力学模型基于拉格朗日法建立。

3.根据权利要求1所述的基于电流法的协作机器人拖动示教控制方法，其特征在于，所述理论力矩的计算公式为：

T(i)＝G(i)+C(i)

其中，T(i)、G(i)和C(i)分别表示关节i的理论力矩、重力矩和非线性力矩，所述重力矩和非线性力矩基于所述机器人动力学模型解析获得。

4.根据权利要求1所述的基于电流法的协作机器人拖动示教控制方法，其特征在于，所述实际输出力矩的计算公式为：

torque＝n·τ_m

其中，torque表示实际输出力矩，n为减速器的减速比，τ_m为与关节电机电流成线性关系的电机输出力矩。

5.一种基于电流法的协作机器人拖动示教控制装置，其特征在于，包括：

获取模块，用于获取机器人在有外力施加时的实时状态参数，所述实时状态参数包括各关节的关节位置、关节速度和关节电机电流；

第一计算模块，用于构建机器人动力学模型，基于所述关节位置和关节速度获得对应关节的理论力矩，基于所述关节电机电流获得对应关节的实际输出力矩；

第二计算模块，基于所述理论力矩和实际输出力矩确定各节点受到的外力矩，并基于所述外力矩试算获得机器人需要的驱动速度；

控制模块，将所述驱动速度作为机器人各关节电机的输入，实现机器人示教时关节空间的拖动控制；

所述第二计算模块包括：

加速度计算单元，用于根据所述外力矩计算得到各关节实时的加速度，各关节实时的所述加速度基于各关节所述理论力矩与实际输出力矩的比较结果确定，具体地：

其中，Acc(i)、T(i)、torque(i)、τ(i)和dq(i)分别表示关节i的加速度、理论力矩、实际输出力矩、外力矩和关节速度，k为比例系数；

驱动速度计算单元，用于基于所述加速度试算获得机器人需要的驱动速度。

6.一种电子设备，其特征在于，包括：

一个或多个处理器；

存储器；和

被存储在存储器中的一个或多个程序，所述一个或多个程序包括用于执行如权利要求1-4任一所述基于电流法的协作机器人拖动示教控制方法的指令。

Images