[发明专利]一种主从微创手术机器人系统及控制方法

权利要求书

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的三个控制参数进行调节，完成三个参数的在线整定，公式如下：

Kp=Kp0+ΔkpKi=Ki0+ΔkiKd=Kd0+Δkd]]>

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

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

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

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