力反馈主操作手附加力的补偿策略研究

贠今天 刘阳阳 杨全利 桑宏强

1.天津工业大学机械工程学院，天津，300387 2.天津市现代机电装备技术重点实验室，天津，300387 3.天津职业技术师范大学，天津，300222

力反馈主操作手附加力的补偿策略研究

贠今天1,2刘阳阳1杨全利3桑宏强1,2

1.天津工业大学机械工程学院，天津，300387 2.天津市现代机电装备技术重点实验室，天津，300387 3.天津职业技术师范大学，天津，300222

针对力反馈主操作手的连杆重力、关节摩擦力以及惯性力影响医生真实感知反馈力信息的问题，建立了一套完整的动力学模型。基于力反馈主操作手动力学模型，分别建立了主操作手的重力补偿模型、摩擦力补偿模型以及惯性力补偿模型。通过力补偿控制策略来消除力反馈主操作手本身所具有的重力、关节摩擦力以及惯性力伴随关节运动对反馈作用力造成的干扰，实现了医生的真实力觉感知，从而提高了微创手术的精度。

力反馈；主操作手附加力；补偿策略；动力学模型；微创手术

0 引言

遥操作机器人(teleoperation robotics)技术是当前具有实用价值的交互式机器人前沿技术，其中，临场感(telepresence)一直是众多学者研究工作的目标之一[1-4]。力反馈遥操作机器人系统中，从操作手末端执行器上的力传感器检测到与外界环境相互作用的力信息,经控制系统及力反馈主操作手(下称主手)传递给操作者，这一力信息即为期望的反馈力。这一力信息中不仅包含从操作手末端执行器与外界环境的交互作用力，而且包含运动过程中力反馈主手关节处的摩擦力、杆件的重力和惯性力等，导致操作者很难分辨出从操作手与外界环境真实交互作用力的大小，使操作者缺乏力觉临场感。微创外科手术机器人临床应用操作中因力反馈主手动力学特性的干扰而缺乏力觉临场感，使医生失去了真实的力觉感知，从而降低了手术的精度。

国内外学者为增强操作者的力觉临场感而展开了相关研究。FAN等[5]提出了一种使用配重平衡定位机构和弹簧位置机构来减少力反馈主手重力对遥操作透明性影响的方法，该方法虽然减小了主手重力对反馈力的影响，但却增大了主手惯性力对反馈力的影响。VALLERY等[6]提出了利用保守力场来减小操作者对力反馈主操作手进行操作时产生的不必要的力/力矩，提高了力反馈遥操作的透明性。WANG等[7]为了研究摩擦力对力反馈遥操作系统的影响，提出了基于二端网络的支持向量机离线估计法，对摩擦力进行在线前馈补偿。MELINTE等[8]利用神经网络法来补偿摩擦力。谢琦等[9]基于等效微分变换法，提出了适合手术机器人的实时主从控制算法。黄勇[10]在分析现有遥操作系统控制策略局限性的基础上，从位置约束空间的角度，提出了一种新型控制策略，以位置约束空间矩阵代替常数增益，结合主从关节空间误差，来得到准确的反馈作用力。

目前，相关研究只针对减小重力或摩擦力对操作者感知反馈力信息造成的影响开展研究，并未实现全面消除主手重力、关节摩擦力及惯性力对操作者感知反馈力所造成的影响，使得操作者并不能感知真实的反馈力信息。

针对微创手术机器人力反馈主手因自身连杆重力、关节摩擦力以及惯性力对反馈作用力造成的干扰，本文以力反馈操作器PHANToM作为微创手术机器人的主手,建立计及关节摩擦力、连杆重力以及惯性力的力反馈主手多体系统动力学模型，分析惯性力、关节摩擦力以及连杆重力对主手动力学行为的影响规律,通过附加力补偿策略来消除主手的连杆重力、关节摩擦力以及惯性力对反馈作用力产生的影响。

1 力反馈主手动力学模型

1.1 力反馈器PHANToM动力学模型

图1 PHANToM结构示意图Fig.1 Schematic of PHANToM structure

根据Lagrange方程

(1)

L=K-Pi=1,2,…，n

式中，Fi为广义力；L为拉格朗日函数；K、P分别为系统的动能和势能。

在不考虑摩擦和外界干扰的情况下，力反馈主手PHANToM的动力学方程可表示为

(2)

式(2)可整理成：

(3)

mdl1p4)cθ2sθ3]

M12=M13=M21=M31=0

M23=M32=(-mcl2p3-mdl1p4)sθ23

C22=C33=0

C21=-C12

C31=-C13

N1=0

N2=g(map1+mbp3+mdl1)cθ2

N3=g(-mbp2+mcl1+mdp4)sθ3

τ=(τ1，τ2，τ3)T

i,j=1,2,3

式(3)为不计关节摩擦力的力反馈主手机构的动力学模型。

1.2 计及关节摩擦的PHANToM动力学模型

为精确实现主手力反馈，使操作者感知真实的反馈力信息，必须建立更准确的主手动力学模型，将主手关节处摩擦力一并计入主手的动力学模型中。力反馈主手中的摩擦主要来源于关节摩擦及传动引起的摩擦，本文忽略传动摩擦，只考虑关节处摩擦力。因其他摩擦模型具有复杂性及不确定性的特点[14]，同时考虑到使摩擦模型利于线性化，本文选用经典的Coulomb+Viscous组合摩擦模型[15]来近似描述主手机构关节处的摩擦效应，Coulomb+Viscous摩擦模型可表示为

(4)

力反馈主手PHANToM关节处的摩擦效应可近似地描述为如下形式：

(5)

式中，Ffci(i=1,2,3)为关节i处的Coulomb摩擦因数；Ffvi(i=1,2,3)为关节i处的Viscous摩擦因数。

在考虑关节摩擦效应时，计及力反馈主手的连杆重力、惯性力及关节摩擦力的完整动力学模型可表示为

(6)

建立的主手动力学模型(式(6))中，惯性力项、摩擦力项、离心力和哥氏力项以及重力项都具有高度的非线性，不利于力反馈主手机器人对力反馈主手控制器的设计，所以将主手动力学模型为线性参数化的形式。本文选取14个动力学参数，则式(6)可化为如下形式：

(7)

Y3×14=[Yd3×8Yf3×6]

πf6×1=(Ffc1，Ffc2，Ffc3，Ffv1，Ffv2，Ffv3)T

其中，Y为3×14维回归矩阵，表征关于力反馈主手的关节角、关节角速度以及关节角加速度的回归矩阵；π为14×1维动力学参数向量，表征关于力反馈主手系统动力学的定常参数向量。

Yd3×8中，

πd8×1中,

根据文献[16]中力反馈主手PHANToM的相关参数确定动力学参数，如表1所示。

2 力反馈主手附加力补偿策略

2.1 力反馈主手附加力

在微创手术机器人系统中引入力反馈，可极大地提高遥操作系统的透明性，增强力觉临场感，使医生更好地实施力控制。从操作手末端执行器与外界未知环境相互作用后会产生交互作用力，力反馈主手根据反馈作用力的大小在各个关节施加相应的驱动信号，使电机输出相应的转矩τf，经力反馈主手转换使操作者感知这一反馈力信息。但是，由于受主手动力学特性的影响，主手在运动过程中关节处将产生动力学附加转矩τi，因此，操作者感知到的力信息并不是真实的反馈力。在反馈力信息中不可避免地包含力反馈主手的杆件重力、关节摩擦力以及惯性力，将这些附加在反馈力信息中的力称为主手附加力(induced master force)，如图2所示。

表1 动力学参数

图2 主手附加力Fig.2 Induced master force

图2中，Hi为力反馈主手运动到附加力矩的传递函数，表征力反馈主手的动力学特性；θmu表示力反馈主手期望关节运动轨迹；τf为反馈力施加在主手电机上所产生的力矩；τi为力反馈主手附加力矩。操作者感受到的力可表示为

F=Ff+Fi

(8)

其中，τ经力反馈主手转换后的力信息为反馈作用力Ff和附加力Fi，Fi包括主手杆件的重力、惯性力及关节摩擦力，二者一起被操作者感知。

在操作者操作力反馈主手使从操作手与外界环境相互作用时，计入附加力矩的力反馈主手完整动力学模型可写成如下形式：

(9)

式(9)写成线性化的形式为

(10)

为使操作者感知真实的反馈力，需使操作者感受到的力矩τu与反馈力矩τf实时平衡，则式(10)转化成主手附加力形式为

(11)

结合人手对力反馈主手的实际操作，选取三个符合主动关节期望运动的驱动正弦函数：

根据式(11)经动力学反解计算可以得到主手的附加力矩-时间关系图，如图3所示。

(a)关节1

(b)关节2

由图3可知，在操作者抓持力反馈主手末端操纵杆使其完成操作者所期望的运动时，随着主手的运动在力反馈主手关节处会产生附加力矩。基于不计关节摩擦力的力反馈主手动力学模型，关节1、2、3处产生的附加力矩最大值约为0.1N·m；基于计及关节摩擦力的力反馈主手完整动力学模型，关节1处产生的最大附加力矩约为0.15N·m，关节2、3处产生的附加力矩约为0.12N·m。这表明操作者感知的实际力信息中包含的因力反馈主手自身产生的附加力Fi较大，会对操作者感知真实的反馈力信息造成较大的干扰。另外，对比计及关节摩擦与不计关节摩擦的主手附加力矩，力反馈主手关节处摩擦力对主手附加力Fi的贡献较大，可进一步推断出关节摩擦力对操作者感知反馈力信息的干扰也较大。因此，需要消除主手的连杆重力、关节摩擦力以及惯性力对操作者感知反馈力所造成的干扰。

2.2 附加力补偿策略

在医生进行微创手术的过程中，要使医生感知真实的反馈力信息，使其不受主手自身动力学特性的影响，可以利用主手电机输出相应的反向驱动力矩来平衡主手附加力矩。本文采用图4所示的力反馈主手附加力补偿策略来消除主手的连杆重力、关节摩擦力及惯性力对医生感知反馈力的影响。

图4 主手附加力补偿策略Fig.4 Compensation strategy for induced master force

力反馈主手附加力补偿策略为：在主手控制器中分别建立主手的重力补偿模型、关节摩擦力补偿模型以及惯性力补偿模型。操作者抓持力反馈主手末端的操纵杆使主手沿期望运动轨迹θmu运动，使从操作手与外界环境发生交互，编码器实时测量主手的各关节转角信息并以脉冲的形式发送给解码器，主手控制器将关节转角信息输入主手重力补偿模型、摩擦补偿模型以及惯性力补偿模型中，通过求解计算得到各关节需要平衡主手重力、关节摩擦力、惯性力所需力矩的大小，然后将得到的力矩信息指令实时地发送给主手各关节处的电机，使电机输出反向驱动力矩，来抵消主手因动力学特性而产生的附加力矩。此时，力反馈主手各主动关节电机的力控制为

τm=cJT(θ)Ff+τiG+τiF+τiI

(12)

其中，τm为力反馈主手各主动关节电机的驱动转矩；J(θ)为力反馈主手的速度雅可比矩阵；τiG为补偿主手重力电机输出的力矩；τiF为补偿主手关节摩擦力电机输出的力矩；τiI为补偿主手惯性力电机输出的力矩；当力反馈主手与从操作手连接时，c=1；当力反馈主手与从操作手断开时，c=0。

2.3 附加力补偿模型

通过建立重力补偿模型实现对主手重力的补偿，消除连杆重力对反馈力的干扰。主手机构的重力补偿模型为

(13)

主手运动过程中，编码器将实时测量到的主手关节转角信息发送给解码器，主手控制器将其输入重力补偿模型来求解计算各关节要平衡主手连杆重力所需的力矩，然后实时地将力矩信息指令发送给主手电机，使其输出反向驱动力矩，从而达到动态补偿主手重力的目的。为补偿主手重力，各电机需要输出的反向力矩如图5a所示。

通过建立主手摩擦力补偿模型来实现对主手摩擦力的补偿，消除关节摩擦力对反馈力的干扰。摩擦力补偿模型为

(14)

主手控制器将编码器发送的主手关节转角信息输入主手摩擦力补偿模型，计算各关节要平衡主手关节摩擦力所需的力矩，然后将力矩信息指令发送给主手电机，使电机输出反向驱动力矩，从而达到动态补偿摩擦力的目的。为补偿主手关节摩擦力，各电机需要输出的反向力矩如图5b所示。

对于力反馈主手惯性力的补偿，由于在实际操作过程中，操作者不可能使主手操作杆始终保持匀速运动，因此，主手惯性力会随操作者手的加速或减速而产生。然而，由于惯性力的二阶非线性使其在实际中无法测量。本文通过设计的加速度观测器来观测力反馈主手的加速度，从而求得主手在运动过程产生的惯性力：

(15)

通过下式求解平衡惯性力所需力矩的大小：

(16)

将惯性力矩信息发送给主手电机，使电机输出反向力矩，实现对力反馈主手惯性力的动态补偿。各电机输出的反向力矩如图5c所示。

(a)补偿主手重力

(b)补偿关节摩擦力

(c)补偿主手惯性力图5 补偿主手重力、摩擦力、惯性力时电机输出的力矩Fig.5 Motor output torque compensating gravity,friction and inertia force of master manipulator

基于建立的主手附加力补偿模型，使主手电机输出反向驱动力矩来平衡抵消主手因自身动力学特性而产生的附加力矩，由此来消除因主手的连杆重力、关节摩擦力以及惯性力而产生的主手附加力矩，各电机输出的补偿力矩与各关节处的附加力矩如图6所示。由图6可知，在主手运动过程中，基于主手附加力补偿模型，主手各电机为平衡抵消附加力矩而输出的补偿力矩与关节处的附加力矩大致相等。其中，电机2的补偿效果欠佳，其原因为力反馈主手在运动过程中，主手机构自身产生的的离心力(自耦力)和哥氏力(互耦力)在关节2处产生的附加力矩较大，故在实际操作过程中需要缓慢操作力反馈主手运动，减小其对操作者感知反馈力的影响。

(a)电机1输出的补偿力矩与关节1处的附加力矩

(b)电机2输出的补偿力矩与关节2处的附加力矩

(c)电机3输出的补偿力矩与关节3处的附加力矩图6 各电机输出的补偿力矩与各关节处的附加力矩Fig.6 Output torque of each motor and induced torque at each joint

在微创手术中，医生在缓慢操作主手时，主手电机输出的补偿力矩与附加力矩可实时动态地平衡抵消，从而使医生真实感知反馈力信息。通过建立主手附加力补偿模型可消除主手重力、关节摩擦力以及惯性力给反馈力带来的干扰，将反馈力信息无干扰地传递给医生，使医生获得真实的力觉临场感，从而实施恰当的力控制行为。

3 结束语

本文建立了计及力反馈主手PHANToM的连杆重力、关节摩擦力及惯性力的完整动力学模型，通过主手附加力补偿策略，消除因力反馈主手动力学特性对医生感知反馈力所造成的干扰，以实现医生的真实力觉感知。仿真结果表明，在力反馈主手运动过程中，主手各电机输出的补偿力矩可动态地平衡主手因自身连杆重力、关节摩擦力以及惯性力而产生的附加力矩。

[1] 宋爱国.人机交互力觉临场感遥操作机器人技术研究[J].科技导报，2015，23：100-109.SONGAiguo.ResearchonHuman-RobotInteractionTelerobotwithForceTelepresence[J].Science&TechnologyReview, 2015, 33(23): 100-109.

[2] 盛国栋，曹其新，潘铁文，等.主从式机器人系统中力反馈的实现[J].中国机械工程，2015，26(9)：1157-1160.SHENGGuodong,CAOQixin,PANTiewen,etal.ImplementationofForceFeedbackinMaster-slaveRobotSystems[J].ChinaMechanicalEngineering, 2015, 26(9): 1157-1160.

[3]ARTIGASJ,HIRZINGERG.ABriefHistoryofDLR’sSpaceTeleroboticsandForce-feedbackTeleoperation[J].ActaPolytechnicaHungarica, 2016, 13(1): 239-249.

[4] 文广，巩明德，张红彦.H∞控制的主从遥操作力觉双向伺服控制系统研究[J].中国机械工程，2011，22(22)：2717-2722. WEN Guang, GONG Mingde, ZHANG Hongyan. Research on Master-slave Tele-operated Force Sense Bilateral Servo Control System byH∞Control[J]. China Mechanical Engineering, 2011, 22(22): 2717-2722.

[5] FAN R, ZHAO C, ZHAO H.Improvement of Dynamic Transparency of Haptic Devices by Using Spring Balance[C]//IEEE International Conference on Robotics and Biomimetics. Guangzhou, 2012: 1075-1080.

[6] VALLERY H, DUSCHAU-WICKE A, RIENER R. Optimized Passive Dynamics Improve Transparency of Haptic Devices[C]//IEEE International Conference on Robotics and Automation. Kobe, 2009:301-306.

[7] WANG G L, LI Y F, BI D X. Support Vector Network Enhanced Adaptive Friction Compensation[C]//IEEE International Conference on Robotics and Automation. Orlando, 2006: 3699-3704.

[8] MELINTE O, MUNTEANU R, GAL I A, et al. Compensating Dynamics of Impedance Haptic Devices Using Neural Networks[C]//8th International Symposium on Advanced Topics in Electrical Engineering (ATEE). Bucharest, 2013: 1-6.

[9] 谢琦，潘博，付宜利，等.基于腹腔微创手术机器人的主从控制技术研究[J]. 机器人，2011，33(1)：53-58. XIE Qi, PAN Bo, FU Yili, et al. Master-slave Control Technology Research Based on Abdominal Minimally Invasive Surgery Robot[J]. Robot, 2011,33(1): 53-58.

[10] 黄勇.主-从异构遥操作系统的双向控制策略研究[D].长春：吉林大学，2014. HUANG Yong. Research on the Bilateral Control Strategy of Master-slave Isomerism Tele-operation System[D]. Changchun: Jilin University, 2014.

[11] CAVUSOGLU M C, FEYGIN D. Kinematics and Dynamics of Phantom (TM) Model 1.5 Haptic Interface[EB/OL]. University of California at Berkeley [2016-07-01]. http://citeseer.ist.psu.edu/viewdoc/download?doi=10.1.1.19.7619&rep=rep1&type=pdf.

[12] 蔡自兴.机器人学[M].3版.北京：清华大学出版社，2015：79-84. CAI Zixing. Robotics[M].3rd ed. Beijing: Tsinghua University Press, 2015: 79-84.

[13] KHALIL W, KLEINFIGER J. A New Geometric Notation for Open and Closed-loop Robots[C]// IEEE International Conference on Robotics and Automation. San Francisco, 1986: 1174-1179.

[14] 丁千，翟红梅.机械系统摩擦动力学研究进展[J].力学进展，2013，43(1)：112-131. DING Qian, ZHAI Hongmei. The Advance Researches of Friction Dynamic in Mechanics System[J]. Advances in Mechanics, 2013, 43(1): 112-131.

[15] THOMSEN J J, FIDIN A. Analytical Approximations for Stick-slip Vibration Amplitudes[J]. International Journal of Non-linear Mechanics, 2003, 38(3):389-403.

[16] TAHMASEBI A M, TAATI B, MOBASSER F, et al. Dynamic Parameter Identification and Analysis of a PHANToM Haptic Device[C]//Proceedings of 2005 IEEE Conference on Control Applications. Toronto, 2005: 1251-1256.

(编辑 王旻玥)

Research on Compensation Strategy for Induced Forces of Force Feedback Master Manipulators

YUN Jintian1,2LIU Yangyang1YANG Quanli3SANG Hongqiang1,2

1.School of Mechanical Engineering,Tianjin Polytechnic University,Tianjin,3003872.Advanced Mechatronics Equipment Technology Tianjin Area Major Laboratory,Tianjin,3003873.Tianjin University of Technology and Education,Tianjin,300222

Aiming at the problems that the connecting rod gravity, joint frictions and inertia forces of force feedback master manipulator affected doctor’s real perception of feedback force informations, a complete dynamics model was established. Based on the dynamics model of force feedback master manipulator, gravity compensation model, friction compensation model, and inertia force compensation model were established respectively. Force compensation strategies were used to eliminate the interferences to feedback forces from gravity, joint friction and inertia force of force feedback master manipulator in joint movements. Thus the doctor’s real force perception is realized and the accuracy of minimally invasive surgery is improved.

force feedback; induced force of master manipulator; compensation strategy; dynamics model; minimally invasive surgery

2016-07-04

国家自然科学基金资助项目(51205287)；天津市自然科学基金资助项目(16JCYBJC18400)

TP242.6

10.3969/j.issn.1004-132X.2017.10.004

贠今天，男，1970年生。天津工业大学机械工程学院教授、博士。主要研究方向为多体系统动力学与控制、机器视觉。发表论文30余篇。刘阳阳(通信作者)，男，1989年生。天津工业大学机械工程学院硕士研究生。E-mail: lyyvino@163.com。杨全利，男，1966年生。天津职业技术师范大学实验师。桑宏强，男，1978年生。天津工业大学机械工程学院副教授、博士。