CN103549994B - 微创血管介入手术导管机器人三维模糊控制装置 - Google Patents

Abstract

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

Description

微创血管介入手术导管机器人三维模糊控制装置

技术领域

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

背景技术

血管介入手术具有伤口小、出血少、术后恢复快且并发症少等优点，广泛应用于心脑血管疾病的治疗。在传统的血管介入手术中，操作者直接操作导管，在医学影像的辅助下，经血管把导管送到病灶处，进行诊断和治疗。但是导管操作需要有经验的操作者经过反复尝试才能获得良好的效果，这样操作者和病人长时间接触射线，长期工作会对操作者造成一定损伤。随着医疗辅助机器人技术的发展，主从导管介入的思想已经引入微创血管介入手术领域。采用这种操作方式，操作者能够脱离手术现场避免射线辐射伤害。手术中，操作者脱离现场，通过控制导管操作机器人间接操作导管到达病灶处，实现对病灶部位的治疗。现有的研究中导管操作机器人的控制通常采用PID控制方法。然而导管是一个高度柔性的设备，难以给出精确的运动学模型，且导管工作在复杂的体内环境中，系统参数会发生变化。PID控制器的参数选定后不能改变，它的适应性较差。因此，需要一种更有效的方法来提高导管操作机器人的控制性能。模糊控制不需要知道被控对象的精确的数学模型，对过程参数变化不敏感，应用于非线性系统时具有很强的鲁棒性和良好的控制品质。然而，二维模糊控制在消除系统稳态误差方面的性能较差。

发明内容

针对现有技术的不足，本发明提出一种微创血管介入手术导管机器人三维模糊控制装置及方法，以达克服传统PID和二维模糊控制方法的不足，提高导管定位的精确度，提高导管的控制精度和系统的鲁棒性，使手术更精确、安全的目的。

一种微创血管介入手术导管机器人三维模糊控制装置，包括主手装置、上位机、从端DSP处理器、第一电机、第二电机、第三电机和姿态传感器，其中，

主手装置：用于设置导管末端的目标位置，并将该目标位置发送至上位机中；

上位机：

（1）当接收到导管末端的初始位置信号时，用于根据导管末端的目标位置和初始位置，计算获得两者的偏差量；将偏差量与初始姿态信息发送至从端DSP处理器中；

（2）当接收到实时采集的导管末端位置信号时，用于根据实际导管末端位置，计算获得与目标位置的位置误差，将该位置误差和导管的实时姿态信息发送至从端DSP处理器；

从端DSP处理器：

（1）当接收到导管末端目标位置与初始位置的偏差量和初始姿态信息时，用于根据导管远端弯曲段的初始姿态信息获得雅克比矩阵，再根据逆雅克比矩阵和导管末端目标位置与初始位置的偏差量，将位置偏差量变换为导管的轴向距离偏差值、旋转角度偏差值和弯曲角度偏差值，并将上述三个量依次转换为第一电机的角位移量、第二电机的角位移量和第三电机的角位移量，控制电机带动导管运动；

（2）当接收到导管末端的位置误差信号和导管的实时姿态信息时，用于计算获得导管末端的位置误差变化率和位置误差变化率的微分值，并将位置误差、位置误差变化率和位置误差变化率的微分值作为其内部的三维模糊控制器的输入，三维模糊控制器根据推理规则公式进行计算获得导管末端位置的补偿量，并根据逆雅克比矩阵，将导管末端位置补偿量变换成导管轴向位移补偿量、导管旋转角度补偿量和导管弯曲段弯曲角度补偿量，将上述三个量依次发送至第一电机、第二电机和第三电机；

第一电机：用于控制导管的轴向位移；

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

第三电机：用于控制导管的弯曲角度；

姿态传感器：用于采集导管远端弯曲段的轴向位移、旋转角度和弯曲角度。

主手装置的输出端连接上位机的一路输入端，上位机的输出端连接从端DSP处理器的一路输入端，从端DSP处理器的一路输出端连接第一电机的输入端，从端DSP处理器的另一路输出端连接第二电机的输入端，从端DSP处理器的又一路输出端连接第三电机的输入端，第一电机的输出端连接从端DSP处理器的另一路输入端，第二电机的输出端连接从端DSP处理器的又一路输入端，第三电机的输出端连接从端DSP处理器的再一路输入端，第一电机的输出轴、第二电机的输出轴和第三电机的输出轴均连接导管；姿态传感器设置于导管上，其输出端连接上位机的另一路输入端。

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

步骤1、通过主手装置设置导管末端的目标位置，并将该目标位置发送至上位机中，将该位置变换到导管坐标系中确定目标位置坐标；

步骤2、采用设置于导管远端弯曲段的位姿传感器获得导管远端的初始姿态信息和导管末端的初始位置，并将上述数据发送至上位机中，所述的姿态信息包括导管远端弯曲角度、旋转角度和轴向位移；

步骤3、上位机根据导管末端的目标位置坐标和初始位置坐标，计算获得两者的偏差量，将偏差量与初始姿态信息发送至从端DSP处理器中，从端DSP处理器根据导管远端初始弯曲角度、初始旋转角度和初始轴向位移获得雅克比矩阵，再根据逆雅克比矩阵和导管末端目 标位置坐标与初始位置坐标的偏差量，将位置偏差量变换为导管的轴向距离偏差值、旋转角度偏差值和弯曲角度偏差值，并根据轴向距离偏差值计算第一电机的角位移量，发送至第一电机，根据旋转角度偏差值计算第二电机的角位移量，发送至第二电机，根据弯曲角度偏差计算第三电机的角位移量，发送至第三电机，带动导管运动；

步骤4、采用位姿传感器实时采集导管远端弯曲段的姿态信息和导管末端位置，并发送至上位机中，上位机根据实际导管末端位置，计算获得与目标位置的位置误差，将该位置误差和姿态信息发送至从端DSP处理器；

步骤5、从端DSP处理器计算获得导管末端的位置误差变化率和位置误差变化率的微分值；

步骤6、从端DSP处理器采用三维模糊控制器对导管末端位置误差、位置误差变化率和位置误差变化率的微分值进行离散化处理，并结合设定的论域范围对上述三个量进行模糊化处理，将模糊化后的变量进行模糊推理并采用重心法将推理所得结果去模糊化，即获得导管末端位置的补偿量；

步骤7、从端DSP处理器根据逆雅克比矩阵，将导管末端位置补偿量变换成导管的三个自由度的补偿量，即导管轴向位移补偿量、导管旋转角度补偿量、导管弯曲段弯曲角度补偿量，将导管轴向位移补偿量发送至第一电机，将导管旋转角度补偿量发送至第二电机，将导管弯曲段弯曲角度补偿量发送至第三电机，带动导管向目标位置移动；

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

步骤6所述的离散化处理，所采用的离散量化等级公式如下：

$E=\left\{\begin{array}{cc}\mathrm{nsgn}\left(e\right)& |e/M|\&GreaterEqual;a_1\\ (n-1)\mathrm{sgn}\left(e\right)& |e/M|\&GreaterEqual;a_2\\ .& .\\ .& .\\ .& .\\ 1\mathrm{sgn}\left(e\right)& |e/M|\&GreaterEqual;a_n\\ 0& |e/M|<a_n\end{array}\right.---\left(1\right)$

$\mathrm{EC}=\left\{\begin{array}{cc}\mathrm{nsgn}\left(\mathrm{ec}\right)& |\mathrm{ec}/M|\&GreaterEqual;b_1\\ (n-1)\mathrm{sgn}\left(\mathrm{ec}\right)& |\mathrm{ec}/M|\&GreaterEqual;b_2\\ .& .\\ .& .\\ .& .\\ 1\mathrm{sgn}\left(\mathrm{ec}\right)& |\mathrm{ec}/M|\&GreaterEqual;b_n\\ 0& |\mathrm{ec}/M|<b_n\end{array}\right.---\left(2\right)$

$\mathrm{ER}=\left\{\begin{array}{cc}\mathrm{nsgn}\left(\mathrm{er}\right)& |\mathrm{er}/M|\&GreaterEqual;c_1\\ (n-1)\mathrm{sgn}\left(\mathrm{er}\right)& |\mathrm{er}/M|\&GreaterEqual;c_2\\ .& .\\ .& .\\ .& .\\ 1\mathrm{sgn}\left(\mathrm{er}\right)& |\mathrm{er}/M|\&GreaterEqual;c_n\\ 0& |\mathrm{er}/M|<c_n\end{array}\right.---\left(3\right)$

其中，e表示导管末端的位置误差；ec表示导管末端的位置误差变化率；er表示导管末端的位置误差变化率的微分值；E表示模糊化处理后导管末端的位置误差；EC表示模糊化处理后导管末端的位置误差变化率；ER表示模糊化处理后导管末端的位置误差变化率的微分值；n为大于1的正整数；0<a_n<…<a₂<a₁<1，0<b_n<…<b₂<b₁<1，0<c_n<…<c₂<c₁<1；M为正整数；sgn(·)为符号函数，括号内变量大于0时，sgn(·)函数值为1，括号内变量等于0时，sgn(·)函数值为0，括号内变量小于0时，sgn(·)函数值为-1；所述的论域范围满足：E、EC和ER的论域均为{-n,-(n-1),…,-1,0,1,…,n-1,n}。

步骤6所述的模糊推理，推理规则公式如下：

$U=\left\{\begin{array}{cc}-<{\mathrm{\&alpha;}}_{1}E+(1-{\mathrm{\&alpha;}}_1\mathrm{EC})>& \left|E\right|\&GreaterEqual;E_a\\ -<{\mathrm{\&alpha;}}_{2}E+(1-{\mathrm{\&alpha;}}_2\mathrm{EC})>& E_b\&le;\left|E\right|<E_a\\ -<{\mathrm{\&alpha;}}_3{E}_i+(1-{\mathrm{\&alpha;}}_3\mathrm{EC})+(1-{2\mathrm{\&alpha;}}_3)\mathrm{ER}>& \left|E\right|<E_b\end{array}\right.---\left(4\right)$

其中，U是三维模糊控制器的模糊推理结果；E表示模糊化处理后导管末端的位置误差；符号<·>表示对括号内的数据进行四舍五入取整；α₁，α₂和α₃是加权因子，且0<α₃<α₂<α₁<1；E_a和E_b是误差的阈值，取值为整数，且0<E_b<E_a≤n，n为大于1的正整数。

本发明优点： 

本发明一种微创血管介入手术导管机器人三维模糊控制装置及方法，针对PID控制方法和二维模糊控制方法在控制导管操作机器人时存在的不足，提出了三维模糊控制方法，提高导管操作机器人的控制精度，进而提高对导管的操作精度，降低患者呼吸心跳等环境因素对导管运动的影响，减少操作者操作导管时产生的超调，使手术更精确、安全。

附图说明

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

图2为本发明一种实施例的可控导管示意图；

图3为本发明一种实施例的从端装置框图；

图4为本发明一种实施例的机器人机械装置示意图；

图5为本发明一种实施例的从端DSP处理器电路原理图；

图6为本发明一种实施例的DSP的供电电路原理图；

图7为本发明一种实施例的JTAG的电路原理图；

图8为本发明一种实施例的DSP外围晶体振荡器电路原理图；

图9为本发明一种实施例的DSP与上位机通信电路原理图；

图10为本发明一种实施例的电机控制信号处理电路原理图；

图11为本发明一种实施例的三个伺服电机的驱动电路原理图，其中，图（a）为第一电机驱动电路图，图（b）为第二电机驱动电路图，图（c）为第三电机驱动电路图；

图12为本发明一种实施例的三个伺服电机的编码器信号处理电路图，其中，图（a）为第一电机的编码器或第二电机的编码器信号处理电路图，图（b）为第三电机的编码器信号处理电路图；

图13为本发明一种实施例的三维模糊控制方法流程图；

图14为本发明一种实施例的主手装置到导管所处的坐标变换示意图；

图15为本发明一种实施例的导管远端弯曲段的运动学模型图；

图16为本发明一种实施例的三维模糊控制器结构。

具体实施方式

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

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

本发明实施例中，主手装置是一个三维的输入设备且具有较高的分辨率和良好的可操作性，本实施例中采用Falcon手柄，具有三个自由度，操作者通过主手给出导管末端目标位置。

本发明实施例中，上位机采用计算机实现，主要进行图像处理和人机交互，接收姿态传感器的测量数据，通过串口与从端DSP处理器通信；通过数字减影血管造影（DSA）形成血管结构的三维图像，操作者通过图像显示器，决定下一步的导管操作。

本发明实施例中，导管操作机器人（包括第一电机、第二电机、第三电机及其机械结构），它有三个自由度，与导管的三个自由度相对应，分别完成导管的进退、旋转和远端弯曲运动，进而实现对导管末端位置的控制。

本发明实施例中，如图2所示，导管由手柄、旋钮、鞘管段和可控弯曲段组成，导管内部有牵引钢丝，通过拉伸钢丝实现导管远端的弯曲，操作者控制导管操作机器人实现导管的进退、旋转和远端弯曲这三种运动。

本发明实施例中，姿态传感器采用Flexcord型号，获得导管末端的位置和姿态信息，并将这些信息通过传感器与上位机的接口发送至上位机。

如图3所示，本发明实施例中，从端DSP处理器采用TMS320F2812芯片，对接收的上位机的（位置误差和姿态）信息进行处理，即实现三维模糊控制算法，通过逆雅克比矩阵将导管的位置误差变换换成导管的进退、旋转和弯曲这三个量的误差，它们分别作为第一电机、第二电机和第三电机的角位置信号，本发明实施例中，三个电机均采用冯哈伯2657W024CR型直流伺服电机，伺服电机驱动机器人装置，使与之相连接的导管运动；三个伺服电机的输出轴上均设置有编码器（HEDS5540），编码器检测角位置信号，位置信号反馈给从端DSP处理器，实现对各个伺服电机的角位置控制。

如图4所示，导管操作机器人（包括第一电机、第二电机、第三电机及其机械结构），装置1是第一伺服电机和减速齿轮箱；2是线性导槽，槽内有丝杠；3是旋转平台，该平台内有第二伺服电机和减速齿轮箱；4是导管的手柄安装室；5是导管远端弯曲平台，其内有第三伺服电机和减速齿轮箱。各部分的功能如下：第一伺服电机转动，带动与之相连接的丝杠转动，使线性导槽上方的平台沿线性导槽进退，进而带动导管前进/后退；第二伺服电机转动，带动导管的手柄放置室转动，使导管手柄旋转，进而实现导管旋转；第三伺服电机转动，使导管手柄上的旋钮旋转，拉动导管内部的牵引钢丝伸缩，实现导管远端弯曲段弯曲角度的变化。

主手装置的输出端连接上位机的一路输入端，上位机的输出端连接从端DSP处理器的一路输入端，从端DSP处理器的一路输出端连接第一电机的输入端，从端DSP处理器的另一路输出端连接第二电机的输入端，从端DSP处理器的又一路输出端连接第三电机的输入端，第一电机的输出端连接从端DSP处理器的另一路输入端，第二电机的输出端连接从端DSP处理器的又一路输入端，第三电机的输出端连接从端DSP处理器的再一路输入端，第一电机的输出轴、第二电机的输出轴和第三电机的输出轴均连接导管；姿态传感器设置于导管上，其输出端连接上位机的另一路输入端。

如图5到图12所示，本发明实施例中各电路之间的连接如下所述：

本发明的一种实施例中，从端DSP处理器的型号为TMS320F2812，TMS320F2812（U1）处理器的管脚示意图如图5所示，它是控制电路的核心。

如图6所示，采用电压转换芯片U3（TPS73HD318）将外接电源VCC（+5V）转换成DSP的供电电源1.8V和3.3V；U3的22引脚和28引脚接TMS320F2812的160引脚，对U1进行上电复位；U3的5引脚、6引脚、11引脚、12引脚接电源VCC和电容C8一端，电容C8的另一端和U3的3引脚、4引脚、9引脚、10引脚接地；17引脚、18引脚、19引脚接电容C10一端，输出电压为3.3V，C10另一端接地；23引脚、24引脚、25引脚接电容C9一端，输出电压为1.8V，C9另一端接地；22引脚和27引脚之间接4.7K的电阻。

图7是JTAG接口电路，1引脚、2引脚、3引脚、7引脚、13引脚、14引脚分别接TMS320F2812的126引脚、135引脚、131引脚、127引脚、137引脚、146引脚；9和11引脚相连接，然后接TMS320F2812的136引脚；5引脚接电源VCC；4引脚、8引脚、10引脚、12引脚均接地；其中13引脚、14引脚分别连接10K的上拉电阻R1和R2，电阻另一端接3.3V电源。

图8是U1的外部时钟电路图，CYC为无源20M晶体振荡器，它的两个引脚分别接TMS320F2812的X1口（77引脚）和X2口（76引脚）。

图9是U1与上位机的串行通信电路，通信是基于RS-232串行接口总线的数据通信，使用MAX202（U2）实现电平转换，其中P1的2引脚、3引脚与U2的14引脚、13引脚连接，U2通过P1的2引脚向上位机发送信息，通过P1的3引脚接收上位机的信息，P1的5引脚接地；U2的1引脚与3引脚之间接电容C1，4引脚与5引脚之间接电容C2；U2的16引脚接电源VCC，2引脚接电容C4的正极端，C4的负极端接电源VCC，6引脚接电容C3的负极端，C3正极端接地；U2的11引脚和12引脚分别接TMS320F2812的串行通讯口SCIRXDA（157引脚）和SCITXDA（155引脚），实现从端与上位机的信息传递。

图10是U1的PWM信号处理电路。U4是八路缓冲器，用于限制U1的输入输出输出电 流。U4的输入端2引脚、4引脚、6引脚、8引脚、11引脚、13引脚分别接TMS320F2812的PWM信号输出口92引脚、93引脚、94引脚、95引脚、98引脚、101引脚；U4的输出端18引脚、16引脚、14引脚、12引脚分别接到U5的2引脚、4引脚、6引脚、8引脚，9引脚、7引脚分别接U6的2引脚、4引脚；U4的20引脚接电源VCC，1引脚、10引脚、19引脚接地。U5和U6都是光耦芯片，其中TLP5214是4个光耦的芯片，TLP5212是2个光耦的芯片；U5的1引脚、3引脚、5引脚、7引脚和U6的1引脚、3引脚分别接1K的电阻，各电阻另一端接3.3V电源；U5的10引脚、12引脚、14引脚、16引脚和U6的6引脚、8引脚接电源VCC；U5的信号输出引脚9引脚、11引脚、13引脚、15引脚和U6的信号输出引脚5引脚、7引脚分别接1K的电阻，电阻另一端接地；U5和U6实现信号的隔离。

本发明一种实施例的三个伺服电机的驱动电路的示意图如图11中图（a）图（b）和图（c）所示。伺服电机为直流伺服电机，型号为冯哈伯2657W024CR，均采用全桥驱动，每个全桥中的功率元件都是MOSFET，型号为IRF640。MOSET的驱动芯片都是IR2110。二极管D1、D2、D3、D4、D5、D6型号为FR157。U7的10引脚、U8的12引脚接U5的15引脚，U7的12引脚、U8的10引脚接U5的13引脚；U9的10引脚、U10的12引脚接U5的11引脚，U9的12引脚、U10的10引脚接U5的9引脚；U11的10引脚、U12的12引脚接U6的7引脚，U11的12引脚、U10的12引脚接U6的5引脚。除控制信号外，各伺服电机控制电路的其它接线均相同。以第一伺服电机为例描述驱动电路的连接。如图11中图（a）所示，在第一伺服电机的驱动电路中，U7的9引脚接电源VCC和电容C11正极端，C11的负极端与11引脚、13引脚接地；7引脚接电阻R16，R16另一端接VT1的栅极；1引脚接电阻R17，R17另一端接VT3的栅极；5引脚和2引脚分别接VT1和VT3的源极；6引脚接二极管D1，D1另一端接15V电源，3引脚接15V电源，2引脚与3引脚之间、5引脚与6引脚之间各接一个自举电容。U8的7引脚接电阻R18，R18另一端接VT2的栅极；1引脚接电阻R19，R19另一端接VT4的栅极；5引脚和2引脚分别接VT2和VT4的源极；3引脚、6引脚、9引脚、11引脚的连接方式与U7相同。

从端DSP处理器TMS320F2812控制各个伺服电机的角位置，需要接收并处理电机光电编码器的位置信号。本发明一种实施例的编码器信号处理电路如图12所示，编码器型号为HEDS5540。在图12中图（a）所示，第一伺服电机的光电编码器信号引脚A1、B1、Z1分别接U16的4引脚、5引脚、6引脚，第二伺服电机的光电编码器信号引脚A2、B2、Z2分别接U16的7引脚、8引脚、9引脚；U16实现电平的转换，U16的11引脚、12引脚、13引脚、14引脚、15引脚、16引脚分别接U1的60引脚、59引脚、57引脚、109引脚、107引脚、106引脚，1引脚和20引脚接3.3V电源，10引脚和19引脚接地，19引脚20引脚之间接电 容C31。由于TMS320F2812的正交编码接口有限，只能处理2个编码器信号，因此，采用芯片HCTL-2022（U14）扩展了一个正交编码接口，接收处理第三伺服电机的编码器信号，电路原理图如图12中图（b）所示。在图（b）中，U14的1引脚、12引脚分别接电源VCC和地；伺服电机三的编码器的引脚A3、B3、Z3分别接U14的10引脚、9引脚、11引脚；U14的2引脚、20引脚、19引脚、18引脚、15引脚、14引脚、13引脚、7引脚接U13的2引脚、3引脚、4引脚、5引脚、6引脚、7引脚、8引脚、9引脚；U14的4引脚、17引脚、5引脚、8引脚接U15的4引脚、5引脚、6引脚、7引脚。U13和U14都是实现电平转换的芯片。U13的1引脚、20引脚接3.3V电源，19引脚接地，电源和地之间接一个电容C29，10引脚接地；U13的11引脚、12引脚、13引脚、14引脚、15引脚、16引脚、17引脚、18引脚接U1的54引脚、39引脚、36引脚、33引脚、30引脚、27引脚、24引脚、21引脚。U15的1引脚、20引脚接3.3V电源，19引脚接地，电源和地之间接一个电容C30，10引脚接地；U15的13引脚、14引脚、15引脚、16引脚接U1的通用输入输出口48引脚、47引脚、46引脚、45引脚。这样，处理就可以接收处理三个光电编码器的电机角位置信息。

采用微创血管介入手术导管机器人三维模糊控制装置进行三维模糊控制的方法，方法流程图如图13所示，包括以下步骤：

步骤1、通过主手装置设置导管末端的目标位置，并将该目标位置发送至上位机中，将该位置变换到导管坐标系中确定目标位置坐标；

在操作过程中需要将主手装置给定的导管末端位置变换到从端导管所处空间坐标系中，主手坐标系到从端导管所处的坐标系变换示意图如图14所示。左边是主手装置的坐标系示意图，主手装置的手柄上下移动为Y_F轴，左右移动为X_F轴，前后移动为Z_F轴。右边是导管所处的空间坐标系示意图，Z轴是导管轴向进退方向，X轴是导管上下运动方向，Y轴是导管的左右运动方向。将手柄的X_F轴、Y_F轴、Z_F轴的正向分别对应到Z轴、X轴和Y轴的正向，得到的变换矩阵如下：

$R=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 1& 0& 0\end{array}\right]---\left(5\right)$

这样，就将主手提供的位置变换成导管末端的期望位置。

步骤2、采用设置于导管远端弯曲段的位姿传感器获得导管远端的初始姿态信息和导管末端的初始位置，并将上述数据发送至上位机系统中，所述的姿态信息包括导管远端弯曲角度α、旋转角度θ和轴向位移d₁；

步骤3、上位机根据导管末端的目标位置坐标和初始位置坐标，计算获得两者的偏差量，将偏差量与初始姿态信息发送至从端DSP处理器中，从端DSP处理器根据导管远端初始弯曲角度、初始旋转角度和初始轴向位移获得雅克比矩阵，再根据逆雅克比矩阵和导管末端目标位置坐标与初始位置坐标的偏差量，将位置偏差量变换为导管的轴向距离偏差值Δd₁（导管轴向位移变化量）、旋转角度偏差值Δθ（旋转角度变化量）和弯曲角度偏差值Δα（弯曲角度变化量），并根据轴向距离偏差值计算第一电机的角位移量，发送至第一电机，根据旋转角度偏差值计算第二电机的角位移量，发送至第二电机，根据弯曲角度偏差计算第三电机的角位移量，发送至第三电机，带动导管运动；

导管操作机器人实现对导管末端位置的控制，需要确定导管手柄处操作与导管末端运动之间的传递关系，即其运动学模型。导管在D-H坐标系中的运动学模型如图15所示，其中，O₀、X₀、Y₀、Z₀分别是导管远端模型中基座标的原点和三个直角坐标轴；O₁、X₁、Y₁、Z₁分别是模型中第一个坐标系的原点和三个直角坐标轴；O₁、X₂、Y₂、Z₂分别是模型中第二个坐标系的原点和三个直角坐标轴，（原点与第一个坐标系相同）；O₁、X₃、Y₃、Z₃分别是模型中第三个坐标系的原点和三个直角坐标轴，（原点与第一个坐标系相同）；O₄、X₄、Y₄、Z₄分别是模型中第四个坐标系的原点和三个直角坐标轴；O₄、X₅、Y₅、Z₅分别是模型中第五个坐标系的原点和三个直角坐标轴，（原点与第四个坐标系相同）；O₆、X₆、Y₆、Z₆分别是模型中末端坐标系的原点和三个直角坐标轴；d₁、θ和α分别是导管远端的轴向位移、旋转角度和弯曲段的弯曲角度；d₄和O_t分别是导管弯曲段对应的弦长和圆心；d₆是导管末端刚性部分的长度，为常量，本实施例中取为10毫米。

{O₀}表示导管远端基坐标系，{O₆}表示导管末端点坐标系，根据D—H法则可获得两个坐标系之间的齐次变换矩阵，具体变换过程可表示为{O₀}沿Z₀平移d₁，绕Z₁顺时针旋转θ角度，绕X₂顺时针转α/2角度，沿Z₃平移d₄，绕X₄顺时针转α/2角度，沿Z₅平移d₆。

两个坐标系之间的齐次变换矩阵如公式（6）：

$T_6^0=\left[\begin{array}{cccc}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\cos \mathrm{\&alpha;}& \sin \mathrm{\&theta;}\sin \mathrm{\&alpha;}& d_6\sin \mathrm{\&theta;}\sin \mathrm{\&alpha;}+d_4\sin \mathrm{\&theta;}\sin (\mathrm{\&alpha;}/2)\\ -\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\cos \mathrm{\&alpha;}& \cos \mathrm{\&theta;}\cos \mathrm{\&alpha;}& d_6\cos \mathrm{\&theta;}\sin \mathrm{\&alpha;}+d_4\cos \mathrm{\&theta;}\sin (\mathrm{\&alpha;}/2)\\ 0& -\sin \mathrm{\&alpha;}& \cos \mathrm{\&alpha;}& d_6\cos \mathrm{\&alpha;}+d_4\cos (\mathrm{\&alpha;}/2)+d_1\\ 0& 0& 0& 1\end{array}\right]---\left(6\right)$

导管末端原点O₆矩阵[x y z]^T在基坐标系{O₀}中的位置如公式（7）所示：

$\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}{d}_6\sin \mathrm{\&theta;}\sin \mathrm{\&alpha;}+d_4\sin \mathrm{\&theta;}\sin (\mathrm{\&alpha;}/2)\\ d_6\cos \mathrm{\&theta;}\cos \mathrm{\&alpha;}+d_4\cos \mathrm{\&theta;}\cos (\mathrm{\&alpha;}/2)\\ d_1+d_6\cos \mathrm{\&alpha;}+d_4\cos (\mathrm{\&alpha;}/2)\end{array}\right]---\left(7\right)$

导管可控弯曲段的长度在导管弯曲时是不变的，长为L，本实施例中取为45毫米。导管的弯曲段为圆弧，可以得出d₄与α之间的关系式：

$d_4=\frac{2L\sin (\mathrm{\&alpha;}/2)}{\mathrm{\&alpha;}}---\left(8\right)$

将式（8）代入式（7）中，关节变量变为三个：d₁、θ和α，分别对应导管的进退、旋转和弯曲运动，导管操作机器人通过改变这三个关节变量来控制导管末端的位置，由上述两式，可得到如式（9）所示的雅克比矩阵J，它表示导管末端位置变化与这三个关节变化量之间的关系，其中，以下各式中的s和c分别表示sin和cos：

$J=\left[\begin{array}{ccc}0& \mathrm{c\&theta;}(d_6\mathrm{s\&alpha;}+L(1-\mathrm{c\&alpha;})/\mathrm{\&alpha;})& \mathrm{s\&theta;}(d_6\mathrm{c\&alpha;}+\mathrm{Ls\&alpha;}/\mathrm{\&alpha;}-L(1-\mathrm{c\&alpha;})/{\mathrm{\&alpha;}}^2)\\ 0& -\mathrm{s\&theta;}(d_6\mathrm{s\&alpha;}+L(1-\mathrm{c\&alpha;})/\mathrm{\&alpha;})& \mathrm{c\&theta;}(d_6\mathrm{c\&alpha;}+\mathrm{Ls\&alpha;}/\mathrm{\&alpha;}-L(1-\mathrm{c\&alpha;})/{\mathrm{\&alpha;}}^2)\\ 1& 0& -d_6\mathrm{s\&alpha;}+\mathrm{Lc\&alpha;}/\mathrm{\&alpha;}-\mathrm{Ls\&alpha;}/{\mathrm{\&alpha;}}^2\end{array}\right]---\left(9\right)$

采用逆运动学来实现控制导管末端位置，除奇异点外，需要对雅克比矩阵求逆，雅克比逆矩阵J^-1如式（10）所示：

$J^{-1}=\left[\begin{array}{ccc}\frac{-\mathrm{s\&theta;}(-d_6\mathrm{s\&alpha;}+\frac{\mathrm{Lc\&alpha;}}{\mathrm{\&alpha;}}-\frac{\mathrm{Ls\&alpha;}}{\mathrm{\&alpha;}})}{d_6\mathrm{c\&alpha;}+\frac{\mathrm{Ls\&alpha;}}{\mathrm{\&alpha;}}-\frac{L(1-\mathrm{c\&alpha;})}{{\mathrm{\&alpha;}}^2}}& \frac{-\mathrm{c\&theta;}(-d_6\mathrm{s\&alpha;}+\frac{\mathrm{Lc\&alpha;}}{\mathrm{\&alpha;}}-\frac{\mathrm{Ls\&alpha;}}{\mathrm{\&alpha;}})}{d_6\mathrm{c\&alpha;}+\frac{\mathrm{Ls\&alpha;}}{\mathrm{\&alpha;}}-\frac{L(1-\mathrm{c\&alpha;})}{{\mathrm{\&alpha;}}^2}}& 1\\ \frac{\mathrm{c\&theta;}}{d_6\mathrm{s\&alpha;}+\frac{L(1-\mathrm{c\&alpha;})}{\mathrm{\&alpha;}}}& \frac{-\mathrm{s\&theta;}}{d_6\mathrm{s\&alpha;}+\frac{L(1-\mathrm{c\&alpha;})}{\mathrm{\&alpha;}}}& 0\\ \frac{\mathrm{s\&theta;}}{d_6\mathrm{c\&alpha;}+\frac{\mathrm{Ls\&alpha;}}{\mathrm{\&alpha;}}-\frac{L(1-\mathrm{c\&alpha;})}{{\mathrm{\&alpha;}}^2}}& \frac{\mathrm{c\&theta;}}{d_6\mathrm{c\&alpha;}+\frac{\mathrm{Ls\&alpha;}}{\mathrm{\&alpha;}}-\frac{L(1-\mathrm{c\&alpha;})}{{\mathrm{\&alpha;}}^2}}& 0\end{array}\right]---\left(10\right)$

求取导管各关节变量的变化量的公式如下：

$\left[\begin{array}{c}{\mathrm{\&Delta;d}}_1\\ \mathrm{\&Delta;\&theta;}\\ \mathrm{\&Delta;\&alpha;}\end{array}\right]=J^{-1}\left[\begin{array}{c}\mathrm{\&Delta;x}\\ \mathrm{\&Delta;y}\\ \mathrm{\&Delta;z}\end{array}\right]---\left(11\right)$

其中，Δd₁表示导管轴向位移变化量，Δθ表示旋转角度变化量，Δα表示弯曲角度变化量；Δx表示导管末端位置Z轴方向变化量，Δy表示导管末端位置Y轴方向变化量，Δz表示导管末端位置Z轴方向变化量。

通过式（11）把导管末端位置的变化转换成导管三个变量的变化，然后通过控制导管操作机器人来改变这三个变量的大小，在三维图像的引导下，操作导管末端运动到目标位置处。

步骤4、采用位姿传感器实时采集导管远端弯曲段的姿态信息和导管末端位置，并发送至上位机中，上位机根据实际导管末端位置，计算获得与目标位置的位置误差，将该位置误差和姿态信息发送至从端DSP处理器；

步骤5、从端DSP处理器计算获得导管末端的位置误差变化率和位置误差变化率的微分值，并将位置误差、位置误差变化率和位置误差变化率的微分值作为从端DSP处理器内部三维模糊控制器的输入；本发明一种实施例的三维模糊控制器结构图如图6所示，该控制方法由从端DSP处理器来实现。

步骤6、三维模糊控制器根据离散量化等级公式和设定的论域范围对位置误差、位置误差变化率和位置误差变化率的微分值进行模糊化处理，将模糊化后的变量按照推理规则公式进行计算获得结果，再采用重心法将推理所得结果去模糊化，即获得导管末端位置的补偿量；

本发明实施例中，位置误差e、位置误差的变化率ec、位置误差变化率的微分er，把它们作为所述的三维模糊控制器的输入，然后进行模糊化，模糊化的变量分别是E、EC、ER。其中，E、EC、ER的论域均为{-5,-4,-3,-2,-1,0,1,2,3,4,5}，模糊化规则如下：

$E=\left\{\begin{array}{cc}5\mathrm{sgn}\left(e\right)& |e/4|\&GreaterEqual;0.80\\ 4\mathrm{sgn}\left(e\right)& |e/4|\&GreaterEqual;0.50\\ 3\mathrm{sgn}\left(e\right)& |e/4/\&GreaterEqual;0.30\\ 2\mathrm{sgn}\left(e\right)& |e/4|\&GreaterEqual;0.10\\ 1\mathrm{sgn}\left(e\right)& |e/4|\&GreaterEqual;0.05\\ 0& |e/4|<0.05\end{array}\right.---\left(12\right)$

$\mathrm{EC}=\left\{\begin{array}{cc}5\mathrm{sgn}\left(\mathrm{ec}\right)& |\mathrm{ec}/4|\&GreaterEqual;0.60\\ 4\mathrm{sgn}\left(\mathrm{ec}\right)& |\mathrm{ec}/4|\&GreaterEqual;0.35\\ 3\mathrm{sgn}\left(\mathrm{ec}\right)& |\mathrm{ec}/4/\&GreaterEqual;0.20\\ 2\mathrm{sgn}\left(\mathrm{ec}\right)& |\mathrm{ec}/4|\&GreaterEqual;0.10\\ 1\mathrm{sgn}\left(\mathrm{ec}\right)& |\mathrm{ec}/4|\&GreaterEqual;0.05\\ 0& |\mathrm{ec}/4|<0.03\end{array}\right.---\left(13\right)$

$\mathrm{ER}=\left\{\begin{array}{cc}5\mathrm{sgn}\left({\mathrm{er}}_i\right)& |\mathrm{er}/4|\&GreaterEqual;0.10\\ 4\mathrm{sgn}\left(\mathrm{er}\right)& |\mathrm{er}/4|\&GreaterEqual;0.07\\ 3\mathrm{sgn}\left(\mathrm{er}\right)& |\mathrm{er}/4|\&GreaterEqual;0.04\\ 2\mathrm{sgn}\left(\mathrm{er}\right)& |\mathrm{er}/4|\&GreaterEqual;0.02\\ 1\mathrm{sgn}\left(\mathrm{er}\right)& |\mathrm{er}/4|\&GreaterEqual;0.01\\ 0& |\mathrm{er}/4|<0.01\end{array}\right.---\left(14\right)$

对模糊化后的变量进行模糊推理，获得推理结果，本发明中三维模糊控制器的推理规则 如下：

$U=\left\{\begin{array}{cc}-<{\mathrm{\&alpha;}}_{1}E+(1-{\mathrm{\&alpha;}}_1\mathrm{EC})& \left|E\right|=\mathrm{5,4}\\ -<{\mathrm{\&alpha;}}_{2}E+(1-{\mathrm{\&alpha;}}_2\mathrm{EC})& \left|E\right|=\mathrm{3,2}\\ -<{\mathrm{\&alpha;}}_{3}E+(1-{\mathrm{\&alpha;}}_3\mathrm{EC})+(1-2{\mathrm{\&alpha;}}_3)\mathrm{ER}>& \left|E\right|=\mathrm{1,0}\end{array}\right.---\left(15\right)$

上式中，U是模糊推理的结果，符号<·>是对其中的数据进行四舍五入取整，α₁，α₂和α₃是加权因子，在不同的阶段取不同的值，本发明实施例中α₁，α₂和α₃分别为0.8、0.65和0.4。

步骤7、从端DSP处理器根据逆雅克比矩阵，将导管末端位置补偿量变换成导管的三个自由度的补偿量，即导管轴向位移补偿量、导管旋转角度补偿量、导管弯曲段弯曲角度补偿量，将导管轴向位移补偿量发送至第一电机，将导管旋转角度补偿量发送至第二电机，将导管弯曲段弯曲角度补偿量发送至第三电机，带动导管向目标位置移动；

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

Claims (2)

1.一种微创血管介入手术导管机器人三维模糊控制装置，其特征在于：包括主手装置、上位机、从端DSP处理器、第一电机、第二电机、第三电机和姿态传感器，其中，

主手装置：用于设置导管末端的目标位置，并将该目标位置发送至上位机中；

上位机：

(1)当接收到导管末端的初始位置信号时，用于根据导管末端的目标位置和初始位置，计算获得两者的偏差量；将偏差量与初始姿态信息发送至从端DSP处理器中；

(2)当接收到实时采集的导管末端位置信号时，用于根据实际导管末端位置，计算获得与目标位置的位置误差，将该位置误差和导管的实时姿态信息发送至从端DSP处理器；

从端DSP处理器：

(1)当接收到导管末端目标位置与初始位置的偏差量和初始姿态信息时，用于根据导管远端弯曲段的初始姿态信息获得雅克比矩阵，再根据逆雅克比矩阵和导管末端目标位置与初始位置的偏差量，将位置偏差量变换为导管的轴向距离偏差值、旋转角度偏差值和弯曲角度偏差值，并将上述三个量依次转换为第一电机的角位移量、第二电机的角位移量和第三电机的角位移量，控制电机带动导管运动；

(2)当接收到导管末端的位置误差信号和导管的实时姿态信息时，用于计算获得导管末端的位置误差变化率和位置误差变化率的微分值，并将位置误差、位置误差变化率和位置误差变化率的微分值作为其内部的三维模糊控制器的输入，三维模糊控制器根据推理规则公式进行计算获得导管末端位置的补偿量，并根据逆雅克比矩阵，将导管末端位置补偿量变换成导管轴向位移补偿量、导管旋转角度补偿量和导管弯曲段弯曲角度补偿量，将上述三个量依次发送至第一电机、第二电机和第三电机；

第一电机：用于控制导管的轴向位移；

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

第三电机：用于控制导管的弯曲角度；

姿态传感器：用于采集导管远端弯曲段的轴向位移、旋转角度和弯曲角度。

2.根据权利要求1所述的微创血管介入手术导管机器人三维模糊控制装置，其特征在于：主手装置的输出端连接上位机的一路输入端，上位机的输出端连接从端DSP处理器的一路输入端，从端DSP处理器的一路输出端连接第一电机的输入端，从端DSP处理器的另一路输出端连接第二电机的输入端，从端DSP处理器的又一路输出端连接第三电机的输入端，第一电机的输出端连接从端DSP处理器的另一路输入端，第二电机的输出端连接从端DSP处理器的又一路输入端，第三电机的输出端连接从端DSP处理器的再一路输入端，第一电机的输出轴、第二电机的输出轴和第三电机的输出轴均连接导管；姿态传感器设置于导管上，其输出端连接上位机的另一路输入端。