主从式机器人系统中力反馈的实现

盛国栋　曹其新　潘铁文　冷春涛　顾　凯

1.上海交通大学,上海,2002402.第二军医大学附属上海长征医院,上海,200003

主从式机器人系统中力反馈的实现

盛国栋1曹其新1潘铁文2冷春涛1顾凯1

1.上海交通大学,上海,2002402.第二军医大学附属上海长征医院,上海,200003

设计了一套适用于医疗和装备维修的主从式机器人系统，并对其力反馈实现进行了研究。采用六维力传感器获得了从端的受力情况，并对获得的力信息进行了滤波、离线坐标系标定和重力补偿等处理，提高了力反馈信息的准确性和抗干扰能力。为进一步减小主从位置误差对系统的影响，在力反馈中实时附加一个与主从位置误差反向的作用力,并将其与传感器获得的力信息进行线性融合。实验结果表明，该方法可以提高系统的力反馈性能，满足主从式机器人系统对力反馈的要求。

主从式;机器人;力反馈;滤波;重力补偿

0　引言

主从式机器人系统可广泛应用于远程医疗、装备维修、空间探索等领域,具有广阔的应用前景[1-3]。能否获得良好的力反馈效果是评价主从式机器人系统性能优劣的一个重要指标[4]。为了获得更好的力反馈效果,国内外学者对主从式机器人系统控制策略进行了相关研究[5-6]。为了减小末端工具尺寸，一些学者在主从式机器人系统双边控制中不是使用力传感器而是使用主从位置误差与滑模控制相结合的算法计算从端与环境的接触力[7-8]。这种方法需要对系统主手模型进行假设[9]，控制透明性差，适用于对力反馈要求不高的场所。为了获得更精确的力反馈效果，需要在工具末端附加力传感器。Song等[10]使用力传感器测量从端受力并运用于双边控制策略中，但并没有涉及提高力信息准确性的具体算法。Sung等[11]使用力传感器测量从端受力，提出了一种适用于二自由度机械臂的重力补偿算法，但该算法并不适用于多自由度机械臂机器人，且没有考虑机器人末端关节坐标系与传感器坐标系之间的不同，影响其重力补偿的正确性。

本文使用六维力传感器测量出从端实际力,将其与由主从位置误差引入的附加力相融合后作为系统的反馈力。该方法一方面通过对力传感器获得的力信息进行滤波、离线坐标系标定和重力补偿等处理，提高了信号的准确性和抗干扰能力；另一方面，采用模糊推理方法对由主从位置误差引入的附加力进行处理，再将力传感器测量的实际力与由主从位置误差引入的附加力进行线性融合并反馈至主手，获得较好的力反馈效果。

1　系统组成

如图1所示,该系统包括主手、从手、主控计算机、显示设备、从手前端工具(含力传感器)、网络摄像头、工具末端摄像头及网络传输部分等部件,采用主从操作方式。其中,主操作手为Force Dimension公司的Omega.7;从操作手为丹麦的Universal Robot;末端工具安装有ATI六维力传感器,可以获得末端工具与外界环境之间的实际作用力。整个过程中,网络摄像头获得从手四周的三维环境，工具末端摄像头为操作从手提供更精确的局部图像。二者的视频图像传输回主控计算机后，操作者可以通过显示设备实时观察从手的工作情况和三维环境中的障碍物情况，并根据障碍物分布遥操作从手,实现有效的避障。工具末端摄像头随末端工具一同移动，可反馈给操作着更加清晰的局部图像，提高从手的工作精度。

图1　主从式机器人系统

2　力信息处理

图2　传感器安装示意图

安装后,传感器坐标系与机器人最后一个关节坐标系的竖直方向(Z方向)是一致的,但是其水平方向(X方向、Y方向)的相互关系是任意的，因此需要进行坐标系标定。传感器的测量值包含末端工具的重力，如图2所示，实际应用中应将工具重力带来的力反馈值补偿掉。传感器坐标系的标定为离线过程，标定后，传感器测得的数据经过重力补偿和坐标变换，才能发送至主手，获得较好的力反馈效果。在标定和重力补偿之前，为提高信号的抗干扰能力以获得更好的力信息，应对传感器测得的原始信号进行巴特沃斯低通滤波。

2.1滤波

传感器在实际测量中,会受到外界干扰,导致测量数据波动较大,影响系统的力反馈控制。巴特沃斯滤波器在通频带内外都有平稳的幅频特性,为去除高频噪声的影响,选用巴特沃斯低通滤波器对传感器信号进行滤波。ATI传感器的采样频率为7 kHz,主从式力反馈中实际使用的频率为1 kHz。将滤波器的通带截至频率设为1 kHz,使用脉冲响应不变法设计滤波器,并在传感器空载的时候进行测试。从图3所示结果可以看出:滤波前,测量数据的波动范围为-0.17～0.17 N;滤波后,测量数据的波动范围为-0.09～0.11 N,有效减小了传感器测量值的数据波动。

图3　力传感器输出

2.2坐标系标定

传感器安装在机械臂的末端,测量值是在其自身坐标系下的读数,因此，为获得世界坐标系下的力信息,首先需要对传感器坐标系进行标定。机器人基坐标系、机器人末端关节坐标系以及传感器坐标系之间关系如图4所示。由于机器人基坐标系Z方向与重力方向相反，则末端工具的重力G在基坐标系中的表示为

0FG=[00-G]T

(1)

转换到传感器坐标系下可表示为

SFG=SR66R00FG

(2)

其中,6R0为机器人基坐标系到机器人末端关节坐标系的旋转矩阵,由机器人结构决定,本系统使用的6R型机械臂,其旋转矩阵可采用解析法求出;SR6为机器人末端关节坐标系到传感器坐标系的旋转矩阵,由于机器人末端关节坐标系与传感器坐标系Z轴方向重合(忽略安装误差),可将此旋转矩阵设为

(3)

式中,θ为机器人末端工具坐标系XT轴与传感器坐标系XS轴之间夹角。

图4　各坐标系转换关系

末端空载时,传感器读数都来自于末端工具的重力,即SFG可由传感器直接读出,因此联立式(1)～式(3)，可求出一组(cosθ,sinθ),变换机器人位置可求得多组(cosθ,sinθ),利用最小二乘法对结果进行优化,得到最优解,确定系统各坐标系之间的相互关系,完成坐标系标定。标定一次后，更换不同质量的末端工具不会影响系统的标定结果。

2.3重力补偿

传感器在实际使用中,其测量值SFS包括实际环境力在传感器坐标系下的值SFr与末端工具重力在传感器坐标系下的值SFG。因此，传感器坐标系下实际力的表达式为

SFr=SFS-SFG

(4)

进一步,转化至机器人基坐标系下:

SFr=0R66RSSFr

(5)

对本系统进行传感器标定和重力补偿后,实验测得机械臂在空载自由运动时,传感器Z方向输出的力如图5a所示,数据输出波动范围为-0.21～0.20 N。实验结果表明,该方法能够有效地补偿系统末端工具的重力,图5b所示为无重力补偿时传感器的输出。

(a)有重力补偿

(b)无重力补偿图5　自由运动输出

3　模糊融合力信息

系统双边控制中,为进一步减小主从位置误差对系统的影响，在系统的力反馈中实时附加一个与主从位置误差反向的作用力(附加力)。附加力的采样频率为10 Hz,如图6所示,并将附加力与传感器测量的力信息进行融合，共同完成系统的力反馈。

1.主手位置信息　2.从手控制及位置信息反馈3.主从位置误差　4.传感器原始数据5.传感器处理后数据　6.由位置误差引入的附加力7.融合后反馈至主手的实际力图6　系统控制框图

因本系统中,力传感器实际使用的采样频率为1 kHz,为了融合附加力与传感器测量的力信息,首先需要对主从位置误差(采样频率为10 Hz)进行处理。如图7所示,在相邻两个误差值之间使用直线拟合,形成连续的信号,再离散成频率为1 kHz的离散信号,得到的离散信号F2可与力传感器获得的信号F1进行融合,传统融合方法常使用线性融合[12]:

F=K1F1+K2F2

(6)

式中,K1、K2分别为力F1和F2的加权系数。

图7　位置误差线性插值

本文中,由传感器获得的力F1是从端工具与外界环境的实际力,因此设K1=1能获得最好的力觉临场感;附加力F2不是由传感器获得,而是由主从位置误差计算所得的。为了不影响系统的透明性,我们希望主从位置误差很大时，附加力的加权系数也较大，这样可以扩大反馈附加力，提高里反馈的控制效果。因加权系数随主从误差变化，所以本文采用模糊控制的方法，根据系统特点建立模糊规则,进而控制F1和F2的融合权重。本系统模糊规则将主从位置误差作为模糊控制的输入语言变量,根据误差大小分别表示为{很大，大，小，负大，负很大}，符号表示为U={HD,D,X,FD,FHD},输出变量K2={20,10,0,0,0},再由式(6)获得最终反馈至主手的力F。当主从位置误差的值为负很大(FHD)和负大(FD)的时候，不做附加力的补偿。主从位置误差为负大(FD),说明从手较主手运动超前，此时附加反馈力会使得操作滞黏感过强，严重影响人手的操作习惯。该主从系统用于精密加工时，由于从手作用力很小,可通过调整K1将力传感器反馈的信息做放大处理，使主手的力反馈能够达到人手适宜操作的力度,提高操作精度。

4　实验

图8　从手工具与脊柱模型

使用图1所示系统进行主从操作实验,通过操作力觉交互设备在图8所示的脊柱模型上打孔,测得的力与位置误差数据如图9所示。从图9可以看出,系统在自由运动、接触运动和钻孔运动过程中都能够有效地实现力反馈,在位置A,当主从位置误差增大时,系统反馈至主手的力在传感器所测力的基础上进一步增大。较大的反馈力对主手的运动起限制作用，防止主从误差的进一步增大，提高了系统的准确性。自由运动时，从端没有任何受力，但由于主从位置误差的存在，主手能够感受到反馈力的存在，虽然这会在一定程度上降低系统的透明性，但由于反馈力很小，所以不影响操作者正常操作，因此本系统在各种情况下都能够实现很好的力反馈。

图9　实验数据

5　结语

为获得更好的力反馈效果，设计了一套适用于医疗和装备维修的主从式机器人系统，并对其力反馈实现进行了研究。使用六维力传感器获得从端的受力情况，并对获得的力信息进行了滤波、离线坐标系标定和重力补偿等处理，提高了信号的准确性和抗干扰能力。此外，为进一步减小主从位置误差对系统的影响，在系统的力反馈中实时附加一个与主从位置误差反向的作用力，并与力传感器获得的力信息进行线性融合。该方法能够有效地实现系统的力反馈并能提高系统控制的准确性。

[1]冯石柱,赵丁选,尚涛,等.遥操作工程机器人p-f力反馈控制方法[J].农业机械学报,2008,39(2):120-124.

FengShizhu,ZhaoDingxuan,ShangTao,etal.Researchonp-fControlforConstructionTelerobotwithForceTelepresence[J].TransactionsoftheChineseSocietyforAgriculturalMachinery,2008,39(2):120-124.

[2]陈铁华,赵丁选,张祝新.主/从机器人系统设计与双向伺服控制[J].农业机械学报,2008,39(12):141-145.

ChenTiehua,ZhaoDingxuan,ZhangZhuxin.DesignofMaster-slaveManipulatorsSystemandBilateralServoControlStrategy[J].TransactionsoftheChineseSocietyforAgriculturalMachinery,2008,39(12):141-145.

[3]倪涛，赵丁选，张红彦. 电液手控器型遥操作机器人力反馈控制策略[J]. 农业机械学报,2010,41(12):190-194.

NiTao,ZhaoDingxuan,ZhangHongyan.BilateralServoControlofTele-robotBasedonElectroHydraulicManipulator[J].TransactionsoftheChineseSocietyforAgriculturalMachinery,2010,41(12):190-194.

[4]邓乐,赵丁选,倪涛.具有临场感的主从机器人系统双边控制策略[J].吉林大学学报(工学版),2006,36(5):681-685.DengLe,ZhaoDingxuan,NiTao.BilateralControlSchemeofMaster-slaveTeleroboticSystemwithTelepresence[J].JournalofJilinUniversity(EngineeringandTechnologyEdition),2006,36(5):681-685.[5]TavakoliM,PatelRV,MoallemM.BilateralControlofaTeleoperatorforSoftTissuePalpation:DesignandExperiments[C]//Proceedingsofthe2006IEEEInternationalConferenceonRoboticsandAutomation.Beijing,2006:3280-3285.

[6]刘威，宋爱国. 力-位置型力觉临场感机器人变增益控制及其稳定性研究[J]. 制造业自动化，2006,28(8):41-45.

LiuWei,SongAiguo.AGain-switchingControlSchemeBasedonForce-positionArchitectureforForce-reflectingTeleoperation[J].ManufacturingAutomation,2006,28(8):41-45.

[7]TachiS,SakakiT.ImpedanceControlledMaster-slaveManipulationSystem[J].JournalofRoboticsSocietyofJapan,1990,8(3):241-263.

[8]ParkJH,ChoHC.SlidingModeControlofBilateralTeleoperationSystemswithForce-reflectionontheInternet[C]//ProceedingoftheIEEE/RSJInternationalConferenceonIntelligentRobotsandSystems.Takamatsu,2000:1187-1192.

[9]SuangsamornN,ItthisekN.ARobustAdaptiveAlgorithmforBilateralControlSystemwithoutForceSensorwithTimeDelay[C]//SICEAnnualConference.Taipei,2010:678-683.

[10]SongGang,GuoShujiang.ANovelForceFeedback-basedTeleoperationSystemforMedicalApplication[J].InternationalJournalofInnovativeComputing,InformationandControl,2007,3(3):737-750.

[11]SungHoAhn,JiSupYoon.ABilateralControlSchemefor2-DOFTelemanipulatorswithControlInputSaturation[J].ControlEngineeringPractice,2002(10):1081-1090.

[12]TanimotoM,AraiF,FukudaT.ForceDisplayMethodforIntravascularNeurosurgery[C]//IEEEInternationalConferenceofSystems,ManandCybermetics.Piscataway,NJ,USA:IEEE,1999:1032-1037.

(编辑张洋)

Implementation of Force Feedback in Master-slave Robot Systems

Sheng Guodong1Cao Qixin1Pan Tiewen2Leng Chuntao1Gu Kai1

1.Shanghai Jiao Tong University,Shanghai,200240 2.Shanghai Changzheng Hospital Affiliated to Second Military Medical University,Shanghai,200003

A set of master-slave robot systems were designed for medical use or equipment maintenance,and its implementation of force feedback was studied.In order to obtain good force telepresence,a six-dimensional force sensor was used to get the forces from the slave site.And the force informations obtained were filtered to improve the noise immunity.Off-line coordinate system calibration and gravity compensation were done to improve the accuracy of the signals.In addition,to further reduce the negative impacts brought by the position errors of the master and slave sites,a force which is opposite with the position error was appended.Experimental results show that this method can meet the force telepresence requirements for the master-slave robot systems.

master-slave;robot;force-feedback;filtering;gravity compensation

2013-07-18

国家自然科学基金资助项目(81371650);国际热核聚变实验堆(ITER)计划资助项目(2011GB113005)

TP242.3DOI：10.3969/j.issn.1004-132X.2015.09.005

盛国栋,男,1989年生。上海交通大学机械与动力工程学院硕士研究生。主要研究方向为手术机器人、农业机器人。发表论文4篇。曹其新，男，1960年生。上海交通大学机械与动力工程学院教授。潘铁文(通信作者)，男，1969年生。第二军医大学附属上海长征医院副教授。冷春涛，男，1981年生。上海交通大学机械与动力工程学院助理研究员。顾凯，男，1990年生。上海交通大学机械与动力工程学院博士研究生。