[发明专利]微创血管介入手术导管机器人系统控制装置及方法

权利要求书

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。