主從式機器人系統(tǒng)中力反饋的實現(xiàn)

盛國棟　曹其新　潘鐵文　冷春濤　顧　凱

1.上海交通大學(xué),上海,2002402.第二軍醫(yī)大學(xué)附屬上海長征醫(yī)院,上海,200003

主從式機器人系統(tǒng)中力反饋的實現(xiàn)

盛國棟1曹其新1潘鐵文2冷春濤1顧凱1

1.上海交通大學(xué),上海,2002402.第二軍醫(yī)大學(xué)附屬上海長征醫(yī)院,上海,200003

設(shè)計了一套適用于醫(yī)療和裝備維修的主從式機器人系統(tǒng)，并對其力反饋實現(xiàn)進行了研究。采用六維力傳感器獲得了從端的受力情況，并對獲得的力信息進行了濾波、離線坐標(biāo)系標(biāo)定和重力補償?shù)忍幚?，提高了力反饋信息的?zhǔn)確性和抗干擾能力。為進一步減小主從位置誤差對系統(tǒng)的影響，在力反饋中實時附加一個與主從位置誤差反向的作用力,并將其與傳感器獲得的力信息進行線性融合。實驗結(jié)果表明，該方法可以提高系統(tǒng)的力反饋性能，滿足主從式機器人系統(tǒng)對力反饋的要求。

主從式;機器人;力反饋;濾波;重力補償

0　引言

主從式機器人系統(tǒng)可廣泛應(yīng)用于遠程醫(yī)療、裝備維修、空間探索等領(lǐng)域,具有廣闊的應(yīng)用前景[1-3]。能否獲得良好的力反饋效果是評價主從式機器人系統(tǒng)性能優(yōu)劣的一個重要指標(biāo)[4]。為了獲得更好的力反饋效果,國內(nèi)外學(xué)者對主從式機器人系統(tǒng)控制策略進行了相關(guān)研究[5-6]。為了減小末端工具尺寸，一些學(xué)者在主從式機器人系統(tǒng)雙邊控制中不是使用力傳感器而是使用主從位置誤差與滑?？刂葡嘟Y(jié)合的算法計算從端與環(huán)境的接觸力[7-8]。這種方法需要對系統(tǒng)主手模型進行假設(shè)[9]，控制透明性差，適用于對力反饋要求不高的場所。為了獲得更精確的力反饋效果，需要在工具末端附加力傳感器。Song等[10]使用力傳感器測量從端受力并運用于雙邊控制策略中，但并沒有涉及提高力信息準(zhǔn)確性的具體算法。Sung等[11]使用力傳感器測量從端受力，提出了一種適用于二自由度機械臂的重力補償算法，但該算法并不適用于多自由度機械臂機器人，且沒有考慮機器人末端關(guān)節(jié)坐標(biāo)系與傳感器坐標(biāo)系之間的不同，影響其重力補償?shù)恼_性。

本文使用六維力傳感器測量出從端實際力,將其與由主從位置誤差引入的附加力相融合后作為系統(tǒng)的反饋力。該方法一方面通過對力傳感器獲得的力信息進行濾波、離線坐標(biāo)系標(biāo)定和重力補償?shù)忍幚恚岣吡诵盘柕臏?zhǔn)確性和抗干擾能力；另一方面，采用模糊推理方法對由主從位置誤差引入的附加力進行處理，再將力傳感器測量的實際力與由主從位置誤差引入的附加力進行線性融合并反饋至主手，獲得較好的力反饋效果。

1　系統(tǒng)組成

如圖1所示,該系統(tǒng)包括主手、從手、主控計算機、顯示設(shè)備、從手前端工具(含力傳感器)、網(wǎng)絡(luò)攝像頭、工具末端攝像頭及網(wǎng)絡(luò)傳輸部分等部件,采用主從操作方式。其中,主操作手為Force Dimension公司的Omega.7;從操作手為丹麥的Universal Robot;末端工具安裝有ATI六維力傳感器,可以獲得末端工具與外界環(huán)境之間的實際作用力。整個過程中,網(wǎng)絡(luò)攝像頭獲得從手四周的三維環(huán)境，工具末端攝像頭為操作從手提供更精確的局部圖像。二者的視頻圖像傳輸回主控計算機后，操作者可以通過顯示設(shè)備實時觀察從手的工作情況和三維環(huán)境中的障礙物情況，并根據(jù)障礙物分布遙操作從手,實現(xiàn)有效的避障。工具末端攝像頭隨末端工具一同移動，可反饋給操作著更加清晰的局部圖像，提高從手的工作精度。

圖1　主從式機器人系統(tǒng)

2　力信息處理

圖2　傳感器安裝示意圖

安裝后,傳感器坐標(biāo)系與機器人最后一個關(guān)節(jié)坐標(biāo)系的豎直方向(Z方向)是一致的,但是其水平方向(X方向、Y方向)的相互關(guān)系是任意的，因此需要進行坐標(biāo)系標(biāo)定。傳感器的測量值包含末端工具的重力，如圖2所示，實際應(yīng)用中應(yīng)將工具重力帶來的力反饋值補償?shù)?。傳感器坐?biāo)系的標(biāo)定為離線過程，標(biāo)定后，傳感器測得的數(shù)據(jù)經(jīng)過重力補償和坐標(biāo)變換，才能發(fā)送至主手，獲得較好的力反饋效果。在標(biāo)定和重力補償之前，為提高信號的抗干擾能力以獲得更好的力信息，應(yīng)對傳感器測得的原始信號進行巴特沃斯低通濾波。

2.1濾波

傳感器在實際測量中,會受到外界干擾,導(dǎo)致測量數(shù)據(jù)波動較大,影響系統(tǒng)的力反饋控制。巴特沃斯濾波器在通頻帶內(nèi)外都有平穩(wěn)的幅頻特性,為去除高頻噪聲的影響,選用巴特沃斯低通濾波器對傳感器信號進行濾波。ATI傳感器的采樣頻率為7 kHz,主從式力反饋中實際使用的頻率為1 kHz。將濾波器的通帶截至頻率設(shè)為1 kHz,使用脈沖響應(yīng)不變法設(shè)計濾波器,并在傳感器空載的時候進行測試。從圖3所示結(jié)果可以看出:濾波前,測量數(shù)據(jù)的波動范圍為-0.17～0.17 N;濾波后,測量數(shù)據(jù)的波動范圍為-0.09～0.11 N,有效減小了傳感器測量值的數(shù)據(jù)波動。

圖3　力傳感器輸出

2.2坐標(biāo)系標(biāo)定

傳感器安裝在機械臂的末端,測量值是在其自身坐標(biāo)系下的讀數(shù),因此，為獲得世界坐標(biāo)系下的力信息,首先需要對傳感器坐標(biāo)系進行標(biāo)定。機器人基坐標(biāo)系、機器人末端關(guān)節(jié)坐標(biāo)系以及傳感器坐標(biāo)系之間關(guān)系如圖4所示。由于機器人基坐標(biāo)系Z方向與重力方向相反，則末端工具的重力G在基坐標(biāo)系中的表示為

0FG=[00-G]T

(1)

轉(zhuǎn)換到傳感器坐標(biāo)系下可表示為

SFG=SR66R00FG

(2)

其中,6R0為機器人基坐標(biāo)系到機器人末端關(guān)節(jié)坐標(biāo)系的旋轉(zhuǎn)矩陣,由機器人結(jié)構(gòu)決定,本系統(tǒng)使用的6R型機械臂,其旋轉(zhuǎn)矩陣可采用解析法求出;SR6為機器人末端關(guān)節(jié)坐標(biāo)系到傳感器坐標(biāo)系的旋轉(zhuǎn)矩陣,由于機器人末端關(guān)節(jié)坐標(biāo)系與傳感器坐標(biāo)系Z軸方向重合(忽略安裝誤差),可將此旋轉(zhuǎn)矩陣設(shè)為

(3)

式中,θ為機器人末端工具坐標(biāo)系XT軸與傳感器坐標(biāo)系XS軸之間夾角。

圖4　各坐標(biāo)系轉(zhuǎn)換關(guān)系

末端空載時,傳感器讀數(shù)都來自于末端工具的重力,即SFG可由傳感器直接讀出,因此聯(lián)立式(1)～式(3)，可求出一組(cosθ,sinθ),變換機器人位置可求得多組(cosθ,sinθ),利用最小二乘法對結(jié)果進行優(yōu)化,得到最優(yōu)解,確定系統(tǒng)各坐標(biāo)系之間的相互關(guān)系,完成坐標(biāo)系標(biāo)定。標(biāo)定一次后，更換不同質(zhì)量的末端工具不會影響系統(tǒng)的標(biāo)定結(jié)果。

2.3重力補償

傳感器在實際使用中,其測量值SFS包括實際環(huán)境力在傳感器坐標(biāo)系下的值SFr與末端工具重力在傳感器坐標(biāo)系下的值SFG。因此，傳感器坐標(biāo)系下實際力的表達式為

SFr=SFS-SFG

(4)

進一步,轉(zhuǎn)化至機器人基坐標(biāo)系下:

SFr=0R66RSSFr

(5)

對本系統(tǒng)進行傳感器標(biāo)定和重力補償后,實驗測得機械臂在空載自由運動時,傳感器Z方向輸出的力如圖5a所示,數(shù)據(jù)輸出波動范圍為-0.21～0.20 N。實驗結(jié)果表明,該方法能夠有效地補償系統(tǒng)末端工具的重力,圖5b所示為無重力補償時傳感器的輸出。

(a)有重力補償

(b)無重力補償圖5　自由運動輸出

3　模糊融合力信息

系統(tǒng)雙邊控制中,為進一步減小主從位置誤差對系統(tǒng)的影響，在系統(tǒng)的力反饋中實時附加一個與主從位置誤差反向的作用力(附加力)。附加力的采樣頻率為10 Hz,如圖6所示,并將附加力與傳感器測量的力信息進行融合，共同完成系統(tǒng)的力反饋。

1.主手位置信息　2.從手控制及位置信息反饋3.主從位置誤差　4.傳感器原始數(shù)據(jù)5.傳感器處理后數(shù)據(jù)　6.由位置誤差引入的附加力7.融合后反饋至主手的實際力圖6　系統(tǒng)控制框圖

因本系統(tǒng)中,力傳感器實際使用的采樣頻率為1 kHz,為了融合附加力與傳感器測量的力信息,首先需要對主從位置誤差(采樣頻率為10 Hz)進行處理。如圖7所示,在相鄰兩個誤差值之間使用直線擬合,形成連續(xù)的信號,再離散成頻率為1 kHz的離散信號,得到的離散信號F2可與力傳感器獲得的信號F1進行融合,傳統(tǒng)融合方法常使用線性融合[12]:

F=K1F1+K2F2

(6)

式中,K1、K2分別為力F1和F2的加權(quán)系數(shù)。

圖7　位置誤差線性插值

本文中,由傳感器獲得的力F1是從端工具與外界環(huán)境的實際力,因此設(shè)K1=1能獲得最好的力覺臨場感;附加力F2不是由傳感器獲得,而是由主從位置誤差計算所得的。為了不影響系統(tǒng)的透明性,我們希望主從位置誤差很大時，附加力的加權(quán)系數(shù)也較大，這樣可以擴大反饋附加力，提高里反饋的控制效果。因加權(quán)系數(shù)隨主從誤差變化，所以本文采用模糊控制的方法，根據(jù)系統(tǒng)特點建立模糊規(guī)則,進而控制F1和F2的融合權(quán)重。本系統(tǒng)模糊規(guī)則將主從位置誤差作為模糊控制的輸入語言變量,根據(jù)誤差大小分別表示為{很大，大，小，負大，負很大}，符號表示為U={HD,D,X,FD,FHD},輸出變量K2={20,10,0,0,0},再由式(6)獲得最終反饋至主手的力F。當(dāng)主從位置誤差的值為負很大(FHD)和負大(FD)的時候，不做附加力的補償。主從位置誤差為負大(FD),說明從手較主手運動超前，此時附加反饋力會使得操作滯黏感過強，嚴(yán)重影響人手的操作習(xí)慣。該主從系統(tǒng)用于精密加工時，由于從手作用力很小,可通過調(diào)整K1將力傳感器反饋的信息做放大處理，使主手的力反饋能夠達到人手適宜操作的力度,提高操作精度。

4　實驗

圖8　從手工具與脊柱模型

使用圖1所示系統(tǒng)進行主從操作實驗,通過操作力覺交互設(shè)備在圖8所示的脊柱模型上打孔,測得的力與位置誤差數(shù)據(jù)如圖9所示。從圖9可以看出,系統(tǒng)在自由運動、接觸運動和鉆孔運動過程中都能夠有效地實現(xiàn)力反饋,在位置A,當(dāng)主從位置誤差增大時,系統(tǒng)反饋至主手的力在傳感器所測力的基礎(chǔ)上進一步增大。較大的反饋力對主手的運動起限制作用，防止主從誤差的進一步增大，提高了系統(tǒng)的準(zhǔn)確性。自由運動時，從端沒有任何受力，但由于主從位置誤差的存在，主手能夠感受到反饋力的存在，雖然這會在一定程度上降低系統(tǒng)的透明性，但由于反饋力很小，所以不影響操作者正常操作，因此本系統(tǒng)在各種情況下都能夠?qū)崿F(xiàn)很好的力反饋。

圖9　實驗數(shù)據(jù)

5　結(jié)語

為獲得更好的力反饋效果，設(shè)計了一套適用于醫(yī)療和裝備維修的主從式機器人系統(tǒng)，并對其力反饋實現(xiàn)進行了研究。使用六維力傳感器獲得從端的受力情況，并對獲得的力信息進行了濾波、離線坐標(biāo)系標(biāo)定和重力補償?shù)忍幚?，提高了信號的?zhǔn)確性和抗干擾能力。此外，為進一步減小主從位置誤差對系統(tǒng)的影響，在系統(tǒng)的力反饋中實時附加一個與主從位置誤差反向的作用力，并與力傳感器獲得的力信息進行線性融合。該方法能夠有效地實現(xiàn)系統(tǒng)的力反饋并能提高系統(tǒng)控制的準(zhǔn)確性。

[1]馮石柱,趙丁選,尚濤,等.遙操作工程機器人p-f力反饋控制方法[J].農(nóng)業(yè)機械學(xué)報,2008,39(2):120-124.

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

[2]陳鐵華,趙丁選,張祝新.主/從機器人系統(tǒng)設(shè)計與雙向伺服控制[J].農(nóng)業(yè)機械學(xué)報,2008,39(12):141-145.

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

[3]倪濤，趙丁選，張紅彥. 電液手控器型遙操作機器人力反饋控制策略[J]. 農(nóng)業(yè)機械學(xué)報,2010,41(12):190-194.

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

[4]鄧樂,趙丁選,倪濤.具有臨場感的主從機器人系統(tǒng)雙邊控制策略[J].吉林大學(xué)學(xué)報(工學(xué)版),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]劉威，宋愛國. 力-位置型力覺臨場感機器人變增益控制及其穩(wěn)定性研究[J]. 制造業(yè)自動化，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

國家自然科學(xué)基金資助項目(81371650);國際熱核聚變實驗堆(ITER)計劃資助項目(2011GB113005)

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

盛國棟,男,1989年生。上海交通大學(xué)機械與動力工程學(xué)院碩士研究生。主要研究方向為手術(shù)機器人、農(nóng)業(yè)機器人。發(fā)表論文4篇。曹其新，男，1960年生。上海交通大學(xué)機械與動力工程學(xué)院教授。潘鐵文(通信作者)，男，1969年生。第二軍醫(yī)大學(xué)附屬上海長征醫(yī)院副教授。冷春濤，男，1981年生。上海交通大學(xué)機械與動力工程學(xué)院助理研究員。顧凱，男，1990年生。上海交通大學(xué)機械與動力工程學(xué)院博士研究生。