摘 要：为使医生能够根据手术机器人提供的腹腔镜视觉环境，进行符合视觉习惯的手术操作，通过深入研究微创腹腔手术机器人系统的持镜臂和器械臂运动学模型，以器械臂相对于持镜臂视觉坐标系的运动学反解为基础，建立一种基于3D动态视觉窗口引导的器械臂运动算法．最后使用Matlab软件Simulink和SimMechanics工具箱分别进行基于理论算法和机构模型的仿真实验，仿真结果显示运动算法的理论变化量和实际变化量均相等．通过多组数据说明了器械臂运动方法的正确性和可行性．
关键词：外科手术机器人；运动方法；动态视觉引导；仿真分析
A Kinematics Method Based on Dynamic Visual Navigationfor the Instrument Arm of Surgical Robot
Abstract:To enable doctors to perform surgical operations based on their visual habits according to the laparoscopicvisual environment provided by the surgical robot, the kinematic model of the laparoscope arm and the instrument arm ofthe Minimally Laparoscopic Surgical Robotic System (MLSRS) is studied, and a new kinematics method of instrument armbased on the 3D dynamic visual window navigation is proposed according to the inverse kinematics of the instrument armwith respect to the visual coordinate system of the laparoscope arm. At last, simulation experiments based on the theoreticalalgorithm and the mechanism model are carried out with Simulink and SimMechanics toolboxes in Matlab, and the simulationresults show that the theoretical and actual changes of the kinematics algorithm are equal. Furthermore, multiple sets of dataare presented to illustrate the correctness and the feasibility of the kinematics method of the instrument arm.
Keywords:surgical robot; kinematics method; dynamic visual navigation; simulation analysis
　　1 引言（Introduction）
　　随着微创外科手术机器人的不断发展，机器人腹腔镜手术的应用也愈加广泛，同时医生在手术过程中的感知和操作方式也发生改变[1]，微创腹腔手术机器人系统（MLSRS）结构和功能的发展需要尽可能适于手术医生的人机操作一体化．目前最知名的商业化外科手术机器人系统是美国Intuitive Surgical 公司的da Vinci系统[2]，但其体积庞大、价格昂贵、术前准备时间长．开发结构紧凑、轻便、价廉的下一代MLSRS已成为该领域的研究热点．德国航空航天中心机器人与机械电子研究所研制出新一代通用型手术机器人系统DLRMIRO，可以在非结构环境下进行手术操作，极大地降低了医生对手术操作空间的需求[3]．美国华盛顿大学和加州大学圣克鲁兹分校研制出一款具有鸟翼状机械臂的医用机器人Raven系统，同时也是最先使用开源软件的手术机器人[4]．手术机器人的发展不仅在于其结构和控制系统本身，还在于建立更为完善的机器人操作臂的运动方法，以适应医生的操作习惯和实际需求，从而更好地完成手术操作．Weede等建立了一种基于马尔可夫模型的智能视觉导航运动方法，能够预测腹腔镜的运动点位[5]，在持镜臂自主运动研究中具有较高价值．
　　Staub等人基于研制的手术机器人提出一种CCD（contractingcurve density）算法，通过分辨微器械局部轮廓的色差来获得更高精度的位姿信息，综合利用编码器和视觉反馈信息，可以在图像空间中进行稳定的镜头推进[6]，但在较暗背景下，由于微器械自身颜色的影响，此算法的追踪将会失效．Tully等人研制出一款蛇形手术机器人HARP，并提出一种估算多关节蛇形手术机器人的形状和末端位姿的滤波算法，可为影像导航手术提供更精确的3D渲染视觉[7]，但在一例面向心脏手术的实验中，运动组织器官会对此算法产生不利影响，需进一步提高其适应能力，才具有实用意义．由于MLSRS结构和功能的不同，相适应的运动方法也各具特色．针对含有器械臂和持镜臂的腹腔手术机器人，建立器械臂基于动态视觉引导的运动方法是一项具有实用意义的课题．
　　2 问题描述（Problem statement）
　　微创腹腔手术机器人从手系统的3维模型如图1所示，包含1条持镜臂和2条器械臂，3臂的前7个运动关节结构形式相同；持镜臂（配备腹腔镜）提供3D视觉反馈，器械臂进行手术操作；器械臂的前4个关节为被动式，其余关节为主动式，在图1中移动自由度上配备了具有旋转、俯仰、偏摆的3自由度微器械．图1 MLSRS从手系统3维模型图Fig.1 The slave arms’ CAD model of MLSRS医生根据手术操作的实际需求，通过控制主手系统，以主从控制方式控制从手系统运动；通过控制持镜臂的运动获得所需的最佳手术视野，3D视觉窗口在手术中将进行阶段性运动（如图2所示），在整个手术进程中是以动态视窗形式提供3D视觉反馈，即医生在动态视觉引导下基于对应的手术视窗进行手术操作；设动态视窗中某视窗为当前视窗，则其下一视窗为其目标视窗．基于动态视觉的3D视窗运动方法已在文[8]中进行了详细论述．基于3D视窗中获得的腹腔手术视野，医生操作两条器械臂上装备的微器械进行相关手术动作．为适应医生的视觉感知和手术操作的习惯，针对微器械的各种操作均是以当前视窗为基准．微器械的运动如图3所示，器械臂的当前位姿基于当前的3D视窗，其目标位姿可通过当前位姿和运动量（均基于当前视窗的视觉坐标系）确定．为此，通过分析基座坐标系、当前3D视窗基于基系的位姿、器械臂基于视觉坐标系（持镜臂的工具坐标系）的位姿、器械臂基于基系的位姿四者之间的运动学关系，以及3D动态视觉引导下目标视窗转换为当前视窗的动态替换过程，可以建立器械臂基于动态视觉引导的运动方法．图2 动态视觉窗口导航过程Fig.2 The procedure of dynamic visual window navigation图3 器械臂基于动态视窗的运动示意图Fig.3 The diagram of instrument arms’ motion based ondynamic visual window
　　3 基坐标系的归一化（Normalization of thebase coordinate system）
　　本文所研究的MLSRS的从手系统包含3条机械臂，其中持镜臂拥有7个自由度，器械臂拥有11个自由度．为便于分析从手系统的运动学模型，应将3臂的运动学基座坐标系统一，持镜臂位于2条器械臂的对称中心线上，因此以持镜臂的基座建立从手系统的基座坐标系，即归一化基系．为此，需首164 机 器 人 2013年3月先完成2条器械臂自身基系相对于归一化基系的坐标系变换，即先完成从手系统的基座坐标系归一化，再进行器械臂和持镜臂的运动学建模．根据从手系统的结构特征，易知3臂自身基座坐标系的相对位置关系（如图4所示），根据几何关系可建立2条器械臂的基系归一化转换矩阵，如式(1)所示．Y1Z1X1O1Y0Z0X0O0Y2Z2X2O2图4 从手系统3臂各自基系的位置关系图Fig.4 The position relations among the three arms’ basecoordinate system of the slave systemBdTl =2641 0 0 ¡kl0 1 0 k30 0 1 00 0 0 1375(1)式中kl;k3 分别表示器械臂与持镜臂各自基系的位置关系参数，可由从手系统的结构尺寸确定，且矩阵BdTl 为常数矩阵．当l=1时，式(1)为BdT1，表示器械臂1与从手系统基系的归一化转换矩阵．当l=2时，式(1)为BdT2，表示器械臂2与从手系统基系的归一化转换矩阵．
　　4 基于动态视觉引导的运动方法（Kinematicsmethod based on dynamic visual naviga-tion）
　　器械臂配备的微器械是直接进行手术操作的执行设备，医生在腹腔镜提供的3D动态视窗下，习惯性地控制微器械进行手术操作，即相对于当前3D视窗控制器械臂（含微器械）运动．器械臂和持镜臂的运动学模型[8]是建立本文运动方法的基础．
　　4.1 运动学建模
　　由于2条器械臂的结构完全相同，其运动学可作统一分析，自身坐标系如图5所示，自身基系为O0，其前4个被动关节坐标系依次为O1;O2;O3;O4，微器械工具坐标系为O11，其余为主动关节坐标系．器械臂坐标系O4 与自身基系O0 的齐次变换矩阵如式(2)所示：04Tp=01Tp¢12Tp¢23Tp¢34Tp(2)z0y0x0z1y1x1z3y3x3d1a3d4y4x4z4a4z4y4x5z6x6y6d6x8y8z8d10αβθ2θ3θ4θ4θ6O0z2y22xO3O4O4 x7y7z7θ7O6(O8)O7d8d9z9x9y9O9y10z10x10O10d11z11x11O11y11θ9θ10O1(O2)图5 器械臂自身运动学坐标系Fig.5 The kinematics coordinate systems of instrument arm微器械末端工具坐标系O11 与第4个被动关节坐标系O4 的齐次变换矩阵如式(3)所示：411Ta=45Ta¢56Ta¢67Ta¢78Ta¢89Ta¢910Ta¢1011Ta(3)联立式(1)～(3)可求解出器械臂基于归一化坐标系的运动学正解：B11Td=BdTl¢04Tp¢411Ta=264nxoxaxpxnyoyaypynzozazpz0 0 0 1375(4)式中：nx=Cq2+3+4(Sq10(CbSq7(CaCq6+Sa)Cq5Sq6)+SbSq7(CaSq6¡SaCq5Cq6) +SaCq7Sq5)¡Cq10(Sq9(CbCq7(CaCq6+SaCq5Sq6)¡SaSq5Sq7+SbCq7(CaSq6¡SaCq5Cq6))¡Cq9(Cb(CaSq6¡SaCq5Cq6)¡Sb(CaCq6+SaCq5Sq6)))¡Sq2+3+4(Cq10(Cq5Sq7Sq9+Cq9Sq5C(b¡q6) +CbCq7Sq5Sq6Sq9¡SbCq6Cq7Sq5Sq9) +Sq10(Cq5Cq7¡CbSq5Sq6Sq7+SbCq6Sq5Sq7))¢ ¢ ¢其中C表示cos，S表示sin，q2+3+4表示q2+q3+q4．由于器械臂通过伺服电机驱动其6个主动关节，其运动学逆解应假设以第4个被动关节坐标系O4 为基础坐标系进行分析求解．为简化计算过程，将坐标系O4 绕其y4 轴旋转a+90±作为运动学逆第35卷第2期 于凌涛，等：基于动态视觉引导的外科手术机器人器械臂运动方法 165解基系O04，则逆解基系O04与坐标系O5的齐次变换矩阵如式(5)所示：45T0a=264rot(y4;a+90±)0000 0 0 1375¡1¢45Ta(5)器械臂的工具坐标系O11 与坐标系O04 之间的齐次变换矩阵411T0a 如式(6)所示：411T0a=45T0a¢56Ta¢67Ta¢78Ta¢89Ta¢910Ta¢1011Ta(6)联立式(1)、(2)、(5)，可得式(7)为器械臂归一化基系与坐标系O04 的齐次变换矩阵：B40T=BdTl¢04Tp¢264rot(y4;a+90±)0000 0 0 1375(7)由于已知被动关节的转角参数，则B40T为已知量，其逆矩阵B40T¡1也已知．设微器械相对于归一化基系的目标位姿矩阵为BtTd，则其在逆解基系O04 下的位姿为4011T0a=B40T¡1¢BtT，并设其逆矩阵为式(8)：4011T0¡1a=264n0xo0xa0xp0xn0yo0ya0yp0yn0zo0za0zp0z0 0 0 1375(8)(I)为求q10、q9、q8，将式(6)作如下变换：4011T0¡1a¢45T0a¢56Ta¢67Ta=1011T¡1a¢910T¡1a¢89T¡1a¢78T¡1a(9)联立式(9)和相关齐次变换矩阵，可得如下方程组：8><>:a0zd6+p0z=¡sinq9(d8+d9)a0yd6+p0y=sinq10(cosq9d8+cosq9d9+d10)a0xd6+p0x=¡cosq10(cosq9d8+cosq9d9+d10)¡d11(10)已知q10、q9、q8 的范围为8><>:q10 2(¡90±;90±)q92(¡90±;90±)d82(0;600 mm)(11)联立方程组(10)和参量范围(11)，可求得唯一逆解：8><>:q10 =arctan[(p0y+a0yd6)=(p0x+a0xd6+d11)]q9=arctanf(p0z+a0zd6)=[d10¡(p0y+a0yd6)=sinq10]gd8= (p0z+a0zd6)=(d9¡sinq9)(12)(II) 为求q7、q6、q5，将式(6)作如下变换：45T0a¢56Ta¢67Ta=4011T0a¢1011T¡1a¢910T¡1a¢89T¡1a¢78T¡1a(13)则式(13)的右端为常量矩阵，令之为矩阵Tr：Tr =264T11T12T13T14T21T22T23T24T31T32T33T34T41T42T43T44375(14)联立式(13)、(14)和相关齐次变换矩阵，可得如下方程组：8><>:T13 =cosq5cos(q6¡b)T23 =sinq5cos(q6¡b)T31 =¡cosq7cos(q6¡b)T32 =sinq7cos(q6¡b)T33 =sin(q6¡b)(15)已知q7、q6、q5 的范围为8><>:q72(¡180±;180±)q62(b¡90±;b+90±)q52(¡90±;90±)(16)联立方程组(15)和参量范围(16)，可求得唯一逆解：8><>:q7=arctan 2[T32=cos(q6¡b);¡T31=cos(q6¡b)]q6=b+arctan(T33cosq5=T13)q5=arctan(T23=T13)(17)4.2 基于动态视觉引导的运动算法通过深入分析器械臂和持镜臂之间的运动学关系，以当前3D视窗的视觉坐标系为参考系，根据微器械的需求运动量，可建立器械臂（含微器械）基于当前视窗的运动学模型，从而建立器械臂基于视觉引导的运动算法，技术框图如图6所示．166 机 器 人 2013年3月px, ∆v py, ∆v pz) φ(ψ,   , φ, vispx, vispy, vispz)  φ图6 器械臂基于动态视觉引导的运动学方法框图Fig.6 The technological flow of kinematics algorithm ofinstrument arm based on dynamic visual navigation器械臂和持镜臂位姿的运动学相对变换关系如式(18)所示：Tl¢dTl =Td(18)式中：Tl 和Td 分别表示持镜臂（含腹腔镜）和器械臂（含微器械）相对于归一化基系的绝对位姿．由式(18)可获得微器械基于当前3D视窗的视觉坐标系的位姿矩阵dTl．为便于分析，用(y;f;j;vpx;vpy;vpz)的参数形式来描述dTl．根据医生操作主手实现的位姿变化量(Dy;Df;Dj;Dvpx;Dvpy;Dvpz)，可获得微器械在视觉坐标系下的目标位姿，其姿态转角如式(19)所示，其位置矢量如式(20)所示．8><>:y0=y+Dyf0=f+Dfj0=j+Dj(19)264lp0xlp0ylp0z375=264lpxlpylpz375+Rl¢264DvpxDvpyDvpz375(20)式中姿态矩阵Rl 如式(21)所示：Rl =264Cy0Cf0Cy0Sf0Sj0¡Cj0Sy0Sy0Sj0+Cy0Cj0Sf0Cf0Sy0Cy0Cj0+Sy0Sf0Sj0Cj0Sy0Sf0¡Cy0Sj0¡Sf0Cf0Sj0Cf0Cj0375(21)由式(19)～(21)可确定微器械在视觉下的目标位姿矩阵dT0l，如式(22)所示：dT0l=264Rllp0xlp0ylp0z0 0 0 1375(22)则由式(18)可得微器械在归一化基系下的绝对目标位姿为TBd=Tl¢dT0l，令矩阵TBd 为TBd=264vnxvoxvaxvpxvnyvoyvayvpyvnzvozvazvpz0 0 0 1375(23)设T0d 的逆矩阵如式(15)所示，则(TBd)¡1=264vn0xvo0xva0xvp0xvn0yvo0yva0yvp0yvn0zvo0zva0zvp0z0 0 0 1375(24)结合器械臂逆运动学中矩阵处理过程公式(5)～(8)，将TBd 代入微器械基于归一化基系的目标位姿矩阵BtTd，并令其在逆解基系O04 下的位姿4011TBa如式(25)所示：4011TBa=B40T¡1d¢TBd=264BnxBoxBaxBpxBnyBoyBayBpyBnzBozBazBpz0 0 0 1375(25)令4011TBa 的逆矩阵如式(26)所示：(4011TBa)¡1=264Bn0xBo0xBa0xBp0xBn0yBo0yBa0yBp0yBn0zBo0zBa0zBp0z0 0 0 1375(26)将4011TBa 替换4011Ta，由式(13)、(14)可得：4011TBa¢1011T¡1a¢910T¡1a¢89T¡1a¢78T¡1a=264vT11vT12vT13vT14vT21vT22vT23vT24vT31vT32vT33vT34vT41vT42vT43vT44375(27)第35卷第2期 于凌涛，等：基于动态视觉引导的外科手术机器人器械臂运动方法 167表1 器械臂1运动学常量参数（未注单位：mm）Tab.1 The link parameters for instrument arm 1 (mm is the length unit)k1k2k3q2q3q4a b d1a3a4d4d6d7d9d10d11dj292 ¡292 ¡195 45±45±45±45±30±1800 400 450 180 700 50 500 100 100 150表2 器械臂正逆运动学输入输出参数Tab.2 The input and output parameters of instrument arm’s forward/inverse kinematicsNO. q5/rad q6/rad q7/rad d8/mm q9/rad q10/rad1 0.1473 1.9610 2.9210 9.754 ¡1.0760 ¡0.69592 1.4440 1.0130 ¡2.9170 91.57 1.097 0.91803 ¡1.1720 1.8220 0.8316 81.47 ¡1.264 1.275theta_5theta_6theta_7theta_9theta_10d_8randomparameters’ module−1.1721.8220.8316−1.2641.275output parameterspxpypzaxayazT13T23T31T32T33theta_5theta_6theta_7theta_9theta_10d_8fcninverse kinematics module−1.1721.8220.8316−1.2641.275input parameterstheta_5theta_6theta_7theta_9theta_10d_8pxpypzaxayazT13T23T31T32T33fcnforward kinematics module图7 器械臂运动学Simulink仿真模型Fig.7 The kinematics model in Simulink simulation of instrument arm将式(26)和(27)相应参数项代入器械臂的运动学逆解公式(11)、(12)、(16)、(17)，即可求出器械臂基于当前3D视窗视觉坐标系的唯一逆解，即器械臂基于视觉引导的运动算法结论公式：q10 =arctan[(Bp0y+Ba0yd6)=(Bp0x+Ba0xd6+d11)];q10 2(¡90±;90±);q9=arctanf(Bp0z+Ba0zd6)=[d10¡(Bp0y+Ba0yd6)=sinq10]g; q92(¡90±;90±);d8= (Bp0z+Ba0zd6)=(d9¡sinq9); d82(0;600mm);q7=arctan 2[vT32=cos(q6¡b);¡vT31=cos(q6¡b)];q72(¡180±;180±);q6=b+arctan(vT33cosq5=vT13);q62(b¡90±;b+90±);q5=arctan(vT23=vT13); q52(¡90±;90±)
　　5 基于MATLAB的仿真分析（Simulationanalysis based on MATLAB）
　　5.1 器械臂运动学仿真器械臂1和2的结构完全相同，且为对称安装，因此下文中以器械臂1为例进行仿真分析．器械臂1运动学中连杆及角度常量参数如表1所示．根据器械臂运动学方程编写MATLAB文件，并搭建器械臂（含微器械）的Simulink仿真模型，如图7所示．利用关节运动量发生模块在参数范围内随机生成3组关节运动量，输入正运动学模块，可获得此时的器械臂末端的位姿矩阵，再将位姿矩阵输入逆运动学模块，可求出的关节运动量与随机输入的q5;q6;q7;q9;q10;d8 完全相等，从而验证了器械臂正逆运动学模型是正确的，其中3组结果数据如表2所示．
　　5.2 器械臂运动算法仿真分析根据器械臂基于视觉引导的运动算法和技术框图（图6），联合使用MATLAB软件中Simulink和SimMechanics工具箱，搭建此算法的仿真模型，整体仿真模型如图8所示，图9为The current & ob-jective mechanism of instrument arm 1 模块中器械臂SimMechanics机构仿真模型．将器械臂1相关参数（见表1）输入仿真模型，当前3D视窗的位姿为T1（由持镜臂位姿决定），微器械基于当前3D视窗视觉坐标系的当前位姿为dlT，微器械基于视觉坐标系的目标位姿相对于当前位姿dlT 的运动量所解析的6个运动学参数为[Dy;Df;Dj;Dvpx;Dvpy;Dvpz]，将表3中的上述参数输入图8所示的仿真模型，输出结果如表4所示．图8中Simulink模块基于运动算法的输出参数DTdl 为器械臂的期望目标位姿变化量（基于当前3D视窗视觉坐标系）；将通过运动算法计算出的器械臂关节运动量输入SimMechanics机构仿真模块，可获得其实际目标位姿变化量DTdB（基于从手系统归一化基系）；对比可知，DTdl 和DTdB完全相等，可见器械臂基于视觉引导的运动算法是正确的．将此仿真过程的3组数据作为整体分析，即表达了3D视觉窗口在手术中进行阶段性运动时，器械臂基于动态视觉引导的一种运动方式，从而满足医生常态的视觉需求和操作习惯，进而更好地使用微创腹腔手术机器人系统完成手术操作．表3 器械臂仿真分析输入参数（未注单位：mm）Tab.3 The input parameters in instrument arm’s simulation analysis (mm is the length unit)NO. TldlT (Dy;Df;Dj;Dvpx;Dvpy;Dvpz)12640:8152 ¡0:0271 0:5786 ¡275:5¡0:0040 0:9986 0:0523 1064¡0:5792 ¡0:0450 0:8139 11920 0 0 1:00003752640:3954 ¡0:4523 0:7994 128:0¡0:5862 ¡0:7943 ¡0:1595 226:00:7071 ¡0:4056 ¡0:5792 ¡108:00 0 0 1:0000375(75±;127±;48±;113;446;779)2264¡0:1095 ¡0:9163 ¡0:3851 206:30:7734 0:1648 ¡0:6121 15170:6243 ¡0:3649 0:6907 994:70 0 0 1:00003752640:0317 0:1401 0:9896 ¡360:0¡0:0622 0:9885 ¡0:1379 189:0¡0:9976 ¡0:0571 0:0400 255:00 0 0 1:0000375(¡35±;87±;¡63±;¡257;456;¡472)32640:5244 ¡0:5129 ¡0:6797 609:8¡0:4750 ¡0:8387 0:2664 817:5¡0:7067 0:1831 ¡0:6834 216:60 0 0 1:0000375264¡0:0082 ¡0:3590 ¡0:9333 891:0¡0:1562 0:9223 ¡0:3534 752:00:9877 0:1429 ¡0:0636 667:00 0 0 1:0000375(135±;¡42±;¡74±;750;¡842;634)表4 器械臂1的目标位姿相对于当前位姿的期望差值与实际差值Tab.4 The desired and practical D-values between the objective and current poses of instrument arm 1NO. DTdlDTdB1264¡0:0945 1:2720 ¡0:0086 14830:3647 1:3580 1:3580 1247¡1:1070 0:5083 0:5083 543:10 0 0 0375264¡0:0945 1:2720 ¡0:0086 14830:3647 1:3580 1:3580 1247¡1:1070 0:5083 0:5083 543:10 0 0 03752264¡1:3070 0:4533 ¡0:2180 76:49¡0:2814 ¡1:1620 ¡0:3319 ¡341:80:2899 0:5744 0:2010 45:300 0 0 0375264¡1:3070 0:4533 ¡0:2180 76:49¡0:2814 ¡1:1620 ¡0:3319 ¡341:80:2899 0:5744 0:2010 45:300 0 0 03753264¡0:1398 ¡1:213 0:5159 18350:1434 0:3000 1:3430 14200:3232 1:108 0:8646 ¡17030 0 0 0375264¡0:1398 ¡1:213 0:5159 18350:1434 0:3000 1:3430 14200:3232 1:108 0:8646 ¡17030 0 0 0375
　　6 结论（Conclusion）
　　面向具有器械臂和持镜臂的微创腹腔手术机器人系统，本文首先探讨了核心问题的解决方法：使医生能够根据腹腔镜提供的视觉环境，进行符合视觉习惯的手术操作．通过分析器械臂的当前及目标位姿、3D动态视窗的视觉坐标系、归一化基系之间的相对运动关系，推导出与之对应的运动学变换方程，建立了基于动态视觉引导的器械臂运动方法，并推the PAO stands for position and orientation.−0.13980.14340.32320−1.213−0.31.10800.51591.3430.8646018351420−17030the practical variation of instrument’s PAOOut1 Conn1the objective mechanism of  robotic arm 1# −0.13980.14340.32320−1.213−0.31.10800.51591.3430.8646018351420−17030the desired variation of instrument’s PAOTtt11t21t31Tt1fcnthe current window’s PAOOut1 Conn1the current mechanism of  robotic arm 1# -C-the current window’s PAO[3x3]the current  instrument’s rotationmatrixTx1x2x3fcnthe currentjoints’ parametersTnew fcnT04newdTtTtt11t21t31Tt2 fcnmatrix transformationEnvmachineenvironment1EnvmachineenvironmentT04newTdev2theta_5theta_6theta_7theta_9theta_10d_8fcninverse kinematics module1T04newTdev2theta_5theta_6theta_7theta_9theta_10d_8fcninverse kinematics moduleOut1Out2input the current window’s postion and instrument’sPAOground2ground1theta_5theta_6theta_7theta_9theta_10d_8Tz1 fcnforward kinematics module1theta_5theta_6theta_7theta_9theta_10d_8Tz2 fcnforward kinematics moduleAdd3Add2TlapTt2Tdev2fcn the absolutely objectivePAO of instrument TlapTt1Tdev1 fcn the absolutelycurrent PAO of instrument 图8 器械臂1基于动态视觉引导的运动算法的混合仿真模型Fig.8 The hybrid simulation model of kinematics algorithm based on dynamic visual navigation for instrument arm11Out11Conn1CS1 CS2T89CS1 CS2T78CS1 CS2T67CS1 CS2T56CS1 CS2T45CS1 CS2T34CS1 CS2T23CS1 CS2T12CS1 CS2T01In1 Out1subsystemB Frevolute9BFrevolute8BFrevolute7BFrevolute6BFrevolute5BFrevolute4B Frevolute3B Frevolute2B Frevolute10B Frevolute1B FrevoluteCS1 CS2body1 body sensorCS1 CS2body图9 The current & objective mechanisms of instrument arm 1模块子系统Fig.9 The subsystem of the current & objective mechanisms of instrument arm 1170 机 器 人 2013年3月导出运动算法的表达式．最后，使用MATLAB软件搭建了Simulink仿真模型，通过在参数范围内随机获取的3组数据验证了器械臂正逆运动学模型的正确性；并使用Simulink和SimMechanics工具箱搭建了联合仿真模型，其中Simulink模块输出器械臂基于视觉引导的目标位姿的期望差值，SimMechanics模块输出其目标位姿的实际差值，通过3组随机数据验证了上述期望差值和实际差值完全相等，可见器械臂基于本文算法的期望运动量与实际运动量一致，进而说明了器械臂在动态视觉引导下运动方法的正确性和可行性．本文提出的运动方法具有一定的理论意义和实用价值，不受外界环境因素的影响，可以提高微创腹腔手术机器人系统在进行微创手术时的可靠性和灵活性．它不仅可以作为微创腹腔手术机器人系统器械臂的专用运动方法，还可以用于具有视觉反馈的其他主从控制机器人系统．
　　参考文献（References）[1] Sung G T, Gill I S. Robotic laparoscopic surgery: A compari-son of the da Vinci and Zeus systems[J]. Urology, 2001, 58(6):893-898.[2] Haidegger T, Benyo Z. Surgical robotic support for long dura-tion space missions[J]. Acta Astronautica, 2008, 63(7-10): 996-1005.[3] Hagn U, Nickl M, J¨org S, et al. The DLR MIRO: A versatilelightweight robot for surgical applications[J]. Industrial Robot,2008, 35(4): 324-336.[4] Lum M J H, Rosen J, Lendvay T S, et al. Effect of time delay ontelesurgical performance[C]//IEEE International Conference onRobotics and Automation. Piscataway, NJ, USA: IEEE, 2009:4246-4252.[5] Weede O, M¨onnich H, M¨uller B, et al. An intelligent and au-tonomous endoscopic guidance system for minimally invasivesurgery[C]//IEEE International Conference on Robotics andAutomation. Piscataway, NJ, USA: IEEE, 2011: 5762-5768.[6] Staub C, Lenz C, Panin G, et al. Contour-based surgical instru-ment tracking supported by kinematic prediction[C]//IEEE RASand EMBS International Conference on Biomedical Roboticsand Biomechatronics. Piscataway, NJ, USA: IEEE, 2010: 746-752.[7] Tully S, Kantor G, Zenati M A, et al. Shape estimation forimage-guided surgery with a highly articulated snake robot[C]//IEEE/RSJ International Conference on Intelligent Robots andSystems. Piscataway, NJ, USA: IEEE, 2011: 1353-1358.[8] Yu L T, Wang Z Y, Yu P, et al. Research on the kinematicmethod of 3D visual window for surgical robot in dynamic vi-sion[C]//IEEE/ICMA International Conference on Mechatron-ics and Automation. Piscataway, NJ, USA: IEEE, 2012: 2037-2042.