CN103558759A - 微创血管介入手术导管机器人系统控制装置及方法 - Google Patents

Abstract

本发明一种微创血管介入手术导管机器人系统控制装置及方法，属于机器人控制技术领域，该装置包括主手控制装置、第一控制器、第一电机、导管夹持装置、第二控制器、第二电机、第一光电编码器和第二光电编码器，所述的第一控制器中包括第一模糊控制器和第一PID控制器，所述的第二控制器中包括第二模糊控制器和第二PID控制器；本发明提高了系统从手对主手的跟踪性能，降低系统的超调量，保证了系统的鲁棒性，从而进一步提高了微创介入手术机器人在介入手术过程中的安全性；对于降低医生在手术过程中受到的辐射危害，提高介入手术的精度，保障患者的身心健康，都具有重要的意义。

Description

微创血管介入手术导管机器人系统控制装置及方法

技术领域

本发明属于机器人控制技术领域，具体涉及一种微创血管介入手术导管机器人系统控制装置及方法。

背景技术

随着国民经济的飞速发展和人民生活水平的不断提高，各种疾病也相伴相生，其中心脑血管疾病已成为威胁人类健康的主要病症之一。根据卫生部统计数据，我国心血管患者超过2亿人，每年约有300万人死于心血管疾病。

微创血管介入手术是治疗心血管疾病的有效方法。微创血管介入手术是医生操纵导管沿人体血管到达体内较远的病变部位，进而对病变部位进行治疗的手术。与传统手术相比，微创血管介入手术具有出血少、创伤小、并发症少、安全可靠和术后恢复快等优点。然而医生手动进行介入手术也存在明显的不足：首先它对医生的技能和操作经验要求较高，因而培养有经验的医生需要较长的时间和精力；其次，在手术过程中医生需要长时间工作在X射线下，对医生自身健康危害较大；此外，手术过程中，医生的误操作很容易造成血管刺穿，危害病人的身心健康。

机器人技术与血管介入技术有机结合是解决上述问题的重要途径。机器人进行介入手术具有以下优势：机器人进行介入手术具有定位精确,安全性高的特点；通过遥控操作技术和力反馈技术的相结合，医生可远程操作，避免了医生遭受X射线辐射；机器人进行介入手术插管速度相对较快，降低了操作时间，减轻了病人的痛苦；结合视觉反馈可进一步提高插管精度，提高了手术的安全性。

目前应用于血管介入手术中的多为主从式导管机器人系统，其控制方法多采用传统的PID控制。然而，介入手术中采用的导管是柔性的，非线性的，导管在血管中运动存在血流的阻力、血管的摩擦力等环境因素，同时主从手之间存在时延效应，故不容易建立精确的导管模型。所以主从式导管机器人系统的从手采用PID控制，往往不能很好的跟踪主手的期望信号，且动态快速性和控制精度通常不能兼顾，有时会出现较大的超调量。因而PID控制应用在微创血管介入手术中可能存在安全隐患，危害病人的身心健康。

发明内容

针对现有技术的不足，本发明提出一种微创血管介入手术导管机器人系统控制装置及方法，以达到克服采用PID控制方法导致的跟踪性能差、超调大，解决动态快速性与控制精度不能兼顾问题的目的。

一种微创血管介入手术导管机器人系统控制装置，包括主手控制装置、第一控制器、第一电机、导管夹持装置、第二控制器、第二电机、第一光电编码器和第二光电编码器，所述的第一控制器中包括第一模糊控制器和第一PID控制器，所述的第二控制器中包括第二模糊控制器和第二PID控制器；

其中，

主手控制装置：用于设定导管的期望轴向位移值和导管的期望旋转角度值，并将导管的期望轴向位移值发送至第一控制器中，将导管的期望旋转角度值发送至第二控制器中；

第一控制器：用于将期望位移值与实际导管轴向运动位移值相减，将获得的位移误差分别发送至控制器内部的第一模糊控制器和第一PID控制器内，同时将位移误差的微分值发送至第一模糊控制器内；第一模糊控制器通过模糊推理，得到第一比例系数、第一积分系数和第一微分系数，将上述系数发送至第一PID控制器；第一PID控制器根据位移误差、第一比例系数、第一积分系数和第一微分系数获得导管轴向运动位移的控制信号，并将控制信号发送至第一电机进行位移补偿；

第一电机：用于控制导管的轴向运动；

第二控制器：用于将期望旋转角度值与实际导管旋转角度值相减，将获得的旋转角度误差分别发送至控制器内部的第二模糊控制器和第二PID控制器内，同时将旋转角度误差的微分值发送至第二模糊控制器内；第二模糊控制器通过模糊推理，得到第二比例系数、第二积分系数和第二微分系数，将上述系数发送至第二PID控制器；第二PID控制器根据旋转角度误差、第二比例系数、第二积分系数和第二微分系数获得导管旋转角度的控制信号，并将控制信号发送至第二电机进行旋转角度补偿；

第二电机：用于控制导管的旋转角度；

第一光电编码器：用于采集第一电机输出轴的轴向位移值，即导管的轴向位移值，并将采集的数值返回发送至第一控制器中；

第二光电编码器：用于采集第二电机输出轴的旋转角度值，即导管的旋转角度值，并将采集的数值返回发送至第二控制器中。

所述的主手控制装置的两路输出端分别连接第一控制器的一路输入端和第二控制器的一路输入端，第一控制器的输出端连接第一电机的输入端，第一电机输出轴连接导管夹持装置一路输入端；第二控制器的输出端连接第二电机的输入端，第二电机的输出端连接导管夹持装置的另一路输入端；第一光电编码器设置于第一电机的输出轴上，其输出端连接第一控制器的另一路输入端；第二光电编码器设置于第二电机的输出轴上，其输出端连接第二控制器的另一路输入端。

采用微创血管介入手术导管机器人系统控制装置进行模糊自适应PID控制的方法，包括以下步骤：

步骤1、通过主手控制装置设定导管的期望位置，位置参数包括导管轴向位移值和导管旋转角度值；

步骤2、主手装置将导管的期望轴向位移值发送至第一控制器中，将导管的期望旋转角度值发送至第二控制器中，第一控制器通过控制第一电机带动导管做轴向运动；第二控制器通过控制第二电机带动导管做旋转运动；

步骤3、采用设置于第一电机输出轴上的第一光电编码器实时采集导管的实际轴向运动位移数值，并将上述位移值发送至第一控制器中；采用设置于第二电机输出轴上的第二光电编码器实时采集导管的实际旋转运动角度值，并将上述角度值发送至第二控制器中；

步骤4、第一控制器将期望位移值与实际导管轴向运动位移值相减，获得位移误差；第二控制器将实际导管旋转角度值与期望角度值相减，获得旋转角度误差；

步骤5、将位移误差分别发送至控制器内部的第一模糊控制器和第一PID控制器内，同时将位移误差的微分值发送至第一模糊控制器内，将旋转角度误差分别发送至控制器内部的第二模糊控制器和第二PID控制器内，同时将旋转角度误差的微分值发送至第二模糊控制器内；

步骤6、第一模糊控制器根据隶属函数和用户设定的论域对位移误差和位移误差的微分进行模糊化处理，根据模糊化后的值查询模糊控制规则表进行模糊推理，再采用重心法进行去模糊化，得到第一比例系数、第一积分系数和第一微分系数，将上述系数发送至第一PID控制器；第二模糊控制器根据隶属函数和用户设定的论域对角度误差和角度误差的微分进行模糊化处理，根据模糊化后的值查询模糊控制规则表进行模糊推理，再采用重心法进行去模糊化，得到第二比例系数、第二积分系数和第二微分系数，将上述系数发送至第二PID控制器；

步骤7、第一PID控制器根据位移误差、第一比例系数、第一积分系数和第一微分系数获得导管轴向运动位移的控制信号，并将控制信号发送至第一电机实现位移补偿；第二PID控制器根据旋转角度误差、第二比例系数、第二积分系数和第二微分系数获得导管轴向运动位移的控制信号，并将控制信号发送至第二电机实现旋转角度补偿；

步骤8、判断导管是否到达期望位置，若到达，则停止并等待下一期望位置信号，否则，返回执行步骤4。

本发明优点：

本发明一种微创血管介入手术导管机器人系统控制装置及方法，动态性能好，不仅具有了模糊控制的灵活性和适应性强的优点，还具有PID控制精度高的优势；本发明设计的模糊自适应PID控制器，提高了系统从手对主手的跟踪性能，降低系统的超调量，保证了系统的鲁棒性，从而进一步提高了微创介入手术机器人在介入手术过程中的安全性；对于降低医生在手术过程中受到的辐射危害，提高介入手术的精度，保障患者的身心健康，都具有重要的意义。

附图说明

图1为本发明一种实施例的整体装置结构示意图；

图2为本发明一种实施例的从手装置机械结构示意图；

图3为本发明一种实施例的光电编码器A路信号电路图；

图4为本发明一种实施例的步进电机驱动电路；

图5为本发明一种实施例的第一控制器DSP芯片；

图6为本发明一种实施例的无刷直流电机驱动电路；

图7为本发明一种实施例的第二控制器DSP芯片；

图8为本发明一种实施例的微创血管介入手术导管机器人系统控制方法流程图；

图9为本发明一种实施例的微创血管介入手术导管机器人系统模糊自适应PID控制结构；

图10为本发明一种实施例的e₁（e₂），e₁c（e₂c）和k_1p（k_2p）的隶属函数示意图；

图11为本发明一种实施例的k_1i（k_2i）的隶属函数示意图；

图12为本发明一种实施例的k_1d（k_2d）的隶属函数示意图；

图13为本发明一种实施例的轴向运动的位移跟踪效果示意图；

图14为本发明一种实施例的旋转运动角度跟踪效果示意图。

具体实施方式

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

如图1所示，微创血管介入手术导管机器人系统控制装置，包括主手控制装置、第一控制器、第一电机、导管夹持装置、导管、第二控制器、第二电机、第一光电编码器和第二光电编码器，所述的第一控制器中包括第一模糊控制器和第一PID控制器，所述的第二控制器中包括第二模糊控制器和第二PID控制器；其中，

主手控制装置用于设定导管的期望轴向位移值和导管的期望旋转角度值，并将导管的期望轴向位移值发送至第一控制器中，将导管的期望旋转角度值发送至第二控制器中；第一控制器用于将期望位移值与实际导管轴向运动位移值相减，将获得的位移误差分别发送至控制器内部的第一模糊控制器和第一PID控制器内，同时将位移误差的微分值发送至第一模糊控制器内；第一模糊控制器通过模糊推理，得到第一比例系数、第一积分系数和第一微分系数，将上述系数发送至第一PID控制器；第一PID控制器根据位移误差、第一比例系数、第一积分系数和第一微分系数获得导管轴向运动位移的控制信号，并将控制信号发送至第一电机进行位移补偿；第一电机用于控制导管的轴向运动；第二控制器用于将期望旋转角度值与实际导管旋转角度值相减，将获得的旋转角度误差分别发送至控制器内部的第二模糊控制器和第二PID控制器内，同时将旋转角度误差的微分值发送至第二模糊控制器内；第二模糊控制器通过模糊推理，得到第二比例系数、第二积分系数和第二微分系数，将上述系数发送至第二PID控制器；第二PID控制器根据旋转角度误差、第二比例系数、第二积分系数和第二微分系数获得导管旋转角度的控制信号，并将控制信号发送至第二电机进行旋转角度补偿；第二电机用于控制导管的旋转角度；第一光电编码器用于采集第一电机输出轴的轴向位移值，即导管的轴向位移值，并将采集的数值返回发送至第一控制器中；第二光电编码器用于采集第二电机输出轴的旋转角度值，即导管的旋转角度值，并将采集的数值返回发送至第二控制器中。

微创介入手术中，医生通过对导管的推拉和旋转，将其经血管送到病变位置，故本实施例从手装置（从手部分包括第一控制器、第一电机、导管夹持装置、导管、第二控制器、第二电机、第一光电编码器和第二光电编码器）包含两个自由度（轴向运动和旋转运动）来模仿医生的两个动作：推拉和旋转。

图1中，主手控制装置把期望的轴向运动的位移值送给第一控制器，第一控制器控制第一电机驱动器，驱动第一电机结合传动装置和导管夹持装置带动导管作轴向运动。安装在第一电机轴上的第一光电编码器，测量电机旋转的角度，通过机械装置关系求取导管实际移动的位移值，并将该值反馈到第一控制器。

主手控制装置把期望的旋转运动的角度值送给第二控制器，第二控制器控制第二电机驱动器，驱动第二电机结合传动装置和导管夹持装置带动导管作旋转运动。安装在第二电机轴上的第二光电编码器，测量电机旋转的角度，通过机械装置关系求取导管实际旋转的角度值，并将该值反馈到第二控制器。

如图2所示，其中1是滑轮，2是第二电机，3是导管夹持装置，4是滑轮，5是导管，6是导管夹持装置，7是丝杠，8是第一电机，9是可移动平台。

图2中，滑轮1和滑轮4组成滑轮组，两个滑轮采用皮带连接。第二电机2带动滑轮1作旋转运动，通过皮带连接，进而带动滑轮4作旋转运动。导管5通过滑轮4，故滑轮4作旋转运动，进而带动导管5作旋转运动。第一电机8带动丝杠7作旋转运动，丝杠7旋转进而带动平台9作轴向运动，平台移动进而带动导管作轴向运动。导管夹持装置3安装在可移动的平台9上，导管夹持装置6安装在固定的平台上（可移动平台下侧）；导管夹持装置3和导管夹持装置6组成一对互锁装置，导管夹持装置3夹持导管时，导管夹持装置6松开导管，即不夹持导管；导管夹持装置6夹持导管时，导管夹持装置3松开导管，即不夹持导管。若导管作旋转运动和轴向运动时，由导管夹持装置3夹持导管，导管夹持装置6不夹持导管。可移动平台9在作轴向运动时，若移动的距离值超过了可移动平台的移动范围，则导管夹持装置6夹持导管，导管夹持装置3不夹持导管，第一电机驱动可移动平台9后退至初始位置。此过程中导管被导管夹持装置6夹持，导管不作任何运动。可移动平台9退至初始位置后，导管夹持装置6松开导管，导管夹持装置3夹持导管，进而可移动平台可以继续作轴向运动。

主手控制装置的两路输出端分别连接第一控制器的一路输入端和第二控制器的一路输入端，第一控制器的输出端连接第一电机的输入端，第一电机输出轴连接导管夹持装置一路输入端；第二控制器的输出端连接第二电机的输入端，第二电机的输出端连接导管夹持装置的另一路输入端；第一光电编码器设置于第一电机的输出轴上，其输出端连接第一控制器的另一路输入端；第二光电编码器设置于第二电机的输出轴上，其输出端连接第二控制器的另一路输入端。

本发明实施例中，主手控制装置采用美国Novint公司生产的NovintFalcon新型3D手柄，该手柄结合上位机可以通过编程实现对从手的控制，上位机与下位机之间可采用CAN总线进行数据传输。本发明实施例中，第一控制器和第二控制器均采用TMS320F2812DSP的最小系统和外围电路；第一光电编码器和第二光电编码器的型号均为HXBI-BEG05L400BM。

光电编码器（指代第一光电编码器和第二光电编码器）的输出端输出三路信号，分别为A路、B路和Z路，三路信号分别经光耦隔离和信号整形后，输送到DSP控制器的QEP1引脚、QEP1引脚和QEP3引脚（如图5所示），本发明实施例中，采用6N137芯片进行光耦隔离，通过74HC14芯片进行滤波整形。以光电编码器A路信号为例，如图3所示，光电编码器输出A路信号连接一个2.2K的电阻后，与6N137芯片的引脚3连接。6N137芯片引脚2接一个12V电源；引脚7、8接5V的电源后，接一个0.1uF的电容，然后接地；引脚5直接接地；Vcc接一个1k电阻后与引脚6相连。6N137芯片引脚6接一个1k电阻后与74HC14芯片的引脚11相连，引脚11接一个0.1uF电容后，并联一个20k的电阻，最后接地。74HC14的引脚10接DSP芯片的QEP1引脚。B路与Z路信号处理电路与A路信号完全相同。

本发明实施例中，第一电机采用了森创公司的42BYG250-BASSML-0151型两相步进电机。步进电机的驱动电路如图4所示（本实施例给出了该步进电机一相的驱动电路，另一相驱动电路与图4相同），图4中步进电机的驱动芯片选用L298集成电路芯片，它是双H桥式驱动器。步进电机的控制器采用芯片L297，它可以产生四相驱动信号。续流二极管V1、V2、V3和V4选用的型号均为IN4007。L297的引脚20与一个10K电阻R2相连后接VCC(5V)，引脚2直接接地。VCC(5V)串接R2后，再与C2(100uF)相连后接地。L297的引脚4、6、5和14分别与L298的引脚5、7、6和1直接相连。L298的引脚1串接一个0.5Ω的电阻后接地，引脚8接电容C4(470uF)后接地。L297引脚12直接连接VCC（5V）,引脚16与引脚12之间串接一个22K的电阻R1，引脚16与电容C1（3.3uF）相连后接地。VCC（5V）电源与电容C3(100uF)相连后接地。L298引脚4直接连接VCC(36V)。续流二极管V1、V3的负极与VCC(36V)相连接，V1、V3的正极分别与V2、V4的负极相连。L2981引脚1与V1正极连接后接电动机负极端子，引脚3与V3的正极连接后接电动机正极端子。

如图5所示，第一控制器DSP芯片采用TMS320F2812型号，该型号DSP芯片最小系统是常见的，故并未详细阐述。芯片L297的引脚17、18、19、10、11分别与图5中DSP引脚1、2、3、4、5连接。TMS320F2812的引脚6、7、8的输入信号，为第一光电编码器A路、B路和Z路经光耦隔离和整形滤波后的信号。

本发明实施例中，第二电机采用了无刷直流电机，采用无刷直流电机的型号为42BLF02-003SV1024，无刷直流电机的驱动电路如图6所示。图6中三相桥式变换电路采用3个IR2110芯片驱动（图6中以一个IR2110芯片的详细连接电路为例，本实施例中采用三个与该电路结构相同的驱动电路），DSP输出端PWM信号与IR2110芯片之间采用光电藕合器件TLP521进行信号转换。图6中Q1、Q2、Q3、Q4、Q5和Q6为MOSFET，型号为2sk1328。自举电容C1和C2采用1uF陶瓷电容。自举二极管选Dbs选用的二极管型号为BYT56D，它的功能是防止V1导通时,较高的电压反串入VCC端烧坏芯片。D1、D2、D3、D4、D5和D6选用的二极管型号为FR207。

如图6所示，DSP引脚PWM1和PWM2分别接电阻R1（1k）和R2(1k)后，再分别与对应的TPL521芯片的引脚2连接。两片TPL521芯片的引脚1接3.3V电源，引脚4接15V电源。15V电源接一个0.1uF的电容后接地。PWM1对应的TPL521芯片的引脚2连接一个1k的电阻后接地，PWM2对应的TPL521芯片的引脚2连接一个1k的电阻后接地。IR2110芯片的引脚9和引脚13连接后接地。IR2110芯片引脚1和引脚2相连后接地，引脚2与一个50Ω的电阻连接后,与Q6MOSFET的栅极相连，引脚3与15V电源相连。IR2110芯片的引脚3和引脚5串联一个自举二极管Dbs，引脚2和引脚3之间串联一个0.1uf的自举电容，引脚5和引脚6之间串联一个0.1uf的自举电容。IR2110芯片的引脚7接一个50的电阻R3后，再与Q1的栅极相连。MOSFET Q1与二极管D1并联、Q2与二极管D2并联、Q3与二极管D3并联、Q4与二极管D4并联、Q5与二极管D5并联、Q6与二极管D6并联。Q1、Q3和Q5的漏极与母线相连，Q2、Q4和Q6的源极与地相连。Q1、Q3和Q5的源极与Q2、Q4和Q6的漏极相连。引脚5与Q1的源极相连后与无刷直流电机绕线A端子相连。无刷直流电机采用星形连接方式，无刷直流电机绕线B和绕线C具体电路连接与绕线A电路图完全相似。

如图7所示，第二控制器DSP也采用TMS320F2812型号，该型号DSP芯片最小系统是常见的，故并未详细阐述。图7中，TMS320F2812的引脚1、2、3、4、5、6分别与无刷直流电机驱动电路的PWM1端子、PWM2端子、PWM3端子、PWM4端子、PWM5端子和PWM6端子相连。TMS320F2812的引脚7、8、9的输入信号，为第二光电编码器A路、B路和Z路经光耦隔离和整形滤波后的信号。

采用微创血管介入手术导管机器人系统控制装置进行模糊自适应PID控制的方法，流程图如图8所示，

结合图9叙述系统模糊自适应PID控制过程；包括以下步骤：

步骤1、主手给定导管期望的运动位置值，包括期望导管轴向运动的位移值y和期望导管旋转运动的角度值θ；

步骤2、主手装置将上述设定值发送至从手装置内部的控制器中，轴向位移值y发送至第一控制器，旋转角度值θ发送至第二控制器。第一控制器驱动从手部分的第一电机结合机械传动装置带动导管作轴向运动；第二控制器驱动从手部分的第二电机结合机械传动装置带动导管作旋转运动；

步骤3、采用设置于第一电机输出轴上的第一光电编码器结合机械传输装置实时采集导管的实际轴向运动位移数值y′，并将上述实际位移值y′发送至第一控制器中；采用设置于第二电机输出轴上的第二光电编码器结合机械传输装置实时采集导管的实际旋转运动角度值θ′，并将上述实际旋转角度值θ′发送至第二控制器中；

步骤4、第一控制器将期望位移值y与实际导管轴向运动位移值y′相减，获得位移误e₁，即e₁=y-y′；第二控制器将期望角度值θ与实际导管旋转角度值相减θ′，获得旋转角度误差e₂，即e₂=θ-θ′；

步骤5、将位移误差e₁分别发送至第一控制器内部的第一模糊控制器和第一PID控制器内，同时将位移误差的微分值e₁c发送至第一模糊控制器内。将旋转角度误差e₂分别发送至第二控制器内部的第二模糊控制器和第二PID控制器内，同时将旋转角度误差的微分值e₂c发送至第二模糊控制器内；

步骤6、第一模糊控制器根据隶属函数和用户设定的论域对位移误差和位移误差的微分进行模糊化处理，根据模糊化后的值查询模糊控制规则表进行模糊推理，再采用重心法进行去模糊化，得到第一比例系数k_1p、第一积分系数k_1i和第一微分系数k_1d，将上述系数发送至第一PID控制器；第二模糊控制器根据隶属函数和用户设定的论域对角度误差和角度误差的微分进行模糊化处理，根据模糊化后的值查询模糊控制规则表进行模糊推理，再采用重心法进行去模糊化，得到第二比例系数k_2p、第二积分系数k_2i和第二微分系数k_2d，将上述系数发送至第二PID控制器。模糊控制具体步骤如下：

步骤6-1，根据量化因子，对模糊输入e₁（e₂）和e₁c（e₂c）进行模糊化；

本发明实施例中，量化因子均为1。设置e₁（e₂）的语言变量为E，误差变化率e₁（e₂）的语言变量为EC。如图10所示，设置两者的论域均为（-1,1），e₁（e₂）和e₁c（e₂c）隶属度函数为trimf型。其语言变量的取值为{NB,NM,NS，ZO,PS,PM,PB}，其中，NB表示负大、NM表示负中、NS表示负小、ZO表示0、PS表示正小、PM表示正中，PB表示正大。

步骤6-2，根据语言变量E和EC的取值查询模糊控制规则表，进行模糊推理，获得控制量；

根据不同的误差值e和误差变化值ec，控制过程对k_p、k_i和k_d自整定要求应满足如下规律：

1）当误差e取较大值时，应取相对较大的k_p值来提高响应的快速性；同时为了降低超调，防止误差e瞬时过大，k_i和k_d应取较小的值；

2）当误差e取较中等值时，为使系统具有较小的超调，应取较小的k_p值和适当大小的k_i和k_d值；

3）当误差e为较小的值时，应取较大的k_p和k_i值，同时应取适当大小的k_d值，来防止系统在平衡点附近发生震荡。

根据以上专家总结的经验,本发明实施例中，根据模糊控制规则（该规则为公知常识，此处不做详细描述）如表1所示。本发明实施例中共采用了49条模糊规则，模糊规则的形式如下：

(1)If(e is NB)and(ec is NB)then(k_p is PB)(k_i is NB)(k_d is PS)

表示：当误差e处于NB范围，ec处于NB范围，那么输出比例系数k_p选用PB范围的值，积分系数k_i选择NB范围的值，微分系数k_d选择PS范围的值。语句（2）至语句（49）与其思路相同；

(2)If(e is NB)and(ecis NM)then(k_i is PB)(k_iis NB)(k_d is NS）

……

(49)If(e is PB)and(ec is PB)then(k_i is NB)(k_iis PB)(k_d is PB）

表1

步骤6-3，采用重心法去模糊化；

重心法计算公式如下，

$u_{\mathrm{fz}}=\frac{{\mathrm{\Σ}}_{i=1}^n{u}_{i}A\left(u_i\right)}{{\mathrm{\Σ}}_{i=1}^{n}A\left(u_i\right)}---\left(1\right)$

其中，u_fz表示所求区域的面积中心的横坐标；u_i论域中的元素，A(u_i)为论域中元素u_i处的隶属度，n表示论域中离散点的个数。

如图10到图12所示，输出量k_1p（k_2p）论域为（-1,1），k_1i（k_2i）的论域为（0,1）,k_1d（k_2d）的论域为（0,2）。两输入三输出的隶属函数均为trimf型。

步骤6-4，将获得控制量与比例因子相乘（本实施例中比例因子均为1），进而获得实际的控制量k_1p（k_2p）、k_1i（k_2i）和k_1d（k_2d）。将获得的实际的控制量k_1p（k_2p）、k_1i（k_2i）和k_1d（k_2d）送到对应的PID控制器；

步骤7、第一PID控制器收到来自第一模糊控制器的k_1p、k_1i和k_1d值后，向步进电机发出控制信号，进而控制导管作轴向运动，实现对轴向运动位移补偿。第二PID控制器收到来自第二模糊控制器的k_2p、k_2i和k_2d的值后，向无刷直流电机发出控制信号，进而控制导管作旋转运动，实现对旋转运动的角度补偿；

步骤8、判断导管是否到达期望位置，若到达，则停止并等待下一期望位置信号，否则，返回执行步骤4。

第一光电编码器和第二光电编码器结合机械传动装置作为监测数据输出端测量导管实际的运动姿态（轴向运动位移值y''和旋转运动角度值θ''），并将其反馈到控制器（第一控制器或第二控制器）与主手控制装置给定的期望值作比较。若期望值与实际值之差为零，则到达期望位置，各电机停止运动，且控制器等待下一期望位置信号。否则，期望值若与实际值之差不为零，则返回执行步骤4。

重复以上步骤，即可实现对血管介入导管系统的模糊自适应PID控制，实现对PID参数的自动校正和调整。

轴向运动的位移跟踪效果如图13所示。输入信号为主手给定的期望导管轴向移动位移值，输出为导管实际移动的位移值。由图13可以看出，模糊自适应PID控制器的输出结果的超调量小于PID控制器输出结果的超调量，并且响应时间短,稳态精度高，鲁棒性强，跟踪输入信号的能力明显提高。

旋转转运动的角度跟踪果如图14所示。输入信号为主手给定的期望导管旋转角度值，输出为导管实际旋转的角度值。从图中可以看出，在旋转运动中，模糊自适应PID控制的超调量同样优于PID控制的输出值，并且响应时间相对较短,稳态精度高，鲁棒性强，跟踪输入信号的能力同样显著提高。

Claims (3)

1.一种微创血管介入手术导管机器人系统控制装置，其特征在于：包括主手控制装置、第一控制器、第一电机、导管夹持装置、第二控制器、第二电机、第一光电编码器和第二光电编码器，所述的第一控制器中包括第一模糊控制器和第一PID控制器，所述的第二控制器中包括第二模糊控制器和第二PID控制器；

其中，

主手控制装置：用于设定导管的期望轴向位移值和导管的期望旋转角度值，并将导管的期望轴向位移值发送至第一控制器中，将导管的期望旋转角度值发送至第二控制器中；

第一控制器：用于将期望位移值与实际导管轴向运动位移值相减，将获得的位移误差分别发送至控制器内部的第一模糊控制器和第一PID控制器内，同时将位移误差的微分值发送至第一模糊控制器内；第一模糊控制器通过模糊推理，得到第一比例系数、第一积分系数和第一微分系数，将上述系数发送至第一PID控制器；第一PID控制器根据位移误差、第一比例系数、第一积分系数和第一微分系数获得导管轴向运动位移的控制信号，并将控制信号发送至第一电机进行位移补偿；

第一电机：用于控制导管的轴向运动；

第二控制器：用于将期望旋转角度值与实际导管旋转角度值相减，将获得的旋转角度误差分别发送至控制器内部的第二模糊控制器和第二PID控制器内，同时将旋转角度误差的微分值发送至第二模糊控制器内；第二模糊控制器通过模糊推理，得到第二比例系数、第二积分系数和第二微分系数，将上述系数发送至第二PID控制器；第二PID控制器根据旋转角度误差、第二比例系数、第二积分系数和第二微分系数获得导管旋转角度的控制信号，并将控制信号发送至第二电机进行旋转角度补偿；

第二电机：用于控制导管的旋转角度；

第一光电编码器：用于采集第一电机输出轴的轴向位移值，即导管的轴向位移值，并将采集的数值返回发送至第一控制器中；

第二光电编码器：用于采集第二电机输出轴的旋转角度值，即导管的旋转角度值，并将采集的数值返回发送至第二控制器中。

2.根据权利要求1所述的微创血管介入手术导管机器人系统控制装置，其特征在于：所述的主手控制装置的两路输出端分别连接第一控制器的一路输入端和第二控制器的一路输入端，第一控制器的输出端连接第一电机的输入端，第一电机输出轴连接导管夹持装置一路输入端；第二控制的输出端连接第二电机的输入端，第二电机的输出端连接导管夹持装置的另一路输入端；第一光电编码器设置于第一电机的输出轴上，其输出端连接第一控制器的另一路输入端；第二光电编码器设置于第二电机的输出轴上，其输出端连接第二控制器的另一路输入端。

3.采用权利要求1所述的微创血管介入手术导管机器人系统控制装置进行模糊自适应PID控制的方法，其特征在于：包括以下步骤：

步骤1、通过主手装置设定导管的期望位置，位置参数包括导管轴向位移值和导管旋转角度值；

步骤2、主手装置将导管的期望轴向位移值发送至第一控制器中，将导管的期望旋转角度值发送至第二控制器中，第一控制器通过控制第一电机带动导管做轴向运动；第二控制器通过控制第二电机带动导管做旋转运动；

步骤3、采用设置于第一电机输出轴上的第一光电编码器实时采集导管的实际轴向运动位移数值，并将上述位移值发送至第一控制器中；采用设置于第二电机输出轴上的第二光电编码器实时采集导管的实际旋转运动角度值，并将上述角度值发送至第二控制器中；

步骤4、第一控制器将期望位移值与实际导管轴向运动位移值相减，获得位移误差；第二控制器将实际导管旋转角度值与期望角度值相减，获得旋转角度误差；

步骤5、将位移误差分别发送至控制器内部的第一模糊控制器和第一PID控制器内，同时将位移误差的微分值发送至第一模糊控制器内，将旋转角度误差分别发送至控制器内部的第二模糊控制器和第二PID控制器内，同时将旋转角度误差的微分值发送至第二模糊控制器内；

步骤6、第一模糊控制器根据隶属函数和用户设定的论域对位移误差和位移误差的微分进行模糊化处理，根据模糊化后的值查询模糊控制规则表进行模糊推理，再采用重心法进行去模糊化，得到第一比例系数、第一积分系数和第一微分系数，将上述系数发送至第一PID控制器；第二模糊控制器根据隶属函数和用户设定的论域对角度误差和角度误差的微分进行模糊化处理，根据模糊化后的值查询模糊控制规则表进行模糊推理，再采用重心法进行去模糊化，得到第二比例系数、第二积分系数和第二微分系数，将上述系数发送至第二PID控制器；

步骤7、第一PID控制器根据位移误差、第一比例系数、第一积分系数和第一微分系数获得导管轴向运动位移的控制信号，并将控制信号发送至第一电机实现位移补偿；第二PID控制器根据旋转角度误差、第二比例系数、第二积分系数和第二微分系数获得导管轴向运动位移的控制信号，并将控制信号发送至第二电机实现旋转角度补偿；

步骤8、判断导管是否到达期望位置，若到达，则停止并等待下一期望位置信号，否则，返回执行步骤4。

Images