CN116372926A - 机器人关节运动补偿方法、系统和存储介质 - Google Patents
机器人关节运动补偿方法、系统和存储介质 Download PDFInfo
- Publication number
- CN116372926A CN116372926A CN202310382416.3A CN202310382416A CN116372926A CN 116372926 A CN116372926 A CN 116372926A CN 202310382416 A CN202310382416 A CN 202310382416A CN 116372926 A CN116372926 A CN 116372926A
- Authority
- CN
- China
- Prior art keywords
- joint
- vibration
- interference
- joints
- robot
- 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.)
- Pending
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 229
- 238000000034 method Methods 0.000 title claims abstract description 73
- 230000008878 coupling Effects 0.000 claims abstract description 39
- 238000010168 coupling process Methods 0.000 claims abstract description 39
- 238000005859 coupling reaction Methods 0.000 claims abstract description 39
- 230000001629 suppression Effects 0.000 claims description 54
- 230000008569 process Effects 0.000 claims description 34
- 238000001514 detection method Methods 0.000 claims description 24
- 230000001276 controlling effect Effects 0.000 claims description 20
- 238000004590 computer program Methods 0.000 claims description 17
- 230000001105 regulatory effect Effects 0.000 claims description 12
- 238000013016 damping Methods 0.000 abstract description 11
- 230000000694 effects Effects 0.000 abstract description 8
- 238000001914 filtration Methods 0.000 abstract description 8
- 238000010586 diagram Methods 0.000 description 13
- 238000004422 calculation algorithm Methods 0.000 description 11
- 238000012545 processing Methods 0.000 description 8
- 238000004891 communication Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000005259 measurement Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000003321 amplification Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 238000003199 nucleic acid amplification method Methods 0.000 description 2
- 230000000452 restraining effect Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 239000000725 suspension Substances 0.000 description 2
- OKTJSMMVPCPJKN-UHFFFAOYSA-N Carbon Chemical compound [C] OKTJSMMVPCPJKN-UHFFFAOYSA-N 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000033228 biological regulation Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 229910021389 graphene Inorganic materials 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000001356 surgical procedure Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
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
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/0091—Shock absorbers
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/02—Sensing devices
-
- 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
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
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为一个实施例中振动抑制算法示意图;
图12为一个实施例中振动抑制算法实施示意图;
图13为一个实施例中机器人关节运动补偿系统的结构示意图;
图14为一个实施例中机器人启动时干扰和振动的抑制流程图;
图15为一个实施例中机器人启动时干扰和振动的抑制算法流程图;
图16为一个实施例中机器人正常工作过程中,突然受到干扰/突然产生振动而进行抑制的场景下,干扰和振动的抑制流程图;
图17为一个实施例中机器人正常工作过程中,突然受到干扰/突然产生振动而进行抑制的场景下,干扰和振动的抑制算法流程图;
图18为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行进一步详细说明。应当理解,此处描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。
本申请实施例提供的机器人关节运动补偿方法,可以应用于如图1所示的机器人系统中。其中,主控制台车1上装载有主控制臂2、控制器3和显示器4;患者躺在患者台车5,患者台车5装载有工具臂6、调整臂8和悬吊9;工具臂6的末端装载有器械7,通过调整臂8和悬吊9控制工具臂6的位姿;图像台车10上装载有显示器11和图像主机12。机器人系统的结构分布如图2所示;主控制台车1的示意图如图3所示;患者台车5的示意图如图4所示;工具臂6的示意图如图5所示。
主控制臂2与工具臂6之间存在主从遥操作映射关系,通过操控主控制臂2,控制工具臂6运动;通过工具臂6对患者实施预先规划好的手术操作;在实施手术操作的过程中,通过图像主机12对操作环境和显示操作影像进行处理,并将处理后显示操作影像显示在显示器11中。
机器人工作过程中,通过控制器3获得伺服指令,并基于伺服指令控制工具臂6各个关节提供驱动力的力矩电机开启伺服运动,在工具臂6运动过程中,实时检测工具臂6的各关节在运动过程中是否存在干扰和振动;在工具臂6的各关节在运动过程中干扰信号的幅值大于安全阈值时,控制器3预测各关节的干扰补偿量,并基于各关节对应的干扰补偿量对工具臂6各关节进行负反馈调节;干扰补偿量为补偿耦合力干扰对机器人各关节运动造成的影响所需调节的力矩变化量;在工具臂6的各关节在运动过程中振动信号的幅值大于安全阈值时,控制器3预测各关节的振动补偿量,并基于各关节对应的振动补偿量对工具臂6各关节进行负反馈调节;振动补偿量为补偿特定频率区间内的纯振动干扰对各关节运动造成的影响所需的运动变化量;若在工具臂6的各关节进行负反馈调节后,各关节的干扰信号的幅值和振动信号的幅值均小于安全阈值的情况下,则控制器3停止对工具臂6各关节进行负反馈调节。
在一个实施例中,如图6所示,提供了一种机器人关节运动补偿方法,以该方法应用于图1中的主控制台车上的控制器为例进行说明,包括以下步骤:
步骤602,实时检测机器人的各关节在运动过程中干扰信号的幅值和振动信号的幅值是否大于安全阈值。
其中,干扰信号指的是串联关节之间的耦合力干扰所对应的信号。如果n轴的整个驱动路径不仅仅是在n-1到n轴之间,而是跨越了n-1甚至n-m,就会造成耦合。例如,如果六轴电机不是固定在5轴结构件,而是装在三轴结构件上再通过传动机构传到六轴,那么在六轴电机静止的情况下四轴和五轴电机的运动都会造成六轴运动。振动是由于关节电机振动或者机械臂末端振动引起的干扰。
本实施例可以通过传感器检测各关节在运动过程中的干扰信号和振动信号,例如,通过关节电机端编码器或关节负载端编码器采集各关节力矩电机的位置或速度信息,干扰信号在位置或速度信息对应的波形图中以幅值突增且逐渐降低的效果体现出来,振动信号在在位置或速度信息对应的波形图中以保持幅值不变的效果体现出来。
需要注意的是:关节电机端编码器或关节负载端编码器为工具臂上原有结构。
可选地,控制器通过关节电机端编码器或关节负载端编码器采集各关节力矩电机的位置或速度信息,并判断编码器采集的信号波形图中干扰信号和振动信号幅值是否超过安全阈值。
步骤604,在检测到干扰信号的幅值大于安全阈值的情况下,预测各关节的干扰补偿量,并基于各关节对应的干扰补偿量对机器人各关节进行负反馈调节;干扰补偿量为补偿耦合力干扰对机器人各关节运动造成的影响所需调节的力矩变化量。
其中,干扰补偿量可以基于各关节的理想力矩与真实力矩之间的差值,确定电机所需调节的力矩变化量。负反馈调节指的是基于干扰补偿量修正各关节的力矩电机的输入力矩,直至各关节的干扰信号的幅值小于安全阈值。基于各关节对应的干扰补偿量对机器人各关节进行负反馈调节的过程,也是对各关节的关节力矩进行补偿的过程。
可选地,机器人的各关节在运动过程中存在干扰时,控制器通过仿真或者数据推演的方式,确定各关节的理想力矩,并通过编码器采集各关节的真实力矩,控制器基于各关节的理想关节力矩与真实关节力矩,确定各关机的干扰补偿量,并基于干扰补偿量修正各关节的力矩电机的输入力矩,直至各关节的干扰信号的幅值小于安全阈值。
步骤606,在检测到振动信号的幅值大于安全阈值的情况下,预测各关节的振动补偿量,并基于各关节对应的振动补偿量对机器人各关节进行负反馈调节;振动补偿量为补偿特定频率区间内的纯振动干扰对各关节运动造成的影响所需的运动变化量。
其中,振动补偿量可以基于各关节的理想刚体运动信号与真实运动信号之间的差值,确定电机所需调节的运动变化量;刚体运动信号可以是关节的位置或速度信息。基于各关节对应的振动补偿量对机器人各关节进行负反馈调节的过程,也是对各关节的关节运动信号进行补偿的过程。
可选地,在检测到振动信号的幅值大于安全阈值的情况下,控制器通过仿真或者数据推演的方式,确定各关节的理想刚体运动信号,并通过编码器采集各关节的真实运动信号,控制器基于各关节的理想刚体运动信号与真实运动信号,确定各关机的振动补偿量,并基于振动补偿量修正各关节的力矩电机的输入力矩,直至力矩电机的真实运动信号调整至与理想刚体运动信号相等。
步骤608,若在机器人的各关节进行负反馈调节后,各关节的干扰和振动小于安全阈值,则停止对机器人各关节进行负反馈调节。
其中,如图7所示,机器人启动时,通过控制器使机器人获得伺服指令;机器人的伺服单元获得伺服指令,开启伺服运动;在干扰信号和振动信号均大于安全阈值时,干扰抑制模块和振动抑制模块将机器人当前的运动信号进行算法补偿,抑制干扰和振动幅值;对补偿后的干扰和振动幅值进行安全阈值判断;当补偿后的干扰和振动大于设定的安全阈值时,继续将关节做闭环算法补偿;当补偿后的干扰和振动小于设定的安全阈值时,将补偿后的伺服指令输出到手术机器人的伺服单元,使其完成补偿运动,其中,干扰和振动的安全阈值根据具体的运动控制精度目标进行设定。
需要注意的是:各关节的理想关节力矩和理想刚体运动信号一般是通过关节动力学模型得到,由于关节动力学模型中柔性模型存在建模误差,在将耦合力干扰和振动融合一起进行干扰预测和补偿时,导致无法精确测量理想刚体运动信号,进一步地导致振动预测和补偿精度低的问题,为了解决该问题,本实施例设置了干扰和振动并行预测和补偿的方案,并且仅获取各关机的理想刚体运动信号,避免柔性模型存在误差导致振动补偿精度低的问题,可以提高补偿精度。
可选地,机器人的各关节进行负反馈调节后,若各关节的干扰大于安全阈值,则返回步骤604再次对各关节的关节力矩进行补偿,直至各关节的干扰小于安全阈值,停止对机器人各关节进行负反馈调节;若各关节的振动大于安全阈值,则返回步骤606再次对各关节的关节运动信号进行补偿,直至各关节的振动小于安全阈值,停止对机器人各关节进行负反馈调节。
上述机器人关节运动补偿方法中,通过机械臂上现有的检测元件实时检测各关节的真实关节力矩和真实运动信号,并基于仿真或者建模的方式获得各关节的理想关节力矩和理想刚体运动信号,通过真实关节力矩与理想关节力矩,确定干扰补偿量,通过真实运动信号和理想刚体运动信号,确定振动补偿量,分别基于干扰补偿量和振动补偿量对机械臂进行干扰和振动补偿,实现在不增加阻尼元件或不通过信号滤波的场景下,实时对耦合力干扰和振动进行补偿的效果,补偿精度高,并且适用不同场景下的耦合力干扰补偿和振动补偿。
在一个实施例中,如图8所示,预测各关节的干扰补偿量,包括:
步骤802,获取伺服指令;伺服指令对应有各关节力矩电机的输入力矩,伺服指令用于控制各关节力矩电机运转至对应的输入力矩。
其中,伺服指令是通过控制器产生的,伺服指令传输至工具臂中各关节力矩电机,控制各关节力矩电机运转至指定的输入力矩。
步骤804,将伺服指令对应的各关节力矩电机的输入力矩输入关节动力学模型,得到机器人在没有干扰的理想环境下各关节的理想关节力矩。
其中,关节动力学模型是基于机器人动力学方程模拟关节运动,研究物体的力与物体运动的关系的模型。
关节动力学模型描述机械臂的运动和每个关节力矩电机输入力或力矩的关系。通过关节动力学模型可以在不损坏关节力矩电机的前提下,确定各关节在没有干扰的理想环境下各关节的理想关节力矩。
步骤806,根据伺服指令控制各关节力矩电机转动,并采集关节力矩电机的真实关节力矩,基于各关节的理想关节力矩和各关节的真实关节力矩,确定各关节对应的干扰值。
其中,关节力矩电机的真实关节力矩通过关节力矩传感器检测得到的。各关节的理想关节力矩和各关节的真实关节力矩之间的差值绝对值,作为各关节对应的干扰值。
步骤808,基于各关节对应的干扰值,确定各关节对应的干扰补偿量。
其中,干扰补偿量是对应的关节所需调节的力矩变化量。干扰补偿量是可以被控制器所识别的数据,而干扰值是用于评估干扰成分,包括除耦合力干扰之外的其他干扰,为对各关节的耦合力干扰对工具臂运动造成的影响进行补偿,需要从干扰值中提取出耦合干扰力的部分,并基于耦合干扰力确定各关节的耦合干扰力的干扰补偿量。具体的,可以通过低通滤波器提取干扰值中耦合干扰力的部分。
可选地,由于各关节的干扰补偿量计算原理相同,因此,在此以工具臂其中一个关节为例,具体的,如图9所示,控制器获取伺服指令Td,解析伺服指令Td,得到关节力矩电机的输入力矩Tm,并将关节力矩电机的输入力矩Tm输入至关节动力学模型,获得由关节动力学模型计算出来的不含有干扰的理想关节力矩Tnj,即Tnj=Tm*Joint_model,其中,Joint_model表示关节动力学模型;工具臂的关节力矩电机运转至输入力矩Tm,在关节力矩运转的过程中,所受到干扰为Tdis,在干扰Tdis的影响下,关节力矩电机的力矩并不等于输入力矩,通过关节力矩传感器检测关节的真实关节力矩Tj;基于真实关节力矩Tj和理想关节力矩Tnj之间的差值,预测关节所受的干扰值Tndis,即Tndis=Tj–Tnj;将干扰值Tndis输入至低通滤波器中,提取耦合力干扰成分,并基于耦合力干扰成分确定关节对应的干扰补偿量Tfb,即Tfb=Filter(Tndis),Filter表示滤波的意思;基于关节对应的干扰补偿量Tfb和伺服指令修正关节力矩电机的输入力矩,从而形成闭环控制。
本实施例中,通过伺服指令和干扰补偿量,修正关节力矩电机的输入力矩,并将输入力矩输入关节动力学模型,由关节动力学模型计算得到不含有干扰的理想关节力矩,基于理想关节力矩和真实关节力矩之间的差值,确定关节所受的干扰值,通过低通滤波器提取干扰值中耦合力干扰成分,并基于耦合力干扰成分确定关节对应的干扰补偿量,并返回执行通过伺服指令和干扰补偿量,修正关节力矩电机的输入力矩的步骤并继续执行,直至关节的耦合力干扰对工具臂运动造成的影响被补偿,即工具臂的末端执行至期望位置,可以在不增加阻尼元件或不通过信号滤波的场景下,实时对耦合力干扰进行补偿的效果,补偿精度高。
在一个实施例中,如图10所示,预测各关节的振动补偿量,包括:
步骤1002,获取伺服指令,并通过运动控制器解析伺服指令对应的各关节力矩电机的输入力矩;伺服指令对应有各关节力矩电机的输入力矩,伺服指令用于控制各关节力矩电机运转至对应的输入力矩。
其中,运动控制器可以是PID控制器,运动控制器根据关节运动信号随时调整发给关节力矩电机的信号,使关节的误差始终控制在精度范围以内。本实施例中,运动控制器基于伺服指令、关节运动信号和振动补偿量,计算得到关节力矩电机的输入力矩。
步骤1004,将伺服指令对应的各关节力矩电机的输入力矩输入关节动力学模型中的等效刚体动力学模型中,得到机器人在没有干扰的理想环境下各关节的理想刚体运动信号;理想刚体运动信号为关节在无振动情况下对应的位置信息和/或速度信息。
其中,等效刚体动力学模型描述机械臂的各关节在力和力矩的作用下平移和转动,通过等效刚体动力学模型可以确定各关节在没有干扰的理想环境下各各关节的理想刚体运动信号。
步骤1006,根据伺服指令控制各关节力矩电机转动,并采集关节力矩电机的真实运动信号,基于各关节的理想刚体运动信号和各关节的真实运动信号,确定各关节对应的纯振动值;真实运动信号为关节在有振动情况下对应的位置信息和/或速度信息。
其中,真实运动信号通过关节力矩电机端编码器测量得到,也可以通过关节负载端编码器测量得到。各关节的理想刚体运动信号和各关节的真实运动信号之间的差值绝对值,作为各关节对应的纯振动值。
步骤1008,基于各关节对应的纯振动值,确定各关节对应的振动补偿量。
其中,振动补偿量为机器人各关节所需调节的运动变化量。纯振动值包含不同振动频率对应的纯振动干扰成分,振动补偿量为补偿特定频率区间内的纯振动干扰成分对工具臂运动造成的影响所需的运动变化量。
可选地,由于各关节的振动补偿量计算原理相同,因此,在此以工具臂其中一个关节为例,具体的,如图11所示,控制器获取伺服指令Vd,解析伺服指令Vd,得到关节力矩电机的输入力矩Tm,并将关节力矩电机的输入力矩Tm输入至关节动力学模型中的等效刚体动力学模型中,进行无振动运算处理,获得由等效刚体动力学模型计算出来的不含有干扰的理想刚体运动信号Rv,即Rv=A(Tm),其中A表示等效刚体动力学模型,A=1/((JM+JL)*s),式中JM代表关节的电机端惯量,JL代表关节的负载端惯量,s为拉普拉斯域算子;工具臂的关节力矩电机运转至输入力矩Tm,在关节力矩运转的过程中,在干扰的影响下,工具臂的末端并未达到期望位置,通过关节力矩传感器检测关节的真实运动信号V,真实运动信号V可以是关节电机端编码器的测量信号Mv,也可以是关节负载端编码器的测量信号Lv;将真实运动信号V和理想刚体运动信号Rv之间的差值进行纯振动估测处理,预测关节所受的纯振动值Vib,即Vib=B(V–Rv),B表示纯振动估测;将纯振动值Vib进行振动抑制处理,即提取特定频率的纯振动信号,并确定关节对应的振动补偿量Vfb,即Vfb=C(Vib),C表示振动抑制处理过程;基于关节对应的振动补偿量Vfb和伺服指令修正关节力矩电机的输入力矩,从而形成闭环控制。
在本实施例中,通过伺服指令和振动补偿量,修正关节力矩电机的输入力矩,并将输入力矩输入关节动力学模型中等效刚体动力学模型,由等效刚体动力学模型计算得到不含有干扰的理想刚体运动信号,基于理想刚体运动信号和真实运动信号之间的差值,确定关节所受的纯振动值,并提取纯振动值中振动成分,并基于振动成分确定关节对应的振动补偿量,并返回执行通过伺服指令和振动补偿量,修正关节力矩电机的输入力矩的步骤并继续执行,直至关节的振动对工具臂运动造成的影响被补偿,即工具臂的末端执行至期望位置,可以在不增加阻尼元件或不通过信号滤波的场景下,实时对振动干扰进行补偿的效果,补偿精度高。
在一些实施例中,在获得纯振动值之后,可以直接基于纯振动值确定关节对应的振动补偿量,但是由于等效刚体动力学模型是通过建模方式得到,与无振动干扰环境还是存在误差,为使得等效刚体动力学模型输出的各关节的理想刚体运动信号更贴近没有干扰的理想环境下的理想数据,本实施例基于各关节对应的纯振动值反馈至等效刚体动力学模型中,修正等效刚体动力学模型输出的各关节的理想刚体运动信号,获得更加精确的理想刚体运动信号,进一步提高纯振动值预测的精度。具体的,如图12所示,基于各关节的理想刚体运动信号和各关节的真实运动信号,确定各关节对应的纯振动值,包括以下步骤:
将各关节的理想刚体运动信号和各关节的真实运动信号分别输入至闭环控制器中,通过闭环控制器预测各关节对应的纯振动值;其中,各关节对应的纯振动值反馈至等效刚体动力学模型中,修正等效刚体动力学模型输出的各关节的理想刚体运动信号。
其中,直接将各关节的理想刚体运动信号和各关节的真实运动信号之间的差值绝对值反馈至等效刚体动力学模型中也可以达到修正理想刚体运动信号的目的,但是由于这种方式纯振动信号估测的速度慢并且准确度低,因此,为解决上述问题,本实施例采用闭环控制器对各关节的理想刚体运动信号和各关节的真实运动信号之间的差值绝对值进行处理,可以快速且精确地预测纯振动值。其中,闭环控制器可以是PID控制器的任意组合形式,即可以是P控制器(比例控制器)、PI控制器(积分控制器)和PID控制器(微分控制器)中任一种形式,根据PID控制器的组合形式对应的处理策略,对各关节的理想刚体运动信号和各关节的真实运动信号之间的差值绝对值进行比例放大、积分或者微分处理。
在一些实施例中,采用频率选择器对纯振动值Vib进行振动抑制处理,即提取特定频率的纯振动信号。具体的,步骤1008包括以下步骤:
将各关节对应的纯振动值输入至频率选择器中,通过频率选择器得到特定频率区间内各关节对应的振动补偿量;其中,各关节对应的振动补偿量反馈至运动控制器的输入,修正各关节力矩的输入力矩。
其中,频率选择器是用于提取纯振动值中不同频率的纯振动信号,并将纯振动信号进行可调节的放大或者缩小,来保证振动补偿量,以实现多种频率振动的抑制,针对不同频率的振动干扰,无需再次估测振动值,只需更改频率选择器的提取范围,适用不同的应用场景。
频率选择器可以通过高通滤波器、低通滤波器和带通滤波器中人一种器件实现。若选择高通滤波器,则可以提取高于预设值的频率范围内的纯振动信号;若选择低通滤波器,则可以提取低于预设值的频率范围内的纯振动信号;若选择带通滤波器,则可以提取特定频率区间范围内的纯振动信号。
在一些实施例中,如图12所示,在获取各关节对应的振动补偿量后,基于各关节对应的振动补偿量对机器人各关节进行负反馈调节的步骤包括以下步骤:
基于各关节对应的振动补偿量和各关节力矩电机的真实运动信号,修正各关节力矩电机的输入力矩,并基于各关节力矩电机修改后的输入力矩控制各关节力矩电机运转;其中,各关节力矩电机修改后的输入力矩作为等效刚体动力学模型的输入。
其中,以真实运动信号为位置信号为例,则振动补偿量为位置补偿量,修正后的输入力矩对应的期望位置等于输入力矩修正前对应的输入位置与振动补偿量之和。
可选地,如图12所示,控制器获取伺服指令Vd,解析伺服指令Vd,得到关节力矩电机的输入力矩Tm,并将关节力矩电机的输入力矩Tm输入至关节动力学模型中的等效刚体动力学模型中,进行无振动运算处理,获得由等效刚体动力学模型计算出来的不含有干扰的理想刚体运动信号Rv;工具臂的关节力矩电机运转至输入力矩Tm,在关节力矩运转的过程中,在干扰的影响下,工具臂的末端并未达到期望位置,通过关节力矩传感器检测关节的真实运动信号V;将真实运动信号V和理想刚体运动信号Rv之间的差值分成两路,一路反馈至闭环控制器中闭环控制器快速且精确地预测纯振动值,并将预测的纯振动值反馈至等效刚体动力学模型中,修正等效刚体动力学模型输出的各关节的理想刚体运动信号Rv,基于修正后的理想刚体运动信号Rv与真实运动信号V,重新确定纯振动值Vib,并将重新确定的纯振动值Vib输入至频率选择器中;另一路输入频率选择器中,频率选择器按时序顺序提取特定频率区间内各关节对应的振动补偿量;基于各关节对应的振动补偿量和各关节力矩电机的真实运动信号,修正各关节力矩电机的输入力矩,并基于各关节力矩电机修改后的输入力矩控制各关节力矩电机运转,从而形成闭环控制。
本实施例中,采用闭环控制器对各关节的理想刚体运动信号和各关节的真实运动信号之间的差值绝对值进行处理,可以快速且精确地预测纯振动值,并将纯振动值反馈至等效刚体动力学模型中,可以修正等效刚体动力学模型输出的各关节的理想刚体运动信号,获得更加精确的理想刚体运动信号,进一步提高纯振动值预测的精度;本实施例通过频率选择器提取纯振动值中特定频率的纯振动信号,将纯振动信号进行可调节的放大或者缩小,来保证振动补偿量,以实现多种频率振动的抑制,针对不同频率的振动干扰,无需再次估测振动值,只需更改频率选择器的提取范围,适用不同的应用场景。
在一个实施例中,本实施例提供一种机器人关节运动补偿方法的详细步骤,具体包括以下步骤:
步骤1,实时检测机器人的各关节在运动过程中干扰和振动是否超过安全阈值,在机器人的各关节在运动过程中干扰大于安全阈值时,执行步骤2;在机器人的各关节在运动过程中振动大于安全阈值时,执行步骤7。
步骤2,获取伺服指令;伺服指令对应有各关节力矩电机的输入力矩,伺服指令用于控制各关节力矩电机运转至对应的输入力矩;
步骤3,将伺服指令对应的各关节力矩电机的输入力矩输入关节动力学模型,得到机器人在没有干扰的理想环境下各关节的理想关节力矩;
步骤4,根据伺服指令控制各关节力矩电机转动,并采集关节力矩电机的真实关节力矩,基于各关节的理想关节力矩和各关节的真实关节力矩,确定各关节对应的干扰值;
步骤5,基于各关节对应的干扰值,确定各关节对应的干扰补偿量。
步骤6,基于各关节对应的干扰补偿量对机器人各关节进行负反馈调节
步骤7,获取伺服指令,并通过运动控制器解析伺服指令对应的各关节力矩电机的输入力矩;伺服指令对应有各关节力矩电机的输入力矩,伺服指令用于控制各关节力矩电机运转至对应的输入力矩;
步骤8,将伺服指令对应的各关节力矩电机的输入力矩输入关节动力学模型中的等效刚体动力学模型中,得到机器人在没有干扰的理想环境下各关节的理想刚体运动信号;理想刚体运动信号为关节在无振动情况下对应的位置信息和/或速度信息;
步骤9,根据伺服指令控制各关节力矩电机转动,并采集关节力矩电机的真实运动信号,将各关节的理想刚体运动信号和各关节的真实运动信号分别输入至闭环控制器中,通过闭环控制器预测各关节对应的纯振动值;真实运动信号为关节在有振动情况下对应的位置信息和/或速度信息;各关节对应的纯振动值反馈至等效刚体动力学模型中,修正等效刚体动力学模型输出的各关节的理想刚体运动信号。
步骤10,将各关节对应的纯振动值输入至频率选择器中,通过频率选择器得到特定频率区间内各关节对应的振动补偿量;其中,各关节对应的振动补偿量反馈至运动控制器的输入修正各关节力矩的输入力矩。
步骤11,基于各关节对应的振动补偿量和各关节力矩电机的真实运动信号,修正各关节力矩电机的输入力矩,并基于各关节力矩电机修改后的输入力矩控制各关节力矩电机运转;其中,各关节力矩电机的输入力矩作为等效刚体动力学模型的输入。
本实施例中,分别基于干扰补偿量和振动补偿量对机械臂进行干扰和振动补偿,实现在不增加阻尼元件或不通过信号滤波的场景下,实时对耦合力干扰和振动进行补偿的效果,补偿精度高,并且适用不同场景下的耦合力干扰补偿和振动补偿。
应该理解的是,虽然如上所述的各实施例所涉及的流程图中的各个步骤按照箭头的指示依次显示,但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明,这些步骤的执行并没有严格的顺序限制,这些步骤可以以其它的顺序执行。而且,如上所述的各实施例所涉及的流程图中的至少一部分步骤可以包括多个步骤或者多个阶段,这些步骤或者阶段并不必然是在同一时刻执行完成,而是可以在不同的时刻执行,这些步骤或者阶段的执行顺序也不必然是依次进行,而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。
基于同样的发明构思,本申请实施例还提供了一种用于实现上述所涉及的机器人关节运动补偿方法的机器人关节运动补偿系统。该装置所提供的解决问题的实现方案与上述方法中所记载的实现方案相似,故下面所提供的一个或多个机器人关节运动补偿系统实施例中的具体限定可以参见上文中对于机器人关节运动补偿方法的限定,在此不再赘述。
在一个实施例中,如图13所示,提供了一种机器人关节运动补偿系统,包括:主控单元、伺服单元、检测模块、干扰抑制模块、振动抑制模块和判断模块,其中:
主控单元输出伺服指令至伺服单元;
伺服单元根据伺服指令控制机器人的各关节运动;
检测模块实时检测机器人的各关节在运动过程中干扰信号的幅值和振动信号的幅值是否大于安全阈值;
干扰抑制模块在检测到干扰信号的幅值大于安全阈值的情况下,预测各关节的干扰补偿量,并基于各关节对应的干扰补偿量对相应的关节进行负反馈调节;干扰补偿量为补偿耦合力干扰对机器人各关节运动造成的影响所需调节的力矩变化量;
振动抑制模块在检测到振动信号的幅值大于安全阈值的情况下,预测各关节的振动补偿量,并基于各关节对应的振动补偿量对机器人各关节进行负反馈调节;振动补偿量为补偿特定频率区间内的纯振动干扰对各关节运动造成的影响所需的运动变化量;
判断模块在机器人的各关节完成负反馈调节后,在各关节的干扰信号的幅值和振动信号的幅值均小于安全阈值的情况下,停止对机器人各关节进行负反馈调节。
在其中一个实施例中,如图13所示,干扰抑制模块包括:无干扰运算单元、干扰估测单元和干扰补偿单元;
无干扰运算单元通过关节动力学模型,计算得到机器人在没有干扰的理想环境下各关节的理想关节力矩;
干扰估测单元确定机器人在真实环境下各关节的真实关节力矩,基于各关节的理想关节力矩和各关节的真实关节力矩,确定各关节对应的干扰值;
干扰补偿单元基于各关节对应的干扰值,确定各关节对应的干扰补偿量,并基于各关节对应的干扰补偿量对机器人各关节进行负反馈调节。
在其中一个实施例中,如图13所示,振动抑制模块包括:无振动运算单元、纯振动估测单元和振动抑制单元;
无振动运算单元通过关节动力学模型中的等效刚体动力学模型,计算得到机器人在没有干扰的理想环境下各关节的理想刚体运动信号;理想刚体运动信号为关节在无振动情况下对应的位置信息和/或速度信息;
纯振动估测单元确定机器人在真实环境下各关节的真实运动信号,基于各关节的理想刚体运动信号和各关节的真实运动信号,确定各关节对应的纯振动值;真实运动信号为关节在有振动情况下对应的位置信息和/或速度信息;
振动抑制单元基于各关节对应的纯振动值,确定各关节对应的振动补偿量,并基于各关节对应的振动补偿量对机器人各关节进行负反馈调节。
在其中一个实施例中,纯振动估测单元将各关节的理想刚体运动信号和各关节的真实运动信号分别输入至闭环控制器中,通过闭环控制器预测各关节对应的纯振动值;其中,各关节对应的纯振动值反馈至等效刚体动力学模型中,修正等效刚体动力学模型输出的各关节的理想刚体运动信号。
在其中一个实施例中,振动抑制单元将各关节对应的纯振动值输入至频率选择器中,通过频率选择器得到特定频率区间内各关节对应的振动补偿量;其中,各关节对应的振动补偿量反馈至运动控制器的输入,修正各关节力矩的输入力矩。
在其中一个实施例中,振动抑制单元基于各关节对应的振动补偿量和各关节力矩电机的真实运动信号,修正各关节力矩电机的输入力矩,并基于各关节力矩电机修改后的输入力矩控制各关节力矩电机运转;其中,各关节力矩电机的输入力矩作为等效刚体动力学模型的输入。
在其中一个实施例中,如图13所示,检测模块包括第一检测模块和第二检测模块;判断模块包括第一判断模块和第二判断模块;
其中,第一检测模块用于检测机器人的各关节在运动过程中干扰信号的幅值是否大于安全阈值;第二检测模块用于检测机器人的各关节在运动过程中振动信号的幅值是否大于安全阈值;第一判断模块用于判断机器人的各关节所受的干扰是否小于设定的安全阈值;第二判断模块用于判断机器人的各关节所受的振动是否小于设定的安全阈值。
在一些实施例中,机器人启动时干扰和振动的抑制流程图如图14所示,机器人启动时,通过控制器使机器人获得伺服指令;机器人的伺服单元获得伺服指令,开启伺服运动;第一检测模块和第二检测模块,分别对手术机器人启动时是否存在干扰和启动振动进行判断;当第一检测模块和第二检测模块检测出的干扰和振动均大于安全阈值时,干扰抑制模块和振动抑制模块将机器人当前的运动信号进行算法补偿,抑制干扰和振动幅值;判断模块对系统补偿后的干扰和振动幅值进行安全阈值判断;当补偿后的干扰和振动小于设定的安全阈值时,将补偿后的伺服指令输出到机器人的伺服单元,使其完成补偿运动。
在一些实施例中,机器人启动时干扰和振动的抑制算法流程图如图15所示,控制器(CPU),用于解析运动指令,处理检测机器人自身的元器件测量的数据(例如,编码器的测量信号)。伺服单元,用于实现被控系统的伺服运动。检测模块,用于检测当前被控系统是否存在干扰和振动。
干扰抑制模块,包括无干扰运算单元、干扰估测单元和干扰补偿单元。无干扰运算单元,用于求解不存在干扰的理想系统;干扰估测单元,用于求解实际系统中存在的干扰成分;干扰补偿单元,用于将实际系统中的干扰成分提取出来,作为补偿量负反馈至系统的控制输入,以消除系统所受的干扰。
振动抑制模块,包括无振动运算单元、纯振动估测单元和振动抑制单元。无振动运算单元,用于求解不含有振动的理想刚体系统的运动信息;纯振动估测单元,用于求解实际系统中存在的振动成分;振动抑制单元,用于将实际系统中的纯振动成分提取出来,作为补偿量负反馈至系统的控制输入,以抑制系统的振动现象。
判断模块,用于判断系统所受的干扰和振动是否小于设定的安全阈值,以保证系统控制输出中的干扰成分和振动成分在安全阈值内。
在一些实施例中,机器人正常工作过程中,突然受到干扰/突然产生振动而进行抑制的场景下,干扰和振动的抑制流程图如图16所示,机器人正常工作过程中,机器人突然受到了干扰或者产生了振动现象。第一检测模块和第二检测模块,分别对机器人运动过程中产生的干扰和振动是否在安全阈值内进行判断;当第一检测模块和第二检测模块检测出的干扰和振动均大于安全阈值时,干扰抑制模块和振动抑制模块将手术机器人当前的运动信号进行算法补偿,抑制干扰和振动幅值;判断模块对补偿后的干扰和振动幅值进行安全阈值判断;当补偿后的干扰和振动小于设定的安全阈值时,将补偿后的伺服指令输出到手术机器人的伺服单元,使其完成补偿运动。
在一些实施例中,机器人正常工作过程中,突然受到干扰/突然产生振动而进行抑制的场景下,干扰和振动的抑制算法流程图如图17所示,检测模块,用于检测当前被控系统是否存在干扰和振动。干扰抑制模块,包括无干扰运算单元、干扰估测单元和干扰补偿单元。无干扰运算单元,用于求解不存在干扰的理想系统;干扰估测单元,用于求解实际系统中存在的干扰成分;干扰补偿单元,用于将实际系统中的干扰成分提取出来,作为补偿量负反馈至系统的控制输入,以消除系统所受的干扰。
振动抑制模块,包括无振动运算单元、纯振动估测单元和振动抑制单元。无振动运算单元,用于求解不含有振动的理想刚体系统的运动信息;纯振动估测单元,用于求解实际系统中存在的振动成分;振动抑制单元,用于将实际系统中的纯振动成分提取出来,作为补偿量负反馈至系统的控制输入,以抑制系统的振动现象。
判断模块,用于判断系统所受的干扰和振动是否小于设定的安全阈值,以保证系统控制输出中的干扰成分和振动成分在安全阈值内。
伺服单元,用于实现被控系统的伺服运动。
上述机器人关节运动补偿系统中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中,也可以以软件形式存储于计算机设备中的存储器中,以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中,提供了一种计算机设备,该计算机设备可以是控制器,其内部结构图可以如图18所示。该计算机设备包括处理器、存储器、输入/输出接口、通信接口、显示单元和输入装置。其中,处理器、存储器和输入/输出接口通过系统总线连接,通信接口、显示单元和输入装置通过输入/输出接口连接到系统总线。其中,该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质和内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的输入/输出接口用于处理器与外部设备之间交换信息。该计算机设备的通信接口用于与外部的终端进行有线或无线方式的通信,无线方式可通过WIFI、移动蜂窝网络、NFC(近场通信)或其他技术实现。该计算机程序被处理器执行时以实现一种机器人关节运动补偿方法。该计算机设备的显示单元用于形成视觉可见的画面,可以是显示屏、投影装置或虚拟现实成像装置。显示屏可以是液晶显示屏或者电子墨水显示屏,该计算机设备的输入装置可以是显示屏上覆盖的触摸层,也可以是计算机设备外壳上设置的按键、轨迹球或触控板,还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解,图18中示出的结构,仅仅是与本申请方案相关的部分结构的框图,并不构成对本申请方案所应用于其上的计算机设备的限定,具体的计算机设备可以包括比图中所示更多或更少的部件,或者组合某些部件,或者具有不同的部件布置。
在一个实施例中,提供了一种计算机设备,包括存储器和处理器,存储器中存储有计算机程序,该处理器执行计算机程序时实现上述各方法实施例中的步骤。
在一个实施例中,提供了一种计算机可读存储介质,其上存储有计算机程序,计算机程序被处理器执行时实现上述各方法实施例中的步骤。
在一个实施例中,提供了一种计算机程序产品,包括计算机程序,该计算机程序被处理器执行时实现上述各方法实施例中的步骤。
需要说明的是,本申请所涉及的用户信息(包括但不限于用户设备信息、用户个人信息等)和数据(包括但不限于用于分析的数据、存储的数据、展示的数据等),均为经用户授权或者经过各方充分授权的信息和数据,且相关数据的收集、使用和处理需要遵守相关国家和地区的相关法律法规和标准。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一非易失性计算机可读取存储介质中,该计算机程序在执行时,可包括如上述各方法的实施例的流程。其中,本申请所提供的各实施例中所使用的对存储器、数据库或其它介质的任何引用,均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器(Read-OnlyMemory,ROM)、磁带、软盘、闪存、光存储器、高密度嵌入式非易失性存储器、阻变存储器(ReRAM)、磁变存储器(Magnetoresistive Random Access Memory,MRAM)、铁电存储器(Ferroelectric Random Access Memory,FRAM)、相变存储器(Phase Change Memory,PCM)、石墨烯存储器等。易失性存储器可包括随机存取存储器(Random Access Memory,RAM)或外部高速缓冲存储器等。作为说明而非局限,RAM可以是多种形式,比如静态随机存取存储器(Static Random Access Memory,SRAM)或动态随机存取存储器(Dynamic RandomAccess Memory,DRAM)等。本申请所提供的各实施例中所涉及的数据库可包括关系型数据库和非关系型数据库中至少一种。非关系型数据库可包括基于区块链的分布式数据库等,不限于此。本申请所提供的各实施例中所涉及的处理器可为通用处理器、中央处理器、图形处理器、数字信号处理器、可编程逻辑器、基于量子计算的数据处理逻辑器等,不限于此。
以上实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对本申请专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请的保护范围应以所附权利要求为准。
Claims (10)
1.一种机器人关节运动补偿方法,其特征在于,所述方法包括:
实时检测机器人的各关节在运动过程中干扰信号的幅值和振动信号的幅值是否大于安全阈值;
在检测到干扰信号的幅值大于安全阈值的情况下,预测各关节的干扰补偿量,并基于各关节对应的干扰补偿量对相应的关节进行负反馈调节;所述干扰补偿量为补偿耦合力干扰对所述机器人各关节运动造成的影响所需调节的力矩变化量;
在检测到振动信号的幅值大于安全阈值的情况下,预测各关节的振动补偿量,并基于各关节对应的振动补偿量对所述机器人各关节进行负反馈调节;所述振动补偿量为补偿特定频率区间内的纯振动干扰对各关节运动造成的影响所需的运动变化量;
在所述机器人的各关节完成负反馈调节后,在各关节的干扰信号的幅值和振动信号的幅值均小于安全阈值的情况下,停止对所述机器人各关节进行负反馈调节。
2.根据权利要求1所述的方法,其特征在于,所述预测各关节的干扰补偿量,包括:
获取伺服指令;所述伺服指令对应有各关节力矩电机的输入力矩,所述伺服指令用于控制各关节力矩电机运转至对应的输入力矩;
将所述伺服指令对应的各关节力矩电机的输入力矩输入关节动力学模型,得到所述机器人在没有干扰的理想环境下各关节的理想关节力矩;
根据所述伺服指令控制各关节力矩电机转动,并采集关节力矩电机的真实关节力矩,基于各关节的理想关节力矩和各关节的真实关节力矩,确定各关节对应的干扰值;
基于各关节对应的干扰值,确定各关节对应的干扰补偿量。
3.根据权利要求1所述的方法,其特征在于,所述预测各关节的振动补偿量,包括:
获取伺服指令,并通过运动控制器解析所述伺服指令对应的各关节力矩电机的输入力矩;所述伺服指令对应有各关节力矩电机的输入力矩,所述伺服指令用于控制各关节力矩电机运转至对应的输入力矩;
将所述伺服指令对应的各关节力矩电机的输入力矩输入关节动力学模型中的等效刚体动力学模型中,得到所述机器人在没有干扰的理想环境下各关节的理想刚体运动信号;所述理想刚体运动信号为关节在无振动情况下对应的位置信息和/或速度信息;
根据所述伺服指令控制各关节力矩电机转动,并采集关节力矩电机的真实运动信号,基于各关节的理想刚体运动信号和各关节的真实运动信号,确定各关节对应的纯振动值;所述真实运动信号为关节在有振动情况下对应的位置信息和/或速度信息;
基于各关节对应的纯振动值,确定各关节对应的振动补偿量。
4.根据权利要求3所述的方法,其特征在于,所述基于各关节的理想刚体运动信号和各关节的真实运动信号,确定各关节对应的纯振动值,包括:
将各关节的理想刚体运动信号和各关节的真实运动信号分别输入至闭环控制器中,通过所述闭环控制器预测各关节对应的纯振动值;其中,各关节对应的纯振动值反馈至所述等效刚体动力学模型中,修正所述等效刚体动力学模型输出的各关节的理想刚体运动信号。
5.根据权利要求3所述的方法,其特征在于,所述基于各关节对应的纯振动值,确定各关节对应的振动补偿量,包括:
将各关节对应的纯振动值输入至频率选择器中,通过所述频率选择器得到特定频率区间内各关节对应的振动补偿量;其中,各关节对应的振动补偿量反馈至所述运动控制器的输入,修正各关节力矩的输入力矩。
6.根据权利要求3所述的方法,其特征在于,所述基于各关节对应的振动补偿量对所述机器人各关节进行负反馈调节,包括:
基于各关节对应的振动补偿量和各关节力矩电机的真实运动信号,修正各关节力矩电机的输入力矩,并基于各关节力矩电机修改后的输入力矩控制各关节力矩电机运转;其中,各关节力矩电机的输入力矩作为等效刚体动力学模型的输入。
7.一种机器人关节运动补偿系统,其特征在于,所述系统包括:主控单元、伺服单元、检测模块、干扰抑制模块、振动抑制模块和判断模块;
所述主控单元输出伺服指令至所述伺服单元;
所述伺服单元根据所述伺服指令控制机器人的各关节运动;
所述检测模块实时检测所述机器人的各关节在运动过程中干扰信号的幅值和振动信号的幅值是否大于安全阈值;
所述干扰抑制模块在检测到干扰信号的幅值大于安全阈值的情况下,预测各关节的干扰补偿量,并基于各关节对应的干扰补偿量对相应的关节进行负反馈调节;所述干扰补偿量为补偿耦合力干扰对所述机器人各关节运动造成的影响所需调节的力矩变化量;
所述振动抑制模块在检测到振动信号的幅值大于安全阈值的情况下,预测各关节的振动补偿量,并基于各关节对应的振动补偿量对所述机器人各关节进行负反馈调节;所述振动补偿量为补偿特定频率区间内的纯振动干扰对各关节运动造成的影响所需的运动变化量;
所述判断模块在所述机器人的各关节完成负反馈调节后,在各关节的干扰信号的幅值和振动信号的幅值均小于安全阈值的情况下,停止对所述机器人各关节进行负反馈调节。
8.根据权利要求7所述的系统,其特征在于,所述干扰抑制模块包括:无干扰运算单元、干扰估测单元和干扰补偿单元;
所述无干扰运算单元通过关节动力学模型,计算得到所述机器人在没有干扰的理想环境下各关节的理想关节力矩;
所述干扰估测单元确定所述机器人在真实环境下各关节的真实关节力矩,基于各关节的理想关节力矩和各关节的真实关节力矩,确定各关节对应的干扰值;
所述干扰补偿单元基于各关节对应的干扰值,确定各关节对应的干扰补偿量,并基于各关节对应的干扰补偿量对所述机器人各关节进行负反馈调节。
9.根据权利要求7所述的系统,其特征在于,所述振动抑制模块包括:无振动运算单元、纯振动估测单元和振动抑制单元;
所述无振动运算单元通过关节动力学模型中的等效刚体动力学模型,计算得到所述机器人在没有干扰的理想环境下各关节的理想刚体运动信号;所述理想刚体运动信号为关节在无振动情况下对应的位置信息和/或速度信息;
所述纯振动估测单元确定所述机器人在真实环境下各关节的真实运动信号,基于各关节的理想刚体运动信号和各关节的真实运动信号,确定各关节对应的纯振动值;所述真实运动信号为关节在有振动情况下对应的位置信息和/或速度信息;
所述振动抑制单元基于各关节对应的纯振动值,确定各关节对应的振动补偿量,并基于各关节对应的振动补偿量对所述机器人各关节进行负反馈调节。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至6中任一项所述的方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310382416.3A CN116372926A (zh) | 2023-04-10 | 2023-04-10 | 机器人关节运动补偿方法、系统和存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310382416.3A CN116372926A (zh) | 2023-04-10 | 2023-04-10 | 机器人关节运动补偿方法、系统和存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116372926A true CN116372926A (zh) | 2023-07-04 |
Family
ID=86969083
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310382416.3A Pending CN116372926A (zh) | 2023-04-10 | 2023-04-10 | 机器人关节运动补偿方法、系统和存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116372926A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN118123812A (zh) * | 2024-01-29 | 2024-06-04 | 浙江大学 | 一种基于机械臂关节伺服系统的多模态主动振动抑制方法 |
-
2023
- 2023-04-10 CN CN202310382416.3A patent/CN116372926A/zh active Pending
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN118123812A (zh) * | 2024-01-29 | 2024-06-04 | 浙江大学 | 一种基于机械臂关节伺服系统的多模态主动振动抑制方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102785046B (zh) | 具备学习控制功能的机器人和点焊机器人 | |
US10481566B2 (en) | Machine learning device, servo control device, servo control system and machine learning method | |
KR102541661B1 (ko) | 조향 제어 속성 및 견인 제어 속성을 결정하기 위한 조향 및 견인 애플리케이션 | |
CN102189550B (zh) | 具有学习控制功能的机器人 | |
JP6411380B2 (ja) | ロボットとその環境との衝突の検出を改善する方法、同方法を実施するシステムおよびコンピュータプログラム製品 | |
JP5591400B2 (ja) | 駆動機械の負荷特性推定装置 | |
CN109968361B (zh) | 一种基于实时力反馈的变阻抗遥操作控制装置及方法 | |
CN109562518B (zh) | 并联连杆机构的控制装置 | |
JP5389251B2 (ja) | 並列駆動システム | |
CN116372926A (zh) | 机器人关节运动补偿方法、系统和存储介质 | |
Olma et al. | Observer-based nonlinear control strategies for Hardware-in-the-Loop simulations of multiaxial suspension test rigs | |
CN113799116A (zh) | 直接力反馈控制方法、装置、控制器和机器人 | |
US11077552B2 (en) | Control system, machine learning apparatus, maintenance assistance apparatus, data generating method, and maintenance assisting method | |
JP2005316937A (ja) | 制御装置およびその制御方法 | |
CN111772794B (zh) | 用于微创手术的主端、从端机器人控制方法与装置 | |
CN106950842B (zh) | 解耦控制方法、装置、机器人及存储介质 | |
US11287801B2 (en) | Control device, CNC device, and control method | |
Miura et al. | High-stiff motion reproduction using position-based motion-copying system with acceleration-based bilateral control | |
CN111015661B (zh) | 一种机器人柔性负载主动振动控制方法和系统 | |
JP4664576B2 (ja) | サーボ制御装置 | |
JP2014117787A (ja) | 制御装置 | |
CN115890666A (zh) | 机器人及其碰撞保护方法、装置及存储介质 | |
CN115042178A (zh) | 保证接触安全的机器人强化学习方法、装置、设备及介质 | |
KR101338082B1 (ko) | 착용식 로봇의 외란제거방법 및 외란제거시스템 | |
US8374706B2 (en) | Method for moving a machine element of an automation machine and a drive system |
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 |