主从同构手术机器人防抖滤波与安全约束控制方法及装置与流程

摘要: 本发明涉及机器人运动控制,尤其涉及一种主从同构手术机器人防抖滤波与安全约束控制方法及装置。背景技术:1、经自然腔道手术机器人通常通过人体自然腔道进入目标区域,具...
主从同构手术机器人防抖滤波与安全约束控制方法及装置与流程

本发明涉及机器人运动控制,尤其涉及一种主从同构手术机器人防抖滤波与安全约束控制方法及装置。

背景技术:

1、经自然腔道手术机器人通常通过人体自然腔道进入目标区域,具有创伤小但路径受限、操作空间狭窄等特点。该类机器人对主从操作的精细性、平顺性和安全性要求较高。

2、在主从遥操作系统中,医生通过主手输入操作意图,从手根据主手指令完成弯曲、旋转、插入或退出等动作。对于主从同构式机器人,主手与从手在自由度构型上具有对应关系,例如均包含弯曲自由度、绕自身旋转自由度和轴向移动自由度。

3、然而,主手操作信号中通常会包含以下干扰:医生手部生理性震颤,典型频率集中在8-12hz;编码器、磁传感器或位移传感器引入的测量噪声;通信或采样过程中的偶发毛刺;主手机构间隙、摩擦或弹性变形导致的微小抖动;从手因速度、加速度突变产生的机械冲击。

4、现有方法常采用简单低通滤波或移动平均滤波对主手信号进行平滑处理。但该类方法存在明显不足:若滤波强度较弱,则难以有效抑制震颤;若滤波强度较强,则会引入较大相位滞后,使主从操作发黏;移动平均滤波窗口增大时延迟明显;单一滤波器难以同时兼顾抗颤、低延迟和平顺控制;缺少针对从手速度、加速度和加加速度(jerk)的安全约束,容易造成从手动作突变。

5、因此,需要一种适用于主从同构手术机器人的防抖滤波与安全约束控制方法,在保证主从跟随性的同时,抑制主手颤振并提高从手运动安全性。

技术实现思路

1、本发明提供一种主从同构手术机器人防抖滤波与安全约束控制方法及装置,旨在解决现有主从同构手术机器人控制方法难以同时兼顾主手操作信号的防抖滤波效果与主从操作的低延迟跟随性,且缺少对从手运动的安全约束,容易造成从手动作突变等问题。

2、第一方面,本发明提供了一种主从同构手术机器人防抖滤波与安全约束控制方法,所述主从同构手术机器人包括主手、控制器以及从手,所述主手与所述从手具有对应的多自由度运动关系,所述方法包括:获取所述主手的多自由度操作信号,并对所述多自由度操作信号进行预处理;对预处理后的多自由度操作信号分别进行状态估计,以获取主手各自由度的速度估计;根据所述主手各自由度的速度估计生成门控系数,并基于所述门控系数对至少一个自由度的速度估计进行门控陷波滤波处理;对门控陷波滤波后的速度估计进行低通滤波,以得到主手多自由度滤波速度信号;根据所述主手与所述从手之间的自由度对应关系,将所述主手多自由度滤波速度信号映射为从手运动控制指令;对所述从手运动控制指令进行运动安全约束处理,以限制所述从手运动过程中的速度幅值、加速度幅值以及加加速度幅值,并生成从手最终控制指令。

3、可选地,所述主手的多自由度操作信号包括绕自身旋转自由度信号、弯曲自由度信号以及轴向移动自由度信号;所述从手具有与所述主手对应的旋转运动自由度、弯曲运动自由度以及轴向运动自由度。

4、可选地,所述对所述多自由度操作信号进行预处理包括:对存在周期变化特征的角度信号进行连续化处理,以消除角度周期跳变造成的数据不连续;

5、对不同自由度对应的操作信号进行单位转换和归一化处理。

6、可选地,所述对所述多自由度操作信号进行预处理还包括:对所述多自由度操作信号进行异常跳变抑制;所述异常跳变抑制包括:分别判断各自由度当前采样时刻的操作信号值与上一采样时刻的操作信号值的差值是否超过对应自由度的预设跳变阈值;若是,则将当前采样时刻的操作信号值判定为异常值,并采用对应自由度历史采样时刻的操作信号值对所述异常值进行替代处理。

7、可选地,所述对预处理后的多自由度操作信号分别进行状态估计,包括:采用α-β状态估计器,基于常速度模型对各自由度的位置和速度进行预测和校正,以获得对应自由度的速度估计结果。

8、可选地,所述根据所述主手各自由度的速度估计生成门控系数,包括:根据各自由度的速度估计的绝对值与预设速度阈值的比较结果生成所述门控系数,所述门控系数的大小与所述速度估计的绝对值呈负相关;所述门控陷波滤波处理包括:根据所述门控系数将各自由度的原始速度估计与经门控陷波滤波后的速度估计进行加权融合,以在所述速度估计的绝对值小于所述预设速度阈值时增强陷波作用,在所述速度估计的绝对值大于所述预设速度阈值时减弱陷波作用。

9、可选地,所述主手与所述从手之间的自由度对应关系包括:所述主手的旋转自由度对应所述从手的旋转运动;所述主手的弯曲自由度对应所述从手的弯曲运动;所述主手的轴向移动自由度对应所述从手沿自然腔道方向的插入或退出运动。

10、可选地,所述运动安全约束处理包括:根据所述从手运动状态对所述从手运动控制指令进行速度幅值约束;根据速度变化过程对所述从手运动控制指令进行加速度幅值约束;根据加速度变化过程对所述从手运动控制指令进行加加速度幅值约束;其中,对应于轴向移动自由度的安全约束参数小于或等于其他自由度对应的安全约束参数。

11、第二方面,本发明提供了主从同构手术机器人防抖滤波与安全约束控制装置,包括:信号采集模块,用于获取主手的多自由度操作信号;预处理模块,用于对所述多自由度操作信号进行预处理;状态估计模块,用于对预处理后的多自由度操作信号分别进行状态估计,以获取主手各自由度的速度估计;门控陷波滤波模块,用于根据所述主手各自由度的速度估计生成门控系数,并基于所述门控系数对至少一个自由度的速度估计进行门控陷波滤波处理;低通滤波模块,用于对门控陷波滤波后的速度估计进行低通滤波,以得到主手多自由度滤波速度信号;主从映射模块,用于根据所述主手与所述从手之间的自由度对应关系,将所述主手多自由度滤波速度信号映射为从手运动控制指令;安全约束模块,用于对所述从手运动控制指令进行运动安全约束处理,以限制所述从手运动过程中的速度幅值、加速度幅值以及加加速度幅值,并生成从手最终控制指令。

12、第三方面,本发明提供一种电子设备,包括处理器以及存储器,所述存储器存储有计算机可读取指令,当所述计算机可读取指令由所述处理器执行时,运行如上述第一方面提供的所述方法中的步骤。

13、第四方面,本发明提供一种存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时运行如上述第一方面提供的所述方法中的步骤。

14、第五方面,本发明提供一种计算机程序产品,包括计算机程序,所述计算机程序被处理器执行时运行如上述第一方面提供的所述方法中的步骤。

15、从以上技术方案可以看出,本发明具有以下优点:

16、本发明提供了一种主从同构手术机器人防抖滤波与安全约束控制方法和装置,所述方法包括:获取所述主手的多自由度操作信号,并对所述多自由度操作信号进行预处理;对预处理后的多自由度操作信号分别进行状态估计,以获取主手各自由度的速度估计;根据所述主手各自由度的速度估计生成门控系数,并基于所述门控系数对至少一个自由度的速度估计进行门控陷波滤波处理;对门控陷波滤波后的速度估计进行低通滤波,以得到主手多自由度滤波速度信号;根据所述主手与所述从手之间的自由度对应关系,将所述主手多自由度滤波速度信号映射为从手运动控制指令;对所述从手运动控制指令进行运动安全约束处理,以限制所述从手运动过程中的速度幅值、加速度幅值以及加加速度幅值,并生成从手最终控制指令。从而实现了对主手多自由度操作信号中人体生理性震颤的自适应精准抑制,在低速精细操作时有效滤除手抖干扰,同时在高速主动操作时保持信号直通以降低相位滞后,达到防抖与低延迟的协同优化;并且通过对从手控制指令施加速度、加速度及加加速度的多重安全约束,避免了从手动作突变和机械冲击,最终显著提升了主从同构手术机器人在遥操作过程中的平顺性、稳定性、安全性和跟手性。