机械臂关节的拖动示教限速方法、装置、电子设备及介质与流程

本发明涉及机械臂领域，特别涉及一种机械臂关节的拖动示教限速方法、装置、电子设备及介质。

背景技术：

拖动示教又称手动示教，是由操作员直接通过手动拖动的方式完成对机械臂的示教编程工作，而后机械臂可根据外界信号触发重复执行示教轨迹的指令。

在对机械臂进行拖动示教时，需先点击示教按钮，以使得机械臂进入拖动示教模式。机械臂在进入拖动示教模式后，再由操作员拖动机械臂移动至期望的位置，而在机械臂被拖动的过程中，机械臂会实时记录并保存其运动位姿或运动轨迹，从而完成对于机械臂的拖动示教过程。

现有技术中，机械臂处于力矩环控制模式，在拖动示教模式下，机械臂关节的运动状态由电机输出力矩和外部拖动力矩所决定，在外部拖动力矩过大时，机械臂关节会因速度过快而存在安全风险。此外，机械臂关节由于被拖动而快速运动时，即使停止施加外部拖动力矩，由于机械臂关节的动力学模型会产生维持当前运动的力，因此，机械臂关节仍会以当前速度继续运动一段距离，在此过程中也会存在安全风险。

技术实现要素：

本发明的主要目的在于提出一种机械臂关节的拖动示教限速方法，旨在解决现有机械臂关节在进行拖动示教时会存在因速度过快而导致的安全风险问题。

为实现上述目的，本发明提出一种机械臂关节的拖动示教限速方法，包括：

获取关节的实际拖动速度；

当实际拖动速度超过预设的安全拖动速度时；

通过动力学模型计算关节电机的调整力矩，调整力矩的方向与关节的当前运动方向相反；

将调整力矩转换为关节电机的运动控制指令，并发送给关节电机。

其中，机械臂关节的拖动示教限速方法还包括：

设定机械臂进行拖动示教时关节的速率阈值；

将设定的速率阈值与关节的当前运动方向进行速度合成，得到关节的安全拖动速度。

其中，当实际拖动速度小于或等于安全拖动速度时，机械臂关节的拖动示教限速方法还包括：

基于动力学模型和关节的实际拖动速度，计算得到关节的输出力矩；

根据关节的输出力矩和其当前速度方向，确定关节的力矩约束范围；

根据力矩约束范围对输出力矩进行饱和调节。

其中，根据关节的输出力矩和其当前速度方向，确定关节的力矩约束范围包括：

若输出力矩与实际拖动速度的方向同为正，则确定关节的力矩约束范围为[0，t1]；

若输出力矩与实际拖动速度的方向同为负，则确定关节的力矩约束范围为[-t1，0]；

若输出力矩的方向为正且实际拖动速度的方向为负，则确定关节的力矩约束范围为[t1，t2]；

若输出力矩的方向为负且实际拖动速度的方向为正，则确定关节的力矩约束范围为[-t2，-t1]；

t1为关节的输出力矩，t2为关节电机的额定力矩，t1小于t2且t1和t2均为正值，力矩和速度的正负方向以关节的零点为参考。

其中，根据力矩约束范围对输出力矩进行饱和调节包括：

当关节的力矩约束范围为[0，t1]且输出力矩的方向为正时：若输出力矩大于t1，则将输出力矩调节为t1；若输出力矩小于或等于t1，则无需对输出力矩进行调节；

当关节的力矩约束范围为[-t1，0]且输出力矩的方向为负时：若输出力矩大于-t1，则无需对输出力矩进行调节；若输出力矩小于或等于-t1，则将输出力矩调节为-t1；

当关节的力矩约束范围为[t1，t2]且输出力矩的方向为正时：若输出力矩大于t1且小于t2，则无需对输出力矩进行调节；若输出力矩小于t1，则将输出力矩调节为t1；若输出力矩大于t2，则将输出力矩调节为t2；

当关节的力矩约束范围为[-t2，-t1]且输出力矩的方向为负时：若输出力矩大于-t2且小于-t1，则无需对输出力矩进行调节；若输出力矩小于-t2，则将输出力矩调节为-t2；若输出力矩大于-t1，则将输出力矩调节为-t1。

本发明还提出一种机械臂关节的拖动示教限速装置，包括：

速度获取模块，用于获取关节的实际拖动速度；

调整力矩模块，用于当实际拖动速度超过预设的安全拖动速度时，通过动力学模型计算关节电机的调整力矩，调整力矩的方向与关节的当前运动方向相反；

指令生成模块，用于将调整力矩转换为关节电机的运动控制指令，并发送给关节电机。

其中，机械臂关节的拖动示教限速装置还包括：

速率设定模块，用于设定机械臂进行拖动示教时关节的速率阈值；

速度合成模块，用于将设定的速率阈值与关节的当前运动方向进行速度合成，得到关节的安全拖动速度。

其中，机械臂关节的拖动示教限速装置还包括：

输出力矩模块，用于基于动力学模型和关节的实际拖动速度，计算得到关节的输出力矩；

力矩约束模块，用于根据关节的输出力矩和其当前速度方向，确定关节的力矩约束范围；

力矩调节模块，用于根据力矩约束范围对输出力矩进行饱和调节。

本发明还提出一种电子设备，包括：

存储器，用于存储计算机程序；

处理器，用于执行计算机程序时，实现前述所记载的机械臂关节的拖动示教限速方法，该机械臂关节的拖动示教限速方法至少包括以下步骤：

获取关节的实际拖动速度；

当实际拖动速度超过预设的安全拖动速度时；

通过动力学模型计算关节电机的调整力矩，调整力矩的方向与关节的当前运动方向相反；

将调整力矩转换为关节电机的运动控制指令，并发送给关节电机。

本发明还提出一种介质，该介质存储有计算机程序，该计算机程序被处理器执行时，实现前述所记载的机械臂关节的拖动示教限速方法，该机械臂关节的拖动示教限速方法至少包括以下步骤：

获取关节的实际拖动速度；

当实际拖动速度超过预设的安全拖动速度时；

通过动力学模型计算关节电机的调整力矩，调整力矩的方向与关节的当前运动方向相反；

将调整力矩转换为关节电机的运动控制指令，并发送给关节电机。

与现有技术相比，本发明实施例的有益技术效果在于：

在对机械臂进行拖动示教时，先判断机械臂各关节的实际拖动速度是否超过其预设的安全拖动速度，其中，每个机械臂关节对应有一个预设的安全拖动速度。若未超过安全拖动速度，则无需对该机械臂关节进行限速，若超过安全拖动速度，则通过动力学模型计算该机械臂关节的关节电机的调整力矩，该调整力矩的方向与关节的当前运动方向（拖动方向）相反，反向的调整力矩将使得关节电机产生与拖动方向相反的阻力，从而降低该机械臂关节的拖动速度，防止机械臂关节的拖动速度过快，避免出现因速度过快而导致的安全风险问题。

附图说明

图1为本发明一实施例中的机械臂关节的拖动示教限速方法的流程示意图；

图2为本发明一实施例中的关节限速示意图；

图3为本发明一实施例中的机械臂关节的拖动示教限速方法的流程示意图；

图4为本发明一实施例中的机械臂关节的拖动示教限速装置的结构示意图；

图5为本发明一实施例中的机械臂关节的拖动示教限速装置的结构示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制，基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明所提出的机械臂关节的拖动示教限速方法适用于多关节机器人，譬如三轴关节机器人、四轴关节机器人、五轴关节机器人、六轴关节机器人及七轴关节机器人等。多关节机器人包括有主控制器和分别与其通讯连接的多个关节，每个关节内都设置有关节控制器、关节电机和编码器等部件，编码器包括有码盘和读头，码盘套设在关节电机的电机轴上并跟随电机轴转动，读头则用于读取码盘上的数据并将其发送给关节控制器或主控制器，以由关节控制器或主控制器根据接收到的数据计算得到该关节的转动速度及位置等，也即通过编码器可实时检测关节的转动速度。

多关节机器人在进行拖动示教时，可能会出现因拖动速度过快而导致的安全风险问题，为此，本发明现提出一种机械臂关节的拖动示教限速方法，以在各关节的实际拖动速度超过安全拖动速度时，通过该关节的关节电机产生与其拖动方向相反的调整力矩，以限制该关节的拖动速度，从而防止该关节的拖动速度过快，也即：相当于是给该超速拖动的关节施加一个阻力。

请参见图1，图1为本发明一实施例中的机械臂关节的拖动示教限速方法的流程示意图，包括以下步骤：

s10，获取关节的实际拖动速度；

本实施例中，关节的拖动速度是指其在关节空间内的速度，实质为关节电机的转动速度，关节电机的速度方向以其正反转为参考，比如关节电机正转为正方向，关节电机反转为反方向。

关节的实际拖动速度是指关节电机转动时的转速，其中，机械臂各个关节的实际拖动速度各不相同，以六轴机械臂为例。假设六轴机械臂的第一关节的实际拖动速度为30°/s，则第二关节的实际拖动速度可能为35°/s，第三关节的实际拖动速度可能为40°/s，第四关节的实际拖动速度可能为45°/s，第五关节的实际拖动速度可能为50°/s，第六关节的实际拖动速度可能为60°/s。

在进行拖动示教时，先由各个关节的编码器检测关节电机的转动数据，而后可由对应关节的关节控制器对转动数据进行处理，以计算得到该关节电机的转动速度，也即该关节的实际拖动速度，然后再将计算得到的关节实际拖动速度发送给机械臂的主控制器。此外，编码器所检测到的转动数据也可直接发送给主控制器，以由主控制器根据该转动数据计算得到该关节的实际拖动速度。

s20，当实际拖动速度超过预设的安全拖动速度时，通过动力学模型计算关节电机的调整力矩，调整力矩的方向与关节的当前运动方向相反；

在一些实施例中，在获取到关节的实际拖动速度后，基于此实际拖动速度与对应的安全拖动速度判断是否需要对该关节进行限速。具体的，若实际拖动速度超过预设的安全拖动速度，则需要对此关节进行限速，而若实际拖动速度未超过预设的安全拖动速度，则无需对此关节进行限速。

由于机械臂各个关节的实际拖动速度各不相同，因此在对机械臂进行拖动示教时，需要对各个关节分别进行限速，以使得各关节的实际拖动速度均小于各自的安全拖动速度，若任一关节的实际拖动速度超出其预设的安全拖动速度，则该机械臂仍将存在前述所提及的安全风险问题。

此外，需要说明的是，各个关节的安全拖动速度也是不同的，仍以六轴机械臂为例，假设第一关节的安全拖动速度阈值为40°/s，则第二关节的安全拖动速度阈值可能为45°/s，第三关节的安全拖动速度阈值为50°/s，第四关节的安全拖动速度阈值为55°/s，第五关节的安全拖动速度阈值为60°/s，第六关节的安全拖动速度阈值为70°/s。

请参见图2，图2为本发明一实施例中的机械臂关节限速示意图。

本实施例中，当实际拖动速度超过安全拖动速度时，再通过动力学模型计算关节电机的调整力矩，其中，该调整力矩的方向与关节的当前运动方向（拖动方向）相反，图2中所示的合力为拖动力矩与调整力矩的合力。

s30，将调整力矩转换为关节电机的运动控制指令，并发送给关节电机。

调整力矩被转换为关节电机的运动控制指令，关节电机根据该控制指令产生与关节拖动方向相反的阻力，以通过该阻力将关节的实际拖动速度降下来，从而达到限速的目的，进而提高机械臂拖动示教时的安全性。

具体应用：拖动机械臂，以对其进行拖动示教，在拖动过程中，各关节的编码器将实时检测对应关节电机的转动数据，而后由关节控制器或主控制器对此转动数据进行处理，以计算得到该关节电机的转动速度，也即该关节的实际拖动速度。假定其中某一关节的实际拖动速度为+45°/s，对应的安全拖动速度为+40°/s，+表示该关节的关节电机正转（假设正转为正方向），也即关节电机的转动方向，45°/s则表示关节电机的转动快慢，也即关节电机每秒转动45°。此时，关节的实际拖动速度为+45°/s，大于该关节的安全拖动速度+40°/s，则此时通过动力学模型计算得到一个关节电机的调整力矩，该调整力矩的方向与关节电机的转动方向相反，假定计算得到的调整力矩为-60n。在计算得到调整力矩后，将此调整力矩转换为关节电机的运动控制指令，关节电机根据此运动控制指令控制关节电机转动，以使得该关节电机在调整力矩的作用下降低速度，具体可使得该关节电机的转动速度降至低于+40°/s，从而防止关节的拖动速度过快，避免出现安全风险，提高机械臂拖动示教时的安全性。

进一步的，在对机械臂的各关节进行限速之前，需先设定各关节的安全拖动速度，安全拖动速度的速度大小根据实际情况进行设定，而安全拖动速度的速度方向则与关节的当前运动方向（拖动方向）相同。

具体的，请参见图3，图3为本发明一实施例中的机械臂关节的拖动示教限速方法的流程示意图，包括以下步骤：

s1，设定机械臂进行拖动示教时关节的速率阈值；

s2，将设定的速率阈值与关节的当前运动方向进行速度合成，得到关节在拖动示教时的安全拖动速度。

每个关节都对应设定一个速率阈值，也即该关节的最大拖动速率，在设定速率阈值后，再通过编码器获取该关节的当前运动方向（拖动方向），而后将该速率阈值与关节的运动方向进行合成，以得到该关节的安全拖动速度。关节的运动方向以关节的零点作为参考，假设关节电机正转（顺时针旋转）为正方向，则关节电机反转（逆时针旋转）为负方向，此处的零点相当于二维直角坐标系的原点。

示例：假设机械臂某一关节的速率阈值设定为50°/s，若通过编码器检测到该关节的当前运动方向为正方向，则合成后得到的安全拖动速度为+50°/s，而若通过编码器检测到该关节的当前运动方向为负方向，则合成后得到的安全拖动速度为-50°/s。

进一步的，机械臂的运动是在外部拖动力矩与关节电机的输出力矩的共同作用下产生的，机械臂的各个关节在运动时，动力学模型会计算得到一个维持各个关节当前运动的理论力矩，也即：外部拖动力矩+输出力矩=理论力矩。为了保证机械臂的可控，关节电机的输出力矩不能超过动力学模型所计算得到的理论力矩，若关节电机的输出力矩超过理论力矩，则在停止拖动时（外部拖动力矩为零），由于关节电机的输出力矩大于理论力矩，因此该关节仍将在两者合力的作用下移动，无法在松手时停止，容易出现安全风险问题。

为此，本发明将对各关节的的关节电机的输出力矩进行约束，以使得关节电机的输出力矩小于动力学模型所计算得到的理论力矩。

具体的，请参见图3，图3为本发明一实施例中的机械臂拖动示教限速方法的流程图，包括以下步骤：

s40，基于动力学模型和关节的实际拖动速度，计算得到关节的输出力矩；

当关节的实际拖动速度小于或等于安全拖动速度时，基于动力学模型和实际拖动速度可计算得到关节的输出力矩，该输出力矩与外部拖动力矩之和应等于前述所提及的理论力矩。

s50，根据关节的输出力矩和其当前速度方向，确定关节的力矩约束范围；

在计算得到关节的输出力矩后，再根据关节的输出力矩和当前速度方向，确定关节的力矩约束范围，输出力矩和速度方向以该关节的零点标定方向为参考，具体在前述实施例中有所阐述，故在此不再赘述。

s60，根据力矩约束范围对输出力矩进行饱和调节。

在确定力矩约束范围后，再根据该力矩约束范围对输出力矩进行饱和调节，以使得关节电机的输出力矩始终小于理论力矩，从而保证在松手后机械臂的各个关节能够停止运动。

示例：假设经步骤s40与s50后所确定的力矩约束范围为[0，100]，若假设在下一次所计算得到的输出力矩为80n，则无需对此输出力矩进行调节，可直接将此输出力矩转化为该关节电机的运动控制指令，而若假设在下一次所计算得到的输出力矩为110n，则此时需要将该输出力矩调节为100n，而后再将调节后的输出力矩转化为该关节的运动控制指令。

进一步的，本发明所提出的力矩约束范围的具体确定方法如下：

若输出力矩与实际拖动速度的方向同为正，则确定关节的力矩约束范围为[0，t1]；

若输出力矩与实际拖动速度的方向同为负，则确定关节的力矩约束范围为[-t1，0]；

若输出力矩的方向为正且实际拖动速度的方向为负，则确定关节的力矩约束范围为[t1，t2]；

若输出力矩的方向为负且实际拖动速度的方向为正，则确定关节的力矩约束范围为[-t2，-t1]；

其中，t1为关节的输出力矩，t2为所述伺服电机的额定力矩，t1小于t2且t1和t2均为正值，力矩和速度的正负方向以关节的零点为参考。

示例：假定t1为100n，t2为150n，实际拖动速度为50°/s，关节电机正转（顺时针转动）为正方向，关节电机反转（逆时针转动）为反方向，先判断输出力矩与实际拖动速度的方向，而后根据输出力矩与实际拖动速度的方向确定关节的力矩约束范围。

具体的，若输出力矩为+100n，实际拖动速度为+50°/s，则确定该关节的力矩约束范围为[0，100]；若输出力矩为-100n，实际拖动速度为-50°/s，则确定该关节的力矩约束范围为[-100，0]；若输出力矩为+100n，而实际拖动速度为-50°/s，则确定该关节的力矩约束范围为[100，150]；若输出力矩为-100n，而实际拖动速度为+50°/s，则确定该关节的力矩约束范围为[-150，-100]。

进一步的，本发明所提出的输出力矩具体调节方法如下：

当关节的力矩约束范围为[0，t1]且输出力矩为正时：若输出力矩大于t1，则将输出力矩调节为t1；若输出力矩小于或等于t1，则无需对输出力矩进行调节；

当关节的力矩约束范围为[-t1，0]且输出力矩为负时：若输出力矩大于-t1，则无需对输出力矩进行调节；若输出力矩小于或等于-t1，则将输出力矩调节为-t1；

当关节的力矩约束范围为[t1，t2]且输出力矩为正时：若输出力矩大于t1且小于t2，则无需对输出力矩进行调节；若输出力矩小于t1，则将输出力矩调节为t1；若输出力矩大于t2，则将输出力矩调节为t2；

当关节的力矩约束范围为[-t2，-t1]且输出力矩为负时：若输出力矩大于-t2且小于-t1，则无需对输出力矩进行调节；若输出力矩小于-t2，则将输出力矩调节为-t2；若输出力矩大于-t1，则将输出力矩调节为-t1。

示例：假定t1为100n，t2为150n，实际拖动速度为50°/s，关节电机正转（顺时针转动）为正方向，关节电机反转（逆时针转动）为反方向，先判断输出力矩与实际拖动速度的方向，而后根据输出力矩与实际拖动速度的方向确定关节的力矩约束范围，然后再根据力矩约束范围对下一次所计算得到的输出力矩进行调节，具体的：

当关节的力矩约束范围为[0，100]且输出力矩的方向为正时：

若下一次所计算得到的输出力矩为+120n，则将此输出力矩调节为+100n，若下一次所计算得到的输出力矩为+90n，则无需对此输出力矩进行调节；

当关节的力矩约束范围为[-100，0]且输出力矩的方向为负时：

若下一次所计算得到的输出力矩为-90n，则无需对此输出力矩进行调节，若下一次所计算得到的输出力矩为-120n，则将此输出力矩调节为-100；

当关节的力矩约束范围为[100，150]且输出力矩的方向为正时：

若下一次所计算得到的输出力矩为+120n，则无需对此输出力矩进行调节，若下一次所计算得到的输出力矩为90n，则将此输出力矩调节为100n，若下一次所计算得到的输出力矩为160n，则将此输出力矩调节为150n。

当关节的力矩约束范围为[-150，-100]且输出力矩的方向为负时：

若下一次所计算得到的输出力矩为-120n，则无需对此输出力矩进行调节，若下一次所计算得到的输出力矩为-90n，则将此输出力矩调节为-100n，若下一次所计算得到的输出力矩为-160n，则将此输出力矩调节为-150n。

请参见图4，图4为本发明一实施例中的机械臂关节的拖动示教限速装置的结构示意图，包括：

速度获取模块10，用于获取关节的实际拖动速度；

调整力矩模块20，用于当实际拖动速度超过预设的安全拖动速度时，通过动力学模型计算关节电机的调整力矩，调整力矩的方向与关节的当前运动方向相反；

指令生成模块30，用于将调整力矩转换为关节电机的运动控制指令，并发送给关节电机。

进一步的，本发明所提出的机械臂关节的拖动示教限速装置还包括：

速率设定模块，用于设定机械臂进行拖动示教时关节的速率阈值；

速度合成模块，用于将设定的速率阈值与关节的当前运动方向进行速度合成，得到关节在拖动示教时的安全拖动速度。

请参见图5，图5为本发明一实施例中的机械臂拖动示教限速装置的结构示意图，还包括：

输出力矩模块40，用于基于动力学模型和关节的实际拖动速度，计算得到关节的输出力矩；

力矩约束模块50，用于根据关节的输出力矩和其当前速度方向，确定关节的力矩约束范围；

力矩调节模块60，用于根据力矩约束范围对输出力矩进行饱和调节。

基于前述所提出的机械臂关节的拖动示教限速方法，本发明还提出一种电子设备，该电子设备包括：

存储器，用于存储计算机程序；

处理器，用于执行计算机程序时，实现前述各实施例所记载的机械臂关节的拖动示教限速方法，该机械臂关节的拖动示教限速方法至少包括以下步骤：

步骤1，获取关节的实际拖动速度；

步骤2，当实际拖动速度超过预设的安全拖动速度时，通过动力学模型计算关节电机的调整力矩，调整力矩的方向与关节的当前运动方向相反；

步骤3，将调整力矩转换为关节电机的运动控制指令，并发送给关节电机。

基于前述所提出的机械臂关节的拖动示教限速方法，本发明还提出一种介质，该介质存储有计算机程序，该计算机程序被处理器执行时，实现前述各实施例所记载的机械臂关节的拖动示教限速方法，该机械臂关节的拖动示教限速方法至少包括以下步骤：

步骤1，获取关节的实际拖动速度；

步骤2，当实际拖动速度超过预设的安全拖动速度时，通过动力学模型计算关节电机的调整力矩，调整力矩的方向与关节的当前运动方向相反；

步骤3，将调整力矩转换为关节电机的运动控制指令，并发送给关节电机。

在本申请所提供的几个实施例中，应该理解到，所揭露的方法和装置，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个模块或组件可以结合或者可以集成到另一个装置，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或模块的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理模块，即可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。

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

以上所述的仅为本发明的部分或优选实施例，无论是文字还是附图都不能因此限制本发明保护的范围，凡是在与本发明一个整体的构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明保护的范围内。