CN115674190B - 协作机械臂及其运动控制方法、碰撞检测方法、控制系统 - Google Patents
协作机械臂及其运动控制方法、碰撞检测方法、控制系统 Download PDFInfo
- Publication number
- CN115674190B CN115674190B CN202211220131.1A CN202211220131A CN115674190B CN 115674190 B CN115674190 B CN 115674190B CN 202211220131 A CN202211220131 A CN 202211220131A CN 115674190 B CN115674190 B CN 115674190B
- Authority
- CN
- China
- Prior art keywords
- joint
- theoretical
- servo motor
- moment
- motion state
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 487
- 238000000034 method Methods 0.000 title claims abstract description 123
- 238000001514 detection method Methods 0.000 title claims abstract description 60
- 239000013598 vector Substances 0.000 claims description 26
- 230000006870 function Effects 0.000 claims description 20
- 238000004891 communication Methods 0.000 claims description 13
- 230000009467 reduction Effects 0.000 claims description 12
- 230000005484 gravity Effects 0.000 claims description 10
- 230000001133 acceleration Effects 0.000 claims description 8
- 230000009471 action Effects 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 3
- 230000001276 controlling effect Effects 0.000 claims 8
- 230000001105 regulatory effect Effects 0.000 claims 1
- 230000006378 damage Effects 0.000 abstract description 15
- 230000008569 process Effects 0.000 description 18
- 238000010586 diagram Methods 0.000 description 12
- 239000003638 chemical reducing agent Substances 0.000 description 9
- 230000002159 abnormal effect Effects 0.000 description 8
- 239000000284 extract Substances 0.000 description 6
- 208000027418 Wounds and injury Diseases 0.000 description 4
- 208000014674 injury Diseases 0.000 description 4
- 230000005611 electricity Effects 0.000 description 3
- 239000011159 matrix material Substances 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000630 rising effect Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
本申请提供了一种协作机械臂及其运动控制方法、碰撞检测方法、控制系统、工业机器人以及计算机存储介质。该运动控制方法包括:根据运动控制任务,计算关节的关节理论运动状态;根据关节理论运动状态,计算关节伺服电机的理论力矩;根据理论力矩,计算关节伺服电机的电流限制范围;控制输出给关节伺服电机的驱动电流不超出电流限制范围。上述方法可以利用关节理论运动状态以及各个关节伺服电机的理论力矩直接对各个关节的驱动电流进行限制,有效控制各个关节对外的输出力矩大小,减少协作机械臂在发生碰撞时产生的伤害,有效保护协作机械臂和人。
Description
技术领域
本申请涉及机器人控制技术领域,特别是涉及一种协作机械臂及其运动控制方法、碰撞检测方法、控制系统、工业机器人以及计算机存储介质。
背景技术
协作机械臂是指被设计成可以在协作区域内与人直接进行交互的机器人,其具有快速部署、人机协作安全等传统工业所不具有的优势。安全作为协作机械臂的主要特征与优势,表现在当与人或者环境发生碰撞时,协作机械臂要能够快速识别碰撞的信号并采取策略避免造成进一步的碰撞伤害。
目前的协作机械臂在运动过程中与障碍物发生碰撞后,会输出更大的电流继续驱动协作机械臂,以使协作机械臂能越过障碍物按预定的轨迹继续运动;而在电流从正常运动的电流值逐渐增加的期间,协作机械臂持续对障碍物施加力,直至电流达到一个较大的电流阈值,甚至该电流阈值为该关节的额定电流,或者检测到碰撞信号后,才会对电流限制,在这个过程中协作机械臂会对障碍物造成较大的损害;尤其是当障碍物是人的时候,协作机械臂会给人造成较大的伤害;同时,对协作机械臂本身也会带来不小的损害。
发明内容
本申请提供了一种协作机械臂及其运动控制方法、碰撞检测方法、控制系统、工业机器人以及计算机存储介质,主要解决的技术问题是如何限制协作机械臂输出的驱动电流以及输出力矩,减小协作机械臂在发生碰撞时产生的伤害。
为解决上述技术问题,本申请提供了一种协作机械臂的运动控制方法,包括:
根据所述运动控制任务,计算关节的关节理论运动状态;
根据所述关节理论运动状态,计算关节伺服电机的理论力矩;
根据所述理论力矩,计算所述关节伺服电机的电流限制范围;
控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
为解决上述技术问题,本申请提供了另一种协作机械臂的运动控制方法,所述运动控制方法应用于伺服控制器,包括:
根据所述运动控制任务,计算关节的关节理论运动状态;
根据所述关节理论运动状态,计算关节伺服电机的理论力矩;
将所述理论力矩发送给伺服驱动器;
其中,所述理论力矩用于供所述伺服驱动器计算关节伺服电机的电流限制范围,并控制输出给所述关节伺服电机的驱动电流不超出电流限制范围。
为解决上述技术问题,本申请提供了另一种协作机械臂的运动控制方法,所述运动控制方法应用于任意一个关节的伺服驱动器,包括:
接收关节伺服电机的理论力矩;
根据所述理论力矩,计算电流限制范围;
控制输出给关节伺服电机的驱动电流不超出所述电流限制范围。
为解决上述技术问题,本申请提供了一种协作机械臂的运动控制系统,包括上位机、伺服控制器、多个关节的伺服驱动器以及多个关节伺服电机;
所述上位机与所述伺服控制器通信连接,所述伺服控制器分别与各个所述伺服驱动器通信连接,各个所述伺服驱动器用于驱动对应的关节伺服电机;
所述协作机械臂的运动控制系统,在工作时配置有运动控制功能,所述运动控制功能包括:
如上述的运动控制方法。
为解决上述技术问题,本申请提供了一种协作机械臂的碰撞检测方法,所述碰撞检测方法包括:
根据运动控制任务,计算关节的关节理论运动状态;
根据所述关节理论运动状态,计算关节伺服电机的理论力矩和关节伺服电机的电机理论运动状态;
根据所述理论力矩,计算所述关节伺服电机的电流限制范围;
控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围;
获取所述关节伺服电机的实际运动状态;
根据所述实际运动状态和所述电机理论运动状态,计算跟踪误差;
当所述跟踪误差超过碰撞阈值时,判定所述协作机械臂发生碰撞。
为解决上述技术问题,本申请提供了另一种协作机械臂的碰撞检测方法,所述碰撞检测方法应用于一种伺服控制器,包括:
根据运动控制任务,计算关节的关节理论运动状态;
根据所述关节理论运动状态,计算关节伺服电机的理论力矩和电机理论运动状态;
将所述理论力矩和所述电机理论运动状态发送给对应的伺服驱动器;
获取所述关节伺服电机的实际运动状态;
根据所述实际运动状态和所述电机理论运动状态,计算跟踪误差;
当所述跟踪误差超过碰撞阈值时,判定所述协作机械臂发生碰撞,并发送停机指令给所述伺服驱动器;
其中,所述理论力矩用于所述伺服驱动器计算所述关节伺服电机的电流限制范围,并控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
为解决上述技术问题,本申请提供了另一种协作机械臂的碰撞检测方法,所述碰撞检测方法应用于任意一个关节的伺服驱动器,包括:
接收关节伺服电机的理论力矩和电机理论运动状态;
根据所述理论力矩,计算电流限制范围;
控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围;
接收所述关节伺服电机的实际运动状态;
根据所述关节伺服电机的电机理论运动状态和实际运动状态,计算跟踪误差;
当所述跟踪误差超过碰撞阈值时,判定所在关节发生碰撞。
为解决上述技术问题,本申请提供了一种协作机械臂的控制系统,包括上位机、伺服控制器、多个关节的伺服驱动器以及多个关节伺服电机;
所述上位机与所述伺服控制器通信连接,所述伺服控制器分别与各个所述关节的伺服驱动器通信连接,各个所述关节的伺服驱动器用于驱动对应的关节伺服电机;
所述协作机械臂的控制系统,在工作时配置有碰撞检测功能,所述碰撞检测功能包括:
如上述的碰撞检测方法。
为解决上述技术问题,本申请还提供了一种协作机械臂,所述协作机械臂包括处理器以及与所述处理器耦接的存储器;
其中,所述存储器用于存储程序数据,所述处理器用于执行所述程序数据以实现上述的运动控制方法和/或上述的碰撞检测方法。
为解决上述技术问题,本申请还提供了一种工业机器人,所述工业机器人包括处理器以及与所述处理器耦接的存储器;
其中,所述存储器用于存储程序数据,所述处理器用于执行所述程序数据以实现上述的运动控制方法和/或上述的碰撞检测方法。
为解决上述技术问题,本申请还提供了一种计算机存储介质,所述计算机存储介质用于存储程序数据,所述程序数据在被处理器执行时,用以实现上述的运动控制方法和/或上述的碰撞检测方法。
与现有技术相比,本申请的有益效果是:运动控制系统根据运动控制任务,计算关节的关节理论运动状态;根据关节理论运动状态,计算关节伺服电机的理论力矩;根据理论力矩,计算关节伺服电机的电流限制范围;控制输出给关节伺服电机的驱动电流不超出电流限制范围。上述方法可以利用关节理论运动状态以及各个关节伺服电机的理论力矩直接对各个关节的驱动电流进行限制,有效控制各个关节对外的输出力矩大小,减少协作机械臂在发生碰撞时对碰撞的障碍物或人产生的伤害,有效保护协作机械臂和人。
附图说明
图1是本申请提供的运动控制方法第一实施例的流程示意图;
图2是本申请提供的运动控制系统中协作机械臂一实施例的硬件结构示意图;
图3是本申请提供的运动控制方法一实施例的流程示意图;
图4是本申请提供的运动控制系统一实施例的硬件结构示意图;
图5是本申请提供的运动控制系统另一实施例的硬件结构示意图;
图6是本申请提供的运动控制方法第二实施例的流程示意图;
图7是本申请提供的运动控制方法第三实施例的流程示意图;
图8是本申请提供的碰撞检测方法第一实施例的流程示意图;
图9是本申请提供的运动控制方法另一实施例的流程示意图;
图10是本申请提供协作机械臂的一实施例在多个时刻的运动状态示意图;
图11是协作机械臂在图10的运动过程中的关节A的输出力矩值示意图;
图12是本申请提供的碰撞检测方法第二实施例的流程示意图;
图13是本申请提供的碰撞检测方法第三实施例的流程示意图;
图14是本申请提供的协作机械臂一实施例的硬件结构示意图;
图15是本申请提供的工业机器人一实施例的硬件结构示意图;
图16是本申请提供的计算机存储介质一实施例的结构示意图。
具体实施方式
下面将结合在本实施例中的附图,对在本实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅是本申请的一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
本申请的运动控制方法以及碰撞检测方法应用于一种协作机械臂,或者包括协作机械臂的运动控制系统等,在其他实施例中,也可以应用于其他领域或者其他功能的机器人,例如工业机器人、扫地机器人、循迹机器人等其他的关节机器人。在一种具体的实施方式中,本申请的运动控制方法以及碰撞检测方法还可以应用于一种协作机械臂,以及包括协作机械臂的运动控制系统。
其中,下面先介绍本申请的运动控制方法以及碰撞检测方法所涉及的部分关键技术术语:
基于上述技术基础,本申请提出一种应用于运动控制系统的运动控制方法,具体请参见图1至图5,图1是本申请提供的运动控制方法第一实施例的流程示意图,图2是本申请提供的运动控制系统中协作机械臂一实施例的硬件结构示意图,图3是本申请提供的运动控制方法一实施例的流程示意图,图4是本申请提供的运动控制系统一实施例的硬件结构示意图,图5是本申请提供的运动控制系统另一实施例的硬件结构示意图。
如图4和图5所示,本申请提供的协作机械臂3属于一种具体的运动控制系统,其中,运动控制系统包括上位机1、伺服控制器2以及协作机械臂3。上位机1与伺服控制器2通信连接,而伺服控制器2与协作机械臂3通信连接。
在一些实施例中,如图5所示,协作机械臂3具体包括多个关节,例如第一关节32、第二关节33以及第三关节34。其中,多个关节分别通过内置接口31与伺服控制器2通信连接。
第一关节32包括伺服驱动器321、关节伺服电机322、编码器323,第二关节33包括伺服驱动器331、关节伺服电机332、编码器333,第三关节34包括伺服驱动器341、关节伺服电机342、编码器343。
其中,伺服驱动器321、伺服驱动器331、伺服驱动器341均通过接口31与伺服控制器2通信连接,且伺服驱动器341用于驱动对应的关节伺服电机342。
在一些实施例中,协作机械臂3包括6或7个关节。
具体地,协作机械臂3所在的运动控制系统,在工作时配置有运动控制功能,其运动控制功能具体包括如图1和图3、或图1和图9所示的运动控制方法,其具体过程请参阅应用于运动控制系统的运动控制方法的具体过程,在此不再赘述。
在一些实施例中,协作机械臂3所在的控制系统,在工作时配置有碰撞检测功能,其碰撞检测功能具体包括如图3和图8、或图8和图9所示的碰撞检测方法,其具体过程请继续参阅应用于运动控制系统的碰撞检测方法,在此不再赘述。
在本实施中,运动控制系统采用位置式闭环PID调节,具体的,可以为设置有位置式闭环PID控制模块进行调节。
如图3所示,本申请所要实现的运动控制方法需要多端的同时运行和交互才能实现完整的运动控制方法,下面分别以图4和图5所示的运动控制系统作为执行主体,进一步描述运动控制系统的控制过程。
如图1所示,在本实施例的运动控制方法具体包括以下步骤:
步骤S11:根据运动控制任务,计算关节的关节理论运动状态。
在本实施例中,请参阅图3中的S101至S102,以及S201至S202。如S101和S102所示,运动控制系统中的上位机接收用户输入的运动控制指令,并将运动控制指令发送给伺服控制器。如S201和S202所示,运动控制系统中的伺服控制器接收上位机发送的运动控制指令,通过解析该运动控制指令,确定协作机械臂的运动控制任务。
具体地,运动控制指令中至少包括用户指定协作机械臂的运动状态,例如运动过程中的速度、方向,以及运动终点的具体位姿等。运动控制系统根据运动控制指令确定协作机械臂的运动控制任务,即具体的运动过程,运动控制任务包括协作机械臂每一时刻的运动规划状态。
在本实施例中,请参阅图3中的S203。运动控制系统根据用户输入的运动控制指令确定的运动控制任务进行运动规划,得到协作机械臂每一运动时刻对应各个关节的关节理论运动状态。
其中,运动规划可以由运动控制任务中用户指定协作机械臂的最终位姿,按照运动规划算法自动计算协作机械臂的各个关节的关节理论运动状态,包括但不限于:机器人中各个关节的关节转角、关节速度以及关节加速度中的至少一种或多种。
在一些实施例中,运动规划也可以由用户从多条预设的运动路径中指定协作机械臂的最终路径,从而确定协作机械臂的最终位姿,按照运动规划算法自动计算协作机械臂的各个关节的关节理论运动状态。
在本实施例中,用户在上位机上输入运动控制指令,上位机发送给伺服控制器。伺服控制器接收该运动指令后,解析该运动控制指令,确定运动控制任务,再根据运动控制任务,计算关节的关节理论运动状态。
在一此实施例中,运动控制指令是一条的运动轨迹,伺服控制器接收到该运动轨迹后,在协作机械臂的坐标系中进行解析,得到协作机械臂可以执行的运动控制任务。
在一些实施例中,运动控制指令是一个指定的空间点,伺服控制器接收到该空间点后,在协作机械臂的坐标系中标定出来,并根据运动控制算法计算得到协作机械臂的当前位置移动到该空间点的运动轨迹,该运动轨迹即可以被协作机械臂直接执行的运动控制任务。
步骤S12:根据关节理论运动状态,计算关节伺服电机的理论力矩。
在本实施例中,请参阅图3中的S204。运动控制系统根据各个关节的关节理论运动状态,计算各个关节伺服电机的理论力矩和确定调节力矩阈值。
具体地,运动控制系统将步骤S11的关节理论运动状态输入预设的逆动力学模型,关节理论运动状态包括各个关节的关节转角、关节速度以及关节加速度,通过逆动力学模型,能够计算出协作机械臂各个关节的关节理论力矩。
在一些实施例中,运动控制系统利用逆动力学模型对关节理论运动状态进行运算,得到力矩列向量,力矩列向量的各个向量元素分别对应于协作机械臂各个关节的关节理论力矩,即力矩列向量为协作机械臂各个关节的关节理论力矩的集合。
逆动力学模型的具体运算过程如下:
利用逆动力学模型中的机器人惯量矩阵和加速度列向量,获取惯量力矩向量;
利用逆动力学模型中的科氏力与离心力模型和速度列向量,获取科氏力与离心力力矩向量;
利用逆动力学模型中的重力模型和各个关节的关节转角,获取重力力矩向量;
将惯量力矩向量、科氏力与离心力力矩向量、重力力矩向量以及摩擦力力矩向量的相应向量元素相加,进而确定力矩列向量,以及协作机械臂各个关节的关节理论力矩。
在一些实施例中,逆动力学模型中所使用的列向量为协作机械臂所有关节的关节理论运动状态,即将协作机械臂所有关节的关节理论运动状态通过列向量的方式形成一个集合,从而统一输入到逆动力学模型进行运算。在运算过程中,列向量中的各个关节的关节理论运动状态相互独立,互不影响。
在一些实施例中,利用逆动力学模型中的摩擦力模型和各个关节的关节速度,获取摩擦力力矩向量,即利用摩擦力模型中的粘性摩擦函数和库伦摩擦函数对各个关节速度进行处理,得到摩擦力力矩向量中的各个向量元素。
运动控制系统所采用的逆动力学模型的具体表现公式如下:
公式中的τj,q都是列向量,列向量的长度与协作机械臂的关节数量相等;τj是通过逆动力学模型计算的各个关节的关节理论力矩,q是各个关节转角,是各个关节速度,/>是各个关节加速度,M(q)是机器人惯量矩阵,/>是科氏力与离心力矩阵,G(q)是重力产生的力矩,/>是摩擦力模型。
在本实施例使用的摩擦力模型是:其中,Fv是粘性摩擦系数,Fs是库伦摩擦系数,sign()是符号函数,指出各个关节速度的正负号。
在一些实施例中,运动控制系统采用逆动力学模型计算所得的是协作机械臂各个关节的关节理论运动状态,在协作机械臂的实际控制中,协作机械臂的各个关节实际上是根据关节伺服电机驱动减速机转动,减速机的输出轴所转动的转角即为关节转角。
因此,运动控制系统还需要将协作机械臂各个关节的关节理论运动状态计算得到各个关节的关节伺服电机的理论力矩,而关节伺服电机的理论力矩才是直接控制关节运行的相关数据。
因此,运动控制系统在计算各个关节伺服电机的理论力矩时需要考虑各个关节减速机的情况,从而得到准确的关节伺服电机的理论力矩。
在一些实施例中,运动控制系统基于上述的关节理论运动状态,计算协作机械臂中各个关节的关节理论力矩。根据协作机械臂各个关节的关节理论力矩和减速比的比值关系,计算协作机械臂各个关节伺服电机的理论力矩。
在一些实施例中,各个关节的减速比可以出厂设置,预设在协作机械臂的各个关节的伺服驱动器,也可以将所有关节的减速比均预设在伺服控制器。
在本实施例中,运动控制系统通过各个关节的关节理论力矩、各个关节减速机的减速比,计算出各个关节伺服电机的理论力矩,理论力矩用于计算关节伺服电机的电流限制范围。
具体地,通过计算各个关节的关节理论力矩和减速比的比值,得到各个关节伺服电机的理论力矩,计算公式如下:
其中,Rgear为各个关节减速机的减速比,τj为各个关节的关节理论力矩,τm为各个关节伺服电机的理论力矩。
步骤S13:根据理论力矩,计算关节伺服电机的电流限制范围。
在本实施例中,请参阅图3中的S205,以及S301至S302。运动控制系统中的伺服控制器发送各个关节的关节伺服电机的电机理论运动状态和理论力矩,及各个关节伺服电机的调节力矩阈值给各个关节的伺服驱动器。而各个关节的伺服驱动器接收伺服控制器发送的该关节伺服电机的电机理论运动状态、理论力矩及调节力矩阈值;根据关节伺服电机的理论力矩和调节力矩阈值计算电流限制范围。
在一些实施例中,运动控制系统中的伺服控制器发送各个关节的关节理论运动状态、各个关节伺服电机的电机理论运动状态和理论力矩给各个关节的伺服驱动器。各个关节的伺服驱动器接收伺服控制器发送的该关节的关节理论运动状态、关节伺服电机的理论力矩及调节力矩阈值;根据关节伺服电机的理论力矩和调节力矩阈值计算电流限制范围。
具体地,各个关节的关节理论力矩可以通过理论力矩的数值进行表征,也可以为各个关节伺服电机的理论力矩与各个关节伺服电机的额定力矩的比值,如以百分比、千分比等表现形式,额定力矩为各个关节伺服电机自身的额定力矩。
例如,具体的关节伺服电机的理论力矩数值为1.98N·m,而对应关节的关节伺服电机的额定力矩为19.8N·m时,关节伺服电机的理论力矩的千分比表现结果为100‰。
需要说明的是,后续描述中的调节力矩阈值、输出电流等均可以采用不同的表现形式,在此不一一列举。
在本实施例中,运动控制系统根据各个关节伺服电机的预设的调节力矩阈值,计算电流限制范围。
其中,各个关节伺服电机的预设的调节力矩阈值,可预先设置并存储在协作机械臂,而每个关节伺服电机的调节力矩阈值可以不同,且对于每个关节伺服电机而言,调节力矩阈值为一固定值,其取值与关节的安全档位相关。
例如,当某一关节的安全档位越高,其对应的调节力矩阈值越小,从而对电流输出约束的范围就越小,保证设备与人身安全。
在一些实施例中,运动控制系统根据各个关节伺服电机的理论力矩与调节力矩阈值确定力矩约束上限和力矩约束下限;
基于力矩约束上限与转矩常数确定电流输出约束上限;
基于力矩约束下限与转矩常数确定电流输出约束下限;
按照电流输出约束上限和电流输出约束下限,确定电流限制范围,计算公式如下:
其中,Δτm为各个关节伺服电机的调节力矩阈值、τm为各个关节伺服电机的理论力矩、kT为各个关节伺服电机的转矩常数。
步骤S14:控制输出给关节伺服电机的驱动电流不超出电流限制范围。
在本实施例中,请参阅图3中的S303。运动控制系统中的各个关节伺服驱动器将关节理论运动状态作为PID控制模块的输入,确定关节伺服电机的驱动电流,并根据电流限制范围对驱动电流进行限制。
具体地,运动控制系统按照各个关节伺服电机的电流限制范围对各个关节伺服电机的驱动电流进行约束并进行输出。
在一些实施例中,运动控制系统按照电机理论运动状态计算输出给关节伺服电机的驱动电流,按照步骤S13确定的电流限制范围对驱动电流进行约束并进行输出,从而控制关节运动,同时对各驱动电流大小进行限制,能够避免协作机械臂在发生碰撞,由于PID调节,导致输出的瞬间力矩增大,加重碰撞对设备及障碍物的伤害,有效保护协作机械臂及人身、物料的安全。
在一些实施例中,运动控制系统控制输出给各个关节伺服电机的驱动电流进行约束的方式如下:
当输出给关节伺服电机的驱动电流在电流限制范围之内时,运动控制系统不对各个关节伺服电机的驱动电流进行约束。
当输出给关节伺服电机的驱动电流超出电流输出约束上限时,按照电流输出约束上限/>对驱动电流进行调整,得到调整后的驱动电流输出。
当输出给关节伺服电机的驱动电流低于电流输出约束下限时,按照电流输出约束下限/>对驱动电流进行调整,得到调整后的驱动电流输出。
例如,运动控制系统计算得到的电流限制范围为0.4A至0.8A,而PID控制模块输出的驱动电流输出为0.6A时,则不需要对驱动电流输出进行调整,输出的驱动电流的电流值为0.6A。
例如,运动控制系统计算得到的电流限制范围为0.4A至0.8A,而PID控制模块输出的驱动电流输出为1.0A时,则需要对驱动电流输出进行按照电流约束上限进行调整,确定调整后的驱动电流为0.8A并进行输出。
又例如,若PID控制模块输出的驱动电流输出为0.3A时,判断驱动电流输出低于电流限制范围的下限,则需要对驱动电流输出进行按照电流约束下限进行调整,确定调整后的驱动电流为0.4A并进行输出。
上述驱动电流输出的具体调整流程如下:
时,iref不用被调整;
时,/>
时,/>
其中,iref为各个关节伺服电机的驱动电流,τm为各个关节伺服电机的理论力矩,Δτm为各个关节伺服电机的调节力矩阈值,kT为各个关节伺服电机的转矩常数。
通过以上调整过程,运动控制系统可以确保各个关节伺服电机的驱动电流会被限制在的范围内。
在一些实施例中,运动控制系统对驱动电流的调整方式还可以通过调整比例系数或其他预设的调节方式进行调节。
在一些实施例中,调节力矩阈值和电机的转矩常数为固定值,而各个关节伺服电机的理论力矩为瞬时值,即计算得到的电流限制范围同样应当为瞬时值。因此,电流限制范围随着各个关节伺服电机的理论力矩变化而变化。
在本实施例中,运动控制系统根据运动控制任务,计算关节的关节理论运动状态;根据关节理论运动状态,计算关节伺服电机的理论力矩;根据理论力矩,计算关节伺服电机的电流限制范围;控制输出给关节伺服电机的驱动电流不超出电流限制范围。上述方法可以利用关节理论运动状态以及各个关节伺服电机的理论力矩直接对各个关节的驱动电流进行限制,有效控制各个关节对外的输出力矩大小,减少协作机械臂在发生碰撞时对碰撞的障碍物或人产生的伤害,有效保护协作机械臂和人。
下面以运动控制系统中的伺服控制器作为执行主体继续阐述图1所示的运动控制方法:
请继续参阅图6,图6是本申请提供的运动控制方法第二实施例的流程示意图。在本实施例的运动控制方法具体应用于一种伺服控制器,如图5所示运动控制系统中的伺服控制器。
如图6所示,本实施例的运动控制方法具体包括以下步骤:
步骤S21:根据运动控制任务,计算关节的关节理论运动状态。
在本实施例中,请参阅图3中的S101至S102,以及S201至S203。
如S101和S102所示,上位机接收用户输入的运动控制指令,并将运动控制指令发送给伺服控制器。如S201和S202所示,伺服控制器接收上位机发送的运动控制指令,通过解析该运动控制指令,确定协作机械臂的运动控制任务。
在本实施例中,请参阅图3中的S203。伺服控制器根据用户输入的运动控制指令确定的运动控制任务进行运动规划,得到协作机械臂每一运动时刻对应各个关节的关节理论运动状态。
步骤S22:根据关节理论运动状态,计算关节伺服电机的理论力矩。
在本实施例中,请参阅图3中的S204。伺服控制器根据各个关节的关节理论运动状态,计算各个关节伺服电机的理论力矩和确定调节力矩阈值。
伺服控制器将步骤S21的关节理论运动状态输入预设的逆动力学模型,通过逆动力学模型,能够计算出协作机械臂各个关节的关节理论力矩。
在一些实施例中,运动控制系统采用逆动力学模型计算所得的是协作机械臂各个关节的关节理论运动状态,在协作机械臂的实际控制中,协作机械臂的各个关节根据关节伺服电机驱动减速机转动,减速机的输出轴所转动的转角即为关节转角。
因此,运动控制系统还需要将协作机械臂各个关节的关节理论运动状态计算得到各个关节的关节伺服电机的理论力矩,而关节伺服电机的理论力矩是直接控制关节运行的相关数据。
其中,减速机起匹配转速和传递转矩的作用,电机在输出扭力时,需要考虑减速机对输出扭力的影响。
在一些实施例中,伺服控制器基于上述的关节理论运动状态,计算协作机械臂中各个关节的关节理论力矩。然后,伺服控制器获取协作机械臂各个关节的减速比,基于协作机械臂各个关节的关节理论力矩和减速比,计算协作机械臂各个关节伺服电机的理论力矩。
步骤S23:将理论力矩发送给对应的伺服驱动器;其中,理论力矩用于伺服驱动器计算关节伺服电机的电流限制范围,并控制输出给关节伺服电机的驱动电流不超出电流限制范围。
在本实施例中,请参阅图3中的S205,以及S301至S302。伺服控制器发送各个关节伺服电机的理论力矩给各个对应关节的伺服驱动器。其中,下发给各个关节伺服驱动器的理论力矩用于计算关节伺服电机的电流限制范围,并控制各个关节伺服驱动器输出给关节伺服电机的驱动电流不超出电流限制范围。
在一些实施例中,伺服控制器还可以将各个关节伺服电机的理论力矩、关节理论运动状态以及各个关节的调节力矩阈值下发到各个关节的伺服驱动器,以使各个关节的伺服驱动器根据关节伺服电机的理论力矩和调节力矩阈值计算电流限制范围,以及根据关节理论运动状态计算驱动电流,最后,根据电流限制范围对驱动电流进行调整,以保证调整后的驱动电流不超出电流限制范围,调节力矩阈值可以预先设置并存储在伺服控制器,或者存储在上位机,并由上位机下发到伺服控制器。
在一些实施例中,调节力矩阈值也可以预先存储在各个关节的伺服驱动器,此时,伺服控制器只需要下发各个关节伺服电机的理论力矩、关节理论运动状态即可。
下面以运动控制系统中各个关节的伺服驱动器作为执行主体继续阐述图1所示的运动控制方法:
请继续参阅图7,图7是本申请提供的运动控制方法第三实施例的流程示意图。在本实施例的运动控制方法具体应用于一种伺服驱动器,如图5所示运动控制系统中的任意一个关节的伺服驱动器。
如图7所示,本实施例的运动控制方法具体包括以下步骤:
步骤S31:接收关节伺服电机的理论力矩。
在本实施例中,请参阅图3中的S301,伺服驱动器接收伺服控制器发送的所在关节的关节理论运动状态、关节伺服电机的理论力矩以及调节力矩阈值。
在一些实施例中,伺服驱动器接收伺服控制器发送的所在关节的关节理论运动状态、关节伺服电机的理论力矩,并从本地存储空间提取预先存储的调节力矩阈值。
具体地,伺服控制器将关节理论运动状态以及关节伺服电机的理论力矩下发到相应关节的伺服驱动器。伺服控制器把各个关节伺服电机的理论力矩τm作为指导值下发给到控制关节运动的伺服驱动器,各个关节伺服电机的理论力矩τm等数据一般通过通讯总线进行传输,常用的总线协议有CAN总线、EtherCAT总线等。在一些实施例中,对传输的通讯总线的总线协议不做具体限制。
在一些实施例中,伺服驱动器接收伺服控制发送的所在关节的关节理论运动状态,从本地存储空间提取预先存储的调节力矩阈值,同时根据关节理论运动状态计算得到关节伺服电机的电机理论运动状态。
步骤S32:根据理论力矩,计算电流限制范围。
在本实施例中,请参阅图3中的S302,伺服驱动器根据关节伺服电机的理论力矩和调节力矩阈值,计算电流限制范围。
在一些实施例中,伺服驱动器根据所在关节伺服电机的理论力矩与调节力矩阈值确定力矩约束上限和力矩约束下限;基于力矩约束上限与预设的转矩常数确定电流输出约束上限;基于力矩约束下限与预设的转矩常数确定电流输出约束下限;按照电流输出约束上限和电流输出约束下限,确定电流限制范围。
步骤S33:控制输出给关节伺服电机的驱动电流不超出电流限制范围。
在本实施例中,请参阅图3中的S303,伺服驱动器将关节理论运动状态作为PID控制模块的输入,确定关节伺服电机的驱动电流,并根据电流限制范围对驱动电流进行限制。
具体地,伺服驱动器接收伺服控制器发送的关节伺服电机的电机理论运动状态,电机理论运动状态为伺服控制器根据关节理论运动状态计算得到。伺服驱动器将电机理论运动状态输入PID控制模块,计算输出给关节伺服电机的驱动电流大小;根据电流限制范围对驱动电流进行约束,以使输出给关节伺服电机的驱动电流不超出电流限制范围。
在一些实施例中,伺服控制器根据关节理论运动状态,计算关节伺服电机的电机理论运动状态,并将电机理论运动状态发送给伺服驱动器。
在一些实施例中,伺服控制器将关节理论运动状态发送给伺服驱动器,以使所述伺服驱动器根据所述关节理论运动状态计算得到电机理论运动状态。
在一些实施例中,伺服驱动器获取所在关节伺服电机的实际转角,根据电机理论运动状态,获取所在关节伺服电机的理论转角状态。然后,伺服驱动器计算所在关节伺服电机的实际转角和理论转角状态中当前时刻对应的理论转角的差值,并将差值输入到位置式闭环PID控制模块,以通过位置式闭环PID控制模块对输出给关节伺服电机的驱动电流进行调整,且控制调整后的驱动电流不超出电流限制范围。
在一些实施例中,伺服驱动器将关节伺服电机的实际转角和理论转角输入到PID控制模块,以通过PID控制模块对输出给关节伺服电机的驱动电流进行调整,控制调整后的驱动电流不超出电流限制范围。
在一些实施例中,伺服驱动器还可以通过位置式闭环PID控制模块算输出给所在关节伺服电机的驱动电流。在一些实施例中,伺服驱动器将电机理论运动状态作为位置式闭环PID控制模块的输入,从而确定输出给所在关节伺服电机的驱动电流。
在一些实施例中,在输出给关节伺服电机的驱动电流在电流限制范围时,伺服驱动器不对所在关节伺服电机的驱动电流进行调整。在输出给关节伺服电机的驱动电流超过电流输出约束上限时,伺服驱动器按照电流输出约束上限对驱动电流进行调整,得到调整后的驱动电流输出。在输出给关节伺服电机的驱动电流低于电流输出约束下限时,伺服驱动器按照电流输出约束下限对驱动电流进行调整,得到调整后的驱动电流输出。
基于上述技术基础和运动控制方法,本申请提出一种应用于运动控制系统的碰撞检测方法,具体请参阅图3、图8、图9和图10,图8是本申请提供的碰撞检测方法第一实施例的流程示意图,图9是本申请提供的运动控制方法另一实施例的流程示意图,图10是本申请提供协作机械臂的一实施例在多个时刻的运动状态示意图。
如图8所示,本实施例的碰撞检测方法具体包括以下步骤:
步骤S41:根据运动控制任务,计算关节的关节理论运动状态。
步骤S42:根据关节理论运动状态,计算关节伺服电机的理论力矩和电机理论运动状态。
步骤S43:根据理论力矩,计算关节伺服电机的电流限制范围。
步骤S44:控制输出给关节伺服电机的驱动电流不超出电流限制范围。
在本实施例中,步骤S41至步骤S44的具体技术内容在上述实施例中的步骤S11至步骤S14中已有详细阐述,在此不再赘述。
步骤S45:获取关节伺服电机的实际运动状态。
在一些实施例中,请参阅图3及图9中的S401和S411,运动控制系统中的关节伺服电机接收驱动电流,驱动电机转子转动,编码器检测电机转子的实际转角,发送电机转子的实际转角给伺服驱动器。本实施例中,运动控制系统可以根据电机转子的实际位置确定关节伺服电机的实际运动状态。
在一些实施例中,在发生碰撞的情况下,关节伺服电机的编码器发送电机转子的实际转角给伺服驱动器,伺服驱动器获得该位置,即实际转角,伺服驱动器根据该实际转角,与电机理论运动状态的当前时刻对应的理论转角,计算实际转角与理论转角的差值,即计算出跟踪误差,然后将跟踪误差与预设的碰撞阈值进行比较,根据比较结果判断协作机械臂是否发生碰撞。当跟踪误差大于碰撞阈值,运动控制系统判定协作机械臂发生碰撞。
具体地,运动控制系统根据步骤S41至步骤S44计算确定的驱动电流驱动关节伺服电机的电机转子转动,关节伺服电机内的编码器通过检测电机转子的实际转角,并将电机转子的实际转角发送给伺服驱动器,从而使得运动控制系统获取关节伺服电机的实际运动状态。
如图10和图11所示,协作机械臂在运动过程中,分别在t1-t4时刻的位置状态的结构示意图,以及协作机械臂的关节A的输出力矩值的示意图,图11中的坐标系中,横轴表示时间,纵轴表示关节A的输出力矩值;“有约束”的实折线表示本申请中的关节A的输出力矩值,“无约束”的虚折线表示现有技术中的关节A的输出力矩值。
在本实施例中,在t1时刻,协作机械臂的关节A位于a点处,计划运动到b点处。当协作机械臂运行了一段时间后。
在t2时刻,关节A移动到c点,和障碍物B发生碰撞,此时尚未检测到协作机械臂发生了碰撞,准备继续按预定的路线运行到b点。
在t3时刻之前,虚折线和实折线所表示的力矩值基本相等。在t3时刻,协作机械臂的关节A由于受到障碍物B的阻拦,而无法进行移动,关节A对应的关节伺服电机的实际转角,在t3时刻,理论上关节A的运动位置在d点(t3`时刻与t3时刻是同一时刻,t3`时刻是指当协作机械臂正常运行时关节A在t3`时刻时的位置),此时伺服驱动器会通过PID控制模块进行主动调节,补偿误差位置,会加大关节伺服电机的输出电流,进而增大对应的关节A的输出力矩,因此t3时刻之后,关节A的输出力矩瞬间迅速增大。
现有技术中,关节A的输出力矩(单位:N·m)会迅速增加到该关节A本身能够输出的最大值(即max值),虽仍无法移动,但会在瞬时输出一个较大的力矩,加重设备和被撞物体的伤害。
在t4时刻判定协作机械臂发生了碰撞,从而停止或快速停止输出电流,对应的关节A的输出力矩会迅速下降,在t5时刻下降到0,从而抱闸停机。
在本实施例中,通过设置关节伺服电机的电流限制范围,进而限制了发生碰撞时关节A的输出力矩的上升上限,即降低了此时的输出力矩max值,从而当t4时刻判定发生了碰撞时,关节A的输出力矩值不会超过阈值上限。
本实施例的方案中,对关节伺服电机设置了输出的电流限制范围,可以使协作机械臂的输出的力矩进行约束,避免协作机械臂输出较大的力而对障碍物或人造成很大的伤害。
请继续参阅图10的协作机械臂运动状态对比图,关节A对应的电机转子转角的对比情况请继续参阅下表:
由上表分析可知,在协作机械臂的关节A与障碍物B发生碰撞之后,由于障碍物B阻碍协作机械臂按照规划路径运动,导致协作机械臂(关节A)在t2时刻后电机转子的实际转角无法继续运动,运动控制系统可以通过编码器检测关节伺服电机的电机转子的实际转角,直观地检测出协作机械臂各个关节的实际运动状态与理论运动状态之间的差异。
其中,关节伺服电机的实际运动状态包括但不限于:关节伺服电机的实际转角、实际速度等。
步骤S46:根据实际运动状态和关节伺服电机的电机理论运动状态,计算跟踪误差。
在本实施例中,请参阅图3中的S311至S312,或者图9中的S311,以及S211至S212,运动控制系统根据实际运动状态和关节伺服电机的电机理论运动状态中同一状态类型的状态数据,例如转角状态数据或者速度状态数据等,计算跟踪误差。
在一些实施例中,运动控制系统可以提取关节伺服电机的实际运动状态中关节伺服电机的实际转角,以及提取关节伺服电机的电机理论运动状态中关节伺服电机的与该实际转角的时刻对应的理论转角;然后,根据实际转角和理论转角的差值,确定转角跟踪误差。
在一些实施例中,运动控制系统可以提取关节伺服电机的实际运动状态中关节伺服电机的实际速度,以及提取关节伺服电机的电机理论运动状态中关节伺服电机的理论速度;然后,根据实际速度和理论速度的差值,确定速度跟踪误差。其中,关节电机转子的实际速度可以通过关节电机转子的实际转角变化快慢计算。
综上,运动控制系统的跟踪误差可以包括上述的转角跟踪误差和速度跟踪误差中的一种或多种的组合。
步骤S47:当跟踪误差超过碰撞阈值时,判定协作机械臂发生碰撞。
在本实施例中,请参阅图3中的S313至S314,或者图9中的S213至S214,运动控制系统通过比较步骤S46计算实时的跟踪误差与预设的碰撞阈值,当跟踪误差超过碰撞阈值的情况下,即可判定协作机械臂发生碰撞。
在一些实施例中,当协作机械臂在运动过程中碰到障碍物时,由于输出扭力被限制,电机转子的实际转角θactual、实际速度就会和理论转角θref、理论速度/>出现比较明显的误差,这个误差一般被称为转角跟踪误差和速度跟踪误差。通过判断转角跟踪误差或速度跟踪误差的大小,可以检测出协作机械臂是否发生了碰撞。
其具体的碰撞检测方法如下:
在一些实施例中,运动控制系统基于各个关节的实际转角和理论转角的差值,确定转角跟踪误差;在转角跟踪误差超过碰撞阈值时,判定协作机械臂发生碰撞。
在一些实施例中,运动控制系统基于各个关节的实际速度和理论速度的差值,确定速度跟踪误差;在速度跟踪误差超过碰撞阈值时,判定协作机械臂发生碰撞。
在一些实施例中,运动控制系统在确定速度跟踪误差超过碰撞阈值,和转角跟踪误差超过碰撞阈值中的任意一个条件满足时,即可判定协作机械臂发生碰撞。
上述碰撞检测过程的具体流程如下:
a.给各个关节预先设定触发转角跟踪误差过大报警的碰撞阈值Δθ和触发速度跟踪误差过大报警的碰撞阈值
b.当|θref-θactual|≥Δθ时,触发转角跟踪误差过大判定协作机械臂发生碰撞。
c.或当时,触发速度跟踪误差过大判定协作机械臂发生碰撞。
这样,当协作机械臂在运动过程中碰到障碍物时,就可以通过对转角/速度的跟踪误差大小的判断来检测碰撞的发生,使关节可以安全停下来。
在一些实施例中,工作人员可以根据不同的负载、机器人运动速度和人机协作的实际安全要求,配置调节力矩阈值Δτm、转角跟踪误差过大报警的阈值Δθ和速度跟踪误差过大报警的阈值比如说当设置的负载较大时,系统对应的配置较大的Δτm;当协作机械臂运动速度较快时,则要配置较大的Δθ和/>人机协作安全要求较高时,则要配置较小的Δτm。
在一些实施例中,调节力矩阈值Δτm、转角跟踪误差过大报警的阈值Δθ和速度跟踪误差过大报警的阈值还可以根据预设的配置规则,基于负载、机器人运动速度和人机协作的实际安全要求进行自动配置。
在一些实施例中,请继续参阅图3和图9中的S3221至S3222,在判定协作机械臂发生碰撞后,运动控制系统停止向各个关节伺服电机供电,以使各个关节伺服电机失电停转,并停止向各个关节的抱闸器供电,以使各个关节的抱闸器失电抱闸,从而使得协作机械臂能够在判定发生碰撞后及时停止。
在一些实施例中,运动控制系统还可以控制各个关节执行回退动作。具体地,关节的回退动作可以为:各个关节按照电机理论运动状态所指示的运动方向的反方向运动一段距离。
在一些实施例中,当判定协作机械臂发生碰撞时,运动控制系统控制各个关节执行回退动作后,停止向各个关节伺服电机供电,并停止向各个关节的抱闸器供电,以使各个关节伺服电机失电停转、各个关节的抱闸器失电抱闸。
在本实施例中,协作机械臂在运动时,根据理论运动状态对协作机械臂的输出力矩进行约束;运动控制系统通过关节伺服电机的编码器监测所在关节的实际运动状态,根据实际运动状态与理论运动状态的跟踪误差进行检测协作机械臂是否发生碰撞。通过该碰撞检测方法,使协作机械臂在发生碰撞时,有效控制协作机械臂的各个关节对外的输出力矩大小,在碰撞检测过程中,减少在碰撞检测过程中协作机械臂在发生碰撞时对碰撞的障碍物或人产生的伤害,有效保护协作机械臂和人。
下面以运动控制系统中的伺服控制器作为执行主体继续阐述图8所示的碰撞检测方法:
请继续参阅图12,图12是本申请提供的碰撞检测方法第二实施例的流程示意图。在本实施例的碰撞检测方法具体应用于一种伺服控制器,如图5所示运动控制系统中的伺服控制器。
如图12所示,在本实施例的碰撞检测方法具体包括以下步骤:
步骤S51:根据运动控制任务,计算关节的关节理论运动状态。
步骤S52:根据关节理论运动状态,计算关节伺服电机的理论力矩以及各个关节伺服电机的电机理论运动状态,其中,理论力矩用于伺服驱动器计算关节伺服电机的电流限制范围,并控制输出给关节伺服电机的驱动电流不超出电流限制范围。
步骤S53:将理论力矩和电机理论运动状态发送给对应的伺服驱动器。
在本实施例中,步骤S51至步骤S53的具体技术内容在上述实施例中的步骤S21至步骤S23中已有详细阐述,在此不再赘述。
步骤S54:获取关节伺服电机的实际运动状态。
在本实施例中,请参阅图9中的S211,伺服控制器通过伺服驱动器获取关节伺服电机的编码器检测的电机转子的实际转角,利用电机转子的实际位置确定关节伺服电机的实际运动状态。
步骤S55:根据实际运动状态和关节伺服电机的电机理论运动状态,计算跟踪误差。
在本实施例中,请参阅图9中的S212,伺服控制器根据实际运动状态和关节伺服电机的电机理论运动状态的同一状态类型,计算跟踪误差。
在一些实施例中,伺服控制器获取关节伺服电机的当前时刻的实际转角,以及获取关节伺服电机的电机理论运动状态中关节伺服电机的与当前时刻对应的理论转角,根据实际转角和理论转角的差值,确定转角跟踪误差。
在一些实施例中,伺服控制器提取关节伺服电机的实际运动状态中关节伺服电机的实际速度、以及提取关节伺服电机的电机理论运动状态中关节伺服电机的理论速度,根据实际速度和理论速度的差值,确定速度跟踪误差。关节电机转子的实际速度通过关节电机转子的实际转角变化快慢计算。
综上,伺服控制器的跟踪误差可以包括上述的转角跟踪误差和速度跟踪误差中的一种或多种的组合。
步骤S56:当跟踪误差超过碰撞阈值时,判定协作机械臂发生碰撞,并发送停机指令给伺服驱动器。
在在本实施例中,请参阅图9中的S213至S214,伺服控制器通过比较步骤S56计算实时的跟踪误差与预设的碰撞阈值,当跟踪误差超过碰撞阈值的情况下,即判定所在关节发生碰撞。
在一些实施例中,请继续参阅图9中的S2151至S2152,伺服控制器判定发生碰撞之后,一方面可以发送停机指令给所有关节的伺服驱动器,以使所有关节根据停止指令停止运动;另一方面可以发送碰撞报告给上位机,以使上位机发出碰撞警告,告知工作人员此时协作机械臂发生碰撞。
在一些实施例中,上位机通过显示屏上加载指定的画面以发出碰撞警告。
在一些实施例中,上位机设置有LED灯,上位机通过LED发出指定颜色的光或以通过控制LED灯指定的频率闪烁以发出碰撞警告。
在一些实施例中,上位机设置有扬声器,上位机通过扬声器发出指定的声音以发出碰撞警告。
参照图3,在一些实施例中,运动控制系统执行以下步骤:
S101:上位机接收用户输入的运动控制指令;
S102:上位机发送运动控制指令给伺服控制器;
S201:伺服控制器接收上位机发送的运动控制指令;
S202:伺服控制器解析运动控制指令,确定运动控制任务;
S203:伺服控制器根据运动控制任务,计算各关节的关节理论运动状态;
S204:伺服控制器根据各关节的关节理论运动状态,计算各关节伺服电机的理论力矩和电机理论运动状态,以及确定调节力矩阈值;
S205:伺服控制器发送各关节伺服电机的电机理论运动状态、理论力矩及调节力矩阈值给各关节伺服驱动器;
S301:伺服驱动器接收伺服控制器发送的该关节伺服电机的电机理论运动状态、理论力矩及调节力矩阈值;
S302:伺服驱动器根据关节伺服电机的理论力矩和调节力矩阈值计算电流限制范围;
S303:伺服驱动器将电机理论运动状态作为PID控制模块的输入,计算关节伺服电机的驱动电流,并根据电流限制范围对驱动电流进行限制;
S401:关节伺服电机接收驱动电流,驱动电机转子转动,编码器检测电机转子的实际转角,发送电机转子的实际转角给伺服驱动器;
S411:关节伺服电机发送异常的电机转子的实际转角给伺服驱动器;
S311:伺服驱动器接收异常的电机转子的实际转角;
S312:伺服驱动器将异常的电机转子的实际转角与电机理论运动状态中电机转子的理论转角进行比较,计算跟踪误差;
S313:伺服驱动器比较跟踪误差是否大于碰撞阈值;
S314:若比较结果为是,则伺服驱动器判定发生碰撞;
S3151:伺服驱动器发送该关节的碰撞信号给伺服控制器;
S3152:伺服驱动器停止向抱闸器输出电流;
S3153:伺服驱动器停止向关节伺服电机输出电流;
S501:碰撞关节的抱闸器失电抱闸;
S421:碰撞关节的关节伺服电机失电停转;
S211:伺服控制器接收伺服驱动器发送的关节的碰撞信号;
S212:伺服控制器根据关节的碰撞信号,确认设备发生碰撞;
S2131:伺服控制器生成停机指令,发送给其他所有非碰撞关节的伺服驱动器;
S321:所有非碰撞关节的伺服驱动器接收停机指令;
S3221:所有非碰撞关节的伺服驱动器停止向关节伺服电机输出电流;
S3222:所有非碰撞关节的伺服驱动器停止向抱闸器输出电流;
S2132:伺服控制器发送碰撞报告给上位机;
S501:所有非碰撞关节的抱闸器失电抱闸;
S421:所有非碰撞关节的关节伺服电机失电停转;
S111:上位机接收碰撞报告;
S112:上位机发出碰撞警告。
参照图9,在一些实施例中,运动控制系统执行以下步骤:
S101:上位机接收用户输入的运动控制指令;
S102:上位机发送运动控制指令给伺服控制器;
S201:伺服控制器接收上位机发送的运动控制指令;
S202:伺服控制器解析运动控制指令,确定运动控制任务;
S203:伺服控制器根据运动控制任务,计算各关节的关节理论运动状态;
S204:伺服控制器根据各关节的关节理论运动状态,计算各关节伺服电机的理论力矩和电机理论运动状态,以及确定调节力矩阈值;
S205:伺服控制器发送各关节伺服电机的电机理论运动状态、理论力矩、及调节力矩阈值给各关节伺服驱动器;
S301:伺服驱动器接收伺服控制器发送的该关节伺服电机的电机理论运动状态、理论力矩及调节力矩阈值;
S302:伺服驱动器根据关节伺服电机的理论力矩和调节力矩阈值计算电流限制范围;
S303:伺服驱动器将电机理论运动状态作为PID控制模块的输入,计算关节伺服电机的驱动电流,并根据电流限制范围对驱动电流进行限制;
S401:关节伺服电机接收驱动电流,驱动电机转子转动,编码器检测电机转子的实际转角,发送电机转子的实际转角给伺服驱动器;
S411:关节伺服电机发送异常的电机转子的实际转角给伺服驱动器;
S311:伺服驱动器接收异常的电机转子的实际转角,发送给伺服控制器;
S211:伺服控制器接收异常的电机转子的实际转角;
S212:伺服控制器将异常的电机转子的实际转角与理论运动状态中电机转子的理论转角进行比较,计算跟踪误差;
S213:伺服控制器比较跟踪误差是否大于碰撞阈值;
S214:若比较结果为是,则伺服控制器判定发生碰撞;
S2151:伺服控制器发送停机指令给关节的伺服驱动器;
S321:所有的伺服驱动器接收停机指令;
S3221:所有的伺服驱动器停止向关节伺服电机输出电流;
S421:所有关节伺服电机失电停转;
S3222:所有的伺服驱动器停止向抱闸器输出电流;
S501:所有的关节的抱闸器失电抱闸;
S2152:伺服控制器发送碰撞报告给上位机;
S111:上位机接收碰撞报告;
S112:上位机发出碰撞警告。
综上,本申请的技术方案,在控制协作机械臂运动的过程中,根据协作机械臂的理论运动状态来计算电流限制范围,并控制协作机械臂的各个关节伺服电机的驱动电流不超出该电流限制范围;利用关节理论运动状态以及各个关节伺服电机的理论力矩直接对各个关节的驱动电流进行限制,有效控制各个关节对外的输出力矩大小,减少协作机械臂在发生碰撞时对碰撞的障碍物或人产生的伤害,有效保护协作机械臂和人。
如下表格中展示了示例性的示出了某协作机械臂以及工业安全的相关参数。
/>
根据上述图表数据结合上述实施例容易看出,通过对协作机械臂的各个关节的驱动电流进行限制,能够限制碰撞时关节的输出力矩增大区间,进而使协作机械臂在发生碰撞时,对碰撞物造成的最大的碰撞夹伤压力也仅在200N,远小于一般工况下协作机械臂输出最大输出力矩时,产生的1000N碰撞夹伤压力,且低于人体能承受的最大安全碰撞压力210N,能有效提高协作机械臂工作时的安全性。应当理解的是,本申请技术方案主要追求的是降低碰撞发生时对人体造成的伤害,无非完全避免伤害发生。
根据调节力矩阈值来设置电流限制范围的电流输出约束上限和电流输出约束下限,调节力矩阈值可以是根据用户通过上位机发送,从而便捷的根据用户的需求设置电流限制范围。
调节力矩阈值也可以是根据当前环境或应用场景的安全档位相关,可以根据不同的安全需求灵活的设置电流限制范围的大小,能满足协作机械臂在不同的使用环境或应用场景下的不同需求。
下面以运动控制系统中各个关节的伺服驱动器作为执行主体继续阐述图8所示的碰撞检测方法:
请继续参阅图13,图13是本申请提供的碰撞检测方法第三实施例的流程示意图。在本实施例的碰撞检测方法具体应用于一种伺服驱动器,如图5所示运动控制系统中的任意一个关节的伺服驱动器。
如图13所示,在本实施例的碰撞检测方法具体包括以下步骤:
步骤S61:接收关节伺服电机的电机理论运动状态和理论力矩。
步骤S62:根据理论力矩,计算电流限制范围。
步骤S63:控制输出给伺关节服电机的驱动电流不超出电流限制范围。
步骤S64:接收关节伺服电机的实际运动状态。
在本实施例中,请参阅图3中的S311,伺服驱动器获取关节伺服电机的编码器检测的电机转子的实际转角,利用电机转子的实时位置确定关节伺服电机的实际运动状态。
步骤S65:根据关节伺服电机的电机理论运动状态和实际运动状态,计算跟踪误差。
步骤S66:当跟踪误差超过碰撞阈值时,判定所在关节发生碰撞。
基于上述运动控制方法和碰撞检测方法的实施例,本申请还提出了一种协作机械臂,具体请参阅图14,图14是本申请提供的协作机械臂一实施例的硬件结构示意图。
本实施例的协作机械臂700包括处理器71、存储器72、输入输出设备73以及总线74。
该处理器71、存储器72、输入输出设备73分别与总线74相连,该存储器72中存储有程序数据,处理器71用于执行程序数据以实现上述实施例的运动控制方法和/或碰撞检测方法。
在本实施例中,处理器71还可以称为CPU(Central Processing Unit,中央处理单元)。处理器71可能是一种集成电路芯片,具有信号的处理能力。处理器71还可以是通用处理器、数字信号处理器(DSP,Digital Signal Process)、专用集成电路(ASIC,ApplicationSpecific Integrated Circuit)、现场可编程门阵列(FPGA,Field Programmable GateArray)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器71也可以是任何常规的处理器等。
基于上述运动控制方法和碰撞检测方法的实施例,本申请还提出了一种工业机器人,具体请参阅图15,图15是本申请提供的工业机器人一实施例的硬件结构示意图。
本实施例的工业机器人800包括处理器81、存储器82、输入输出设备83以及总线84。
该处理器81、存储器82、输入输出设备83分别与总线84相连,该存储器82中存储有程序数据,处理器81用于执行程序数据以实现上述实施例的运动控制方法和/或碰撞检测方法。
本申请还提供一种计算机存储介质,请继续参阅图16,图16是本申请提供的计算机存储介质一实施例的结构示意图,该计算机存储介质900中存储有程序数据91,该程序数据91在被处理器执行时,用以实现上述实施例的运动控制方法/或碰撞检测方法。
本申请的实施例以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)或处理器(processor)执行本申请各个实施方式所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅为本申请的实施方式,并非因此限制本申请的专利范围,方式利用本申请说明书及附图内容所作的等效结构或等效流程变换,或直接或间接运用在其他相关的技术领域,均同理包括在本申请的专利保护范围内。
Claims (45)
1.一种协作机械臂的运动控制方法,其特征在于,包括:
根据运动控制任务,计算关节的关节理论运动状态;
根据所述关节理论运动状态,计算关节伺服电机的理论力矩;
根据所述理论力矩,计算所述关节伺服电机的电流限制范围;
控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
2.根据权利要求1所述的运动控制方法,其特征在于,
所述根据所述理论力矩,计算所述关节伺服电机的电流限制范围,包括:
根据所述理论力矩以及所述关节伺服电机的调节力矩阈值,计算所述电流限制范围。
3.根据权利要求2所述的运动控制方法,其特征在于,
所述电流限制范围包括电流输出约束上限和电流输出约束下限;
所述控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围,包括:
当所述给所述关节伺服电机的驱动电流超过所述电流输出约束上限时,按照所述电流输出约束上限对所述驱动电流进行调整,得到调整后的驱动电流;
当所述给所述关节伺服电机的驱动电流低于所述电流输出约束下限时,按照所述电流输出约束下限对所述驱动电流进行调整,得到调整后的驱动电流。
4.根据权利要求3所述的运动控制方法,其特征在于,所述方法还包括:
根据所述理论力矩与所述调节力矩阈值确定力矩约束上限和力矩约束下限;
基于所述力矩约束上限与预设的转矩常数计算所述电流输出约束上限;
基于所述力矩约束下限与所述预设的转矩常数计算所述电流输出约束下限。
5.根据权利要求1所述的运动控制方法,其特征在于,
所述根据运动控制任务,计算关节的关节理论运动状态之后,所述运动控制方法还包括:
根据所述关节理论运动状态计算所述关节伺服电机的电机理论运动状态;
将所述电机理论运动状态输入对应的PID控制模块,计算所述关节伺服电机的驱动电流。
6.根据权利要求5所述的运动控制方法,其特征在于,
所述电机理论运动状态包括理论转角状态,所述PID控制模块为位置式闭环PID控制模块。
7.根据权利要求6所述的运动控制方法,其特征在于,
所述控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围,包括:
获取所述关节伺服电机的实际转角;
根据所述电机理论运动状态,获取当前时刻对应的所述关节伺服电机的理论转角;
将所述实际转角和所述理论转角输入到所述PID控制模块或将所述实际转角与所述理论转角的差值输入到所述PID控制模块,通过所述PID控制模块对输出给所述关节伺服电机的驱动电流进行调整,控制调整后的驱动电流不超出所述电流限制范围。
8.根据权利要求1所述的运动控制方法,其特征在于,所述根据所述关节理论运动状态,计算关节伺服电机的理论力矩,包括;
将所述关节理论运动状态输入预设的逆动力学模型;
利用所述逆动力学模型对所述关节理论运动状态进行运算,得到关节理论力矩;
获取所述关节的减速比;
根据所述关节理论力矩和所述减速比,计算所述关节伺服电机的理论力矩;
其中,所述关节理论运动状态包括关节转角、关节速度以及关节加速度。
9.根据权利要求8所述的运动控制方法,其特征在于,
所述利用所述逆动力学模型对所述关节理论运动状态进行运算,得到关节理论力矩,包括:
利用所述逆动力学模型中的机器人惯量和加速度,获取惯量力矩;
利用所述逆动力学模型中的科氏力与离心力模型和速度,获取科氏力与离心力力矩;
利用所述逆动力学模型中的重力模型和关节转角,获取重力力矩;
利用所述逆动力学模型中的摩擦力模型和关节速度,获取摩擦力力矩;
将所述惯量力矩、所述科氏力与离心力力矩、所述重力力矩以及所述摩擦力力矩相加,得到所述关节理论力矩。
10.根据权利要求9所述的运动控制方法,其特征在于,
所述利用所述逆动力学模型中的摩擦力模型和关节速度,获取摩擦力力矩,包括:
利用所述摩擦力模型中的粘性摩擦函数和库伦摩擦函数对所述关节速度进行处理,得到所述摩擦力力矩。
11.根据权利要求1所述的运动控制方法,其特征在于,
所述根据运动控制任务,计算关节的关节理论运动状态之前,所述运动控制方法还包括:
接收运动控制指令,解析所述运动控制指令,确定所述运动控制任务。
12.一种协作机械臂的运动控制方法,其特征在于,所述运动控制方法应用于伺服控制器,包括:
根据所述运动控制任务,计算关节的关节理论运动状态;
根据所述关节理论运动状态,计算关节伺服电机的理论力矩;
将所述理论力矩发送给伺服驱动器;
其中,所述理论力矩用于供所述伺服驱动器计算所述关节伺服电机的电流限制范围,并控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
13.根据权利要求12所述的运动控制方法,其特征在于,所述将所述理论力矩发送给伺服驱动器,包括:
将所述理论力矩和所述关节伺服电机的调节力矩阈值发送给所述伺服驱动器,用于供所述伺服驱动器计算所述电流限制范围,并控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
14.根据权利要求12所述的运动控制方法,其特征在于,所述根据所述关节理论运动状态,计算关节伺服电机的理论力矩,包括:
将所述关节理论运动状态输入预设的逆动力学模型;
利用所述逆动力学模型对所述关节理论运动状态进行运算,得到关节理论力矩;
获取所述关节的减速比;
根据所述关节理论力矩和所述减速比,计算所述关节伺服电机的理论力矩;
其中,所述关节理论运动状态包括关节转角、关节速度以及关节加速度。
15.根据权利要求14所述的运动控制方法,其特征在于,所述利用所述逆动力学模型对所述关节理论运动状态进行运算,得到关节理论力矩,包括:
利用所述逆动力学模型中的机器人惯量和加速度,获取惯量力矩;
利用所述逆动力学模型中的科氏力与离心力模型和速度,获取科氏力与离心力力矩;
利用所述逆动力学模型中的重力模型和关节转角,获取重力力矩;
利用所述逆动力学模型中的摩擦力模型和关节速度,获取摩擦力力矩;
将所述惯量力矩向量、所述科氏力与离心力力矩向量、所述重力力矩向量以及所述摩擦力力矩相加,得到所述关节理论力矩。
16.根据权利要求15所述的运动控制方法,其特征在于,
所述利用所述逆动力学模型中的摩擦力模型和关节速度,获取摩擦力力矩,包括:
利用所述摩擦力模型中的粘性摩擦函数和库伦摩擦函数对所述关节速度进行处理,得到所述摩擦力力矩。
17.根据权利要求12所述的运动控制方法,其特征在于,
所述根据所述运动控制任务,计算关节的关节理论运动状态之前,所述运动控制方法还包括:
接收运动控制指令,解析所述运动控制指令,确定所述运动控制任务。
18.根据权利要求12所述的运动控制方法,其特征在于,还包括:
根据所述关节理论运动状态,计算所述关节伺服电机的电机理论运动状态,并将所述电机理论运动状态发送给所述伺服驱动器;
或者,将所述关节理论运动状态发送给所述伺服驱动器,以使所述伺服驱动器根据所述关节理论运动状态计算得到所述电机理论运动状态;
其中,所述电机理论运动状态用于输入PID控制模块计算所述关节伺服电机的驱动电流。
19.一种协作机械臂的运动控制方法,其特征在于,所述运动控制方法应用于任意一个关节的伺服驱动器,包括:
接收关节伺服电机的理论力矩;
根据所述理论力矩,计算电流限制范围;
控制输出给关节伺服电机的驱动电流不超出所述电流限制范围。
20.根据权利要求19所述的运动控制方法,其特征在于,
所述根据所述理论力矩,计算电流限制范围,包括:
根据所述理论力矩和调节力矩阈值,计算所述电流限制范围;
其中,所述调节力矩阈值由伺服控制器发送。
21.根据权利要求20所述的运动控制方法,其特征在于,
所述运动控制方法,还包括:
接收所述关节伺服电机的电机理论运动状态,所述电机理论运动状态由所述伺服控制器根据关节理论运动状态计算得到;
或者,接收所述关节的关节理论运动状态,根据所述关节理论运动状态计算电机理论运动状态;
所述控制输出给关节伺服电机的驱动电流不超出所述电流限制范围,包括:
将所述电机理论运动状态输入PID控制模块,计算所述关节伺服电机的驱动电流;
根据所述电流限制范围对所述驱动电流进行约束,以使输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
22.根据权利要求21所述的运动控制方法,其特征在于,所述运动控制方法还包括:
获取所述关节伺服电机的实际转角;
根据所述电机理论运动状态,获取当前时刻对应的所述关节伺服电机的理论转角;
将所述实际转角和所述理论转角输入到所述PID控制模块或将所述实际转角与所述理论转角的差值输入到所述PID控制模块,通过所述PID控制模块对输出给所述关节伺服电机的驱动电流进行调整,控制调整后的驱动电流不超出所述电流限制范围。
23.根据权利要求20所述的运动控制方法,其特征在于,
所述电流限制范围包括电流输出约束上限和电流输出约束下限;
所述控制输出给关节伺服电机的驱动电流不超出所述电流限制范围,包括:
当所述给关节伺服电机的驱动电流超过所述电流输出约束上限时,按照所述电流输出约束上限对所述驱动电流进行调整,得到调整后的驱动电流;
当所述给关节伺服电机的驱动电流低于所述电流输出约束下限时,按照所述电流输出约束下限对所述驱动电流进行调整,得到调整后的驱动电流。
24.根据权利要求23所述的运动控制方法,其特征在于,所述方法还包括:
根据所述理论力矩与所述调节力矩阈值确定力矩约束上限和力矩约束下限;
基于所述力矩约束上限与预设的转矩常数计算所述电流输出约束上限;
基于所述力矩约束下限与所述预设的转矩常数计算所述电流输出约束下限。
25.一种协作机械臂的运动控制系统,包括上位机、伺服控制器、多个关节的伺服驱动器以及多个关节伺服电机;
所述上位机与所述伺服控制器通信连接,所述伺服控制器分别与各个所述伺服驱动器通信连接,各个所述伺服驱动器用于驱动对应的关节伺服电机;
其特征在于,所述协作机械臂的运动控制系统,在工作时配置有运动控制功能,所述运动控制功能包括:
如权利要求1-11任一项所述的运动控制方法。
26.一种协作机械臂的碰撞检测方法,其特征在于,所述碰撞检测方法包括:
根据运动控制任务,计算关节的关节理论运动状态;
根据所述关节理论运动状态,计算关节伺服电机的理论力矩和电机理论运动状态;
根据所述理论力矩,计算所述关节伺服电机的电流限制范围;
控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围;
获取所述关节伺服电机的实际运动状态;
根据所述实际运动状态和所述电机理论运动状态,计算跟踪误差;
当所述跟踪误差超过碰撞阈值时,判定所述协作机械臂发生碰撞。
27.根据权利要求26所述的碰撞检测方法,其特征在于,
所述判定所述协作机械臂发生碰撞之后,包括:
停止向所述协作机械臂的各个关节伺服电机供电;和/或,
停止向所述协作机械臂的各个关节的抱闸器供电。
28.根据权利要求26所述的碰撞检测方法,其特征在于,
所述判定所述协作机械臂发生碰撞之后,所述碰撞检测方法还包括:
控制所述协作机械臂的各个关节执行回退动作。
29.根据权利要求26所述的碰撞检测方法,其特征在于,
所述根据所述实际运动状态和关节伺服电机的电机理论运动状态,计算跟踪误差,包括:
基于所述关节伺服电机的实际运动状态,获取所述关节伺服电机的实际转角;
基于所述关节伺服电机的电机理论运动状态,获取所述关节伺服电机的理论转角;
根据所述实际转角和理论转角的差值,确定转角跟踪误差。
30.根据权利要求26所述的碰撞检测方法,其特征在于,
所述根据所述实际运动状态和关节伺服电机的电机理论运动状态,计算跟踪误差,包括:
基于所述关节伺服电机的实际运动状态,获取所述关节伺服电机的实际速度;
基于所述关节伺服电机的电机理论运动状态,获取所述关节伺服电机的理论速度;
根据所述实际速度和理论速度的差值,确定速度跟踪误差。
31.根据权利要求30所述的碰撞检测方法,其特征在于,
所述实际速度通过所述关节的位置变化快慢计算。
32.根据权利要求26所述的碰撞检测方法,其特征在于,
所述碰撞阈值根据所述关节的负载和/或所述关节的运动速度确定。
33.一种协作机械臂的碰撞检测方法,其特征在于,所述碰撞检测方法应用于一种伺服控制器,包括:
根据运动控制任务,计算关节的关节理论运动状态;
根据所述关节理论运动状态,计算关节伺服电机的理论力矩和电机理论运动状态;
将所述理论力矩和所述电机理论运动状态发送给对应的伺服驱动器;
获取所述关节伺服电机的实际运动状态;
根据所述实际运动状态和所述电机理论运动状态,计算跟踪误差;
当所述跟踪误差超过碰撞阈值时,判定所述协作机械臂发生碰撞,并发送停机指令给所述伺服驱动器;
其中,所述理论力矩用于所述伺服驱动器计算所述关节伺服电机的电流限制范围,并控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
34.根据权利要求33所述的碰撞检测方法,其特征在于,还包括:
当判定所述协作机械臂发生碰撞时,发送碰撞报告给上位机,以使所述上位机根据所述碰撞报告发出碰撞警告。
35.根据权利要求33所述的碰撞检测方法,其特征在于,
所述根据所述实际运动状态和关节伺服电机的电机理论运动状态,计算各个关节的跟踪误差,包括:
基于所述关节伺服电机的实际运动状态,获取所述关节伺服电机的实际转角;
基于所述关节伺服电机的电机理论运动状态,获取关节伺服电机的理论转角;
根据所述实际转角和所述理论转角的差值,确定转角跟踪误差。
36.根据权利要求33所述的碰撞检测方法,其特征在于,
所述根据所述实际运动状态和所述关节伺服电机的电机理论运动状态,计算跟踪误差,包括:
基于所述关节伺服电机的实际运动状态,获取所述关节伺服电机的实际速度;
基于所述关节伺服电机的电机理论运动状态,获取所述关节伺服电机的理论速度;
根据所述实际速度和所述理论速度的差值,确定速度跟踪误差。
37.一种协作机械臂的碰撞检测方法,其特征在于,所述碰撞检测方法应用于任意一个关节的伺服驱动器,包括:
接收关节伺服电机的理论力矩和电机理论运动状态;
根据所述理论力矩,计算电流限制范围;
控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围;
接收所述关节伺服电机的实际运动状态;
根据所述关节伺服电机的电机理论运动状态和实际运动状态,计算跟踪误差;
当所述跟踪误差超过碰撞阈值时,判定所在关节发生碰撞。
38.根据权利要求37所述的碰撞检测方法,其特征在于,
所述判定所在关节发生碰撞之后,所述碰撞检测方法还包括:
停止向所在关节伺服电机供电;和/或,
停止向所在关节的抱闸器供电。
39.根据权利要求37所述的碰撞检测方法,其特征在于,
所述判定所在关节发生碰撞之后,所述碰撞检测方法还包括:
发送关节的碰撞信号给伺服控制器,以使所述伺服控制器根据所述碰撞信号向其他关节的伺服驱动器发送执行回退指令和/或停止指令。
40.根据权利要求37所述的碰撞检测方法,其特征在于,
所述根据所述关节伺服电机的电机理论运动状态和实际运动状态,计算跟踪误差,包括:
基于所述关节伺服电机的实际运动状态,获取所述关节伺服电机的实际转角;
基于关节伺服电机的电机理论运动状态,获取所述关节伺服电机的理论转角;
根据所述实际转角和所述理论转角的差值,确定转角跟踪误差。
41.根据权利要求37所述的碰撞检测方法,其特征在于,
所述根据所述关节伺服电机的电机理论运动状态和实际运动状态,计算跟踪误差,包括:
基于所述关节伺服电机的实际运动状态,获取所述关节伺服电机的实际速度;
基于所述关节伺服电机的电机理论运动状态,获取所述关节伺服电机的理论速度;
根据所述实际速度和所述理论速度的差值,确定速度跟踪误差。
42.一种协作机械臂的控制系统,包括上位机、伺服控制器、多个关节的伺服驱动器以及多个关节伺服电机;
所述上位机与所述伺服控制器通信连接,所述伺服控制器分别与各个所述关节的伺服驱动器通信连接,各个所述关节的伺服驱动器用于驱动对应的关节伺服电机;
其特征在于:
所述协作机械臂的控制系统,在工作时配置有碰撞检测功能,所述碰撞检测功能包括:
如权利要求26-32任一项所述的碰撞检测方法。
43.一种协作机械臂,其特征在于,所述协作机械臂包括处理器以及与所述处理器耦接的存储器;
其中,所述存储器用于存储程序数据,所述处理器用于执行所述程序数据以实现如权利要求1-11、12-18、19-24任一项所述的运动控制方法和/或26-32、33-36、37-41任一项所述的碰撞检测方法。
44.一种工业机器人,其特征在于,所述工业机器人包括处理器以及与所述处理器耦接的存储器;
其中,所述存储器用于存储程序数据,所述处理器用于执行所述程序数据以实现如权利要求1-11、12-18、19-24任一项所述的运动控制方法和/或26-32、33-36、37-41任一项所述的碰撞检测方法。
45.一种计算机存储介质,其特征在于,所述计算机存储介质用于存储程序数据,所述程序数据在被处理器执行时,用以实现如权利要求1-11、12-18、19-24任一项所述的运动控制方法和/或26-32、33-36、37-41任一项所述的碰撞检测方法。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211220131.1A CN115674190B (zh) | 2022-09-30 | 2022-09-30 | 协作机械臂及其运动控制方法、碰撞检测方法、控制系统 |
PCT/CN2022/138465 WO2024066063A1 (zh) | 2022-09-30 | 2022-12-12 | 协作机械臂及其运动控制方法、碰撞检测方法、控制系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211220131.1A CN115674190B (zh) | 2022-09-30 | 2022-09-30 | 协作机械臂及其运动控制方法、碰撞检测方法、控制系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115674190A CN115674190A (zh) | 2023-02-03 |
CN115674190B true CN115674190B (zh) | 2024-05-07 |
Family
ID=85064152
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211220131.1A Active CN115674190B (zh) | 2022-09-30 | 2022-09-30 | 协作机械臂及其运动控制方法、碰撞检测方法、控制系统 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN115674190B (zh) |
WO (1) | WO2024066063A1 (zh) |
Citations (34)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH07256577A (ja) * | 1994-03-23 | 1995-10-09 | Mitsubishi Electric Corp | 産業ロボットの制御方法 |
JP2012055981A (ja) * | 2010-09-06 | 2012-03-22 | Denso Wave Inc | ロボット制御装置およびロボット制御方法 |
KR20170103424A (ko) * | 2016-03-04 | 2017-09-13 | 주식회사 사이보그-랩 | 협업로봇의 충돌을 감지하는 장치 및 방법 |
CN108247654A (zh) * | 2018-02-05 | 2018-07-06 | 遨博(北京)智能科技有限公司 | 一种机器人的控制方法、装置和系统 |
CN108789408A (zh) * | 2018-06-20 | 2018-11-13 | 上海交通大学 | 基于力矩传感器的协作机器人驱控一体化控制系统 |
CN109079800A (zh) * | 2018-10-29 | 2018-12-25 | 珠海格力智能装备有限公司 | 机器人碰撞检测方法、装置、存储介质、处理器及机器人 |
CN208614800U (zh) * | 2018-02-05 | 2019-03-19 | 遨博(北京)智能科技有限公司 | 一种机器人的控制系统 |
CN109531565A (zh) * | 2018-08-27 | 2019-03-29 | 苏州博众机器人有限公司 | 一种机械臂的控制方法、装置、服务机器人及存储介质 |
CN109732599A (zh) * | 2018-12-29 | 2019-05-10 | 深圳市越疆科技有限公司 | 一种机器人碰撞检测方法、装置、存储介质及机器人 |
CN109807899A (zh) * | 2019-03-08 | 2019-05-28 | 哈工大机器人(合肥)国际创新研究院 | 针对拖动示教的协作机器人摩擦力矩补偿方法 |
CN110000815A (zh) * | 2019-04-09 | 2019-07-12 | 深圳前海达闼云端智能科技有限公司 | 一种碰撞检测方法、装置、电子设备及存储介质 |
CN110026981A (zh) * | 2019-04-19 | 2019-07-19 | 中科新松有限公司 | 一种基于模型自适应的机械臂碰撞检测方法 |
CN110787026A (zh) * | 2019-11-11 | 2020-02-14 | 上海电气集团股份有限公司 | 一种基于多传感器信息的运动异常保护方法及康复设备 |
CN110815233A (zh) * | 2019-11-28 | 2020-02-21 | 北京云迹科技有限公司 | 机器人驱动方法、装置、存储介质及机器人 |
JP2020044610A (ja) * | 2018-09-19 | 2020-03-26 | 株式会社デンソーウェーブ | ロボットの制御方法 |
CN111152231A (zh) * | 2020-01-07 | 2020-05-15 | 成都卡诺普自动化控制技术有限公司 | 一种基于力控的机器人示教方法 |
CN111230869A (zh) * | 2020-01-21 | 2020-06-05 | 北京卫星制造厂有限公司 | 一种复杂空间曲线焊缝运动轨迹和焊接工艺协同规划方法 |
WO2020133270A1 (zh) * | 2018-12-28 | 2020-07-02 | 深圳配天智能技术研究院有限公司 | 机器人的动力学参数辨识方法、机器人和存储装置 |
CN111923045A (zh) * | 2020-08-07 | 2020-11-13 | 珠海格力智能装备有限公司 | 机器人的控制方法、装置、计算机可读存储介质和处理器 |
CN112123333A (zh) * | 2020-08-18 | 2020-12-25 | 珠海格力电器股份有限公司 | 一种多关节机器人碰撞检测方法及其装置 |
CN113021353A (zh) * | 2021-03-26 | 2021-06-25 | 福州大学 | 机器人碰撞检测方法 |
DE102020103857A1 (de) * | 2020-02-14 | 2021-08-19 | Franka Emika Gmbh | Kraftbegrenzung bei Kollision eines Robotermanipulators |
CN214724190U (zh) * | 2020-11-18 | 2021-11-16 | 江汉大学 | 一种自动化机器人控制系统 |
CN113664869A (zh) * | 2021-09-29 | 2021-11-19 | 宁波睿达医疗器械有限公司 | 机械臂、机械臂控制方法及系统 |
CN113752250A (zh) * | 2021-05-31 | 2021-12-07 | 腾讯科技(深圳)有限公司 | 机器人关节的控制方法、装置、机器人及存储介质 |
CN113771047A (zh) * | 2021-10-29 | 2021-12-10 | 遨博(北京)智能科技有限公司 | 对机械臂的碰撞处理方法、控制柜、以及机械臂系统 |
CN215825344U (zh) * | 2021-09-29 | 2022-02-15 | 宁波睿达医疗器械有限公司 | 机械臂 |
CN114161477A (zh) * | 2021-12-03 | 2022-03-11 | 厦门驱动未来科技有限公司 | 一种工业机器人碰撞检测方法 |
CN114475244A (zh) * | 2022-04-14 | 2022-05-13 | 禾多科技(北京)有限公司 | 定速巡航控制方法、整车控制器及定速巡航控制系统 |
CN114604139A (zh) * | 2022-04-21 | 2022-06-10 | 福建中维动力科技股份有限公司 | 电池电流控制方法及终端 |
CN114603599A (zh) * | 2020-12-08 | 2022-06-10 | 山东新松工业软件研究院股份有限公司 | 一种机器人碰撞检测方法、装置、计算机设备及存储介质 |
CN114734437A (zh) * | 2022-03-25 | 2022-07-12 | 中国科学院自动化研究所 | 机器人关节控制方法及装置 |
WO2022161245A1 (zh) * | 2021-01-29 | 2022-08-04 | 苏州艾利特机器人有限公司 | 一种提高机器人关节转矩检测精度的方法及多关节机器人 |
CN114952821A (zh) * | 2021-02-25 | 2022-08-30 | 腾讯科技(深圳)有限公司 | 机器人运动控制方法、机器人及系统 |
Family Cites Families (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP3367641B2 (ja) * | 1998-07-31 | 2003-01-14 | 株式会社安川電機 | ロボットの制御装置 |
JP4756618B2 (ja) * | 2001-03-21 | 2011-08-24 | 株式会社ダイヘン | 多関節ロボットにおける衝突検出・停止制御法 |
CN107253196B (zh) * | 2017-08-01 | 2021-05-04 | 中科新松有限公司 | 一种机械臂碰撞检测方法、装置、设备及存储介质 |
CN111906775A (zh) * | 2020-06-08 | 2020-11-10 | 深圳众为兴技术股份有限公司 | 一种机器人碰撞检测方法、装置、存储介质及机器人 |
CN112936260A (zh) * | 2021-01-26 | 2021-06-11 | 华南理工大学 | 一种六轴工业机器人无传感器碰撞检测方法及系统 |
CN112894821B (zh) * | 2021-01-30 | 2022-06-28 | 同济大学 | 基于电流法的协作机器人拖动示教控制方法、装置及设备 |
CN113319853B (zh) * | 2021-06-18 | 2022-11-22 | 广东智源机器人科技有限公司 | 机器人控制方法、装置、电子设备以及机器人系统 |
CN113681567A (zh) * | 2021-09-14 | 2021-11-23 | 北京清飞科技有限公司 | 一种新型无传感器机器人碰撞检测方法及其系统 |
-
2022
- 2022-09-30 CN CN202211220131.1A patent/CN115674190B/zh active Active
- 2022-12-12 WO PCT/CN2022/138465 patent/WO2024066063A1/zh unknown
Patent Citations (34)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH07256577A (ja) * | 1994-03-23 | 1995-10-09 | Mitsubishi Electric Corp | 産業ロボットの制御方法 |
JP2012055981A (ja) * | 2010-09-06 | 2012-03-22 | Denso Wave Inc | ロボット制御装置およびロボット制御方法 |
KR20170103424A (ko) * | 2016-03-04 | 2017-09-13 | 주식회사 사이보그-랩 | 협업로봇의 충돌을 감지하는 장치 및 방법 |
CN108247654A (zh) * | 2018-02-05 | 2018-07-06 | 遨博(北京)智能科技有限公司 | 一种机器人的控制方法、装置和系统 |
CN208614800U (zh) * | 2018-02-05 | 2019-03-19 | 遨博(北京)智能科技有限公司 | 一种机器人的控制系统 |
CN108789408A (zh) * | 2018-06-20 | 2018-11-13 | 上海交通大学 | 基于力矩传感器的协作机器人驱控一体化控制系统 |
CN109531565A (zh) * | 2018-08-27 | 2019-03-29 | 苏州博众机器人有限公司 | 一种机械臂的控制方法、装置、服务机器人及存储介质 |
JP2020044610A (ja) * | 2018-09-19 | 2020-03-26 | 株式会社デンソーウェーブ | ロボットの制御方法 |
CN109079800A (zh) * | 2018-10-29 | 2018-12-25 | 珠海格力智能装备有限公司 | 机器人碰撞检测方法、装置、存储介质、处理器及机器人 |
WO2020133270A1 (zh) * | 2018-12-28 | 2020-07-02 | 深圳配天智能技术研究院有限公司 | 机器人的动力学参数辨识方法、机器人和存储装置 |
CN109732599A (zh) * | 2018-12-29 | 2019-05-10 | 深圳市越疆科技有限公司 | 一种机器人碰撞检测方法、装置、存储介质及机器人 |
CN109807899A (zh) * | 2019-03-08 | 2019-05-28 | 哈工大机器人(合肥)国际创新研究院 | 针对拖动示教的协作机器人摩擦力矩补偿方法 |
CN110000815A (zh) * | 2019-04-09 | 2019-07-12 | 深圳前海达闼云端智能科技有限公司 | 一种碰撞检测方法、装置、电子设备及存储介质 |
CN110026981A (zh) * | 2019-04-19 | 2019-07-19 | 中科新松有限公司 | 一种基于模型自适应的机械臂碰撞检测方法 |
CN110787026A (zh) * | 2019-11-11 | 2020-02-14 | 上海电气集团股份有限公司 | 一种基于多传感器信息的运动异常保护方法及康复设备 |
CN110815233A (zh) * | 2019-11-28 | 2020-02-21 | 北京云迹科技有限公司 | 机器人驱动方法、装置、存储介质及机器人 |
CN111152231A (zh) * | 2020-01-07 | 2020-05-15 | 成都卡诺普自动化控制技术有限公司 | 一种基于力控的机器人示教方法 |
CN111230869A (zh) * | 2020-01-21 | 2020-06-05 | 北京卫星制造厂有限公司 | 一种复杂空间曲线焊缝运动轨迹和焊接工艺协同规划方法 |
DE102020103857A1 (de) * | 2020-02-14 | 2021-08-19 | Franka Emika Gmbh | Kraftbegrenzung bei Kollision eines Robotermanipulators |
CN111923045A (zh) * | 2020-08-07 | 2020-11-13 | 珠海格力智能装备有限公司 | 机器人的控制方法、装置、计算机可读存储介质和处理器 |
CN112123333A (zh) * | 2020-08-18 | 2020-12-25 | 珠海格力电器股份有限公司 | 一种多关节机器人碰撞检测方法及其装置 |
CN214724190U (zh) * | 2020-11-18 | 2021-11-16 | 江汉大学 | 一种自动化机器人控制系统 |
CN114603599A (zh) * | 2020-12-08 | 2022-06-10 | 山东新松工业软件研究院股份有限公司 | 一种机器人碰撞检测方法、装置、计算机设备及存储介质 |
WO2022161245A1 (zh) * | 2021-01-29 | 2022-08-04 | 苏州艾利特机器人有限公司 | 一种提高机器人关节转矩检测精度的方法及多关节机器人 |
CN114952821A (zh) * | 2021-02-25 | 2022-08-30 | 腾讯科技(深圳)有限公司 | 机器人运动控制方法、机器人及系统 |
CN113021353A (zh) * | 2021-03-26 | 2021-06-25 | 福州大学 | 机器人碰撞检测方法 |
CN113752250A (zh) * | 2021-05-31 | 2021-12-07 | 腾讯科技(深圳)有限公司 | 机器人关节的控制方法、装置、机器人及存储介质 |
CN113664869A (zh) * | 2021-09-29 | 2021-11-19 | 宁波睿达医疗器械有限公司 | 机械臂、机械臂控制方法及系统 |
CN215825344U (zh) * | 2021-09-29 | 2022-02-15 | 宁波睿达医疗器械有限公司 | 机械臂 |
CN113771047A (zh) * | 2021-10-29 | 2021-12-10 | 遨博(北京)智能科技有限公司 | 对机械臂的碰撞处理方法、控制柜、以及机械臂系统 |
CN114161477A (zh) * | 2021-12-03 | 2022-03-11 | 厦门驱动未来科技有限公司 | 一种工业机器人碰撞检测方法 |
CN114734437A (zh) * | 2022-03-25 | 2022-07-12 | 中国科学院自动化研究所 | 机器人关节控制方法及装置 |
CN114475244A (zh) * | 2022-04-14 | 2022-05-13 | 禾多科技(北京)有限公司 | 定速巡航控制方法、整车控制器及定速巡航控制系统 |
CN114604139A (zh) * | 2022-04-21 | 2022-06-10 | 福建中维动力科技股份有限公司 | 电池电流控制方法及终端 |
Non-Patent Citations (2)
Title |
---|
双臂机器人的协调控制算法综述;王琪等;《计算机工程与应用》;20210101;第57卷(第1期);第1-16页 * |
基于电子皮肤的协作机器人碰前安全方案研究;姜宇,黄睿等;《机器人技术与应用》(第3期);第21-25页 * |
Also Published As
Publication number | Publication date |
---|---|
WO2024066063A1 (zh) | 2024-04-04 |
CN115674190A (zh) | 2023-02-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
TWI625612B (zh) | 機器人系統之監視裝置 | |
EP2372478B1 (en) | Motor driving system and motor controller | |
US9821459B2 (en) | Multi-joint robot having function for repositioning arm | |
KR102370879B1 (ko) | 협동로봇제어장치 및 협동로봇을 제어하는 방법 | |
US9409295B2 (en) | Method for controlling a robot | |
US8583285B2 (en) | Method and device for stopping a manipulator | |
US20170028565A1 (en) | Industrial robot system and control method thereof | |
EP2042278B1 (en) | Robot controller for halting a robot based on the speed of a robot hand portion | |
JP2608161B2 (ja) | 産業用ロボットの停止制御方法 | |
KR102418451B1 (ko) | 로봇 제어 시스템 | |
US20150306767A1 (en) | Motion limiting device and motion limiting method | |
US20210086374A1 (en) | Robot comprising safety system ensuring stopping time and distance | |
US20170225331A1 (en) | Systems and methods for safe robot operation | |
CN112476438B (zh) | 机械臂避障方法、装置、机械臂及机器人 | |
CN109311164B (zh) | 对机器人组的监视 | |
JP2016193473A (ja) | 駆動制御装置および安全制御システム | |
WO1991010181A1 (en) | Method of detecting collision using observer | |
Je et al. | Current based compliance control method for minimizing an impact force at collision of service robot arm | |
CN115674190B (zh) | 协作机械臂及其运动控制方法、碰撞检测方法、控制系统 | |
JP5778891B2 (ja) | ロボット制御装置 | |
JP2018062026A (ja) | ロボットの速度や加速度を制限する機能を備えたロボット制御装置 | |
JPH10180663A (ja) | ロボットアームの制御装置 | |
CA2909347C (en) | Safety monitoring for a serial kinematic structure | |
JP7488474B2 (ja) | ロボット制御装置 | |
JP6966415B2 (ja) | 制御方法、制御装置及びプログラム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
CB02 | Change of applicant information | ||
CB02 | Change of applicant information |
Address after: 518055 1003, building 2, Chongwen Park, Nanshan Zhiyuan, No. 3370, Liuxian Avenue, Fuguang community, Taoyuan Street, Nanshan District, Shenzhen City, Guangdong Province Applicant after: Shenzhen Yuejiang Technology Co.,Ltd. Address before: 518055 1003, building 2, Chongwen Park, Nanshan Zhiyuan, No. 3370, Liuxian Avenue, Fuguang community, Taoyuan Street, Nanshan District, Shenzhen City, Guangdong Province Applicant before: SHENZHEN YUEJIANG TECHNOLOGY Co.,Ltd. |
|
GR01 | Patent grant | ||
GR01 | Patent grant |