主从遥操作机器人系统的附加反馈力补偿方法

贠今天 ，从敬德

（1.天津工业大学机械工程学院，天津 300387；2.天津工业大学天津市现代机电装备技术重点实验室，天津300387）

随着社会发展的迫切需要，微创外科手术已经发展为主从遥操作的一个重要应用领域[1-5].目前，微创外科手术机器人临床应用操作中存在的主要问题是缺乏力觉临场感[6-8]，使医生失去了真实的力觉感知，只能通过有限的视觉反馈来获取相应信息，影响了医生安全操作器官组织并实施恰当力控制的行为.研制具有力觉临场感的遥操作手术系统成为目前机器人手术发展的一个重点.

具有力觉临场感的遥操作指的是机器人主操作手不仅能给从操作手发送精确运动（位置）指令，而且能够将从操作手末端执行器上安装的力传感器检测到的接触力信息或无力（力矩）传感器情况下观测到的力信息反馈给操作者，被操作者精确感知.然而，在临床实际操作中，由于主操作手的动力学特性对从操作手的反馈力的影响以及反馈力和主操作手运动信息相互耦合对主操作手的遥操作位置指令产生影响，不仅使医生失去了真实的力觉感知，影响其实施恰当力控制的行为，而且位置（运动）不精确可能对人体器官造成伤害，从而降低了手术精度、手术质量与手术安全性.

在遥操作机器人控制系统中，临场感一直是众多学者研究追求的目标，国内外的学者、机构、商业公司从力反馈设备、主从遥操作控制结构和方法都做了很多研究工作.廖立等[9]设计了一种构型为串并联混合形式的主手，该主手操作灵活，工作空间大，提供3个自由度的力反馈.Ryu等[10]开发研制了一套6自由度力反馈装置，其全部力矩电机都布置在基座上，然后利用丝传动来传递力，以降低整个系统的惯量，从而降低重力及惯性力对力感知的影响.Ott等[11]开发了一套重力补偿系统（Zero-G），用来消除触感装置自身重力导致操作者肌肉疲劳对触觉感知的影响.Ryu等[12]针对6自由度3-PRPS并联型操作手关节处的摩擦，基于自适应控制策略提出一种非线性摩擦力补偿方法.Kuchenbecker等[13]建立了力反馈遥操作机器人的附加运动模型，并实时地对主操作手附加运动进行估计补偿，从而消除附加运动对系统稳定性和精准力反馈的影响.Park等[14]基于鲁棒控制策略，通过自适应阻抗控制算法补偿未知模型对反馈作用力的影响，从而获得高精度的反馈力.Li等[15]将滑模控制和扰动观测器的力补偿相结合用于处理各种建模的不确定性和外部环境力的影响.黄勇[16]提出了新型控制策略，以位置约束空间矩阵代替常数增益，结合主、从关节空间误差，得到准确的力反馈.Gao等[17]提出了一种新的力反馈手控制器系统，采用基于零力/力矩的大闭环力控制方法，有效地提高了反馈力/力矩的准确性.Takhmar等[18]提出了基于投影的力反射算法（PBFR），与传统的直接力反馈相比性能显著改善.

本文针对主从遥操作微创外科手术过程中，主操作手的动力学特性对从手端反馈力的影响，即主操作手的惯性力、重力和关节摩擦力与从手端真实的交互反馈力一起叠加被操作者感知，从而影响手术精度、质量和安全性的这一问题，提出了一种附加反馈力补偿方法.通过对模型化的主操作手动力学特性进行研究分析，基于力反馈主操作手PHANToM Desktop和万用机械手臂WAM arm搭建实验平台，并对从操作手与环境交互的反馈力进行补偿实验，验证本文提出的附加反馈力补偿方法的可行性和有效性.

1 主操作手动力学建模

本文探讨主操作手动力学特性对操作者感知反馈力的影响，并对附加反馈力进行补偿，首先要建立计及关节摩擦力的主操作手完整的动力学模型，并对其进行动力学分析.

1.1 拉格朗日建模

为搭建主从遥操作力反馈实验平台，本文以主操作手PHANToM Desktop为研究对象，其有6个自由度，末端能实现3个位置自由度的输出.由于本文只考虑位置问题，不涉及姿态部分，故只对其前3个位置自由度进行讨论.主操作手PHANToM Desktop的结构简图如图1所示.

图1 PHANToM Desktop结构简图Fig.1 Structural sketch of PHANToM Desktop

根据拉格朗日方程对主操作手进行动力学建模，忽略关节摩擦力，主操作手的动力学方程为：

机器人动力学系统可被线性参数化为：

式中 ：Iaxx、Iayy、Iazz、Idfxx、Idfyy、Idfzz、Icxx、Icyy、Iczz、Ibexx、Ibeyy、Ibezz、Ibaseyy、ma、mc、mbe、mdf、l1、l2、l3、l4、l5分别为主操作手PHANToM Desktop的物理参数和几何结构参数.

1.2 摩擦力模型

根据主操作手的设计结构，虽然其采用丝传动以减小摩擦，但在应用中不得不考虑关节摩擦的问题.本文选择Coulomb+Viscous摩擦的基本模型如下：

或者其线性化形式：

将关节摩擦力模型计入动力学方程（2），则主操作手完整的动力学方程可写成如下形式：

2 主操作手动力学参数辨识

为补偿主操作手的动力学特性所产生的附加反馈力，必须对其进行预测计算，所以需要对未知的动力学参数进行辨识验证.

2.1 动力学参数辨识

针对主操作手线性动力学方程（22），如果能够获得一些独立的数据点，可采用最小二乘法来求解.文献[19]指出，对于上述方程，其最小二乘法的解可用以下形式给出：

为采集这些数据，本文采用PD控制主操作手PHANToM Desktop沿着给定的理想激励轨迹进行运动，记录一段时间内主操作手的关节角、关节速度和电机力矩.由N组数据组成的回归矩阵和力矩矢量的形式如下：

利用式（25）进行动力学参数辨识时，需要测量主操作手的关节位置、关节速度、关节加速度和关节力矩.然而由于主操作手PHANToM Desktop并未安装直接测量关节速度和关节加速度的装置，一般根据数据采样频率，采用一次差分和二次差分的方法进行数值计算关节速度和关节加速度，但是二次差分计算会放大测量噪声，从而引入较大的误差.所以根据相关文献所述，使主操作手动力学模型（22）通过一个严格稳定的低通滤波器，其传递函数为：

式中：ω为低通滤波器的截止频率，且需要满足条件ω＞0.动力学模型通过低通滤波器如图2所示.

主操作手动力学模型通过低通滤波器之后，可以

图2 动力学模型通过低通滤波器Fig.2 Dynamic model passing through low-pass filter

得到：

式中：τL和YL（θ，θ˙）分别为经过低通滤波之后的关节力矩向量 τ和回归矩阵 Y（θ，θ˙，θ¨）.

对于一阶低通滤波器，通过正逆拉普拉斯变换，有如下关系：

式中：L[]和L-1[]分别表示拉普拉斯变换和拉普拉斯逆变换.

考虑一个形式如f（q）q¨j的函数通过一阶低通滤波器，其中 q=[q1，q2，…，qm]T且q˙t=0.首先，根据微分的定义，有如下关系：

然后，对等式（31）两边分别进行拉普拉斯变换可以得到式（32）：

最后，对等式（32）两边分别乘以 L（s）可得：

根据式（33），主操作手PHANToM Desktop的动力学模型经过一阶低通滤波器后得到的回归矩阵如下：

参数辨识的实验装置包括一台Windows系统的DELL PC台式机、一台主操作手PHANToM Desktop.主操作手通过PCI并行端口与PC连接，基于主操作手设备厂商提供的API接口，本文采用PD控制器来控制主操作手按照给定的理想轨迹进行运动.PD控制器如下：

式中：比例参数 kP=diag[1 400，3 000，1 600]；微分参数 kD=diag[18，26，17].

本文选取信号频率为ω=1～4.5 rad/s，同时根据主操作手的工作范围，选择规划的期望轨迹θd（rad）和期望速度θ˙d（rad/s）如下：

在本文的辨识实验中，设备工作频率和数据采集频率均为1 kHz.一阶低通滤波器的截止频率选择为ω=10 rad/s，该频率比激励信号的带宽高，比噪声信号的频率范围低，因此，数据采集满足采样定理且能滤掉噪声信号.整个实验过程用时40 s，前10 s采集的数据用于动力学参数辨识，后30 s采集的数据用于估计关节力矩以验证辨识的动力学参数.

将采集的关节位置、关节速度和关节力矩数据导入Matlab中，经过低通滤波器处理，采用最小二乘法并通过编写程序求得18个动力学参数的估计值，如表1所示.

表1 PHANToM Desktop动力学参数估计Tab.1 Estimated dynamic parameters of PHANToM Desktop

2.2 力矩估计及动力学参数验证

通过动力学参数辨识，可以得到18个参数的估计值.为验证参数的可靠性，需要用辨识的动力学参数来估计各个关节的滤波力矩，并与实际测量的滤波关节力矩进行对比，如图3所示.需要指出的是，此处测量的滤波关节力矩为采集电机接收的控制力矩的滤波值.

估计关节的滤波力矩可由式（40）求得：

根据文献[19]，滤波估计力矩的相对均方根误差（RMS）表示为：

经计算，主操作手PHANToM Desktop各关节滤波估计力矩的相对均方根误差如表2所示.

图3 PHANToM Desktop各关节测量力矩与估计力矩对比Fig.3 Comparison between measured torques and estimated torques for each joint of PHANToM Desktop

表2 各关节的滤波估计力矩的相对均方根误差Tab.2 RMS errors of filtered estimated torque for each joint

通过图3和表2可以看出，关节2和关节3的力矩误差相对较小，而关节1的力矩误差相对明显.分析其原因，需要指出的是：由于本文只考虑主操作手的位置部分，不涉及姿态部分，而PHANToM Desktop末端姿态输入的Gimbal万向节和尖笔Stylus无法拆除.按照Geomagic公司工程师的建议，参数辨识实验过程中，将末端姿态部分固定起来，这部分结构未进行建模处理，这样便增加了系统的惯量.从而影响主操作手的动态特性，其累加作用于关节1；另外，由于在建模过程中对一些物理结构进行了简化处理，导致了数学模型的不精确误差.但总体而言，由图3和表2可以得出，主操作手的动力学模型基本正确，动力学参数估计可信.

3 主操作手附加反馈力补偿策略

主操作手的动力学特性会影响操作者感知从操作手与环境相互作用的反馈力，即操作者感知的反馈力中包含了主操作手的惯性力、重力和关节处的摩擦力.

主操作手附加反馈力如图4所示.

图4 主操作手附加反馈力Fig.4 Induced feedback force of master manipulator

由图4可以看出，从操作手真实反馈力矩τf、附加反馈力矩τi及主操作手的电机实际输出转矩τ之间存在如下关系：

式中：力矩经主操作手转换为交互作用力Ff和附加反馈力Fi一起反馈给操作者，其中Fi包括主操作手运动部件的重力、惯性力及关节处的摩擦力.因此，在主从遥操作控制的过程中需要对上述的附加反馈力进行补偿，具体的补偿策略如图5所示.

图5 主操作手附加反馈力补偿策略Fig.5 Compensation strategy of induced feedback force for master manipulator

图5中：Hf（s）为交互作用反馈力到主手运动的传递函数；Hi（s）为附加反馈力到主手运动的传递函数；Hˆi（s）为经过参数估计的主操作手动力学特性传递函数，其根据主手的运动状态估计出附加反馈力，通过在电机上施加反向补偿力矩，驱使主手产生补偿运动xˆhi，进而消除附加反馈力产生的附加运动 xhi，使主手运动更接近真实反馈运动xhf，从而消除附加反馈力对从手端反馈力的影响，使操作者感知从操作手与环境之间真实的交互作用力.

4 实验验证及结果分析

为验证附加反馈力补偿方法，现搭建主从遥操作力反馈实验平台，如图6所示.主操作手（PHANToM Desktop）通过PCI并行端口与上位机（DELL PC）连接，上位机和下位机（WAM PC104）之间通过无线局域网（WLAN）进行连接.基于UDP通讯协议，在保证通讯实时性的情况下，上位机和下位机之间进行数据传输.从操作手（WAM Arm）末端安装有六轴Force/Torque传感器，能采集从手末端的三维轴向力.同时，主操作手为力反馈设备，能够实时输出三维轴向力.

图6 主从遥操作力反馈实验平台Fig.6 Experiment platform for force feedback of master-slave teleoperation

实验平台搭建完成后，设计力反馈实验.主从遥操作控制系统采用精确位置跟踪控制来实现从手跟踪主手位置并与环境交互接触，并将传感器采集的反馈力数据返回主手并输出，从而让操作者感知，系统控制框图如图7所示.

图7 主从遥操作控制系统框图Fig.7 Block diagram of master-slave teleoperation control system

控制系统中数据采集、数据传输和设备的工作频率均为500 Hz.本文设计的力反馈实验过程如下：操作者通过主手精确位置控制从手运动，并使从手末端的Haptic Ball去接触一个提前放置好的物体，在此过程中，交互作用力被记录并发送至主手，然后通过电机输出被操作者感知.实验过程中，主手的位置、主手的电机输出力矩和从手的传感器数据被实时记录用于实验分析，整个实验持续时间为4 min，将上述数据导入Matlab中进行处理分析.

利用表1中辨识的动力学参数、记录的主手位置信息以及数据采集频率，对实验过程中主操作手的附加反馈力矩进行估计，然后通过转置雅可比转换可得到X、Y、Z 3个方向的附加反馈力，如图8所示.

图8 主操作手附加反馈力Fig.8 Induced feedback force for master manipulator

将从手端传感器记录的三维力和主手电机输出的三维力进行比较，如图9所示.

由图9可以看出，主手输出操作者感知的反馈力和真实的交互作用反馈力之间存在明显的误差，尤其是Y方向附加反馈力较为明显.需要说明的是，由于主操作手的物理结构较小和无法模拟真实的手术环境，因此，主操作手产生的附加反馈力与从操作手接触反馈力相比较小，但从数值上可以看出主手输出操作者感知的反馈力和真实的交互作用反馈力之间确实存在误差，这正是本文提出的进一步提高反馈力精准度的补偿方法的根据.

图9 真实反馈力和主手输出反馈力对比Fig.9 Comparison between real feedback force and master manipulator output feedback force

通过主操作手的附加反馈力补偿，将补偿后的主手输出反馈力和从手真实的交互作用反馈力进行对比，如图10所示，发现误差明显减小.

根据式（41）对相对均方根误差的定义，同样有主操作手输出反馈力与真实反馈力的相对均方根误差和主操作手补偿反馈力与真实反馈力的相对均方根误差.通过Matlab进行编程计算，可以得到2组相对均方根误差如表3所示.

表3 X、Y、Z 3方向上主手反馈力补偿前后相对均方根误差对比Tab.3 Comparison of RMS errors in X，Y，Z direction before and after feedback force compensation for master manipulator %

图10 真实反馈力和补偿后的主手输出反馈力对比Fig.10 Comparison between real feedback force and compensated feedback force of master manipulator

根据表3中的相对均方根误差进行对比分析，相比未补偿之前，经过附加反馈力补偿之后主操作手X、Y、Z 3个方向上的反馈力精度分别提升3.9%、5%和2.1%.实验结果说明本文提出的附加反馈力补偿方法可行且有效，能够进一步提高主从遥操作反馈力的精度.

5 结 语

本文针对微创外科手术过程中，主操作手的动力学特性影响操作者感知真实反馈力的问题，提出了一种附加反馈力补偿方法.从主操作手的动力学模型出发，基于现有的力反馈设备PHANToM Desktop，建立计及关节摩擦力的完整动力学模型，并对模型的动力学参数进行了辨识和验证.在此基础上，提出了具体的附加反馈力补偿策略.在已知主手动力学模型的基础上，搭建主从遥操作力反馈实验平台，并进行主从遥操作附加反馈力补偿实验.将主手输出反馈力和从手真实的反馈力进行对比发现，经过附加反馈力补偿之后，主操作手X、Y、Z 3个方向上的反馈力精度较未补偿之前分别提升3.9%、5.0%和2.1%.实验结果表明，操作者感知反馈力的精确度比未补偿时有明显提升，说明本文提出的附加反馈力补偿方法是可行且有效的.

下一步将在本文工作的基础上，设计更接近外科手术实际状况的实验进行补偿方法验证.同时，研究分析反馈力与主操作手运动耦合而产生附加位移，进而影响位置跟踪精度等问题.

[1]IBRAHIM A M，MAKARY M A.Robot-assisted surgery and health care costs[J].New England Journal of Medicine，2010，363（22）：2174-2179.

[2]戴建生，魏国武，李建民.国际微创手术机器人的现状和发展趋势[J].机器人技术与应用，2011（4）：1-5.DAI J S，WEI G W，LI J M.Current situation and development trend of the international minimally invasive surgery robot[J].Robot Technique and Application，2011（4）：1-5（in Chinese）.

[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]DARIO P，GUGLIELMELLI E，ALLOTTA B，et al.Robotics for medical applications[J].IEEE Robotics&Amp Amp Automation Magazine，1996，3（3）：44-56.

[5]TAYLOR R H.Medical Robotics and Computer-Integrated Surgery[M].Berlin：Springer，2008.

[6]YU Y，CHEN H Y，LOU Y J，et al.Remote master-slave control of a 6D manipulator for cardiac surgery application[C]//2014 IEEE International Conference on Robotics and Biomimetics.Bali：ROBIO，2014：1799-1804.

[7]徐兆红，宋成利，闫士举.机器人在微创外科手术中的应用[J].中国组织工程研究，2011，15（35）：6598-6601.XU Z H，SONG C L，YAN S J.Research on minimally invasive robotic surgery[J].Journal of Clinical Rehabilitative Tissue Engineering Research，2011，15（35）：6598-6601（in Chinese）.

[8]CHOI J，PARK J W，DONG J K，et al.Lapabot：A compact telesurgical robot system for minimally invasive surgery：Part I.System description[J].Minimally Invasive Therapy&Allied Technologies Mitat Official Journal of the Society for Minimally Invasive Therapy，2012，21（3）：188-194.

[9]廖立，闫志远，杜志江，等.面向机器人辅助远程手术的遥操作主手研究[J].中国医疗设备，2014（8）：11-13.LIAO L，YAN Z Y，DU Z J，et al.Research on teleoperation master manipulator for robot assisted remote operation[J].China Medical Devices，2014（8）：11-13（in Chinese）.

[10]RYU D，SONG J B，CHO C，et al.Development of a six DOF haptic master for teleoperation of a mobile manipulator[J].Mechatronics，2010，20（2）：181-191.

[11]OTT R，GUTIERREZ M，THALMANN D，et al.Improving user comfort in haptic virtual environments through gravity compensation[C]//Proceedings of the first Joint Eurohaptics Conference and Symposium on Haptic Interfaces for Virtual Environment and Teleoperator Systems.Washington DC：IEEE Computer Society，2005，82（3）：401-409.

[12]RYU J H，SONG J，KWON D S.A nonlinear friction compensation method using adaptive control and its practical application to an in-parallel actuated 6-DOF manipulator[J].Control Engineering Practice，2001，9（2）：159-167.

[13]KUCHENBECKER K J，NIEMEYER G.Modeling induced master motion in force-reflecting teleoperation[J].Journal of Dynamic Systems Measurement and Control，2004，128（4）：348-353.

[14]PARK H，LEE J M.Adaptive impedance control of a haptic interface[J].Mechatronics，2004，14（3）：237-253.

[15]LI S H，ZHOU M M，YU X H.Design and implementation of terminal sliding mode control method for PMSM speed regulation system[J].IEEE Transactions on Industrial Informatics，2013，9（4）：1879-1891.

[16]黄勇.主—从异构遥操作系统的双向控制策略研究 [D].长春：吉林大学，2014.HUANG Y.Research on the bilateral control strategy of master-slave isomerism teleoperation system[D].Changchun：Jilin University，2014（in Chinese）.

[17]GAOX，WANGYF，SONGJZ，etal.Researchofanew6-D of force feedback hand controller system[J].Journal of Robotics，2014，2014：1-9.

[18]TAKHMAR A，POLUSHIN I G，TALASAZ A，et al.Cooperative teleoperation with projection-based force reflection for MIS[J].IEEE Transactions on Control Systems Technology，2015，23（4）：1411-1426.

[19]TAATI B，TAHMASEBI A M，HASHTRUDI-ZAAD K.Experimental identification and analysis of the dynamics of a phantom premium 1.5a haptic device[J].Presence Teleoperators&Virtual Environments，2008，17（4）：327-343.