[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN114442490B - 基于自适应力反馈的血管介入机器人系统主端控制方法 - Google Patents

基于自适应力反馈的血管介入机器人系统主端控制方法 Download PDF

Info

Publication number
CN114442490B
CN114442490B CN202210361274.8A CN202210361274A CN114442490B CN 114442490 B CN114442490 B CN 114442490B CN 202210361274 A CN202210361274 A CN 202210361274A CN 114442490 B CN114442490 B CN 114442490B
Authority
CN
China
Prior art keywords
motor
model
force
slave
operating rod
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
Application number
CN202210361274.8A
Other languages
English (en)
Other versions
CN114442490A (zh
Inventor
柏健
黄志俊
刘金勇
钱坤
郑泽阳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Langbokang Medical Technology Co ltd
Original Assignee
Lancet Robotics Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Lancet Robotics Co Ltd filed Critical Lancet Robotics Co Ltd
Priority to CN202210361274.8A priority Critical patent/CN114442490B/zh
Publication of CN114442490A publication Critical patent/CN114442490A/zh
Application granted granted Critical
Publication of CN114442490B publication Critical patent/CN114442490B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A61B34/37—Master-slave robots
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A61B2034/301—Surgical robots for introducing or steering flexible instruments inserted into the body, e.g. catheters or endoscopes
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A61B2034/303—Surgical robots specifically adapted for manipulations within body lumens, e.g. within lumen of gut, spine, or blood vessels

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Robotics (AREA)
  • Surgery (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Animal Behavior & Ethology (AREA)
  • Veterinary Medicine (AREA)
  • Biomedical Technology (AREA)
  • Molecular Biology (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

本发明实施例公开了一种基于自适应力反馈的血管介入机器人系统主端控制方法，其中方法包括：构建电机模型；构建滑轨模型；获取从端传感器信号，对从端传感器信号进行滤波；构建所述电机的参考模型，基于所述参考模型，设置控制器对所述电机模型的参数进行实时校正；基于校正后的电机模型及所述滑轨模型，估算摩擦力，并补偿所述摩擦力；基于所述处理后的传感器信号，将还原的所述从端设备所受到的扭力、还原的所述从端设备所受到的水平方向的阻力作用于操作杆，作用于操作杆的还原后的力即为力反馈。本方法能够实现精准力反馈，使血管介入机器人系统系统拥有强抗扰性和鲁棒性，能够较大程度地避免输出的波动。

Description

基于自适应力反馈的血管介入机器人系统主端控制方法
技术领域
本发明涉及控制领域，尤其是基于自适应力反馈的血管介入机器人系统主端控制方法。
背景技术
近年来，伴随着机器人技术的进步以及人们对健康问题的更加重视，机器人辅助介入治疗手段逐渐成为人们关注的焦点。目前的血管介入手术设备化利用程度较低，大多是由经验丰富的医生进行操作。介入医生所需的培训时间也较长，并且介入手术的操作较多地依赖经验性操作。此外，由于人的血管弯曲狭窄且分支较多，并且血管壁较为脆弱，这就对实验过程中医生对导管操作的精度提出了很大的要求，需要医生十分精细的操作手法。长时间精细化的手术操作势必会造成医生的操作疲劳，从而可能会影响到手术的效果。此外，操作医生需要经常性地较长时间暴露于X射线之下进行手术操作，即便穿戴有防辐射的铅服，也无法保证医生不受到射线的辐射，而且沉重的铅服不仅会对医生的操作精度造成影响，而且长期穿戴也会对医生的身体造成很大的负担，会对医生的身体健康造成慢性影响。
因此，出现了血管介入机器人系统，血管介入机器人系统一般采用主从操作模式，由血管介入机器人系统暴露于X射线下实施手术，医生在病房外通过远程操作的方式控制所述血管介入机器人系统进行工作，避免了医生受到手术室内X射线辐射。与医生暴露于X射线下工作相比，血管介入机器人系统具有高精度、高准确度的特点，并且能够消除人手颤抖对介入手术的影响。此外，血管介入机器人系统在临床的应用，能够帮助医生完成更为复杂的介入手术，有效减少复杂手术对医生经验的依赖程度。以支架手术为例，血管介入机器人系统需要根据手术过程中实时的CT影像判断导丝、导管和球囊所处的位置，由医生对所述血管介入机器人系统进行操作，实现在远程对血管介入机器人的操控。但是，现有的血管介入机器人系统无法像传统手术过程一样，通过在手术过程中感受导管、球囊和导丝当前所受的阻力等信息来判断目前介入物当前的位置状态，因此，导致医生无法实时地通过“手感”来判断当前手术进行的状态，而是通过注射更多次剂量的造影剂通过术中CT来进行判断。这样不仅会延长手术时间，同时还会对患者产生二次伤害。
发明内容
根据本发明的一方面，提供一种基于自适应力反馈的血管介入机器人系统主端控制方法，包括：
构建电机模型，所述电机模型用于模拟主端设备中的电机，所述电机包括第一电机及第二电机；所述第一电机用于带动操作杆旋转，还原从端设备所受到的扭力；所述第二电机用于带动所述操作杆沿水平方向运动，还原从端设备所受到的水平方向的阻力；
构建滑轨模型，所述滑轨模型用于估算所述主端设备使用过程中的摩擦力；
获取从端传感器信号，对所述从端传感器信号进行滤波，得到处理后的传感器信号；
构建所述电机的参考模型，所述电机的参考模型是表征电机无误差运行的模型；基于所述参考模型，设置控制器对所述电机模型的参数进行实时校正；
基于校正后的电机模型及所述滑轨模型，估算摩擦力，并补偿所述摩擦力；基于所述处理后的传感器信号，将还原的所述从端设备所受到的扭力、还原的所述从端设备所受到的水平方向的阻力作用于操作杆，作用于操作杆的还原后的力即为力反馈。
可选地，在本发明上述各方法实施例中，所述电机模型为：
Figure 752331DEST_PATH_IMAGE001
，
Figure 262947DEST_PATH_IMAGE002
，
Figure 370580DEST_PATH_IMAGE003
，
其中，u、i、R、
Figure 562527DEST_PATH_IMAGE004
、L分别表示所述电机模型的端电压、线电流、绕组内阻、电机内部磁链和绕组电感，Ke、Kt分别是反电动势系数和转矩系数，Te表示电机的输出转矩，ω为电机的当前转速，t为当前时间，J为电机的转动惯量常数，Bz为电机的阻尼系数。
可选地，在本发明上述各方法实施例中，所述滑轨模型为：
Figure 439216DEST_PATH_IMAGE005
，
Figure 323995DEST_PATH_IMAGE006
，
Figure 653346DEST_PATH_IMAGE007
，
Figure 442791DEST_PATH_IMAGE008
，
以及初始位置条件
Figure 439566DEST_PATH_IMAGE009
，
其中，Mtotal为第一滑块、第二滑块、第一电机、操作杆以及联轴器质量之和，dw为同步轮直径，所述同步轮包括第一同步轮及第二同步轮，所述第二电机带动所述第二同步轮旋转，所述第二同步轮、第一同步轮以及同步带共同构成皮带传动装置，所述同步带用于带动固定在同步带上的滑块运动，所述滑块包括第一滑块和第二滑块，所述第二滑块用于将所述第一电机固定于同步带，并使所述第一电机与固定在所述同步带上的第一滑块之间保持水平方向的相对静止；所述操作杆与所述第一电机的电机轴通过联轴器进行固定，所述联轴器带动操作杆进行旋转运动；
Figure 760826DEST_PATH_IMAGE010
为水平方向总滑动摩擦系数，Fh为用户对操作杆施加水平方向力，p、v、a分别为滑块固定水平方向的位置、速度、加速度；设
Figure 577473DEST_PATH_IMAGE011
，Fref为需要跟踪的力信号，所述需要跟踪的力信号为从端传感器传输的、经过过滤的力信号，sign(*)为符号函数，表示当*大于0时为1，*小于0时为-1，*等于0时为0；g为重力常数，f(t)为所述主端设备运行时机械结构产生的扰动，p0为滑轨初始位置。
可选地，在本发明上述各方法实施例中，所述对所述从端传感器信号进行滤波，包括：
构建非线性跟踪微分器，将所述非线性跟踪微分器作为低通滤波器进行滤波，所述非线性跟踪微分器为：
Figure 642380DEST_PATH_IMAGE012
，
其中，获取的所述从端传感器信号为forg，fsun(z1,z2,r,h)满足
Figure 759241DEST_PATH_IMAGE013
，
其中，Fref(k+1)为k+1时刻所述从端传感器通过滤波算法获取的原信号，Fref(k)为k时刻所述从端传感器通过滤波算法获取的原信号，h为所述从端传感器实际的采样间隔时间，Dref(k)为k时刻所述从端传感器通过滤波算法获取的原信号的一阶导数，Dref(k+1)为k+1时刻所述从端传感器通过滤波算法获取的原信号的一阶导数，v(k)为运算过程中的中间变量，forg(k)为k时刻时所述从端传感器实际测量的带有噪声的原信号，r为滤波系数，fix(q)为向上取整函数，q、s为运算过程中的中间变量，sat(a,b)为饱和函数，表示当|a|<b时，sat(a,b)=a，其余情况sat(a,b)=sign(a)*b。
可选地，在本发明上述各方法实施例中，所述参考模型为：
Figure 785490DEST_PATH_IMAGE014
，
Figure 355012DEST_PATH_IMAGE015
，
其中，矩阵Am和B是参数矩阵形式的表述，
Figure 426873DEST_PATH_IMAGE016
为参考模型的状态变量。
可选地，在本发明上述各方法实施例中，所述基于所述参考模型，设置控制器对所述电机模型的参数进行实时校正，包括：
设置第一控制器，所述第一控制器为：
Figure 867082DEST_PATH_IMAGE017
，
其中，C为矩阵
Figure 61303DEST_PATH_IMAGE018
，
Figure 383700DEST_PATH_IMAGE019
指输入的参考信号，x(t)为电机模型的状态变量，e(t)表示输出误差，
Figure 318639DEST_PATH_IMAGE020
为待调节参数；
基于所述第一控制器，对所述电机模型的磁链和绕组电阻进行实时校正。
可选地，在本发明上述各方法实施例中，所述基于校正后的电机模型及所述滑轨模型，估算摩擦力，其中，估算得到的摩擦力为
Figure 410092DEST_PATH_IMAGE021
，其中，
Figure 509635DEST_PATH_IMAGE010
是指的第一滑块、第二滑块与滑轨之间相对运动的摩擦系数，或是操作杆和第一滑块旋转运动时存在的摩擦系数。
可选地，在本发明上述各方法实施例中，构建第二控制器，所述第二控制器用于将还原的所述从端设备所受到的扭力、还原的所述从端设备所受到的水平方向的阻力作用于操作杆，所述第二控制器为：
Figure 53749DEST_PATH_IMAGE022
，
其中，k为当前时刻离散化的时间序列，r(k)为所述处理后的传感器信号，y(k)=ft(k)是滑轨的输出，也就是操作杆末端用户感受到的力，ki为待调节的参数，j为标准求和运算中的变量符号，N为设置的积分求和序列上限数值，r(k-j)为k-j时刻处理后的传感器信号，y(k-j)为k-j时刻滑轨的输出，kd为待调节的参数，Dref(k-1)为k-1时刻通过滤波算法获取的原信号的一阶导数。
根据本发明的另一方面，提供一种基于自适应力反馈的血管介入机器人系统主端控制装置，包括：
电机模型构建模块，配置为构建电机模型，所述电机模型用于模拟主端设备中的电机，所述电机包括第一电机及第二电机；所述第一电机用于带动操作杆旋转，还原从端设备所受到的扭力；所述第二电机用于带动所述操作杆沿水平方向运动，还原从端设备所受到的水平方向的阻力；
滑轨模块构建模块，配置为构建滑轨模型，所述滑轨模型用于估算所述主端设备使用过程中的摩擦力；
滤波模块，配置为获取从端传感器信号，对所述从端传感器信号进行滤波，得到处理后的传感器信号；
校正模块，配置为构建所述电机的参考模型，所述电机的参考模型是表征电机无误差运行的模型；基于所述参考模型，设置控制器对所述电机模型的参数进行实时校正；
力反馈模块，配置为基于校正后的电机模型及所述滑轨模型，估算摩擦力，并补偿所述摩擦力；基于所述处理后的传感器信号，将还原的所述从端设备所受到的扭力、还原的所述从端设备所受到的水平方向的阻力作用于操作杆，作用于操作杆的还原后的力即为力反馈。
根据本发明的又一方面，提供一种计算机可读存储介质，所述存储介质中存储有多条指令，所述多条指令用于由处理器加载并执行如前所述方法。
根据本发明的再一方面，提供一种电子设备，所述电子设备，包括：
处理器，用于执行多条指令；
存储器，用于存储多条指令；
其中，所述多条指令用于由所述存储器存储，并由所述处理器加载并执行如前所述方法。
本发明具有以下技术效果：（1）能够实现精准力反馈，为医生操作提供了便利，避免了医生在进行血管介入手术时，身着铅衣导致手术时间长、质量差的问题，同时避免了医生暴露在术中CT射线下，能够保护医生的健康。（2）本发明通过非线性跟踪微分器对力信号的估计和处理，相较于传统低通滤波器有着相位滞后小，反应迅速，实时性强的优点，相较于卡尔曼滤波器有着无需参考模型，计算量少的优点。通过非线性跟踪微分器给出的导数信号，包括不可导函数也给出近似导数，方便其余控制器利用所述导数信号改善控制性能。（3）对电机转矩采用了自适应控制方法，从而能够避免因电机运行时产生的热量对电机系统参数的影响，也能够在一定程度上避免电机异常运行时产生的结构摄动，相较于传统PID控制，具有免于繁琐的调参的优点。此外，本发明还利用Lyapunov方法设计了稳定控制器，所述稳定控制器是一种大范围渐近稳定控制器，使电机系统拥有强抗扰性和鲁棒性。相较于优化控制，例如LQR控制，本发明的稳定性更加适合医疗行业这种高风险操作。（4）在力反馈控制方面，通过利用自抗扰技术中扩张状态观测器的思想估计出了反作用力的大小，具有较强的鲁棒性，较为平滑，能够较大程度的避免输出的波动。
下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。
附图说明
通过结合附图对本发明实施例进行更详细的描述，本发明的上述以及其他目的、特征和优势将变得更加明显。附图用来提供对本发明实施例的进一步理解，并且构成说明书的一部分，与本发明实施例一起用于解释本发明，并不构成对本发明的限制。在附图中，相同的参考标号通常代表相同部件或步骤。
图1是本发明一示例性实施例提供的方法的流程示意图。
图2A是本发明一示例性实施例提供的血管介入机器人系统主端控制部分机械结构示意图。
图2B是本发明一示例性实施例提供的血管介入机器人系统主端控制部分机械结构的左视图。
图2C是本发明一示例性实施例提供的血管介入机器人系统主端控制部分机械的俯视图。
图2D是本发明一示例性实施例提供的血管介入机器人系统主端控制部分机械的三维结构示意图。
图3是本发明一示例性实施例提供的滤波效果示意图。
图4是本发明一示例性实施例提供的电机控制原理示意图。
图5是本发明又一示例性实施例提供的方法的流程示意图。
图6是本发明一示例性实施例提供的装置的结构示意图。
图7是本发明一示例性实施例提供的电子设备的结构。
附图标记：
1，触摸按键；2，操作杆；3，第一滑块；4，联轴器；5，第二滑块；6，第一电机及编码器；7，滑轨；8，限位螺钉及接触开关；9，第一同步轮；10，同步带；11，第二同步轮；12，第二电机及编码器。
具体实施方式
下面，将参考附图详细地描述根据本发明的示例实施例。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是本发明的全部实施例，应理解，本发明不受这里描述的示例实施例的限制。
应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。
本领域技术人员可以理解，本发明实施例中的“第一”、“第二”、“S1”～“S5”等术语仅用于区别不同步骤、设备或模块等，既不代表任何特定技术含义，也不表示它们之间的必然逻辑顺序。例如S1、S2步骤不限顺序而且可以并行处理。
还应理解，在本发明实施例中，“多个”可以指两个或两个以上，“至少一个”可以指一个、两个或两个以上。
还应理解，对于本发明实施例中提及的任一部件、数据或结构，在没有明确限定或者在前后文给出相反启示的情况下，一般可以理解为一个或多个。
另外，本发明中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本发明中字符“/”，一般表示前后关联对象是一种“或”的关系。
还应理解，本发明对各个实施例的描述着重强调各个实施例之间的不同之处，其相同或相似之处可以相互参考，为了简洁，不再一一赘述。
同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。
以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。
对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，技术、方法和设备应当被视为说明书的一部分。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。
本发明实施例可以应用于终端设备、计算机系统、服务器等电子设备，其可与众多其它通用或专用计算系统环境或配置一起操作。适于与终端设备、计算机系统、服务器等电子设备一起使用的众所周知的终端设备、计算系统、环境和/或配置的例子包括但不限于：个人计算机系统、服务器计算机系统、瘦客户机、厚客户机、手持或膝上设备、基于微处理器的系统、机顶盒、可编程消费电子产品、网络个人电脑、小型计算机系统﹑大型计算机系统和包括上述任何系统的分布式云计算技术环境，等等。
终端设备、计算机系统、服务器等电子设备可以在由计算机系统执行的计算机系统可执行指令(诸如程序模块)的一般语境下描述。通常，程序模块可以包括例程、程序、目标程序、组件、逻辑、数据结构等等，它们执行特定的任务或者实现特定的抽象数据类型。计算机系统/服务器可以在分布式云计算环境中实施，分布式云计算环境中，任务是由通过通信网络链接的远程处理设备执行的。在分布式云计算环境中，程序模块可以位于包括存储设备的本地或远程计算系统存储介质上。
示例性方法
图1是本发明一示例性实施例提供的基于自适应力反馈的血管介入机器人系统主端控制方法的流程示意图。本发明血管介入机器人系统主端控制部分机械的结构如图2A-图2D所示。
本发明的基于自适应力反馈的血管介入机器人系统，分为主端设备和从端设备，所述主端设备由用户在远端进行操作，从而控制从端设备进行手术，所述主端设备包括触摸按键1、操作杆2、第一滑块3、联轴器4、第二滑块5、第一电机及编码器6、滑轨7、限位螺钉及接触开关8、第一同步轮9、同步带10、第二同步轮11和第二电机及编码器12、主控芯片及力传感器（未示出），所述第一同步轮9为从动轮，和第二同步轮11以及同步带10共同构成皮带传动装置，所述第二同步轮11为主动轮，由所述第二电机带动旋转；所述同步带10用于带动固定在同步带上的滑块运动，所述滑块包括第一滑块3和第二滑块5，所述第一滑块3用于将操作杆2的水平运动分量传递给同步带10，所述第二滑块用于将所述第一电机固定于同步带，并使所述第一电机与固定在所述同步带上的第一滑块之间保持水平方向的相对静止；所述滑轨7与所述同步带10相连接，并为所述滑块提供水平运动的轨道；限位螺钉及接触开关固定在滑块运动的边界位置，用于防止滑块脱离滑轨，以及矫正编码器，以获取所述滑块的绝对位置；所述操作杆2由用户操作，能够传递用户的动作（包括推拉，旋转等动作），所述操作杆2与所述第一电机的电机轴通过联轴器4进行固定，所述联轴器4带动操作杆2进行旋转运动，用户对操作杆2的旋转分量的运动能够传递到所述第一电机；所述第二电机及编码器12用于带动所述第二同步轮11的旋转，进而带动所述同步带10进行水平运动，再带动固定于所述同步带10上的第一滑块3和第二滑块5水平运动，从而带动操作杆2进行水平方向的运动。所述主端设备还包括触摸按键，用于判断用户是否接触操作杆2，若接触了所述操作杆2，则启动力反馈，即还原从端设备所受到的阻力；否则将所述第一滑块3与所述第二滑块5回中至滑轨7的中间位置。所述从端设备（未示出）为血管介入手术的对患者的执行机构，所述从端设备包括血管介入机器人，导管、导丝和球囊的推进装置，以及安装在所述推进装置之上的从端传感器，所述从端传感器可以为力传感器。
所述基于自适应力反馈的血管介入机器人系统的工作方式为：从端设备在执行血管介入时，所述血管介入机器人操作导管、导丝或球囊，在当前状态下，由于受到血管壁阻碍，血流粘性等因素的影响，在末端会感受到一定的阻力，从而被从端安装的力传感器所感知。从端传感器感知这个信号后转化为相应的信号经通信线缆传送给主端设备进行处理，主端通过执行力反馈程序对该阻力进行还原复现，从而使用户在操作杆端感受到从端导丝，导管或球囊所受的阻力。因此，获取所述从端设备的从端传感器感受到的阻力信号，通过通信线缆将所述阻力信号传输到所述主端设备的主控芯片当中。所述主控芯片通过控制所述第一电机及所述第二电机的电流值大小，调节所述第一电机和所述第二电机的输出力矩，输出力矩经同步轮-同步带系统带动滑轨-滑块系统，将所述输出力矩反映给用户，从而在主端设备中还原从端设备所受到的导丝，导管或球囊推进过程中受到的阻力，因此，能够在主端设备实时模拟真实手术中的“手感”，用户再对操作杆施加动作，所述主端设备的力传感器感知所述动作，生成传感信号；将所述传感信号作为运动指令发送给从端，对从端进行控制。
本实施例可应用在电子设备上，如图1所示，包括以下步骤：
步骤S1，构建电机模型，所述电机模型用于模拟主端设备中的电机，所述电机包括第一电机及第二电机；所述第一电机用于带动操作杆旋转，还原从端设备所受到的扭力；所述第二电机用于带动所述操作杆沿水平方向运动，还原从端设备所受到的水平方向的阻力。
本实施例中，所述第一电机用于带动操作杆旋转，还原从端设备所受到的扭力，也就是说，所述第一电机用于还原出所述从端设备的力传感器感知的当前导管、导丝或球囊所受的扭力。所述第二电机用于带动所述操作杆沿水平方向运动，还原从端设备所受到的水平方向的阻力，也就是说，所述第二电机用于还原所述从端设备的力传感器感知的当前导管、导丝或球囊所受的推进或回拉时所受的水平方向的阻力。
所述第一电机及所述第二电机是同型号电机，因此，所述电机模型适用于所述第一电机及所述第二电机。所述第一电机及所述第二电机的输入信号都是所述从端传感器的部分信号。两个电机的区别在于，所述第一电机负责控制操作杆旋转部分的力反馈，所述第二电机负责控制操作杆前进、后退的力反馈。因此，所述第一电机和所述第二电机，二者模型一致。为了能够实时模拟真实手术中的“手感”，需要构建电机转矩控制器，通过电机转矩控制器实现所述第一电机、第二电机的电机转矩的控制。本实施例中构建的电机模型为后续构造的电机转矩控制器提供动态系统模型。
本实施例中，所述电机模型为：
Figure 795309DEST_PATH_IMAGE001
，
Figure 6847DEST_PATH_IMAGE002
，
Figure 811380DEST_PATH_IMAGE003
，
其中，u、i、R、
Figure 108369DEST_PATH_IMAGE004
、L分别表示所述电机模型的端电压、线电流、绕组内阻、电机内部磁链和绕组电感，Ke、Kt分别是反电动势系数和转矩系数，Te表示电机的输出转矩，ω为电机的当前转速，t为当前时间，J为电机的转动惯量常数，Bz为电机的阻尼系数。
电压是控制器经驱动器驱动电机的输出信号，也是电机模型的输入信号，用来控制电机内部的变量。电阻，磁链和电感能够通过电机的说明文档获取相应的参数。电流能够通过驱动器内置的电流传感器获取，反电动势系数和转矩系数能够通过电机的说明书获取，ω、t、J、B的数值都能够从电机的说明文档获取。
设x1=i，x2=ω，将所述电机模型转化为状态空间方程：
Figure 856882DEST_PATH_IMAGE023
，
电机的输出转矩为
Figure 454085DEST_PATH_IMAGE024
。
步骤S2，构建滑轨模型，所述滑轨模型用于估算所述主端设备使用过程中的摩擦力。
所述主端设备使用过程中，所受到的摩擦力，例如包括：第一滑块，第二滑块与滑轨在相对运动之间产生的摩擦力，所述第一滑块、第二滑块分别部署于导轨上并固定于同步带上，用于带动操作杆、第一电机以及联轴器运动；操作杆旋转时产生的摩擦力等。所述滑轨模型用于抵消上述摩擦力，并在抵消摩擦力的过程中，将还原的所述从端设备所受到的当前导丝，导管和球囊水平方向的阻力和扭力作用于操作杆，进而通过操作杆将还原的所述从端设备所受到的当前导丝，导管和球囊水平方向的阻力和扭力传递给用户手上，令其有较为真实的操作手感。
本实施例中，所述滑轨模型为：
Figure 423660DEST_PATH_IMAGE005
，
Figure 411207DEST_PATH_IMAGE006
，
Figure 494570DEST_PATH_IMAGE007
，
Figure 149542DEST_PATH_IMAGE008
，
以及初始位置条件
Figure 558527DEST_PATH_IMAGE009
，
其中，Mtotal为第一滑块，第二滑块，第一电机，操作杆以及联轴器的质量之和，dw为第一同步轮和第二同步轮的直径，
Figure 833038DEST_PATH_IMAGE010
为水平方向总滑动摩擦系数，Fh为用户对操作杆施加水平方向力，p、v、a分别为滑块固定水平方向的位置、速度、加速度，所述滑块包括第一滑块和第二滑块；令输出
Figure 985670DEST_PATH_IMAGE011
，Fref为需要跟踪的力信号，所述需要跟踪的力信号为从端传感器传输的、经过过滤的力信号，sign(v)为速度v的符号，g为重力常数，f(t)为所述主端设备的机械结构运行时产生的扰动。本实施例中，假设电机轴与同步轮轴无相对滑动，忽略同步带弹性形变。设同步轮顺时针转动以及其带动皮带运动的方向为正方向。所述机械结构运行时产生的扰动包括同步带与同步轮之间的相对运动，第二电机运行时收到电磁干扰或电源中存在高次谐波而产生的抖动，无法预估的机械震动等难以建模的扰动。
本发明的目的是通过调节Te，令输出
Figure 291887DEST_PATH_IMAGE011
，同时通过把从端的力映射到主端设备，让用户能够感知从端传感器所感知的推拉力和扭力，以及通过获取当前位置和滑轨中点位置的差值，通过使用位置式PID控制，将第一滑块，第二滑块及其上固定的所有装置通过第二电机带动同步轮-传送带-滑轨系统将其位置运行到滑轨的中点位置。回中控制使用本领域通用的回中控制方法，因此回中控制并不进行详细阐述。
步骤S3，获取从端传感器信号，对所述从端传感器信号进行滤波，得到处理后的传感器信号。
所述从端传感器可以为力传感器，获得的信号为力传感信号，用于感知从端当前导丝，导管和球囊水平推拉力和扭力。
所述对所述从端传感器信号进行滤波，包括：
构建非线性跟踪微分器，将所述非线性跟踪微分器作为低通滤波器进行滤波，所述非线性跟踪微分器为：
Figure 340614DEST_PATH_IMAGE012
，
其中，获取的所述从端传感器信号为forg，fsun(z1、z2、r、h)满足
Figure 99492DEST_PATH_IMAGE013
，
其中，Fref(k+1)为k+1时刻所述从端传感器通过滤波算法获取的原信号，Fref(k)为k时刻所述从端传感器通过滤波算法获取的原信号，h为所述从端传感器实际的采样间隔时间，Dref(k)为k时刻所述从端传感器通过滤波算法获取的原信号的一阶导数，Dref(k+1)为k+1时刻所述从端传感器通过滤波算法获取的原信号的一阶导数，v(k)为运算过程中的中间变量，forg(k)为k时刻时所述从端传感器实际测量的带有噪声的原信号，r为滤波系数，fix(q)为向上取整函数，q、s为运算过程中的中间变量，sat(a,b)为饱和函数，表示当|a|<b时，sat(a,b)=a，其余情况sat(a,b)=sign(a)*b。
考虑到血管介入机器人系统主端接收到的力传感器的信号由于机械振动，血液流动阻力等，不可避免存在扰动信号。本发明采取利用自抗扰控制理论，并将其应用到所述血管介入机器人系统中。通过建立非线性跟踪微分器作为低通滤波器，能够实时地滤除扰动中的高频噪声及其导数信号，以便用户使用，例如便于医生观察，操作人员调试及控制器的设计和使用。
图3显示了采用所述非线性跟踪微分器进行滤波的滤波效果。
步骤S4，构建所述电机的参考模型，所述电机的参考模型是表征电机无误差运行的模型；基于所述参考模型，设置控制器对所述电机模型的参数进行实时校正。
如图4所示，所述电机的参考模型为永磁直流电机模型，能够通过产品说明文档获得相关参数，进而构建所述电机的参考模型。通过所述参考模型，便于针对电机的参数的改变的情况进行快速的参数矫正。
所述参考模型为：
Figure 810744DEST_PATH_IMAGE014
，
Figure 237046DEST_PATH_IMAGE015
，
其中，矩阵Am和B是参数矩阵形式的表述，
Figure 722254DEST_PATH_IMAGE016
为状态观测器观测得到的状态变量。
由于第一电机和第二电机是同样的电机，因此，能够用同样的控制器对所述第一电机和所述第二电机进行控制。但在所述第一电机和所述第二电机的使用过程中，部分参数可能存在差异，例如受到的摩擦力不同，因此，可以通过调整控制器的参数的方式，对所述第一电机和所述第二电机进行控制。例如，实例化两个控制器，这两个控制器分别用于控制所述第一电机和所述第二电机，这两个控制器的区别在于参数值不同。
所述基于所述参考模型，设置控制器对所述电机模型的参数进行实时校正，包括：
设置第一控制器，所述第一控制器为：
Figure 968427DEST_PATH_IMAGE017
，
其中，矩阵
Figure 731372DEST_PATH_IMAGE025
，
Figure 481022DEST_PATH_IMAGE026
，C为矩阵
Figure 402710DEST_PATH_IMAGE018
，
Figure 136180DEST_PATH_IMAGE027
指输入的参考信号，x(t)为电机模型的状态变量，e(t)表示输出误差，且e=r-y，
Figure 431376DEST_PATH_IMAGE020
为待调节参数。
基于所述第一控制器，对所述电机模型的参数进行实时校正，校正方式为：根据
Figure 301112DEST_PATH_IMAGE025
，
Figure 393702DEST_PATH_IMAGE026
，对磁链和绕组电阻进行实时校正。
本实施例中，由于医疗器械操作需要具有严格的稳定性和鲁棒性。因此基于Lyapunov第二法设计了控制器。
由于电机运行时线阻和磁链参数易受到温度变化的影响，本实施例采用了基于模型自适应的控制思路，能够在线地实时估计并进行补偿和控制。
将所述电机模型写为标准矩阵发展方程描述形式：
Figure 614468DEST_PATH_IMAGE028
，
其中根据微分方程可以得出：
Figure 515952DEST_PATH_IMAGE029
，
u(t)为输入，是所述电机模型的端电压。
其中，由于磁、线阻会随热量的变化而变化，对矩阵A(t)引起漂移和摄动，会导致最终输出偏离参考模型。这里针对原始模型建立基于参考模型的状态观测器，并实时对矩阵参数进行校正。
为了无差跟踪参考信号，设李雅普诺夫函数如下：
Figure 771353DEST_PATH_IMAGE030
，
其中r(t)指输入的参考信号，
Figure 34844DEST_PATH_IMAGE031
表示电机输出的力矩，e=r-y表示输出误差。
然后对李雅普诺夫函数求导可得：
Figure 739976DEST_PATH_IMAGE032
，
令控制率满足
Figure 114326DEST_PATH_IMAGE033
，
代入可得
Figure 224233DEST_PATH_IMAGE034
。
根据李雅普诺夫判据可以得出，对于参考模型只需简单调整参数
Figure 393046DEST_PATH_IMAGE020
即可以指数速率跟踪输出信号。且此控制器对系统是全局稳定的。
接下来对电机的设计参数进行自适应矫正。
由于温度变化不存在突变，阻值和磁链的改变相比采样控制周期来说几乎可以忽略，所以可以在每一个时刻将其不确定的数值近似看作常值扰动。
通过电机上自带的编码器和控制电机的驱动器上自带的电流传感器采集到的状态，并对采集到的信号进行滤波，可以提取其近似测量值和其导数信号。
通过将状态变量x(t)在同一个采样周期时刻内看作参数，并通过电机模型可以得出其当前近似参数数值为：
Figure 591334DEST_PATH_IMAGE035
，
Figure 300533DEST_PATH_IMAGE036
，
将
Figure 468209DEST_PATH_IMAGE037
，R(t)替代u(t)中的矩阵Am中的元素
Figure 276765DEST_PATH_IMAGE004
、R，解出的新的控制率u(t)即为所求的自适应控制率。
步骤S5，基于校正后的电机模型及所述滑轨模型，估算摩擦力，并补偿所述摩擦力；基于所述处理后的传感器信号，将还原的所述从端设备所受到的扭力、还原的所述从端设备所受到的水平方向的阻力作用于操作杆，作用于操作杆的还原后的力即为力反馈。
由于用户在推动或旋转操作杆运动时存在着摩擦力，由于摩擦力具有强非线性作用，对所述血管介入机器人系统的控制，特别是力反馈有着极其严重的影响。因此，针对摩擦力进行补偿，进而估算摩擦力。
所述基于校正后的电机模型及所述滑轨模型，估算摩擦力，其中，估算得到的摩擦力为
Figure 897102DEST_PATH_IMAGE021
。
构建第二控制器，所述第二控制器用于将还原的所述从端设备所受到的扭力、还原的所述从端设备所受到的水平方向的阻力作用于操作杆。所述第二控制器为：
Figure 344746DEST_PATH_IMAGE022
，
其中，k为当前时刻离散化的时间序列，r(k)为所述处理后的传感器信号，y(k)=ft(k)是滑轨的输出，也就是最终操作杆末端用户感受到的力，ki为待调节的参数，在此其下标i表示积分（integral），j为标准求和运算中的变量符号，N为设置的积分求和序列上限数值，r(k-j)为k-j时刻处理后的传感器信号，y(k-j)为k-j时刻滑轨的输出，kd为待调节的参数，Dref(k-1)为k-1时刻所述从端传感器通过滤波算法获取的原信号的一阶导数。
本实施例中，基于所述滑轨模型，设状态变量为
Figure 632508DEST_PATH_IMAGE038
，建立状态空间如下：
Figure 611965DEST_PATH_IMAGE039
，
其中，
Figure 985177DEST_PATH_IMAGE040
，uh(t)表示电机输出力矩，uh(t)也是滑轨模型的输入信号。F(t)表示操作者例如医生对操作杆施加的作用力，设电机的预期输出力矩为输入信号，通过控制系统的秩判据可以得出系统是精确可控的。由于摩擦力根据运动方向分段非线性，接下来根据不同时刻的状态来决定当前第二控制器，并进行分段线性化的控制，即静止时无需考虑力反馈控制。
考虑滑动时，通过编码器的信号可以判断当前运行速度的方向，由于用户的手和操作杆总是相对静止，其运动状态一致。则手的受力，即输出y(t)满足
Figure 973862DEST_PATH_IMAGE041
，则代入状态方程可以得到状态输出：
Figure 116130DEST_PATH_IMAGE042
。
基于所述处理后的传感器信号，将还原的所述从端设备所受到的扭力、还原的所述从端设备所受到的水平方向的阻力作用于操作杆，作用于操作杆的还原后的力即为力反馈，包括：
所述处理后的传感器信号记为r(t)，对原系统关于
Figure 800577DEST_PATH_IMAGE043
进行状态扩张：
Figure 395506DEST_PATH_IMAGE044
，
并构建扩张状态观测器：
Figure 187882DEST_PATH_IMAGE045
，
第二控制器为：
Figure 919078DEST_PATH_IMAGE046
，
其中，kp、ki、kd表示待调节的第二控制器的参数。y(k)=ft(k)是滑轨系统输出，也就是最终操作杆末端用户感受的力，z(t)为状态观测器状态变量，e1为观测估计状态和实际状态的误差。
本实施例中，通过第一电机和第二电机上集成的编码器和驱动电路上自带的电流传感器获取的参数，辨识并抵消滑动摩擦。采用自抗扰技术的思想，将用户手握操作杆输入到系统产生抖动的加速度分量和摩擦引起的加速度分量看作一个整体扰动，将其扩维并建立状态观测器。并通过状态观测器估计的扰动进行抵消，以及利用观测的状态进行状态反馈的极点配置。
并利用所述扩张状态观测器进行极点配置，并进行离散化，可以设计出所述第二控制器为
Figure 771496DEST_PATH_IMAGE022
，
其中Dref是滤波器获取参考信号的导数。
图5是本发明又一示例性实施例提供的基于自适应力反馈的血管介入机器人系统主端控制的方法的流程示意图。如图5所示，所述方法包括：
步骤S51，确定操作杆的触摸按键是否被接触，若是，进入步骤S52；若否，进入步骤S53；
步骤S52：判断固定在同步带上的第一滑块与第二滑块是否到达极限位置；若是，进入步骤S53；若否，则确定力反馈，进入步骤S54；
步骤S53：将所述第一滑块与所述第二滑块回中至滑轨的中间位置；
步骤S54：基于所述力反馈，向所述主端设备施加动作，所述主端设备的力传感器感知所述动作，生成传感信号；将所述传感信号作为运动指令发送给从端，对从端进行控制。
进一步地，所述步骤S51之前，包括步骤S50，初始化所述血管介入机器人系统，所述第二电机带动第二同步轮旋转，所述第二同步轮带动同步带移动，从而带动固定在同步带上的所述第一滑块和所述第二滑块进行水平运动直至触碰行程开关，触碰所述行程开关后，对增量式编码器进行校准，然后利用位置环反馈PID控制方法启动回中程序，使所述第一滑块和所述第二滑块达到滑轨的中间位置。
进一步地，所述步骤S53：将所述第一滑块与所述第二滑块回中，包括：利用位置环反馈PID控制方法启动回中程序，使所述第一滑块与所述第二滑块达到滑轨的中间位置。
进一步地，所述步骤S54之后，将所述第二控制器输出的数值，主端设备的传感器测量得到的数值，以及操作过程中产生的错误代码等信息通过日志系统记录到存储设备当中。
示例性装置
图6是本发明一示例性实施例提供的基于自适应力反馈的血管介入机器人系统主端控制的装置的结构示意图。如图6所示，本实施例包括：
电机模型构建模块，配置为构建电机模型，所述电机模型用于模拟主端设备中的电机，所述电机包括第一电机及第二电机；所述第一电机用于带动操作杆旋转，还原从端设备所受到的扭力；所述第二电机用于带动所述操作杆沿水平方向运动，还原从端设备所受到的水平方向的阻力；
滑轨模块构建模块，配置为构建滑轨模型，所述滑轨模型用于估算所述主端设备使用过程中的摩擦力；
滤波模块，配置为获取从端传感器信号，对所述从端传感器信号进行滤波，得到处理后的传感器信号；
校正模块，配置为构建所述电机的参考模型，所述电机的参考模型是表征电机无误差运行的模型；基于所述参考模型，设置控制器对所述电机模型的参数进行实时校正；
力反馈模块，配置为基于校正后的电机模型及所述滑轨模型，估算摩擦力，并补偿所述摩擦力；基于所述处理后的传感器信号，将还原的所述从端设备所受到的扭力、还原的所述从端设备所受到的水平方向的阻力作用于操作杆，作用于操作杆的还原后的力即为力反馈。
示例性电子设备
图7是本发明一示例性实施例提供的电子设备的结构。该电子设备可以是第一设备和第二设备中的任一个或两者、或与它们独立的单机设备，该单机设备可以与第一设备和第二设备进行通信，以从它们接收所采集到的输入信号。图7图示了根据本公开实施例的电子设备的框图。如图7所示，电子设备包括一个或多个处理器71和存储器72。
处理器71可以是中央处理单元（CPU）或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制电子设备中的其他组件以执行期望的功能。
存储器72可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器（RAM）和/或高速缓冲存储器（cache）等。所述非易失性存储器例如可以包括只读存储器（ROM）、硬盘、闪存等。在所述计算机可读存储介质上可以存储一个或多个计算机程序指令，处理器71可以运行所述程序指令，以实现上文所述的本公开的各个实施例的软件程序的基于自适应力反馈的血管介入机器人系统主端控制的方法以及/或者其他期望的功能。在一个示例中，电子设备还可以包括：输入装置73和输出装置74，这些组件通过总线系统和/或其他形式的连接机构（未示出）互连。
此外，该输入装置73还可以包括例如键盘、鼠标等等。
该输出装置74可以向外部输出各种信息。该输出设备74可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。
当然，为了简化，图7中仅示出了该电子设备中与本公开有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，电子设备还可以包括任何其他适当的组件。
示例性计算机程序产品和计算机可读存储介质
除了上述方法和设备以外，本公开的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本公开各种实施例的基于自适应力反馈的血管介入机器人系统主端控制的方法中的步骤。
所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本公开实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。
此外，本公开的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本公开各种实施例的基于自适应力反馈的血管介入机器人系统主端控制的方法中的步骤。
所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子（非穷举的列表）包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器（RAM）、只读存储器（ROM）、可擦式可编程只读存储器（EPROM或闪存）、光纤、便携式紧凑盘只读存储器（CD-ROM）、光存储器件、磁存储器件、或者上述的任意合适的组合。
以上结合具体实施例描述了本公开的基本原理，但是，需要指出的是，在本公开中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本公开的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本公开为必须采用上述具体的细节来实现。
本说明书中各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似的部分相互参见即可。对于系统实施例而言，由于其与方法实施例基本对应，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
本公开中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。
可能以许多方式来实现本公开的方法和装置。例如，可通过软件、硬件、固件或者软件、硬件、固件的任何组合来实现本公开的方法和装置。用于所述方法的步骤的上述顺序仅是为了进行说明，本公开的方法的步骤不限于以上具体描述的顺序，除非以其它方式特别说明。此外，在一些实施例中，还可将本公开实施为记录在记录介质中的程序，这些程序包括用于实现根据本公开的方法的机器可读指令。因而，本公开还覆盖存储用于执行根据本公开的方法的程序的记录介质。
还需要指出的是，在本公开的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本公开的等效方案。提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本公开。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本公开的范围。因此，本公开不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。
为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本公开的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。

Claims (7)

1.一种基于自适应力反馈的血管介入机器人系统主端控制方法，其特征在于，包括：
构建电机模型，所述电机模型用于模拟主端设备中的电机，所述电机包括第一电机及第二电机；所述第一电机用于带动操作杆旋转，还原从端设备所受到的扭力；所述第二电机用于带动所述操作杆沿水平方向运动，还原从端设备所受到的水平方向的阻力；
构建滑轨模型，所述滑轨模型用于估算所述主端设备使用过程中的摩擦力；
获取从端传感器信号，对所述从端传感器信号进行滤波，得到处理后的传感器信号；
构建所述电机的参考模型，所述电机的参考模型是表征电机无误差运行的模型；基于所述参考模型，设置控制器对所述电机模型的参数进行实时校正；
基于校正后的电机模型及所述滑轨模型，估算摩擦力，并补偿所述摩擦力；基于所述处理后的传感器信号，将还原的所述从端设备所受到的扭力、还原的所述从端设备所受到的水平方向的阻力作用于操作杆，作用于操作杆的还原后的力即为力反馈，
所述滑轨模型为：
Figure 743923DEST_PATH_IMAGE001
，
Figure 478706DEST_PATH_IMAGE002
，
Figure 586339DEST_PATH_IMAGE003
，
Figure 309445DEST_PATH_IMAGE004
，
以及初始位置条件
Figure 953178DEST_PATH_IMAGE005
，
其中，Mtotal为第一滑块、第二滑块、第一电机、操作杆以及联轴器质量之和，dw为同步轮直径，所述同步轮包括第一同步轮及第二同步轮，所述第二电机带动所述第二同步轮旋转，所述第二同步轮、第一同步轮以及同步带共同构成皮带传动装置，所述同步带用于带动固定在同步带上的滑块运动，所述滑块包括第一滑块和第二滑块，所述第二滑块用于将所述第一电机固定于同步带，并使所述第一电机与固定在所述同步带上的第一滑块之间保持水平方向的相对静止；所述操作杆与所述第一电机的电机轴通过联轴器进行固定，所述联轴器带动操作杆进行旋转运动；
Figure 837957DEST_PATH_IMAGE006
为水平方向总滑动摩擦系数，Fh为用户对操作杆施加水平方向力，p、v、a分别为滑块固定水平方向的位置、速度、加速度；设
Figure 432887DEST_PATH_IMAGE007
，Fref为需要跟踪的力信号，所述需要跟踪的力信号为从端传感器传输的、经过过滤的力信号，sign(*)为符号函数，表示当*大于0时为1，*小于0时为-1，*等于0时为0；g为重力常数，f(t)为所述主端设备运行时机械结构产生的扰动，p0为滑轨初始位置，
所述对所述从端传感器信号进行滤波，包括：构建非线性跟踪微分器，将所述非线性跟踪微分器作为低通滤波器进行滤波，
所述参考模型为：
Figure 726727DEST_PATH_IMAGE008
，
Figure 723502DEST_PATH_IMAGE009
，
其中，矩阵Am和B是参数矩阵形式的表述，
Figure 44762DEST_PATH_IMAGE010
为参考模型的状态变量，
所述基于所述参考模型，设置控制器对所述电机模型的参数进行实时校正，包括：
设置第一控制器，所述第一控制器为：
Figure 628452DEST_PATH_IMAGE011
，
其中，C为矩阵
Figure 958939DEST_PATH_IMAGE012
，
Figure 810221DEST_PATH_IMAGE013
指输入的参考信号，x(t)为电机模型的状态变量，e(t)表示输出误差，
Figure 69426DEST_PATH_IMAGE014
为待调节参数；
基于所述第一控制器，对所述电机模型的磁链和绕组电阻进行实时校正，校正方式为：根据
Figure 638948DEST_PATH_IMAGE015
，
Figure 507546DEST_PATH_IMAGE016
，对磁链和绕组电阻进行实时校正，
其中，u、i、R、
Figure 980378DEST_PATH_IMAGE017
、L分别表示所述电机模型的端电压、线电流、绕组内阻、电机内部磁链和绕组电感，Ke、Kt分别是反电动势系数和转矩系数，Te表示电机的输出转矩，ω为电机的当前转速，t为当前时间，J为电机的转动惯量常数，Bz为电机的阻尼系数，
构建第二控制器，所述第二控制器用于将还原的所述从端设备所受到的扭力、还原的所述从端设备所受到的水平方向的阻力作用于操作杆，所述第二控制器为：
Figure 643441DEST_PATH_IMAGE018
，
其中，k为当前时刻离散化的时间序列，r(k)为所述处理后的传感器信号，y(k)=ft(k)是滑轨的输出，也就是操作杆末端用户感受到的力，ki为待调节的参数，j为标准求和运算中的变量符号，N为设置的积分求和序列上限数值，r(k-j)为k-j时刻处理后的传感器信号，y(k-j)为k-j时刻滑轨的输出，kd为待调节的参数。
2.如权利要求1所述的方法，其特征在于，所述电机模型为：
Figure 965838DEST_PATH_IMAGE019
，
Figure 874013DEST_PATH_IMAGE020
，
Figure 699887DEST_PATH_IMAGE021
。
3.如权利要求1所述的方法，其特征在于，
所述非线性跟踪微分器为：
Figure 533851DEST_PATH_IMAGE022
，
其中，获取的所述从端传感器信号为forg，fsun(z1,z2,r,h)满足
Figure 579429DEST_PATH_IMAGE023
其中，Fref(k+1)为k+1时刻所述从端传感器通过滤波算法获取的原信号，Fref(k)为k时刻所述从端传感器通过滤波算法获取的原信号，h为所述从端传感器实际的采样间隔时间，Dref(k)为k时刻所述从端传感器通过滤波算法获取的原信号的一阶导数，Dref(k+1)为k+1时刻所述从端传感器通过滤波算法获取的原信号的一阶导数，v(k)为运算过程中的中间变量，forg(k)为k时刻时所述从端传感器实际测量的带有噪声的原信号，r为滤波系数，fix(q)为向上取整函数，q、s 为运算过程中的中间变量，sat(a,b)为饱和函数，表示当|a|<b时，sat(a,b)=a，其余情况sat(a,b)=sign(a)*b。
4.如权利要求1所述的方法，其特征在于，
所述基于校正后的电机模型及所述滑轨模型，估算摩擦力，其中，估算得到的摩擦力为
Figure 586568DEST_PATH_IMAGE024
。
5.一种基于自适应力反馈的血管介入机器人系统主端控制装置，其特征在于，包括：
电机模型构建模块，配置为构建电机模型，所述电机模型用于模拟主端设备中的电机，所述电机包括第一电机及第二电机；所述第一电机用于带动操作杆旋转，还原从端设备所受到的扭力；所述第二电机用于带动所述操作杆沿水平方向运动，还原从端设备所受到的水平方向的阻力；
滑轨模块构建模块，配置为构建滑轨模型，所述滑轨模型用于估算所述主端设备使用过程中的摩擦力；
滤波模块，配置为获取从端传感器信号，对所述从端传感器信号进行滤波，得到处理后的传感器信号；
校正模块，配置为构建所述电机的参考模型，所述电机的参考模型是表征电机无误差运行的模型；基于所述参考模型，设置控制器对所述电机模型的参数进行实时校正；
力反馈模块，配置为基于校正后的电机模型及所述滑轨模型，估算摩擦力，并补偿所述摩擦力；基于所述处理后的传感器信号，将还原的所述从端设备所受到的扭力、还原的所述从端设备所受到的水平方向的阻力作用于操作杆，作用于操作杆的还原后的力即为力反馈，
所述滑轨模型为：
Figure 532528DEST_PATH_IMAGE001
，
Figure 316156DEST_PATH_IMAGE002
，
Figure 81986DEST_PATH_IMAGE003
，
Figure 830499DEST_PATH_IMAGE004
，
以及初始位置条件
Figure 663588DEST_PATH_IMAGE005
，
其中，Mtotal为第一滑块、第二滑块、第一电机、操作杆以及联轴器质量之和，dw为同步轮直径，所述同步轮包括第一同步轮及第二同步轮，所述第二电机带动所述第二同步轮旋转，所述第二同步轮、第一同步轮以及同步带共同构成皮带传动装置，所述同步带用于带动固定在同步带上的滑块运动，所述滑块包括第一滑块和第二滑块，所述第二滑块用于将所述第一电机固定于同步带，并使所述第一电机与固定在所述同步带上的第一滑块之间保持水平方向的相对静止；所述操作杆与所述第一电机的电机轴通过联轴器进行固定，所述联轴器带动操作杆进行旋转运动；
Figure 104934DEST_PATH_IMAGE006
为水平方向总滑动摩擦系数，Fh为用户对操作杆施加水平方向力，p、v、a分别为滑块固定水平方向的位置、速度、加速度；设
Figure 358061DEST_PATH_IMAGE007
，Fref为需要跟踪的力信号，所述需要跟踪的力信号为从端传感器传输的、经过过滤的力信号，sign(*)为符号函数，表示当*大于0时为1，*小于0时为-1，*等于0时为0；g为重力常数，f(t)为所述主端设备运行时机械结构产生的扰动，p0为滑轨初始位置，
所述对所述从端传感器信号进行滤波，包括：构建非线性跟踪微分器，将所述非线性跟踪微分器作为低通滤波器进行滤波，
所述参考模型为：
Figure 411730DEST_PATH_IMAGE008
，
Figure 801123DEST_PATH_IMAGE009
，
其中，矩阵Am和B是参数矩阵形式的表述，
Figure 413370DEST_PATH_IMAGE010
为参考模型的状态变量，
所述基于所述参考模型，设置控制器对所述电机模型的参数进行实时校正，包括：
设置第一控制器，所述第一控制器为：
Figure 655258DEST_PATH_IMAGE011
，
其中，C为矩阵
Figure 11153DEST_PATH_IMAGE012
，
Figure 255052DEST_PATH_IMAGE013
指输入的参考信号，x(t)为电机模型的状态变量，e(t)表示输出误差，
Figure 274086DEST_PATH_IMAGE014
为待调节参数；
基于所述第一控制器，对所述电机模型的磁链和绕组电阻进行实时校正，校正方式为：根据
Figure 767384DEST_PATH_IMAGE015
，
Figure 395812DEST_PATH_IMAGE016
，对磁链和绕组电阻进行实时校正，
其中，u、i、R、
Figure 792420DEST_PATH_IMAGE017
、L分别表示所述电机模型的端电压、线电流、绕组内阻、电机内部磁链和绕组电感，Ke、Kt分别是反电动势系数和转矩系数，Te表示电机的输出转矩，ω为电机的当前转速，t为当前时间，J为电机的转动惯量常数，Bz为电机的阻尼系数，
构建第二控制器，所述第二控制器用于将还原的所述从端设备所受到的扭力、还原的所述从端设备所受到的水平方向的阻力作用于操作杆，所述第二控制器为：
Figure 480890DEST_PATH_IMAGE018
，
其中，k为当前时刻离散化的时间序列，r(k)为所述处理后的传感器信号，y(k)=ft(k)是滑轨的输出，也就是操作杆末端用户感受到的力，ki为待调节的参数，j为标准求和运算中的变量符号，N为设置的积分求和序列上限数值，r(k-j)为k-j时刻处理后的传感器信号，y(k-j)为k-j时刻滑轨的输出，kd为待调节的参数。
6.一种计算机可读存储介质，所述存储介质中存储有多条指令，所述多条指令用于由处理器加载并执行如权利要求1-4中任一项所述方法。
7.一种电子设备，其特征在于，所述电子设备，包括：
处理器，用于执行多条指令；
存储器，用于存储多条指令；
其中，所述多条指令用于由所述存储器存储，并由所述处理器加载并执行如权利要求1-4中任一项所述方法。
CN202210361274.8A 2022-04-07 2022-04-07 基于自适应力反馈的血管介入机器人系统主端控制方法 Active CN114442490B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210361274.8A CN114442490B (zh) 2022-04-07 2022-04-07 基于自适应力反馈的血管介入机器人系统主端控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210361274.8A CN114442490B (zh) 2022-04-07 2022-04-07 基于自适应力反馈的血管介入机器人系统主端控制方法

Publications (2)

Publication Number Publication Date
CN114442490A CN114442490A (zh) 2022-05-06
CN114442490B true CN114442490B (zh) 2022-06-24

Family

ID=81360473

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210361274.8A Active CN114442490B (zh) 2022-04-07 2022-04-07 基于自适应力反馈的血管介入机器人系统主端控制方法

Country Status (1)

Country Link
CN (1) CN114442490B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115363768B (zh) * 2022-10-27 2023-04-14 北京唯迈医疗设备有限公司 一种介入手术机器人系统及其控制方法和介质
CN116277034B (zh) * 2023-05-15 2023-09-12 北京壹点灵动科技有限公司 应对负载变化的机器人控制方法、装置及电子设备
CN116999178B (zh) * 2023-10-07 2024-01-12 北京科鹏医疗器械有限公司 一种经自然通道内窥镜操作的双频滤波直观主从映射方法

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112017516A (zh) * 2020-08-26 2020-12-01 北京理工大学 一种远程血管介入手术训练系统

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2005018882A1 (ja) * 2003-08-21 2005-03-03 Tmsuk Co., Ltd. ロボット装置およびロボット遠隔操作システム
CN102085663B (zh) * 2010-12-22 2012-11-14 中国科学院深圳先进技术研究院 遥操作系统及方法
CN102825603B (zh) * 2012-09-10 2015-01-21 江苏科技大学 网络遥操作机器人系统及时延克服方法
KR102306959B1 (ko) * 2013-09-04 2021-10-01 삼성전자주식회사 수술 로봇 및 수술 로봇 제어 방법
US10932874B2 (en) * 2015-08-25 2021-03-02 Kawasaki Jukogyo Kabushiki Kaisha Remote control robot system
CN107184274B (zh) * 2017-06-05 2020-03-31 上海速介机器人科技有限公司 具有手感的血管介入手术机器人操作手柄及其控制方法
CN107307909B (zh) * 2017-07-06 2018-11-27 北京理工大学 一种介入机器人远程操作系统
CN108665759A (zh) * 2018-05-07 2018-10-16 上海斐旭电子设备有限公司 一种血管介入手术模拟训练系统实现过程
CN109968361B (zh) * 2019-05-17 2020-12-18 北京理工大学 一种基于实时力反馈的变阻抗遥操作控制装置及方法
CN111973279B (zh) * 2019-05-21 2021-07-16 复旦大学 一种血管介入手术机器人的主从位置自适应跟踪控制方法
CN111110353B (zh) * 2020-01-20 2024-04-19 深圳爱博合创医疗机器人有限公司 一种介入手术机器人的主端控制器

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112017516A (zh) * 2020-08-26 2020-12-01 北京理工大学 一种远程血管介入手术训练系统

Also Published As

Publication number Publication date
CN114442490A (zh) 2022-05-06

Similar Documents

Publication Publication Date Title
CN114442490B (zh) 基于自适应力反馈的血管介入机器人系统主端控制方法
US11148282B2 (en) Control device for motor drive device, control device for multi-axial motor, and control method for motor drive device
US11950970B2 (en) Medical support arm system, medical support arm control method, and medical support arm control device
L’Orsa et al. Introduction to haptics for neurosurgeons
Lund et al. High-bandwidth active impedance control of the proprioceptive actuator design in dynamic compliant robotics
CN113189865B (zh) 基于动力学参数辨识的康复机器人控制系统
JP2005316937A (ja) 制御装置およびその制御方法
US12114942B2 (en) Information processing apparatus, information processing system, and information processing method
CN117921684B (zh) 机械臂的控制方法、装置、存储介质及电子设备
Aksman et al. Force estimation based compliance control of harmonically driven manipulators
CN113664869A (zh) 机械臂、机械臂控制方法及系统
JP6237039B2 (ja) ロボット制御装置およびロボット制御方法
JP6107282B2 (ja) 処理装置、ロボット、プログラム及びロボットの制御方法
JP6502634B2 (ja) 制御装置
CN115564877A (zh) 虚拟人的驱动方法、装置、存储介质和电子设备
WO2022145213A1 (ja) 制御装置、制御システム及び制御方法
CN117116459B (zh) 康复机器人主动康复训练控制方法及装置
WO2022145150A1 (ja) 制御システム及び制御方法
Pitakwatchara Task space impedance control of the manipulator driven through the multistage nonlinear flexible transmission
WO2022004568A1 (ja) 制御システム、制御方法及びプログラム
US10452141B2 (en) Method, system and apparatus to condition actions related to an operator controllable device
Arocena et al. Design and control of a novel 3-dof flexible robot, part 2: Modeling and control
Hace et al. Towards Sensor-Less Haptic Teleoperation
CN117901086A (zh) 增强现实机器人的无线遥操作方法及系统
CN118809575A (zh) 力估计方法、装置、电子设备及可读存储介质

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
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20221008

Address after: 311399 Room 701, Building B, Tianmu Pharmaceutical Port Pharmaceutical Industry Incubation Park, No. 88, Jiuzhou Street, Jinnan Street, Lin'an District, Hangzhou, Zhejiang

Patentee after: Hangzhou Langbokang Medical Technology Co.,Ltd.

Address before: 311100 508, 5th floor, building 4, No. 5, Lvtai Road, Zhongtai street, Yuhang District, Hangzhou City, Zhejiang Province

Patentee before: Hangzhou lancet robot Co.,Ltd.