张忠林 熊 璟 张东文

(中国科学院深圳先进技术研究院 深圳 518055)

主从同构穿刺手术机器人遥操作控制策略研究

张忠林 熊 璟 张东文

(中国科学院深圳先进技术研究院 深圳 518055)

文章在分析了穿刺机器人系统功能需求的基础上，搭建了主从遥操作系统的半实物仿真平台。基于等效微分变换的思想，提出雅克比矩阵(Jacobian Matrix)方法和比例微分(Proportional-Derivative，PD)控制律的联合控制方法。通过设计数字滤波器来消除外科医生的手部低频抖动对穿刺手术机器人精度的影响。实验结果表明从机器人末端执行器在笛卡尔空间坐标下能够精确快速安全地跟随主机器人末端执行器的位置变化，并且主机器人端的手部抖动能够被消除。

穿刺手术机器人；主从控制；雅克比矩阵；PD 控制算法；抖动消除

1 引 言

遥操作穿刺手术是通过专用手术器械插入患者体内，对患者体内的病灶进行手术操作的外科手术。穿刺手术具有创伤小、痛苦小、康复快和术后并发症少等优点，是当前主要发达国家竞相研究的医学领域之一[1]。目前已有很多较为成熟的医疗遥操作系统，如美国 Computer Motion 公司研发的 AESOP 腹腔镜操作机器人[2]、Zeus 微创手术机器人系统[3]以及 Intuitive Surgical 公司研发的 Da Vinci 微创手术机器人系统[4]是较具代表性的医疗遥操作系统。它们不仅为医生提供了先进的视觉反馈，而且让手术医生拥有了更加灵活的操作方式，在医疗手术机器人领域具有革命性的意义。

穿刺手术主从机器人系统是一个拥有主从控制器的遥操作装置。近年来，在穿刺手术机器人遥操作研究中，许多研究学者都尝试通过反馈信息(如速度、加速度、力和视频等)来进行主从系统的精确跟随控制研究，从而建立高精度的遥操作系统[5]。虽然通过上述反馈信息构建的反馈控制系统精度很高，但是系统较为复杂，而且响应速度未达理想要求，如当速度或加速度很快时，其控制性能就可能会恶化，甚至不稳定。此外，潜在的可变时延问题也可能会导致系统的不稳定。

本文主要针对穿刺手术遥操作控制策略、抖动消除等进行了相关半实物仿真研究。本文方法的优点在于利用较为简单的控制系统结构，获得较好的控制精度和响应速度，且易于在实验中实现。

2 遥操作机器人构型

穿刺手术机器人系统采用主从控制结构，外科医生通过操纵主机器人(主手)来实现对从机器人(从手)的控制，从而完成相关手术任务。本系统机器人采用的设备是 SensAble 公司生产的PHANToM OMNI[5]，如图 1 所示。PHANToM OMNI 是一款具有6 自由度的小型机器人，其所有关节都为旋转关节，前三个关节控制机器人末端位置坐标，后三关节采用三轴交汇的方式控制机器人姿态坐标。主手工作空间即为点 P 的活动空间。另外，本文中所有坐标均指在笛卡尔空间下的坐标。

3 辅助穿刺遥操作系统

主从式控制系统在医疗机器人遥操作系统中处于核心的地位，起着协调主从手、监控手术对象和为操作者提供手术操作信息的作用[6]。手术医生通过主从将操作主手、主手的位置和速度信息映射传递给病灶端的从手，从而实现主从手的快速、精确跟随。

本文穿刺手术机器人主从控制系统如图 2 所示。其中，Xm和 Xs分别表示主手和从手末端执行器在空间坐标中的位置坐标；和分别表示主从手末端执行器在空间坐标中的速度矢量；θm和 θs分别表示从手关节角度和关节角速度矢量；为从手逆雅克比矩阵；LPF 为低通数字滤波器。

图 1 PHANToM OMNI 初始位置信息Fig. 1. PHANToM OMNI robot and its initial position

图 2 主从控制系统框图Fig. 2. Block diagram of master-slave robot control system

3.1 主从控制策略

本文通过笛卡尔空间控制来建立主从机器人之间的联系，笛卡尔空间控制的重点在于逆雅克比矩阵的运算。本文对雅克比矩阵的运算采用微分变换思想，即利用微小时间段内的位移代替瞬时速度的方法对其进行运算。通过逆雅克比矩阵的运算可以得到逆运动学唯一解，从而降低了运动学计算的复杂程度，且系统的实时性也得到了提高。

雅克比矩阵是机器人关节空间速度向机器人末端笛卡尔空间速度的映射，也可以看成是将机器人关节速度与机器人末端在笛卡尔空间的速度联系起来，其表达式为：

因为计算机控制是一种采样控制，只能根据采样时刻的偏差值计算控制量，所以可利用机器人末端和关节角微小时间段内的位移和分别代替瞬时末端速度和关节速度，从而式(1)运动学问题还可表示为：

由式(2)得机器人逆运动学问题求解关系：

根据公式(3)，可得到机器人笛卡尔空间坐标系下末端位置速度到关节速度的转换。然而，由于逆雅克比矩阵是相对于局部空间位置的映射，随着机器人在其工作空间的运动，主从跟随误差就会不断积累，从而降低跟随精度，导致穿刺手术失败。为了消除这种积累误差，本系统引入了比例微分(Proportional Derivative，PD)反馈控制环节。

PD 闭环控制可以较好地消除上述主从跟随误差，所以我们在此引入了闭环 PD 控制算法，将末端执行器的位姿坐标与目标位姿坐标信息比较后的误差送到 PD 控制器，进而消除这种误差。通过调节比例微分系数 kp、kv来使系统能够迅速地达到稳状态，最终使从手末端执行器位姿能够精确、迅速地跟随主手末端执行器位姿坐标变化。PD 控制律如下：

3.2 关节速度求解与抖动消除

PHANToM OMNI 关节速度的求解过程如图3 所示。通过对机器人的任何一个关节引入一个延迟模块，可以得到当前时刻关节角度的输出与上一时刻输出的差量，即可得到机器人此关节的速度；

图 3 关节速度求解框图Fig. 3. The block diagram of solving joint velocity

在手术过程中医生手部难免会发生抖动，而在长时间的手术过程中医生疲劳度增加，手部抖动会更加明显。这些无关的抖动通过主从映射到从手的运动上，最终会影响手术精度[7]。本系统首先对主手采样数据进行一次滑动均值滤波，来减小外科医生手部抖动对从手的影响，然后再对通过主从映射得到的从手关节角度数据信息进行二次滤波，最终有效消除抖动。

滑动均值算法对周期性的干扰具有较好的抑制作用。当系统在一定时间内进行连续采样时，只需进行一次采样就可对这一次测量数据进行计算。该算法极大地提高了测量数据的计算速度，满足快速控制的实时性要求。本算法详细叙述如下：

在第 i 次采样周期中，每进入下一步计算之前，将采样得到的每一个离散点先利用式(6)进行计算，得到均值采样结果，然后用计算结果来代替。

4 半实物仿真

为了验证本文方法的有效性，利用上述主从控制方案分别进行主从跟随的半实物仿真实验。本半实物仿真实验在带有 PHANToM 工具箱[8]的MATLAB Simulink 实时仿真环境下进行。

4.1 反馈消除误差仿真

图 4 为引入 PD 反馈控制环节前后主从跟随误差效果。从图中的半实物仿真结果可以看出，引入 PD 环节的控制系统能够有效地消除由于逆雅克比矩阵运算而产生的积累误差，从而能够满足穿刺手术任务的要求。

图 4 主从运动跟随误差Fig. 4. Master-slave tracking error

4.2 抖动消除仿真

本系统进行抖动滤波时，取 n＝15，也即所设计数字滤波器为 15 阶滤波器。图 5 所示为从手末端的抖动滤波效果图。其中，图 5(a)表示抖动消除前的从手末端运动轨迹，结果显示其有较多的抖动毛刺；图 5(b)表示经过平滑数字滤波器进行抖动滤波后的运动轨迹，可以看出其运动曲线毛刺明显减少，也即是外科医生手部的无关抖动得到了显著消除。

图 5 从手末端抖动消除前后效果图Fig. 5. Tremor elimination results of slave end-effector

4.3 主从跟随控制仿真

在进行主从跟随半实物仿真实验时，从手是在无约束的条件下跟随主手运动。图 6 和图 7 为主从手末端执行器的位姿跟随效果图。图中分别给出了主从手在 X、Y 和 Z 方向的位置随时间变化情况。在半实物仿真过程中的刚开始的几秒内，操作者比较快速度地操作主手运动，直到20 秒左右时，再以较慢的速度操作主手运动。

图 6 主从位置跟随Fig. 6. Master-slave position following

从图 6 和图 7 可以看出，无论主手是在低速运动还是高速运动，穿刺手术机器人从手末端在笛卡尔空间下的 X、Y 和 Z 方向上的位姿坐标都能够平稳、迅速地跟随主手末端的位姿坐标变化，说明了本系统控制策略的合理性，基本能够满足穿刺手术操作的任务要求。

图 7 主从姿态跟随Fig. 7. Master-slave posture following

本文为主从同构机器人的控制，其主从关节一一对应，所以选择简单的关节—关节控制即可获得较为理想的控制效果。但是，本文目的是为下一步进行主从异构机器人控制研究做理论准备，而主从异构机器人关节并非一一对应关系，不适合采用简单的关节控制方法，因此主从机器人需要通过运动学正、逆运算才能建立联系。逆运动学的求解方法有很多，比较典型的有 Pieper解法[9]、Lee 和 Ziegler 提出的几何法[10]以及 Paul等[11]提出的反变换法等。然而，这些方法得到的关节变量表达式可能含有超越函数，使计算较为复杂且会产生多个解，从而需要对这些解进行实时最优选取，这严重影响了系统的实时性及控制精度。

因此，本文提出了逆雅克比矩阵运算的方法，逆雅克比矩阵运算能得到唯一解，提高了系统实时性；同时又引入 PD 闭环控制环节来消除了逆雅克比矩阵运算引起的积累误差。半实物仿真结果充分说明本文控制策略的合理性，从而为下一步主从异构穿刺机器人遥操作控制的研究提供了理论基础。

5 总 结

为了满足穿刺手术机器人主从运动控制的精确、快速以及稳定性，本文提出了基于逆雅克比矩阵运算法和 PD 闭环控制的主从控制策略。半实物仿真结果验证了算法的可行性，基本能够满足穿刺手术任务的要求。但本文研究只涉及运动学方面，并没有虚拟力反馈和动力学方面的研究，因此下一步工作不仅仅要在实物平台验证算法的可行性，还要进行虚拟向导和动力学方面的研究，最终为主从异构穿刺手术机器人遥操作控制的研究做好理论准备。

[1] Kazanzides P, Fichtinger G, Hager GD, et a1. Surgical and interventional robotics-core concepts, technology, and design [J]. IEEE Robotics and Automation Magazine, 2008, 15(2): 122-130.

[2] Shew SB, Ostlie DJ, Holcomb GW. Robotic telescopic assistance in pediatric laparoscopic surgery [J]. Pediatric Endosurgery and Innovative Techniques, 2003, 7(4): 371-376.

[3] Lanfranco AR, Castellanos AE, Desai JP, et al. Robotic surgery: a current perspective [J]. Annals of Surgery, 2004, 239(1): 14-21.

[4] Broeders I, Ruurda J. Robotics revolutionizing surgery: the intuitive surgical “Da Vinci” system [J]. Industrial Robot, 2001, 28(5): 387-392.

[5] Silva AJ, Ramirez OAD, Vega VP, et al. PHANToM OMNI haptic device: kinematic and manipulability [C] // Robotics and Automotive Mechanics Conference on Electronics, 2009: 193-198.

[6] Aliaga I, Rubio A, Sánchez E. Experimental quantitative comparison of different control architectures for master-slave teleoperation [J]. IEEE Transactions on Control Systems Technology, 2004, 12(1): 2-11.

[7] Gonzalez JG, Heredia EA, Rahman T, et al. Optimal digital fi ltering for tremor suppression [J]. IEEE Transactions on Biomedical Engineering, 2000, 47(5): 664-673.

[8] Mohammadi A, Tavakoli M, Jazayeri A. PHANTSIM: a simulink toolkitfor the sensable PHANToM haptic devices [C] // Proceedings of the 23rd CANCAM, 2011: 787-790.

[9] Pieper DL.The kinematics of manipulators under computer control [D]. Stanford: Stanford University, 1968.

[10] Lee CSG, Ziegler M. A geometric approach in solving the inverse kinematics of PUMA robots [J]. IEEE Transactions on Aerospace and Electronic Systems, 1984, 20(6): 695-706.

[11] Paul RP, Shimano B, Mayer GE. Kinematic control equations for simple manipulators [J]. IEEE Transactions on Systems, Man and Cybernetics, 1981, 11(6): 449-455.

Study on Teleoperation Control Strategy of Master-Slave Isomorphic Robots in Puncture Surgeries

ZHANG Zhonglin XIONG Jing ZHANG Dongwen

( Shenzhen Institutes of Advanced Technology, Chinese Academy of Sciences, Shenzhen 518055, China )

In this paper, a semi-physical simulation platform of the master-slave robot control simulation system was built based on the analysis on functional requirements of the PHANToM OMNI robot system. Based on differential transformation method, the real-time master-slave control was executed by using Jacobian matrix and proportionalderivative(PD) close-loop control algorithm, and the digital fi lter was designed to eliminate effects of surgeons’ hand lowfrequency tremors on the precision of the master-slave control. The simulation experimental results demonstrate that the slave robot can follow precisely and quickly movements of the master robot and eliminate hand tremors.

teleoperation; master-slave control; Jacobian matrix; PD control algorithm; tremor elimination

TP 242

A

2013-11-20

张忠林，硕士研究生，研究方向为微创手术机器人的主从控制；熊璟(通讯作者)，助理研究员，研究方向为医疗机器人与机器人机构学等，E-mail：jing.xiong@siat.ac.cn；张东文，工程师，研究方向为医疗机器人等。