穿刺手术机器人遥操作系统的控制策略研究

张忠林，陈 以，俞 益(桂林电子科技大学 电子工程与自动化学院，广西 桂林541004)

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

穿刺手术主从机器人系统作为一个拥有主从控制方式的遥操作装置，近年来，在穿刺手术机器人遥操作研究中，其系统结构也从主从同构形式发展成了主从异构形式。主从异构机器人实时控制需要系统能够在短时间内完成机器人的正逆运动学计算、误差消除、抖动消除等。

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

1 遥操作主从机器人构型

穿刺手术机器人系统采用主从式控制结构，外科医生通过操纵主机器人（主手）来实现对从机器人（从手）的控制。本系统的主机器人采用的是SensAble公司的Phantom Omni[5]，具有 6自由度(DOF)，所有关节都为旋转关节，前3个关节控制机器人末端位置，后3个关节以三轴交汇的方式控制机器人姿态，如图1(a)所示。另外，本文所有坐标均是指在笛卡尔空间下的坐标。从机器人作为穿刺手术系统的执行部分负责完成手术操作，拥有6DOF，且都是旋转关节，前3个关节配合实现从机器人末端定位；后3个关节以三轴交汇的方式实现末端姿态调整，如图1(b)所示。

2 辅助穿刺遥操作系统

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

图1 主从机器人构型

主从控制系统框图如图 2所示，其中 Xm、Xg分别表示主手和从手末端执行器在笛卡尔空间坐标中的位置坐标；X˙m、X˙s分别为主从手末端执行器在笛卡尔空间坐标中的速度矢量；θs、θ˙s分别表示从手关节角度和关节角速度矢量；为从手逆雅克比矩阵；k为主从映射比例系数；LPF为低通数字滤波器。

图2 主从控制系统框图

2.1 主从控制策略的选取

一般情况下，主从控制方案的选择与主从手的结构相关。主从同构型的机器人，主从手关节数目相同且一一对应，利用简单的关节-关节控制就能实现快速精确的主从控制。然而对于主从异构型的机器人，由于主从手各关节不再一一对应，不能利用关节控制来解决，必须引入主从手的运动学正、逆运算，在笛卡尔空间坐标系中建立起映射关系。

笛卡尔空间坐标下的主从控制的关键在于逆运动学的求解，逆运动学求解有多种方法[6-8]，但是这些方法得到的关节变量表达式可能含有超越函数，计算较为复杂，且会产生多个解，需要对这些解进行实时最优选取，影响了系统的实时性。本文基于微分变换的思想，利用逆雅克比矩阵进行逆运动学求解，得到唯一解，降低了运动学计算量，且实时性也得到了提高。

雅克比矩阵能将机器人关节速度与末端在笛卡尔空间的速度联系起来，也可称为机器人关节空间速度向末端笛卡尔空间速度的映射，表示为：

其 中 ，θ˙∈R6×1为 关 节 角 速 度 矢 量 ，J(θ)∈R6×6为 雅 克 比矩阵，X˙∈R6×1为末 端速度矢量。

将机器人末端和关节角在微小时间段内的位移△X和△θ分别代替瞬时末端速度和关节速度，则式(1)又可表示为：

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

其中，J(θ)-1为逆雅克比矩阵。

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

在PD控制环节，通过调节比例微分系数 kp、kd来使系统能够迅速地达到稳状态，最终使得从手末端执行器位姿能够精确、迅速地跟随主手末端执行器位姿坐标变化。PD控制律如下：

其中，X˙m、X˙s分别表示主、从手末端执行器位姿速度，Xm、Xs分别表示主、从手末端执行器位姿。

2.2 主从控制中的抖动消除

在手术过程中医生手部难免发生抖动，特别是长时间手术时手部抖动会更加显著。这些无关抖动通过主从映射会反映到从手的运动上，进而会影响手术精度。本系统首先对主手采样数据进行一次滑动均值滤波，然后再对从手关节角度数据信息进行二次滤波，这样将有效滤除抖动。

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

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

其中，N′i为滤波计算结果，n为均值数字滤波器阶数(i≥n)。

3 半实物仿真

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

3.1 反馈消除误差仿真

图3所示为引入PD反馈控制环节前后主从跟随误差效果。从图中可以看出，引入PD环节后的控制系统能够有效地消除系统积累误差。

图3 主从运动跟随误差

3.2 抖动消除仿真

进行抖动滤波时，取n=15，即滑动均值滤波器为15阶滤波器。图4所示为从手末端的抖动滤波效果图，图4(a)表示抖动消除前的从手末端在Z轴的位移，可以看出其有较多的抖动毛刺；图4(b)表示经过平滑数字滤波器进行抖动滤波后的运动轨迹，可以看出其运动曲线毛刺明显减少，即外科医生手部的无关抖动得到了显著消除。

图4 从手末端抖动消除前后效果图

3.3 主从跟随控制仿真

在进行主从跟随半实物仿真时，设定主从映射比例k=1，即从手运动轨迹完全一样。通过对PD控制参数的调节，当 kp=0.3、kd=0.001时，能得到较好的跟随效果，结果如图 5所示(由于在 X、Y、Z方向的主从手情况类似，故只给出在X方向的跟随情况)。此外在实验刚开始的几秒钟里，先缓慢地操纵主手，大概13 s后再加快主手运动速度。

图5 主从位置跟随和跟随误差图

从图5(a)可以看出，无论主手运动速度快慢，从手都能够平稳、快速、精确地跟随主手运动。从图5(b)可以看出，主从手跟随误差不超过0.5 mm，说明了本系统控制算法的合理性。

为了满足穿刺手术机器人主从运动控制的精确、快速及稳定性，本文提出了基于雅克比矩阵和PD控制律的主从控制算法，半实物仿真结果验证了算法的可行性，基本能够满足穿刺手术任务的要求。本文研究只涉及运动学方面，并没有虚拟力反馈和动力学方面的研究，下一步工作是进行虚拟向导和动力学方面的研究。

[1]KAZANZIDES P，FICHTINGER G，HAGER G D，et a1.Surgical and interventional robotics-core concepts,technology，and design[J].IEEE Robotics and Automation Magazine，2008，15(2)：122-130.

[2]SHEW S B，OSTLIE D J，HOLCOMB G W.Robotic telescopic assistance in pediatric laparoscopic surgery[J].Pediatric Endosurgery and Innovative Techniques，2003，7(4)：371-376.

[3]LANFRANCO A R，CASTELLANOS A E，DESAI J P，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-391.

[5]SANSANAYUTH J，NILKHAMHANG I，TUNGPIMOLRAT K.Teleoperation with inverse dynamics control for Phantom Omni haptic device[C].SICE Annual Conference，Akita University，Akita，Japan，2012：2121-2126.

[6]PIEPER D L.The kinematics of manipulators under computer control[D].Stanford，USA：Stanford University，1968.

[7]LEE C S G，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.

[8]PAUL R P，SHIMANO B，MAYER G E.Kinematic control equations for simple manipulators[J].IEEE Transactions on Systems，Man，and Cybernetics，1981，11(6)：449-455.

[9]MOHAMMADI A，TACAKOLI M，JAZAYERI A.Phantsim：a simulink toolkit for the sensable Phantom haptic devices[C].Proceedings of the 23rd CANCAM，Canada，2011：787-790.