主從式機器人系統中力反饋的實現

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

1.上海交通大學,上海,2002402.第二軍醫大學附屬上海長征醫院,上海,200003

主從式機器人系統中力反饋的實現

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

1.上海交通大學,上海,2002402.第二軍醫大學附屬上海長征醫院,上海,200003

設計了一套適用于醫療和裝備維修的主從式機器人系統，并對其力反饋實現進行了研究。采用六維力傳感器獲得了從端的受力情況，并對獲得的力信息進行了濾波、離線坐標系標定和重力補償等處理，提高了力反饋信息的準確性和抗干擾能力。為進一步減小主從位置誤差對系統的影響，在力反饋中實時附加一個與主從位置誤差反向的作用力,并將其與傳感器獲得的力信息進行線性融合。實驗結果表明，該方法可以提高系統的力反饋性能，滿足主從式機器人系統對力反饋的要求。

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

0　引言

主從式機器人系統可廣泛應用于遠程醫療、裝備維修、空間探索等領域,具有廣闊的應用前景[1-3]。能否獲得良好的力反饋效果是評價主從式機器人系統性能優劣的一個重要指標[4]。為了獲得更好的力反饋效果,國內外學者對主從式機器人系統控制策略進行了相關研究[5-6]。為了減小末端工具尺寸，一些學者在主從式機器人系統雙邊控制中不是使用力傳感器而是使用主從位置誤差與滑?？刂葡嘟Y合的算法計算從端與環境的接觸力[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年生。上海交通大學機械與動力工程學院博士研究生。