面向微创外科手术机械臂的主动力摆位控制策略

王伟， 杜志江， 闫志远， 王伟东

(哈尔滨工业大学 机器人技术与系统国家重点实验室，黑龙江 哈尔滨 150080)

机器人辅助微创手术是将机器人技术与腹腔镜微创外科手术技术相融合产生的新型治疗手段，在继承传统微创手术创伤小、感染率低、并发症少及术后恢复快等优点的同时，从根本上解决了腹腔镜微创手术带来的操作弊端[1-2]。目前机器人辅助微创手术技术已成为机器人领域的研究热点，许多研制成功的医疗手术机器人已取得良好的操作效果，某些微创外科手术系统在实际手术中得以应用[3-6]。

当利用微创外科手术机器人进行手术操作时，医护人员需要在术前准备阶段将机械臂及微创器械拖拽至微创切口位置并手动调整机械臂的主、被动关节，使手术器械能够获得良好的工作空间，尽量避免各机械臂之间的术中干涉。上述环节称为微创外科手术机器人术前初始位姿的建立过程，即医护人员直接对机械臂施加外力，根据操作者的意图拖拽机器臂进行相应的位姿调整。问题的理想方式是在关节电机处于受控状态下进行机械臂的主动摆位操作，根据机械臂的受力情况估计操作者的控制意图，通过关节电机驱动机械臂辅助医护人员完成微创外科手术机器人的位姿调整，即所谓的主动顺应控制。根据控制对象的不同，此类控制问题可大致分为基于力平衡的力矩控制方式和基于虚拟环境模型的速度或位置控制方式两类。

对于力平衡的控制方式，通常使伺服电机工作在力矩模式下，一般采用关节电机的反馈电流估测人机之间的接触力，通过关节电机输出转矩主动补偿机械臂连杆的重力矩及传动环节中存在的摩擦力矩。文献[7]提出一种基于机械臂动力学模型的人机交互控制算法，机械臂与环境的接触力通过电机电流进行估计。文献[8]采用自测量的方式获得关节所受重力矩及摩擦力矩的补偿模型并通过电机输出相应的力矩进行主动平衡，机械臂的惯性力由外力克服，旨在为机械臂与操作者之间的力交互建立无摩擦和重力影响的操作环境。

基于虚拟环境模型的控制方式是指以人为设定的人机操作环境近似替代实际动力学模型的控制方式，此时伺服电机工作在位置或速度模式下，通过已建立的关节位置或速度与接触力之间的关系模型实现机器人与环境的力交互。阻抗控制[9-11]与虚拟工具法[12-14]在该类控制方式中比较常见，两种控制算法在原理上比较接近，都是通过抽象出的简化人机交互模型建立输入力、力矩与输出位置或速度之间的对应关系，并根据实际要求和测试反馈调整各关节的阻尼系数、虚拟质量或接触力增益等虚拟环境参数，以达到理想的人机力交互状态。文献[15]还提出了一种基于位置的零力控制算法，该算法根据伺服电机的控制模型将机械臂所受外力转换成关节电机对应的位置输出，控制电机转动以映射外力对机械臂的作用效果。

微创外科手术机器人的主动摆位操作对于控制算法的稳定性要求较高，这是由于在进行术中器械位姿调整时，即使是很小的意外移动都有可能对病患造成严重伤害。文献[7-8]提出的方法是伺服电机在力矩模式下的力平衡控制，其定位效果依赖于力学模型的补偿精度。当机械臂构型或连杆间传动关系变得复杂时，传动摩擦和自身重力的理论模型与实际情况不可避免的会存在偏差，致使机械臂不断地进行调整甚至发生震荡，稳定性较差。文献[15]提出的算法实际上是将与接触力大小相当的电机输出力矩转换成位置模式下的关节期望位置，伺服电机虽然工作在位置模式，但本质上仍属于力平衡控制，同样会存在上述问题。此外，与机械人直接示教过程只关注末端执行器的位置不同，微创外科手术机器人的主动摆位过程为了避免机械臂之间的碰撞，确保微创器械的有效工作空间，往往更注重各连杆的姿态调整。若采用文献[9-14]的控制方式则会为操作者带来诸多不便，这是由于此种方式将人机交互的作用位置限定在机械臂的执行末端，不利于各连杆位姿的独立控制。因此，合理的实现方式是使操作者与机械臂之间的接触位置不受约束，可根据操作者的控制意图实现机械臂位姿调整。

1 主动力摆位控制方法

为了满足机器人辅助微创外科手术对主动摆位的实际需求，本文采用在机械臂各关节处安装力矩传感器的方式测量机械臂与操作者之间的作用力，利用力学模型与采样数据相结合的方式处理重力的补偿精度问题。为了保证主动力摆位的稳定性，在伺服电机的速度模式下提出了基于关节力矩的虚拟工具控制模型。本节将分为传感器的安装方式，重力矩模型的建立方法以及交互力的控制算法三个部分介绍医疗机器人主动力摆位的实现方法。

1.1 传感器安装方式

相对于依靠电机电流反馈进行接触力估计的方式，采用传感器进行接触力测量的方式更加稳定可靠。传感器的安装方式不但决定了接触力的测量精度、机器人与外界环境的力交互位置甚至还会影响控制算法的响应速度。本文在机械臂各转动关节中集成一维力矩传感器，由于此种方式的传感器与关节驱动电机的安装位置较在机械臂末端安装六维力传感器的方式更为接近，与外界环境之间接触力的传递与检测变得更加简单直接，可避免由于机构参数不准确而产生关节分力的计算偏差，同时可以有效地减少由机构间隙产生的力反馈延迟与检测误差，从而提高控制系统的响应时间。此外，此种安装方式使机械臂与外界环境的力交互位置不受限于特定的局部区域，可扩展至整条机械臂的任意位置，更有利于机械臂各连杆位姿的独立调整。本文设计的机械臂关节驱动单元如图1所示，一维力矩传感器安装在谐波减速机的输出端与机械臂的固定基座之间，只检测关节转动方向受到的力矩值，采用增加额外轴承的方式消除摩擦力对关节力矩测量的干扰，并采用低通模拟滤波器对传感器进行滤波。

图1 力矩传感器安装示意图Fig.1 Installation diagram of the torque sensor

1.2 重力补偿模型

由于采用在机械臂关节处安装力矩传感器的测量方式，因此传感器的测量值会包含由机械臂自重产生的重力矩分量，为了能够提取机械臂与操作者之间的作用力，需要建立准确的重力矩模型以进行对应关节的重力补偿。本文首先根据机械臂的运动学方程推导出每个主动关节的重力矩理论模型。为了降低理论模型与实际情况的偏差，将连杆重量、质心位置等机械臂属性参数作为未知变量，通过实测采样的方式记录各关节在其工作空间内受到的重力矩分布情况并利用优化算法求得最佳的参数值，整个模型的建立过程并不需要确切的机械臂属性参数和繁琐的数值计算。

图2所示为本文设计的微创外科手术机械臂，其中包含2个主动驱动关节以及1个被动调整关节。每个主动关节均集成力矩传感器，被动调整关节中安装绝对位置码盘以记录该关节位置。在进行手术操作时，复合平行四边形机构(由图2中第2、3、4连杆构成)能够保证手术机械臂在微创器械介入处不会对病人造成额外的非手术伤害，是目前最常用的远心不动点执行机构，其工作原理如图3所示。连杆1、2、3、4之间通过转动副连接，连杆5在实际结构中并不存在，机构依靠平行四边形A、B、C之间的彼此约束实现绕远心不动点RCM的平面转动并利用轴线通过RCM点的转动关节驱动该平面机构，从而实现微创器械绕切口位置的空间转动而不发生相对移动，因此不会对病人造成额外伤害。

当进行主动力摆位时，复合平行四边形机构连杆1、2、3的质心位置由驱动关节2的转角决定，驱动关节1控制整个远心不动点机构的转动，而被动调整关节的位置变化会改变驱动关节处力矩传感器的测量值。实际上，该结构的重力矩补偿模型与4个机械臂连杆重量、质心位置及3个转动关节的角度相关。为了便于描述机械臂各连杆间的位姿关系以及驱动关节处所受的重力矩，本文采用D-H法建立机械臂运动学模型(如图4所示)，相应的坐标系齐次变换矩阵为

(1)

式中：si=sinθi,ci=cosθi,i=1,2,3。

图2 微创外科手术机械臂Fig.2 Minimally invasive surgery manipulator

图3 复合平行四边形RCM机构Fig.3 The compound parallelogram mechanism for RCM

图4 机械臂D-H坐标系Fig.4 D-H coordinates system of the manipulator

设P2m1、P3m2、P4m3、P5m4分别为机械臂连杆1、2、3、4质心在坐标系2、3、4、5中的位置向量：

(2)

则由式(1)、(2)可得到各质心位置向量在坐标系2和基系中表示为

(3)

(4)

若以G1、G2、G3、G4表示机械臂连杆1、2、3、4的重力，则各连杆质心处的重力向量在坐标系2中的表述如式为

(5)

式中：E代表3阶单位矩阵，inv(·)表示矩阵的逆运算，i=1,2,3,4。由力矩的定义可得驱动关节1所受到的重力矩在坐标系2中的表述如式为

(6)

驱动关节1在其轴线方向受到的重力矩为向量τg的z轴分量，其方向与z2轴正方向相同，表达式如为

τ1=c1(G1ym1c2+G2zm2c2+G3zm3c2+

G4zm4c2+G1xm1s2+G3ym3s2+

G3a3c3s2+G2xm2c3s2+G4xm4c3s2-

G2ym2s3s2-G4ym4s3s2)

(7)

以同样的方式亦可求得驱动关节2在其轴线方向上所受重力矩的表达式表示为

τ2=G3ym3s1-G4a3c1c2+G3a3s1c3-G3xm3c1c2+
G2xm2c3s1+G4xm4c3s1-G2ym2a3s1s3-
G4ym4s1s3+G3a3c1c2s3+G2ym2c1c2c3+
G4ym4c1c2c3+G2xm2c1c2s3+G4xm4c1c2s3

(8)

在主动关节各自的运动空间内随机生成若干机械臂的手术摆位姿态，并在机械臂各连杆静止时记录力矩传感器数值以及相应的关节位置。令式(7)、(8)左端分别等于驱动关节1和驱动关节2的力矩传感器测量值，将相应的主、被动关节位置代入表达式右侧，由此可得到与采样点数目相同的方程式，联立方程组并利用Matlab软件提供的fminunc函数求解机械臂各待定参数的最优值。优化参数如表1所示，其中目标函数终值为式(7)、(8)右侧计算值与左侧采样值之差的平方和。

表1 优化参数设置值Table 1 The optimization parameter settings

图5所示为机械臂被动调整关节θ1分别为-19°、-24°以及-29°时，驱动关节的重力矩补偿模型与传感器实测数据的对比图，其中平滑曲面由式(7)、(8)计算获得，圆点分别表示驱动关节1和驱动关节2传感器的实测数值。两类数据之间存在一定偏差，这与机械结构及采样点的密集程度等因素有关，但最主要的原因是来自传感器自身的测量值波动。由图5以及表2中的统计数据可知被动调整关节θ1的改变对于重力矩补偿模型的精度影响很小，每个驱动关节处的重力矩均可以得到较好的补偿，偏差在可接受范围之内。关节1的重力矩模型要略好于关节2，这是由于关节2的传动环节更加复杂，力在传递过程中会受到更多来自机构自身的干扰。在引入适当的检测阈值加以限定后，可消除于传感器测量值波动而引发的接触力判断错误问题，可增强力控制模型的输入稳定性。

1.3 基于关节力矩的虚拟工具控制算法

如文献[12]所述，以虚拟工具的动力学模型近似代替机械臂动力学模型的方式称为力控制的虚拟工具模型。当以此种方式进行力控制时，并不需要考虑机械臂各连杆的实际质量，以虚拟质量和人为设定的环境阻尼建立动力学模型。在此基础上，本文提出的控制算法以提取出的关节力矩作为输入，关节的驱动电机以速度模式进行控制，相应的输出速度由虚拟环境模型计算得到的加速度确定。对虚拟摩擦参数进行实时调整，以满足主动摆位过程中的不同阶段的需求，其控制原理如图6所示。

图5 重力矩补偿模型与实测数据的比较Fig.5 A comparison between the actual data and the compensation model of the gravity torque

图6 基于关节力矩的虚拟工具法控制框图Fig.6 A diagram block of the virtual-tool controller based on joint torque

θ1/(°)重力矩补偿误差误差均值/(N·m)标准差/(N·m)误差最大值/(N·m)-19-24-29τ1～-τ10.00160.09720.2828τ2～-τ2-0.01650.18340.3793τ1～-τ1-0.00140.11270.2350τ2～-τ20.01480.2063-0.4072τ1～-τ10.00350.12060.3651τ2～-τ20.01270.22740.4211

基于关节力矩的主动力摆位控制分为机械臂与外界的作用力检测和虚拟工具模型算法的实现两个部分。当操作者与机械臂进行力交互时，重力补偿模型根据各关节的实时位置θ计算相应的重力矩补偿值，驱动关节在转动方向上受到的合力矩τ通过力矩传感器进行检测，将经过模拟滤波后的合力矩值τ～与由重力矩模型得到的补偿值相减并与传感器检测阈值h进行比较，以大于检测阈值的关节力矩提取值τ′作为虚拟工具算法的控制输入。外界环境施加在关节处的接触力与机械臂连杆间的运动关系通过虚拟质量M体现，即以M作为关节所受力矩与关节加速度的转换参数。为了能够提供柔顺且自然的控制方式，需要引入相应的环境反馈参数，以关节转动速度v和虚拟摩擦参数C作为外界环境对机械臂的阻尼反馈。主动摆位控制算法通过当前关节的输入力矩以及前一周期的关节速度计算本周期的关节加速度，从而实现速度模式下的力控制，其表达式为

(9)

式中：虚拟质量M,虚拟摩擦参数C1、C2以及检测阈值h统称为主动力摆位控制模型的模型参数。每个驱动关节的控制均采用相同的模块配置，但每个关节对应的模型参数和检测阈值会有所不同。模型参数值的选取对主动力摆位控制模型的表现性能有显著影响。由于虚拟质量和检测阈值会决定关节加速度值，因此其数值的大小会影响关节运动的流畅性，较大的虚拟质量会使关节运动变得敏感而易产生震荡，而较小的虚拟质量会使机械臂的拖动变得困难。另一方面，虚拟摩擦参数会决定运动的平滑性以及作用力消失后的关节运动时间及位移，较小的虚拟摩擦参数会使运动更加流畅，但同时也会增加机械臂摆位后的停止时间和移动位移。为了满足微创外科手术机械臂的主动摆位要求，本文提出的主动力摆位控制算法会根据机械臂的受力情况主动修改虚拟工具模型的摩擦参数，当机械臂检测到与外界环境之间的作用力时，以较小的摩擦参数C1进行虚拟环境的阻尼反馈，使机械臂的拖拽过程更为轻便流畅；当机械臂与外界环境之间的接触力消失后，采用较大的摩擦参数C2控制机械臂运动，由于环境阻尼的增大会迫使转动关节快速地完成定位过程，以避免产生较大的位置误差。

2 算法验证与分析

为了验证本文提出的主动力摆位控制算法的有效性，以图2所示自行研制的微创外科手术机械臂作为验证平台。验证实验与实际的微创外科手术机器人的主动力摆位过程相似，为了使实验结果简洁清晰，便于分析主动力摆位控制模型的实际表现，操作者分别对两个主动关节进行一次大幅度的拖拽动作(约Δθ≈ 75°，不包含姿态的微调过程)，并记录这一过程中关节所受到的接触力矩，转动速度以及关节位置等信息(如图7所示)，相应的控制模型参数如表3所示。

图7 关节力矩与关节运动的对应关系Fig.7 The corresponding relationship between the joint torque and joint motion

关节MC1C2h/(N·m)采样周期/ms移动均值滤波窗口值12.50.090.350.40.28023.10.120.400.50.280

由图7可以看出，基于关节力矩的虚拟工具模型仅需要较小的作用力即可实现机械臂的主动力摆位过程，每个驱动关节的转动速度与关节处所受力矩的变化趋势基本一致，关节加速度会随着接触力增量的变化趋势进行调整。当操作者试图拖动机械臂时，随着接触力的增大驱动关节会以较大的加速度进行位置跟随(图7所示A阶段)，能够快速消除由于期望位置与当前位置偏差而产生的迟滞感；若关节所受力矩变化趋于平稳，主动关节会以相对稳定的速度运动以保持当前连杆运动速度与作用力矩之间的对应关系(图7所示B阶段)；当外界与机械臂之间的作用力消失时，关节会以较大的加速度进行减速(图7所示C阶段)，在保证运动流畅自然的前提下能够快速停止关节运动以减少位置误差。值得注意的是，当关节处的作用力矩发生波动时(如图7所示驱动关节2的B阶段)，与之对应的关节速度也能保证稳定，不会产生机械臂的震荡以及受力不均的操作感受，由此可见主动力摆位控制模型具有较好的稳定性。

3 结论

1)面向微创外科手术机器人的主动摆位引导操作设计了相应的主动关节驱动单元，可实现关节接触力矩的独立检测与运动控制。

2)提出了一种机械臂重力补偿模型，可有效识别主动关节驱动单元所检测力矩中的重力矩分量，在机械臂属性参数未知的前提下亦可实现较精确的重力矩补偿。

3)提出了一种基于关节力矩的虚拟工具控制算法，能够流畅地完成微创外科手术机器人的主动摆位引导任务，可在机械臂的任意位置对RCM机构连杆进行位姿调整，操作方式更便捷灵活。

由于虚拟工具模型的表现效果会受到模型参数的影响，在后续研究中会考虑根据机械臂的运动状态实现模型参数的连续调整，以期待进一步改善微创外科手术机器人摆位过程的操作感受。

[1] MURPHY D A, MILLER J S, LANGFORD D A, et al. Endoscopic robotic mitral valve surgery[J]. The journal of thoracic and cardiovascular surgery, 2006, 132(4): 1119-1120.

[2] MEIRELES O, HORGAN S. Applications of surgical robotics in general surgery[M]. New York: Springer, 2011: 791-812.

[3] MARESCAUX J, RUBINO F. The ZEUS robotic system: experimental and clinical applications.[J]. Surgical clinics of North America, 2003, 83(6): 1305-1315.

[4] MIYAMOTO S, SUGIURA M, WATANABE S, et al. Development of minimally invasive surgery systems[J]. Hitachi review, 2003, 52(4): 189-195.

[5] BLAKE H, JACOB R, FRIEDMAN D W, et al. Raven-II: an open platform for surgical robotics research[J]. IEEE transactions on biomedical engineering, 2013, 60(4): 954-959.

[6] MA J, BERKELMAN P. A compact, simple, and robust teleported robotic surgery system[M]. New York: Springer, 2011: 139-158.

[7] KOBAYASHI S, MUIS A, OHNISHI K. Sensorless cooperation between human and mobile manipulator[C]//IEEE International Conference on Industrial Technology. Lyon, France, 2005: 811-816.

[8] 游有鹏, 张宇, 李成刚. 面向直接示教的机器人零力控制[J]. 机械工程学报, 2014, 50(3): 10-17.

YOU Youpeng, ZHANG Yu, LI Chenggang. Force-free control for the direct teaching of robots[J]. Chinese journal of mechanical engineering, 2014, 50(3): 10-14.

[9] DUCHAINE V, GOSSELIN C M. General model of human-robot cooperation using a novel velocity based variable impedance control[C]//Euro Haptics Conference. Tsukuba, Japan, 2007: 446-451.

[11] LECOURS A, MAYER-ST-ONGE B, GOSSELIN C. Variable admittance control of a four-degree-of-freedom intelligent assist device[C]//International Conference on Robotics and Automation. St. Paul, USA, 2012: 3903-3908.

[12] KOSUGE K, FUJISAWA Y, FUKUDA T. Control of robot directly maneuvered by operator[C]//International Conference on Intelligent Robots and Systems. Tokyo, Japan, 1993: 49-54.

[13] INFANTE M L, KYRKI V. Usability of force-based controllers in physical human-robot interaction[C]//International Conference on Human-Robot Interaction. Lausanne, Switzerland, 2012: 355-362.

[14] FERRETTI G, MAGNANI G A, ROCCO P. Assigning virtual tool dynamics to an industrial robot through an admittance controller[C]//International Conference on Advanced Robotics. Munich, Germany, 2009: 1-6.

[15] GOTO S, USUI T, KYURA N, et al. Forcefree control with independent compensation for industrial articulated robot arms[J]. Control engineering practice, 2007, 15(6): 627-638.

本文引用格式：

王伟， 杜志江， 闫志远， 等. 面向微创外科手术机械臂的主动力摆位控制策略[J]. 哈尔滨工程大学学报， 2017， 38(12): 1915-1921.

WANG Wei, DU Zhijiang, YAN Zhiyuan, et al. Active force positioning control strategy to design a minimally invasive surgical manipulator[J]. Journal of Harbin Engineering University， 2017， 38(12): 1915-1921.