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

盛國(guó)棟　曹其新　潘鐵文　冷春濤　顧　凱

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

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

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

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

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

主從式;機(jī)器人;力反饋;濾波;重力補(bǔ)償

0　引言

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

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

1　系統(tǒng)組成

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

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

2　力信息處理

圖2　傳感器安裝示意圖

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

2.1濾波

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

圖3　力傳感器輸出

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

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

0FG=[00-G]T

(1)

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

SFG=SR66R00FG

(2)

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

(3)

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

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

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

2.3重力補(bǔ)償

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

SFr=SFS-SFG

(4)

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

SFr=0R66RSSFr

(5)

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

(a)有重力補(bǔ)償

(b)無(wú)重力補(bǔ)償圖5　自由運(yùn)動(dòng)輸出

3　模糊融合力信息

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

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

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

F=K1F1+K2F2

(6)

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

圖7　位置誤差線性插值

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

4　實(shí)驗(yàn)

圖8　從手工具與脊柱模型

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

圖9　實(shí)驗(yàn)數(shù)據(jù)

5　結(jié)語(yǔ)

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

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

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

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

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

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

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

[4]鄧樂,趙丁選,倪濤.具有臨場(chǎng)感的主從機(jī)器人系統(tǒng)雙邊控制策略[J].吉林大學(xué)學(xué)報(bào)(工學(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]劉威，宋愛國(guó). 力-位置型力覺臨場(chǎng)感機(jī)器人變?cè)鲆婵刂萍捌浞€(wěn)定性研究[J]. 制造業(yè)自動(dòng)化，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

國(guó)家自然科學(xué)基金資助項(xiàng)目(81371650);國(guó)際熱核聚變實(shí)驗(yàn)堆(ITER)計(jì)劃資助項(xiàng)目(2011GB113005)

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

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