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

摘要

本发明一种微创血管介入手术导管机器人三维模糊控制装置及方法，属于微创血管介入手术机器人控制领域，该装置包括主手装置、上位机、从端DSP处理器、第一电机、第二电机、第三电机和姿态传感器，本发明针对PID控制方法和二维模糊控制方法在控制导管操作机器人时存在的不足，提出了三维模糊控制方法，提高导管操作机器人的控制精度，进而提高对导管的操作精度，降低患者呼吸心跳等环境因素对导管运动的影响，减少操作者操作导管时产生的超调，使手术更精确、安全。

主权项

一种微创血管介入手术导管机器人三维模糊控制装置，其特征在于：包括主手装置、上位机、从端DSP处理器、第一电机、第二电机、第三电机和姿态传感器，其中，主手装置：用于设置导管末端的目标位置，并将该目标位置发送至上位机中；上位机：（1）当接收到导管末端的初始位置信号时，用于根据导管末端的目标位置和初始位置，计算获得两者的偏差量；将偏差量与初始姿态信息发送至从端DSP处理器中；（2）当接收到实时采集的导管末端位置信号时，用于根据实际导管末端位置，计算获得与目标位置的位置误差，将该位置误差和导管的实时姿态信息发送至从端DSP处理器；从端DSP处理器：（1）当接收到导管末端目标位置与初始位置的偏差量和初始姿态信息时，用于根据导管远端弯曲段的初始姿态信息获得雅克比矩阵，再根据逆雅克比矩阵和导管末端目标位置与初始位置的偏差量，将位置偏差量变换为导管的轴向距离偏差值、旋转角度偏差值和弯曲角度偏差值，并将上述三个量依次转换为第一电机的角位移量、第二电机的角位移量和第三电机的角位移量，控制电机带动导管运动；（2）当接收到导管末端的位置误差信号和导管的实时姿态信息时，用于计算获得导管末端的位置误差变化率和位置误差变化率的微分值，并将位置误差、位置误差变化率和位置误差变化率的微分值作为其内部的三维模糊控制器的输入，三维模糊控制器根据推理规则公式进行计算获得导管末端位置的补偿量，并根据逆雅克比矩阵，将导管末端位置补偿量变换成导管轴向位移补偿量、导管旋转角度补偿量和导管弯曲段弯曲角度补偿量，将上述三个量依次发送至第一电机、第二电机和第三电机；第一电机：用于控制导管的轴向位移；第二电机：用于控制导管的旋转角度；第三电机：用于控制导管的弯曲角度；姿态传感器：用于采集导管远端弯曲段的轴向位移、旋转角度和弯曲角度。