CN103536367A - 一种主从微创手术机器人系统及控制方法 - Google Patents

Abstract

一种主从微创手术机器人系统及控制方法，本发明涉及机器人控制领域。过程为：用户在主从微创手术机器人系统的主手端设置导管移动位置期望值；位姿传感器测得导管实际的运动关节量，利用变论域模糊PID控制方法对导管位置进行调整；成像装置接收检测到的图像信息并成像；通过成像装置呈现的图像判断是否到达期望位置。本发明在常规PID控制的基础上增强了位置跟踪性能及鲁棒性，基于误差分级选择伸缩因子，解决了模糊PID控制精度与控制规则数目之间的矛盾，避免了每个采样周期计算一次伸缩因子，节省了时间，对系统硬件要求降低，在确保微创手术安全性的同时，增强了主从微创手术机器人系统在微创手术中的有效性和实用性。

Description

一种主从微创手术机器人系统及控制方法

技术领域

本发明涉及机器人控制领域，特别涉及一种主从微创手术机器人系统及控制方法。

背景技术

随着科学技术的发展，微创手术在心脑血管疾病内的应用越来越广泛。传统血管介入微创手术中，导管被插入到股动脉中，在血管中运动直到目标点，通过胶囊或支架，实现心脑血管中的血栓溶解，血管扩充以及堵塞畸形血管等功能。由于介入创口微乎其微，损伤程度比传统手术远远要轻，减轻病人痛苦，术后恢复更快。但是也存在着许多缺点：导管介入完全依赖于医生的专业性和熟练度；导管末端与血管壁碰撞的摩擦力医生不能准确估计，而过大的力导致血管穿孔，大量出血；手术时间长，而且医生要暴露在射线下忍受仪器的强烈辐射，对医生身体带来伤害。

针对上述问题，国内外进行了大量研究，并提出主从微创手术机器人系统应用于微创手术中。但多数主从系统中，对驱动器的控制方式较为单一，大多采用常规PID控制，而实际导管驱动系统中存在许多非线性因素，如系统的摩擦力、血流的阻力、死区、时滞效应、导管自身参数及其运动环境条件不断变化等，致使系统的动态特性不够理想，一定程度上制约了主从控制的操作性能。模糊PID控制，能够对PID参数进行在线调整，提高PID的跟踪性能，增强介入手术的安全性。然而保证控制系统的精度往往是通过增加语言变量和控制规则的数目，制定足够多的控制规则既不现实又不便于在微创手术中的应用；随着控制过程的进行，误差逐渐变小，向隶属度函数的零位移动，此时使用原有的误差隶属度函数致使精度不足。

发明内容

针对现有技术存在的不足，本发明的目的是提供一种用于主从微创手术机器人系统装置的控制方法，以达到提高主从微创手术安全性的目的。

本发明的技术方案是这样实现的：一种主从微创手术机器人系统，包括：

主手：用于实现医生对导管位置的控制。主手接收医生给出的导管移动位置的期望值，并且通过映射将主手手柄移动的位置信息转换为导管运动关节量；

导管：用于主从微创手术中向病灶处输送胶囊或支架；

检测装置：用于检测导管末端和目标点的位姿；

上位机：用于接收主手和检测装置传送的位置信号，将信号进行处理，并将结果传送给伺服驱动装置；

伺服驱动装置：用于接收上位机传送的位置信号，驱使导管对医生给出的导管移动位置的期望值进行跟踪；

成像装置：用于图像处理和人机交互，将导管和血管姿态呈现，使医生更加直观地了解手术进行情况；

在所述上位机中还进一步包括：

位置运动转换器：用于将上位机接收到的来自主手和检测装置的位置信息转换为导管期望关节量和导管实际关节量；

变论域模糊控制器：用于将导管期望关节量和导管实际关节量之间的误差及误差变化率进行模糊处理，经模糊化，带有变论域过程的模糊推理，去模糊化过程，将对导管关节量误差和误差变化率的控制作用转换为PID控制的比例控制、积分控制、微分控制作用，并实现PID控制的比例参数、积分参数、微分参数在线调整；

PID控制器：用于根据接收到的变论域模糊控制器的输出信号完成导管关节量的跟踪。

一种对主从微创手术机器人系统的导管进行控制的方法，包括以下步骤：

步骤1：首先，用户在主从微创手术机器人系统的主手端设置导管移动位置的期望位置；其次用户操作主手手柄控制导管向期望位置移动，在主手手柄的初始位置建立坐标系，把主手手柄最终移动到的位置信息映射到导管所处任务空间，通过映射得到的坐标，利用雅克比逆矩阵最终确定导管运动关节量，确定导管运动关节量，包括导管的进退量、旋转量和弯曲量；

步骤2：位姿传感器测得导管实际对应的运动关节量，对导管位置进行调整；

步骤2-1：期望关节量与实际关节量比较，计算二者的误差e，将误差e和误差变化率ec作为输入送入变论域模糊控制器中，确定变论域模糊控制器输入和输出的模糊子集，具体为：

在变论域模糊控制器内，首先定义各个关节量误差的语言变量E和误差变化率的语言变量EC；然后，用户自定义误差的论域e和误差变化率ec的论域，并在误差的论域范围内将误差的语言变量离散化，在误差变化率的论域范围内将误差变化率的语言变量离散化，确定误差对应的模糊子集和误差变化率对应的模糊子集，表达式均为：

{NB,NM,NS,O,PS,PM,PB}

式中，NB代表负大，NM代表负中，NS代表负小，O代表零，PS代表正小，PM代表正中，PB代表正大；

设定变论域模糊控制器输出的PID参数的三个变化量Δk_p,Δk_i,Δk_d的模糊子集表达式也均为：

{NB,NM,NS,O,PS,PM,PB}

步骤2-2：确定PID参数的三个变化量Δk_p,Δk_i,Δk_d各自的隶属度函数、误差的隶属度和误差变化率的隶属度函数；

步骤2-3：选取伸缩因子，分别为误差对应的伸缩因子、误差变化率对应的伸缩因子、比例因数变化量对应的伸缩因子、积分因数变化量对应的伸缩因子、微分因数变化量对应的伸缩因子；选取伸缩因子的过程为：

首先将误差e划分为四个区域，即误差区域1～4，其中：

误差区域1定义为：误差隶属度函数中NB和PB最高的部分对应的横坐标区间，该区域表示误差较大；

误差区域2定义为：误差隶属度函数中NM和PM最高的部分对应的横坐标区间，该区域表示误差适中；

误差区域3定义为：误差隶属度函数中NS和PS最高的部分对应的横坐标区间，该区域表示误差较小；

误差区域4定义为：误差隶属度函数中O最高的部分对应的横坐标区间，该区域表示误差很小；

设置误差区域1～4的伸缩因子的取值范围：

误差区域1内的伸缩因子设置如下：误差对应的伸缩因子取值范围为区间(0.7,1]；误差变化率对应的伸缩因子取值范围为区间(0.7,1]；比例因数变化量对应的伸缩因子取值范围为区间(0.7,1]；积分因数变化量对应的伸缩因子取值范围为区间(0,0.2]；微分因数变化量对应的伸缩因子取值范围为区间(0,0.2]；

误差区域2内的伸缩因子设置如下：误差对应的伸缩因子取值范围为区间(0.4,0.7]；误差变化率对应的伸缩因子取值范围为区间(0.4,0.7]；比例因数变化量对应的伸缩因子取值范围为区间(0.4,0.7]；积分因数变化量对应的伸缩因子取值范围为区间(0.2,0.4]；微分因数变化量对应的伸缩因子取值范围为区间(0.2,0.4]；

误差区域3内的伸缩因子设置如下：误差对应的伸缩因子的取值范围为区间(0.2,0.4]；误差变化率对应的伸缩因子的取值范围为区间(0.2,0.4]；比例因数变化量对应的伸缩因子的取值范围为区间(0.2,0.4]；积分因数变化量对应的伸缩因子的取值范围为区间(0.4,0.7]；微分因数变化量对应的伸缩因子的取值范围为区间(0.4,0.7]；

误差区域4内的伸缩因子设置如下：误差对应的伸缩因子的取值范围为区间(0,0.2]；误差变化率对应的伸缩因子的取值范围为区间(0,0.2]；比例因数变化量对应的伸缩因子的取值范围为区间(0,0.2]；积分因数变化量对应的伸缩因子的取值范围为区间(0.7,1]；微分因数变化量对应的伸缩因子的取值范围为区间(0.4,0.7]；

伸缩因子的确定过程如下：

根据首个采样时刻的误差值，判断该误差值属于哪个误差区域，伸缩因子取值即为该误差区域对应的伸缩因子；再接收下一个采样时刻的误差值，若下一个采样时刻的误差值与上一个采样时刻的误差值位于同一个误差区域内，则伸缩因子不变，否则，修改令伸缩因子为此时误差所在的误差区域所对应的伸缩因子，重复该过程，直至采样过程结束；

步骤2-4：确定变论域模糊控制器各输入输出参数的模糊规则表；

所述的变论域模糊控制器内的模糊控制规则如下：

当e的模糊子集为NB，ec的模糊子集为NB时，k_p的模糊子集为PB，k_i的模糊子集为NB，k_d的模糊子集为PS；

当e的模糊子集为NB，ec的模糊子集为NM时，k_p的模糊子集为PB，k_i的模糊子集为NB，k_d的模糊子集为NS；

当e的模糊子集为NB，ec的模糊子集为NS时，k_p的模糊子集为PM，k_i的模糊子集为NM，k_d的模糊子集为NB；

当e的模糊子集为NB，ec的模糊子集为O时，k_p的模糊子集为PM，k_i的模糊子集为NM，k_d的模糊子集为NB；

当e的模糊子集为NB，ec的模糊子集为PS时，k_p的模糊子集为PS，k_i的模糊子集为NS，k_d的模糊子集为NM；

当e的模糊子集为NB，ec的模糊子集为PM时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为NM；

当e的模糊子集为NB，ec的模糊子集为PB时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为PS；

当e的模糊子集为NM，ec的模糊子集为NB时，k_p的模糊子集为PB，k_i的模糊子集为NB，k_d的模糊子集为PS；

当e的模糊子集为NM，ec的模糊子集为NM时，k_p的模糊子集为PB，k_i的模糊子集为NB，k_d的模糊子集为NS；

当e的模糊子集为NM，ec的模糊子集为NS时，k_p的模糊子集为PM，k_i的模糊子集为NM，k_d的模糊子集为NB；

当e的模糊子集为NM，ec的模糊子集为O时，k_p的模糊子集为PS，k_i的模糊子集为NS，k_d的模糊子集为NM；

当e的模糊子集为NM，ec的模糊子集为PS时，k_p的模糊子集为PS，k_i的模糊子集为NS，k_d的模糊子集为NS；

当e的模糊子集为NM，ec的模糊子集为PM时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为NS；

当e的模糊子集为NM，ec的模糊子集为PB时，k_p的模糊子集为NS，k_i的模糊子集为O，k_d的模糊子集为O；

当e的模糊子集为NS，ec的模糊子集为NB时，k_p的模糊子集为PM，k_i的模糊子集为NB，k_d的模糊子集为O；

当e的模糊子集为NS，ec的模糊子集为NM时，k_p的模糊子集为PM，k_i的模糊子集为NM，k_d的模糊子集为NS；

当e的模糊子集为NS，ec的模糊子集为NS时，k_p的模糊子集为PM，k_i的模糊子集为NS，k_d的模糊子集为NM；

当e的模糊子集为NS，ec的模糊子集为O时，k_p的模糊子集为PS，k_i的模糊子集为NS，k_d的模糊子集为NM；

当e的模糊子集为NS，ec的模糊子集为PS时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为NS；

当e的模糊子集为NS，ec的模糊子集为PM时，k_p的模糊子集为NS，k_i的模糊子集为PS，k_d的模糊子集为NS；

当e的模糊子集为NS，ec的模糊子集为PB时，k_p的模糊子集为NS，k_i的模糊子集为PS，k_d的模糊子集为O；

当e的模糊子集为O，ec的模糊子集为NB时，k_p的模糊子集为PM，k_i的模糊子集为NM，k_d的模糊子集为O；

当e的模糊子集为O，ec的模糊子集为NM时，k_p的模糊子集为PM，k_i的模糊子集为NM，k_d的模糊子集为NS；

当e的模糊子集为O，ec的模糊子集为NS时，k_p的模糊子集为PS，k_i的模糊子集为NS，k_d的模糊子集为NS；

当e的模糊子集为O，ec的模糊子集为O时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为NS；

当e的模糊子集为O，ec的模糊子集为PS时，k_p的模糊子集为NS，k_i的模糊子集为PS，k_d的模糊子集为NS；

当e的模糊子集为O，ec的模糊子集为PM时，k_p的模糊子集为NM，k_i的模糊子集为PM，k_d的模糊子集为NS；

当e的模糊子集为O，ec的模糊子集为PB时，k_p的模糊子集为NM，k_i的模糊子集为PM，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为NB时，k_p的模糊子集为PS，k_i的模糊子集为NM，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为NM时，k_p的模糊子集为PS，k_i的模糊子集为NS，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为NS时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为O时，k_p的模糊子集为NS，k_i的模糊子集为PS，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为PS时，k_p的模糊子集为NS，k_i的模糊子集为PS，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为PM时，k_p的模糊子集为NM，k_i的模糊子集为PM，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为PB时，k_p的模糊子集为NM，k_i的模糊子集为PB，k_d的模糊子集为O；

当e的模糊子集为PM，ec的模糊子集为NB时，k_p的模糊子集为PS，k_i的模糊子集为O，k_d的模糊子集为PB；

当e的模糊子集为PM，ec的模糊子集为NM时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为PS；

当e的模糊子集为PM，ec的模糊子集为NS时，k_p的模糊子集为NS，k_i的模糊子集为PS，k_d的模糊子集为PM；

当e的模糊子集为PM，ec的模糊子集为O时，k_p的模糊子集为NM，k_i的模糊子集为PS，k_d的模糊子集为PS；

当e的模糊子集为PM，ec的模糊子集为PS时，k_p的模糊子集为NM，k_i的模糊子集为PM，k_d的模糊子集为PS；

当e的模糊子集为PM，ec的模糊子集为PM时，k_p的模糊子集为NM，k_i的模糊子集为PB，k_d的模糊子集为PS；

当e的模糊子集为PM，ec的模糊子集为PB时，k_p的模糊子集为NB，k_i的模糊子集为PB，k_d的模糊子集为PB；

当e的模糊子集为PB，ec的模糊子集为NB时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为PB；

当e的模糊子集为PB，ec的模糊子集为NM时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为PM；

当e的模糊子集为PB，ec的模糊子集为NS时，k_p的模糊子集为NM，k_i的模糊子集为PS，k_d的模糊子集为PM；

当e的模糊子集为PB，ec的模糊子集为O时，k_p的模糊子集为NM，k_i的模糊子集为PM，k_d的模糊子集为PM；

当e的模糊子集为PB，ec的模糊子集为PS时，k_p的模糊子集为NM，k_i的模糊子集为PM，k_d的模糊子集为PS；

当e的模糊子集为PB，ec的模糊子集为PM时，k_p的模糊子集为NB，k_i的模糊子集为PB，k_d的模糊子集为PS；

当e的模糊子集为PB，ec的模糊子集为PB时，k_p的模糊子集为NB，k_i的模糊子集为PB，k_d的模糊子集为PB；

步骤2-5：步骤2-1产生的输入变量的模糊量根据步骤2-4产生的模糊规则、步骤2-3产生的伸缩因子，产生输出变量的模糊量，经去模糊化，得到变论域模糊控制器的输出Δk_p,Δk_i,Δk_d；

步骤2-6：将Δk_p,Δk_i,Δk_d值送到PID控制器中，对PID的三个控制参数进行调节，完成三个参数的在线整定，公式如下：

$\left\{\begin{array}{c}{K}_p=K_{p0}+{\mathrm{\Δk}}_p\\ K_i=K_{i0}+\mathrm{\Δ}{k}_i\\ K_d=K_{d0}+{\mathrm{\Δk}}_d\end{array}\right.$

式中，K_p0为比例因数初值、K_i0为积分因数初值、K_d0为微分因数初值，K_p、K_i、K_d为PID控制器的控制参数，分别为比例因数、积分因数、微分因数；

步骤2-7：将步骤2-6得到的位置控制信号传输给伺服驱动装置，伺服驱动装置输送导管运动到指定位置，实现导管的位置跟踪；

步骤3：成像装置接收检测装置中的图像信息并成像；

步骤4：用户通过成像装置呈现的图像判断是否到达期望位置，若未到达，则执行步骤2；若已到达，则等待用户的下一步控制指令。

本发明的有益效果：本发明在常规PID控制的基础上，增强了位置跟踪性能及鲁棒性，提高了微创血管手术的安全性，基于误差分级选择伸缩因子，解决了模糊PID控制精度与控制规则数目之间的矛盾，同时避免了每个采样周期计算一次伸缩因子的大量计算，节省了时间，对系统硬件要求降低，在确保微创手术安全性的同时，增强了主从微创手术机器人系统在微创手术中的有效性和实用性。

附图说明

图1为本发明一种实施方式主从微创手术机器人系统的总体结构示意图；

图2为本发明一种实施方式主从微创手术机器人系统的工作原理示意图；

图3为本发明一种实施方式变论域模糊PID控制器结构示意图；

图4为本发明一种实施方式伺服驱动装置中的用来移动导管的传输装置结构图；

图5为本发明一种实施方式无刷直流电机的控制芯片电路原理图；

图6为本发明一种实施方式高压驱动芯片电路原理图；

图7为本发明一种实施方式电机伺服驱动装置中电机和三相逆变电路原理图；

图8为本发明一种实施方式用于主从微创手术机器人系统装置的控制方法的流程图；

图9为本发明一种实施方式初始误差隶属度函数示意图；

图10为本发明一种实施方式变论域后误差隶属度函数示意图。

具体实施方式

下面结合附图对本发明的实施方式作进一步详细的说明。

本实施方式采用的主从微创手术机器人系统的总体结构如图1所示。包括主手1、上位机2、伺服驱动装置3、导管4(本实施方式中伺服驱动装置3和导管4构成从手)、检测装置5(本实施方式中采用位姿传感器)和成像装置6。主手1连接上位机2，医生通过控制主手1给出导管的期望位置信息并传送给上位机2；上位机2接收主手1和检测装置5传送的位置信号，将信号进行转换，分别得到导管期望运动关节量和导管实际运动关节量，上位机2完成导管实际运动关节量对导管期望关节量的跟踪，并将结果传送给伺服驱动装置3；伺服驱动装置3接收导管关节量信息，完成导管4的进退、弯曲、旋转，实现对导管4末端的位置控制；导管4是柔性材料，连接伺服驱动装置3，在血管中运动，向病灶处输送胶囊或支架；检测装置5检测导管及目标点的位姿，并将检测到的位置信息传递给上位机2，将图像信息传送给成像装置6；成像装置6接收检测装置5传送过来的图像信息，将导管和血管姿态呈现。

本实施方式采用的主从微创手术机器人系统的工作原理如图2所示。主手1连接上位机2，医生通过控制主手1给出导管的位置信息，主手1通过映射将主手手柄移动的位置信息转换为导管运动关节量，进而实现对介入装置以及导管的控制，即实现主从控制方式(医生通过主端主手对从端手术进行控制)；上位机2接收主手1和检测装置5传送的位置信号，将信号通过雅克比逆矩阵进行位置运动转换，分别得到导管期望运动关节量和导管实际运动关节量，上位机2通过变论域模糊PID控制器完成导管实际运动关节量对导管期望关节量的跟踪，并将结果传送给伺服驱动装置3；伺服驱动装置3接收导管关节量信息，经变换环节，按照导管关节与电机脉冲量之间的对应关系，转化为期望脉冲数，期望脉冲数经过PMAC卡、驱动器、电机，驱动导管4进退、弯曲、旋转，实现对导管4末端的位置控制，编码器检测实际脉冲数反馈到PMAC卡，调节脉冲数，进一进调整导管4末端的位置；导管4连接伺服驱动装置，在血管中运动直至导管4末端到达目标点，向目标点处输送胶囊或支架；检测装置5放置在导管4末端，检测导管及目标点处的位姿，并将位置信号传递给上位机2，将图像信息传送给成像装置6；成像装置6主要进行图像处理和人机交互，将导管和血管姿态呈现，使医生更别直观地了解手术进行情况。

本实施方式中的变论域模糊PID控制器结构，如图3所示。包括变论域模糊控制器和PID控制器。误差e经过微分(d/dt)获得误差变化率ec，二者首先要进行模糊化，如误差e经模糊化处理后的语言变量为E、误差变化率ec经模糊化处理后的语言变量为EC。根据误差e的大小选择伸缩因子，改变模糊推理过程中输入、输出(本实施方式中的输入为误差e和误差变化率ec，输出为Δk_p,Δk_i,Δk_d)论域，确定隶属度函数。再经模糊推理规则，输出变量Δk_p,Δk_i,Δk_d的模糊量Δk′_p,Δk′_i,Δk′_d、去模糊化得到输出变量Δk_p,Δk_i,Δk_d送给PID控制器，PID控制器用于调整PID参数，输出电压形式(如图3中的u)的位置指令。

本实施方式中的伺服驱动装置中的用来移动导管的传输装置结构图，如图4所示。包括线性滑道7，旋转平台8，旋转手柄9和转向旋钮10。旋转平台8位于线性滑道7上端，且与线性滑道7滑动连接，旋转手柄9安装在旋转平台8上，可随旋转平台一起在线性滑道7上滑动，也可自行转动。在所述旋转手柄9上还安装有一个低速的转向旋钮10。伺服驱动装置通过三个自由度操控导管：控制旋转手柄9实现导管末端的旋转，控制转向旋钮10实现导管末端的弯曲，控制旋转平台8在线性滑道7上前后运动导管实现导管末端的位移。

本实施方式的硬件配置为：上位机采用的DSP芯片选取型号为TMS320LF2407A，无刷直流电机的控制芯片选用MC33035，驱动芯片选用高压驱动芯片IR2130，缓冲器型号为CD4050，三级管的型号为9012。

图5中，光电耦合器选择TLP521-1。DSP引脚IOPE1连接光电耦合器U3的2脚，DSP的IOPE2脚连接光电耦合器U4的2脚，DSP的PWM9脚连接光电耦合器U5的2脚，DSP的引脚

接光电耦合器U6的4脚，光电耦合器U3的1脚与330Ω电阻R7连接后接3.3V电源，光电耦合器U4的1脚与330Ω电阻R8连接后接3.3V电源、光电耦合器U5的1脚与330Ω电阻R9连接后接3.3V电源，光电耦合器U3的3脚、光电耦合器U4的3脚、光电耦合器U5的3脚均接地，光电耦合器U3的4脚与无刷直流电机的控制芯片的3脚连接，光电耦合器U4的4脚与无刷直流电机的控制芯片的7脚连接，光电耦合器U5的4脚与无刷直流电机的控制芯片的11脚相连。霍尔传感器U7的1脚、2脚、3脚、4脚分别与无刷直流电机的控制芯片的8脚、4脚、5脚、6脚相连，其5脚接地，其2脚串接0.01μf的电容C1后接地，引脚3串接0.01μf的电容C2后接地，引脚4串接0.01μf的电容C3后接地。无刷直流电机的控制芯片的引脚8串接一个10K电阻R10后与引脚11相连、串接一个4.7K电阻R11后与引脚10相连。无刷直流电机的控制芯片的引脚10串接0.01μf的电容C4后接地，无刷直流电机的控制芯片的引脚12与引脚13相连，无刷直流电机的控制芯片的引脚24串接1K电阻R14后接15V电源、无刷直流电机的控制芯片的引脚1串接1K电阻R15后接15V电源、无刷直流电机的控制芯片的引脚2连接1K电阻R16后接15V电源。无刷直流电机的控制芯片的引脚15、16、22、23接地。无刷直流电机的控制芯片的引脚17、18接15V电源。光电耦合器U6的引脚4串接1K的电阻R12后接3.3V电源。光电耦合器U6的引脚1串接2K电阻R13后接15V电源，光电耦合器U6的引脚3接地，光电耦合器U6的引脚2接LED1后与无刷直流电机的控制芯片的引脚14相连。

如图6所示，无刷直流电机的控制芯片的引脚2(即MC33035的AT脚)、引脚1(即MC33035的BT脚)、引脚24(即MC33035的CT脚)与缓冲器U1的引脚3、引脚5、引脚7相连，无刷直流电机的控制芯片的引脚21(即MC33035的AB脚)、引脚20(即MC33035的BB脚)、引脚19(即MC33035的CB脚)与缓冲器U2的引脚3、引脚5、引脚7相连。缓冲器U1的引脚1接5V电源，引脚8接地，引脚2、4、6分别与高压驱动芯片的引脚2、引脚3、引脚4相连。缓冲器U2的引脚1接5V电源，引脚8接地，引脚2、引脚4、引脚6分别与高压驱动芯片的引脚5、引脚6、引脚7相连。高压驱动芯片的引脚1接15V电源，高压驱动芯片的15V电源接整流二极管D7与自身的引脚28相连，高压驱动芯片的15V电源连接整流二极管D8并与其自身的引脚24相连，高压驱动芯片的15V电源连接整流二极管D9并与其自身的引脚20相连。高压驱动芯片的引脚27接20Ω/5W的电阻R1、引脚23接20Ω/5W的电阻R3、引脚19接20Ω/5W的电阻R5、引脚16接20Ω/5W的电阻R2，引脚15接20Ω/5W的电阻R4、引脚14接20Ω/5W的电阻R6、引脚28串接10μf/25V的电容C5与自身的引脚26相连、引脚24串接10μf/25V的电容C6与自身的引脚22相连、引脚20串接10μf/25V的电容C7与引脚18相连、引脚12接地、引脚8串联2K电阻R17后连接到三极管Q7的基极，三极管Q7的集电极接5V电源，三极管Q7的发射极串联2K电阻R18后与无刷直流电机的控制芯片(即MC33035)的引脚9相连，三极管Q7的发射极串接LED2和2K电阻R19后接地，无刷直流电机的控制芯片的引脚9与0.001μf的电容C8相连后接地。

本实施方式中的电机伺服驱动装置中设置有无刷直流电机和三相逆变电路，如图7所示。无刷直流电机型号选用57BL-0880N1-LS-B，高压驱动芯片(即IR2130)的6路输出驱动信号Vg1～Vg6分别加到6个功率MOSFET(如图7中的V1～V6)的栅极驱动功率MOS管，24V电源接V1漏极，V1源极接Vs1，Vs1接V2漏极，V2源极接Vs0，24V电源接V3漏极，V3源极接Vs2，Vs2接V4漏极，V4源极接Vs0，24V电源接V5漏极，V5源极接Vs3，Vs3接V6漏极，V6源极接Vs0，Vs0接0.1Ω/5W电阻R0后接地，24V电源接电容C9后接地，Vs1、Vs2、Vs3分别接到无刷直流电机的A、B、C上。

无刷直流电机带动图4中的旋转平台8在线性滑道7上前后运动，实现导管的进退运动；无刷直流电机带动旋转手柄9旋转，实现导管的旋转运动；无刷直流电机带动转向旋钮10转动，实现导管的弯曲运动。

本实施方式采用的主从微创手术机器人系统的控制方法，如图8所示，包括以下步骤：

步骤1：首先用户在主从微创手术机器人系统的主手端设置导管移动位置的期望位置(x_z,y_z,z_z)^T；其次操作主手手柄控制导管向期望位置运动，在主手手柄的初始位置建立坐标系，把操作手柄最终移动到的位置信息映射到导管所处任务空间，通过映射得到坐标(x_c,y_c,z_c)^T，将位置(x_z,y_z,z_z)^T通过如下公式进行转换，得到期望的导管运动关节量：导管的期望进退量λ、期望旋转量θ和期望弯曲量α，公式为：

$\left(\begin{array}{c}\mathrm{d\λ}\\ \mathrm{d\θ}\\ \mathrm{d\α}\end{array}\right)=J^{-1}\left(\begin{array}{c}{\mathrm{dx}}_z\\ {\mathrm{dy}}_z\\ {\mathrm{dz}}_z\end{array}\right)$

式中，dx_z、dy_z、dz_z分别为期望位置的横坐标的微分、纵坐标的微分、竖坐标的微分，dλ、dθ、dα分别为导管的期望进退量的微分、期望旋转量的微分和期望弯曲量的微分，J^-1为雅克比逆矩阵。

步骤2：位姿传感器测得导管实际对应的运动关节量实际进退量λ′，实际旋转量θ′，实际弯曲量α′，对导管位置进行调整(以对导管进退关节量调整为例，其余两个关节量类似)；

步骤2-1：期望进退关节量λ与实际进退关节量λ′比较，计算导管进退量误差为：

e_λ=λ-λ′

式中，e_λ为导管进退量误差。

导管进退量误差变化率公式为：

ec_λ=de_λ/dt，

式中，ec_λ为导管进退量误差变化率。

将导管进退量误差e_λ和导管进退量误差变化率ec_λ作为输入送入变论域模糊控制器中，确定变论域模糊控制器输入和输出的模糊子集，具体为：

在变论域模糊控制器内，首先定义进退量误差的语言变量E和误差变化率的语言变量EC；然后，用户自定义进退量误差的论域e_λ和误差变化率ec_λ的论域，并在进退量误差的论域范围内将进退量误差的语言变量离散化，在进退量误差变化率的论域范围内将进退量误差变化率的语言变量离散化，确定进退量误差对应的模糊子集和进退量误差变化率对应的模糊子集，表达式均为：

{NB,NM,NS,O,PS,PM,PB}

式中，NB代表负大，NM代表负中，NS代表负小，O代表零，PS代表正小，PM代表正中，PB代表正大；

设定变论域模糊控制器输出的PID参数的三个变化量Δk_p,Δk_i,Δk_d的模糊子集表达式也均为：

{NB,NM,NS,O,PS,PM,PB}

步骤2-2：确定PID参数的三个变化量Δk_p,Δk_i,Δk_d各自的隶属度函数、进退量误差的隶属度函数和进退量误差变化率的隶属度函数，以进退量误差论域[-3,3]为例，进退量误差的隶属度函数如图9所示；

步骤2-3：选取伸缩因子，分别为进退量误差对应的伸缩因子、进退量误差变化率对应的伸缩因子、比例因数变化量对应的伸缩因子、积分因数变化量对应的伸缩因子、微分因数变化量对应的伸缩因子；选取伸缩因子的过程为：

首先将误差e划分为四个区域，即误差区域1～4，其中：

误差区域1定义为：误差隶属度函数中NB和PB最高的部分对应的横坐标区间，该区域表示误差较大；

误差区域2定义为：误差隶属度函数中NM和PM最高的部分对应的横坐标区间，该区域表示误差适中；

误差区域3定义为：误差隶属度函数中NS和PS最高的部分对应的横坐标区间，该区域表示误差较小；

误差区域4定义为：误差隶属度函数中O最高的部分对应的横坐标区间，该区域表示误差很小；

设置误差区域1～4的伸缩因子的取值范围：

误差区域1内的伸缩因子设置如下：误差对应的伸缩因子取值范围为区间(0.7,1]；误差变化率对应的伸缩因子取值范围为区间(0.7,1]；比例因数变化量对应的伸缩因子取值范围为区间(0.7,1]；积分因数变化量对应的伸缩因子取值范围为区间(0,0.2]；微分因数变化量对应的伸缩因子取值范围为区间(0,0.2]；

误差区域2内的伸缩因子设置如下：误差对应的伸缩因子取值范围为区间(0.4,0.7]；误差变化率对应的伸缩因子取值范围为区间(0.4,0.7]；比例因数变化量对应的伸缩因子取值范围为区间(0.4,0.7]；积分因数变化量对应的伸缩因子取值范围为区间(0.2,0.4]；微分因数变化量对应的伸缩因子取值范围为区间(0.2,0.4]；

误差区域3内的伸缩因子设置如下：误差对应的伸缩因子的取值范围为区间(0.2,0.4]；误差变化率对应的伸缩因子的取值范围为区间(0.2,0.4]；比例因数变化量对应的伸缩因子的取值范围为区间(0.2,0.4]；积分因数变化量对应的伸缩因子的取值范围为区间(0.4,0.7]；微分因数变化量对应的伸缩因子的取值范围为区间(0.4,0.7]；

误差区域4内的伸缩因子设置如下：误差对应的伸缩因子的取值范围为区间(0,0.2]；误差变化率对应的伸缩因子的取值范围为区间(0,0.2]；比例因数变化量对应的伸缩因子的取值范围为区间(0,0.2]；积分因数变化量对应的伸缩因子的取值范围为区间(0.7,1]；微分因数变化量对应的伸缩因子的取值范围为区间(0.4,0.7]；

伸缩因子的确定过程如下：

根据首个采样时刻的误差值，判断该进退量误差值属于哪个误差区域，伸缩因子取值即为该误差区域对应的伸缩因子；再接收下一个采样时刻的进退量误差值，若下一个采样时刻的进退量误差值与上一个采样时刻的进退量误差值位于同一个误差区域内，则伸缩因子不变，否则，修改令伸缩因子为此时误差所在的误差区域所对应的伸缩因子，重复该过程，直至采样过程结束；

若进退量误差论域为[-3,3]，首次采样进退量误差值位于误差区域2，例如取进退量误差伸缩因子为0.6，则下个采样时间内进退量误差论域为[-1.8,1.8]，进退量误差隶属度函数如图10所示。接下来一个采样时刻若进退量误差仍位于误差区域2，则进退量伸缩因子、进退量误差论域、进退量误差隶属度函数均不变化，若不位于误差区域2，则进退量误差论域、进退量误差隶属度函数随着对应区域内的进退量伸缩因子变化而变化；

步骤2-4；确定变论域模糊控制器各输入输出参数的模糊规则表；

根据专家总结的经验，确定变论域模糊控制器内的模糊控制规则如下表：

表中，第一行表示EC的模糊子集，第一列表示E的模糊子集，NB代表负大，NM代表负中，NS代表负小，O代表零，PS代表正小，PM代表正中，PB代表正大；

第二行第二列的PB/NB/PS表示当e的模糊子集为NB，ec的模糊子集为NB时，k_p的模糊子集为PB，k_i的模糊子集为NB，k_d的模糊子集为PS；

第二行第三列的PB/NB/NS表示当e的模糊子集为NB，ec的模糊子集为NM时，k_p的模糊子集为PB，k_i的模糊子集为NB，k_d的模糊子集为NS；

第四行第四列的PM/NS/NM表示当e的模糊子集为NS，ec的模糊子集为NS时，k_p的模糊子集为PM，k_i的模糊子集为NS，k_d的模糊子集为NM；

以此类推；

步骤2-5：步骤2-1产生的输入变量的模糊量根据步骤2-4产生的模糊规则、步骤2-3产生的伸缩因子，产生输出变量的模糊量，经去模糊化，得到变论域模糊控制器的输出Δk_p,Δk_i,Δk_d；

步骤2-6：将Δk_p,Δk_i,Δk_d值送到PID控制器中，对PID的三个控制参数进行调节，完成三个参数的在线整定，公式如下：

$\left\{\begin{array}{c}{K}_p=K_{p0}+{\mathrm{\Δk}}_p\\ K_i=K_{i0}+\mathrm{\Δ}{k}_i\\ K_d=K_{d0}+{\mathrm{\Δk}}_d\end{array}\right.$

式中，K_p0为比例因数初值、K_i0为积分因数初值、K_d0为微分因数初值，K_p、K_i、K_d为PID控制器的控制参数，分别为比例因数、积分因数、微分因数；

步骤2-7：将步骤2-6得到的调整后的导管进退量信号传输给伺服驱动装置，类似地，调整后的导管弯曲量和旋转量也传输给伺服驱动装置，按下列公式将导管关节量转换为伺服驱动装置内的期望电机脉冲数，从而驱动导管运动到指定位置，伺服驱动装置内的编码器测得输出脉冲数，伺服驱动装置对电机脉冲数进行调整，实现导管的位置跟踪；

步骤3：成像装置接收检测装置中的图像信息并成像，检测装置检测到点(x_c,y_c,z_c)^T的实时变化，与期望位置(x_z,y_z,z_z)^T形成图像；

步骤4：用户通过成像装置呈现的图像判断是否到达期望位置，即判断点(x_c,y_c,z_c)^T与点(x_z,y_z,z_z)^T是否已经完全重合，若未完全重合，则执行步骤2；若已完全重合，则等待用户的下一步控制指令。

以上所述仅为本发明较佳的具体实施方式，但是本发明的保护范围并不局限于此，本领域内的熟练技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明保护范围之内。本发明的保护范围应该以权利要求的保护范围为准。

Claims (2)

1.一种主从微创手术机器人系统，包括：

主手：用于实现医生对导管位置的控制，主手接收医生给出的导管移动位置的期望值，并且通过映射将主手手柄移动的位置信息转换为导管运动关节量；

导管：用于主从微创手术中向病灶处输送胶囊或支架；

检测装置：用于检测导管末端和目标点的位姿；

上位机：用于接收主手和检测装置传送的位置信号，将信号进行处理，并将结果传送给伺服驱动装置；

伺服驱动装置：用于接收上位机传送的位置信号，驱使导管对医生给出的导管移动位置的期望值进行跟踪；

成像装置：用于图像处理和人机交互，将导管和血管姿态呈现，使医生更加直观地了解手术进行情况；

其特征在于：所述的上位机中还进一步包括：

位置运动转换器：用于将上位机接收到的来自主手和检测装置的位置信息转换为导管期望关节量和导管实际关节量；

变论域模糊控制器：用于将导管期望关节量和导管实际关节量之间的误差及误差变化率进行模糊处理，经模糊化，带有变论域过程的模糊推理，去模糊化过程，将对导管关节量误差和误差变化率的控制作用转换为PID控制的比例控制、积分控制、微分控制作用，并实现PID控制的比例参数、积分参数、微分参数在线调整；

PID控制器：用于根据接收到的变论域模糊控制器的输出信号完成导管关节量的跟踪。

2.一种主从微创手术机器人系统的控制的方法，其特征在于：包括以下步骤：

步骤1：首先，用户在主从微创手术机器人系统的主手端设置导管移动位置的期望位置；其次用户操作主手手柄控制导管向期望位置移动，在主手手柄的初始位置建立坐标系，把主手手柄最终移动到的位置信息映射到导管所处任务空间，通过映射得到的坐标，利用雅克比逆矩阵最终确定导管运动关节量，确定导管运动关节量，包括导管的进退量、旋转量和弯曲量；

步骤2：位姿传感器测得导管实际对应的运动关节量，对导管位置进行调整；

步骤2-1：期望关节量与实际关节量比较，计算二者的误差e，将误差e和误差变化率ec作为输入送入变论域模糊控制器中，确定变论域模糊控制器输入和输出的模糊子集，具体为：

在变论域模糊控制器内，首先定义各个关节量误差的语言变量E和误差变化率的语言变量EC；然后，用户自定义误差的论域e和误差变化率ec的论域，并在误差的论域范围内将误差的语言变量离散化，在误差变化率的论域范围内将误差变化率的语言变量离散化，确定误差对应的模糊子集和误差变化率对应的模糊子集，表达式均为：

{NB,NM,NS,O,PS,PM,PB}

式中，NB代表负大，NM代表负中，NS代表负小，O代表零，PS代表正小，PM代表正中，PB代表正大；

设定变论域模糊控制器输出的PID参数的三个变化量Δk_p,Δk_i,Δk_d的模糊子集表达式也均为：

{NB,NM,NS,O,PS,PM,PB}

步骤2-2：确定PID参数的三个变化量Δk_p,Δk_i,Δk_d各自的隶属度函数、误差的隶属度函数和误差变化率的隶属度函数；

步骤2-3：选取伸缩因子，分别为误差对应的伸缩因子、误差变化率对应的伸缩因子、比例因数变化量对应的伸缩因子、积分因数变化量对应的伸缩因子、微分因数变化量对应的伸缩因子；选取伸缩因子的过程为：

首先将误差e划分为四个区域，即误差区域1～4，其中：

误差区域1定义为：误差隶属度函数中NB和PB最高的部分对应的横坐标区间，该区域表示误差较大；

误差区域2定义为：误差隶属度函数中NM和PM最高的部分对应的横坐标区间，该区域表示误差适中；

误差区域3定义为：误差隶属度函数中NS和PS最高的部分对应的横坐标区间，该区域表示误差较小；

误差区域4定义为：误差隶属度函数中O最高的部分对应的横坐标区间，该区域表示误差很小；

设置误差区域1～4的伸缩因子的取值范围：

误差区域1内的伸缩因子设置如下：误差对应的伸缩因子取值范围为区间(0.7,1]；误差变化率对应的伸缩因子取值范围为区间(0.7,1]；比例因数变化量对应的伸缩因子取值范围为区间(0.7,1]；积分因数变化量对应的伸缩因子取值范围为区间(0,0.2]；微分因数变化量对应的伸缩因子取值范围为区间(0,0.2]；

误差区域2内的伸缩因子设置如下：误差对应的伸缩因子取值范围为区间(0.4,0.7]；误差变化率对应的伸缩因子取值范围为区间(0.4,0.7]；比例因数变化量对应的伸缩因子取值范围为区间(0.4,0.7]；积分因数变化量对应的伸缩因子取值范围为区间(0.2,0.4]；微分因数变化量对应的伸缩因子取值范围为区间(0.2,0.4]；

误差区域3内的伸缩因子设置如下：误差对应的伸缩因子的取值范围为区间(0.2,0.4]；误差变化率对应的伸缩因子的取值范围为区间(0.2,0.4]；比例因数变化量对应的伸缩因子的取值范围为区间(0.2,0.4]；积分因数变化量对应的伸缩因子的取值范围为区间(0.4,0.7]；微分因数变化量对应的伸缩因子的取值范围为区间(0.4,0.7]；

误差区域4内的伸缩因子设置如下：误差对应的伸缩因子的取值范围为区间(0,0.2]；误差变化率对应的伸缩因子的取值范围为区间(0,0.2]；比例因数变化量对应的伸缩因子的取值范围为区间(0,0.2]；积分因数变化量对应的伸缩因子的取值范围为区间(0.7,1]；微分因数变化量对应的伸缩因子的取值范围为区间(0.4,0.7]；

伸缩因子的确定过程如下：

根据首个采样时刻的误差值，判断该误差值属于哪个误差区域，伸缩因子取值即为该误差区域对应的伸缩因子；再接收下一个采样时刻的误差值，若下一个采样时刻的误差值与上一个采样时刻的误差值位于同一个误差区域内，则伸缩因子不变，否则，修改令伸缩因子为此时误差所在的误差区域所对应的伸缩因子，重复该过程，直至采样过程结束；

步骤2-4：确定变论域模糊控制器各输入输出参数的模糊规则表；

所述的变论域模糊控制器内的模糊控制规则如下：

当e的模糊子集为NB，ec的模糊子集为NB时，k_p的模糊子集为PB，k_i的模糊子集为NB，k_d的模糊子集为PS；

当e的模糊子集为NB，ec的模糊子集为NM时，k_p的模糊子集为PB，k_i的模糊子集为NB，k_d的模糊子集为NS；

当e的模糊子集为NB，ec的模糊子集为NS时，k_p的模糊子集为PM，k_i的模糊子集为NM，k_d的模糊子集为NB；

当e的模糊子集为NB，ec的模糊子集为O时，k_p的模糊子集为PM，k_i的模糊子集为NM，k_d的模糊子集为NB；

当e的模糊子集为NB，ec的模糊子集为PS时，k_p的模糊子集为PS，k_i的模糊子集为NS，k_d的模糊子集为NM；

当e的模糊子集为NB，ec的模糊子集为PM时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为NM；

当e的模糊子集为NB，ec的模糊子集为PB时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为PS；

当e的模糊子集为NM，ec的模糊子集为NB时，k_p的模糊子集为PB，k_i的模糊子集为NB，k_d的模糊子集为PS；

当e的模糊子集为NM，ec的模糊子集为NM时，k_p的模糊子集为PB，k_i的模糊子集为NB，k_d的模糊子集为NS；

当e的模糊子集为NM，ec的模糊子集为NS时，k_p的模糊子集为PM，k_i的模糊子集为NM，k_d的模糊子集为NB；

当e的模糊子集为NM，ec的模糊子集为O时，k_p的模糊子集为PS，k_i的模糊子集为NS，k_d的模糊子集为NM；

当e的模糊子集为NM，ec的模糊子集为PS时，k_p的模糊子集为PS，k_i的模糊子集为NS，k_d的模糊子集为NS；

当e的模糊子集为NM，ec的模糊子集为PM时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为NS；

当e的模糊子集为NM，ec的模糊子集为PB时，k_p的模糊子集为NS，k_i的模糊子集为O，k_d的模糊子集为O；

当e的模糊子集为NS，ec的模糊子集为NB时，k_p的模糊子集为PM，k_i的模糊子集为NB，k_d的模糊子集为O；

当e的模糊子集为NS，ec的模糊子集为NM时，k_p的模糊子集为PM，k_i的模糊子集为NM，k_d的模糊子集为NS；

当e的模糊子集为NS，ec的模糊子集为NS时，k_p的模糊子集为PM，k_i的模糊子集为NS，k_d的模糊子集为NM；

当e的模糊子集为NS，ec的模糊子集为O时，k_p的模糊子集为PS，k_i的模糊子集为NS，k_d的模糊子集为NM；

当e的模糊子集为NS，ec的模糊子集为PS时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为NS；

当e的模糊子集为NS，ec的模糊子集为PM时，k_p的模糊子集为NS，k_i的模糊子集为PS，k_d的模糊子集为NS；

当e的模糊子集为NS，ec的模糊子集为PB时，k_p的模糊子集为NS，k_i的模糊子集为PS，k_d的模糊子集为O；

当e的模糊子集为O，ec的模糊子集为NB时，k_p的模糊子集为PM，k_i的模糊子集为NM，k_d的模糊子集为O；

当e的模糊子集为O，ec的模糊子集为NM时，k_p的模糊子集为PM，k_i的模糊子集为NM，k_d的模糊子集为NS；

当e的模糊子集为O，ec的模糊子集为NS时，k_p的模糊子集为PS，k_i的模糊子集为NS，k_d的模糊子集为NS；

当e的模糊子集为O，ec的模糊子集为O时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为NS；

当e的模糊子集为O，ec的模糊子集为PS时，k_p的模糊子集为NS，k_i的模糊子集为PS，k_d的模糊子集为NS；

当e的模糊子集为O，ec的模糊子集为PM时，k_p的模糊子集为NM，k_i的模糊子集为PM，k_d的模糊子集为NS；

当e的模糊子集为O，ec的模糊子集为PB时，k_p的模糊子集为NM，k_i的模糊子集为PM，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为NB时，k_p的模糊子集为PS，k_i的模糊子集为NM，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为NM时，k_p的模糊子集为PS，k_i的模糊子集为NS，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为NS时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为O时，k_p的模糊子集为NS，k_i的模糊子集为PS，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为PS时，k_p的模糊子集为NS，k_i的模糊子集为PS，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为PM时，k_p的模糊子集为NM，k_i的模糊子集为PM，k_d的模糊子集为O；

当e的模糊子集为PS，ec的模糊子集为PB时，k_p的模糊子集为NM，k_i的模糊子集为PB，k_d的模糊子集为O；

当e的模糊子集为PM，ec的模糊子集为NB时，k_p的模糊子集为PS，k_i的模糊子集为O，k_d的模糊子集为PB；

当e的模糊子集为PM，ec的模糊子集为NM时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为PS；

当e的模糊子集为PM，ec的模糊子集为NS时，k_p的模糊子集为NS，k_i的模糊子集为PS，k_d的模糊子集为PM；

当e的模糊子集为PM，ec的模糊子集为O时，k_p的模糊子集为NM，k_i的模糊子集为PS，k_d的模糊子集为PS；

当e的模糊子集为PM，ec的模糊子集为PS时，k_p的模糊子集为NM，k_i的模糊子集为PM，k_d的模糊子集为PS；

当e的模糊子集为PM，ec的模糊子集为PM时，k_p的模糊子集为NM，k_i的模糊子集为PB，k_d的模糊子集为PS；

当e的模糊子集为PM，ec的模糊子集为PB时，k_p的模糊子集为NB，k_i的模糊子集为PB，k_d的模糊子集为PB；

当e的模糊子集为PB，ec的模糊子集为NB时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为PB；

当e的模糊子集为PB，ec的模糊子集为NM时，k_p的模糊子集为O，k_i的模糊子集为O，k_d的模糊子集为PM；

当e的模糊子集为PB，ec的模糊子集为NS时，k_p的模糊子集为NM，k_i的模糊子集为PS，k_d的模糊子集为PM；

当e的模糊子集为PB，ec的模糊子集为O时，k_p的模糊子集为NM，k_i的模糊子集为PM，k_d的模糊子集为PM；

当e的模糊子集为PB，ec的模糊子集为PS时，k_p的模糊子集为NM，k_i的模糊子集为PM，k_d的模糊子集为PS；

当e的模糊子集为PB，ec的模糊子集为PM时，k_p的模糊子集为NB，k_i的模糊子集为PB，k_d的模糊子集为PS；

当e的模糊子集为PB，ec的模糊子集为PB时，k_p的模糊子集为NB，k_i的模糊子集为PB，k_d的模糊子集为PB；

步骤2-5：步骤2-1产生的输入变量的模糊量根据步骤2-4产生的模糊规则、步骤2-3产生的伸缩因子，产生输出变量的模糊量，经去模糊化，得到变论域模糊控制器的输出Δk_p,Δk_i,Δk_d；

步骤2-6：将Δk_p,Δk_i,Δk_d值送到PID控制器中，对PID的三个控制参数进行调节，完成三个参数的在线整定，公式如下：

$\left\{\begin{array}{c}{K}_p=K_{p0}+{\mathrm{\Δk}}_p\\ K_i=K_{i0}+\mathrm{\Δ}{k}_i\\ K_d=K_{d0}+{\mathrm{\Δk}}_d\end{array}\right.$

式中，K_p0为比例因数初值、K_i0为积分因数初值、K_d0为微分因数初值，K_p、K_i、K_d为PID控制器的控制参数，分别为比例因数、积分因数、微分因数；

步骤2-7：将步骤2-6得到的位置控制信号传输给伺服驱动装置，伺服驱动装置输送导管运动到指定位置，实现导管的位置跟踪；

步骤3：成像装置接收检测装置中的图像信息并成像；

步骤4：用户通过成像装置呈现的图像判断是否到达期望位置，若未到达，则执行步骤2；若已到达，则等待用户的下一步控制指令。

Images