王宏民,　呂雄飛,　王茂生,　蘇鳳武,　康紅明

(1.黑龍江科技大學(xué) 電氣與控制工程學(xué)院, 哈爾濱 150022;2.黑龍江建筑職業(yè)技術(shù)學(xué)院 機(jī)電工程技術(shù)學(xué)院, 哈爾濱 150025)

?

主操作手重力補(bǔ)償下的時(shí)延控制

王宏民1,呂雄飛1,王茂生2,蘇鳳武1,康紅明1

(1.黑龍江科技大學(xué) 電氣與控制工程學(xué)院, 哈爾濱 150022;2.黑龍江建筑職業(yè)技術(shù)學(xué)院 機(jī)電工程技術(shù)學(xué)院, 哈爾濱 150025)

針對(duì)自主研制的應(yīng)用于外科手術(shù)的三維力反饋型主操作手的重力補(bǔ)償問(wèn)題,對(duì)主操作端控制中產(chǎn)生的時(shí)延進(jìn)行深入分析,利用多線程、多事件技術(shù)盡可能降低主操作端的時(shí)間時(shí)延。通過(guò)矢量分析的方法得到電機(jī)為平衡位置機(jī)構(gòu)靜力平衡所需的輸出力矩。分別在ADMAS和MATLAB環(huán)境下,進(jìn)行仿真對(duì)比分析。仿真結(jié)果驗(yàn)證了建立的系統(tǒng)動(dòng)力學(xué)模型的正確性。該重力補(bǔ)償方法可以滿足主操作手的重力平衡要求。

主端機(jī)器人; 重力平衡; 時(shí)延控制; 外科手術(shù)

0　引　言

近年來(lái),隨著外科手術(shù)機(jī)器人技術(shù)的發(fā)展,眾多學(xué)者研究主操作手作為系統(tǒng)中的力反饋設(shè)備在重力補(bǔ)償方面,取得了一定的成果。Nathan和Herve利用配重方法對(duì)串聯(lián)機(jī)構(gòu)實(shí)現(xiàn)重力補(bǔ)償[1-2]。Streit利用彈簧對(duì)連桿機(jī)構(gòu)進(jìn)行重力補(bǔ)償[3]。Kumar利用滑輪實(shí)現(xiàn)機(jī)構(gòu)的重力補(bǔ)償[4]。Kazerooni等利用直接驅(qū)動(dòng)的方法對(duì)并聯(lián)機(jī)構(gòu)進(jìn)行重力補(bǔ)償[5]。上述文獻(xiàn)大多采用機(jī)械的方法來(lái)補(bǔ)償精度不高的問(wèn)題。筆者將對(duì)主操作端控制過(guò)程中產(chǎn)生的時(shí)延進(jìn)行分析,利用多線程、多事件技術(shù)盡可能降低主操作端的時(shí)間延時(shí),通過(guò)矢量分析方法建立系統(tǒng)靜力學(xué)模型,計(jì)算各部分重心在關(guān)節(jié)空間中所產(chǎn)生的轉(zhuǎn)矩,利用力矩電機(jī)輸出相應(yīng)的轉(zhuǎn)矩進(jìn)行重力補(bǔ)償。

1　主操作端時(shí)延特性分析

主操作端時(shí)延主要包括對(duì)主操作手的位姿信息采集和電機(jī)控制所用的時(shí)間。由于主操作手需要完成的重要工作是在手術(shù)過(guò)程中時(shí)刻保持重力平衡的,所以重力補(bǔ)償程序啟動(dòng)后,主操作手才進(jìn)行力反饋算法的執(zhí)行。主操作手端控制系統(tǒng)的流程如圖1所示。

圖1　主操作手控制系統(tǒng)流程

圖2　主操作手耗時(shí)

從圖1可以看出,完成一次重力補(bǔ)償需要三個(gè)過(guò)程,即開(kāi)啟重力補(bǔ)償—執(zhí)行—開(kāi)啟力反饋算法。圖2是重力補(bǔ)償周期tz和信息顯示耗時(shí)th示意。從圖2a可以看出,主操作端的重力補(bǔ)償周期在90 ms左右。主操作手除了重力補(bǔ)償之外,還有對(duì)主手位姿、反饋力等信息的顯示。圖2b中經(jīng)過(guò)測(cè)量耗時(shí)為10 ms左右。因此,可知主手端的采樣周期總體平均耗時(shí)100.63 ms。

2　多線程、多事件技術(shù)的時(shí)延控制

主操作手采用多線程技術(shù)[6],將重力補(bǔ)償線程的結(jié)束作為一個(gè)事件,只有等到該事件觸發(fā)后,才進(jìn)入數(shù)據(jù)發(fā)送線程和信息顯示線程。當(dāng)重力補(bǔ)償線程得到位姿和電流信息之后,通知另外兩個(gè)線程進(jìn)行相應(yīng)的操作。然后,重力補(bǔ)償線程繼續(xù)進(jìn)行重力補(bǔ)償?shù)挠?jì)算,將電流值發(fā)送給三個(gè)驅(qū)動(dòng)器,保證主操作端的重力平衡。數(shù)據(jù)發(fā)送周期ts如圖3所示。從圖3中可以看出,多線程單事件驅(qū)動(dòng)的數(shù)據(jù)發(fā)送周期均值為94.587 ms。

多線程技術(shù)使主端時(shí)延相比于100.63 ms的采樣周期略有減小,但時(shí)延依然很大。在保證重力補(bǔ)償?shù)姆椒ǖ幕A(chǔ)上,采用多事件技術(shù),在發(fā)送一次電流指令之后進(jìn)行一次位置采樣,然后觸發(fā)數(shù)據(jù)發(fā)送線程。多事件數(shù)據(jù)發(fā)送周期td如圖4所示。從圖4可以看出,基于多事件技術(shù)的主操作端的采樣周期均值為36.5 ms,大幅降低了主操作端的采樣周期。

圖3　多線程數(shù)據(jù)發(fā)送周期

圖4　多事件采樣周期

3　主操作手靜力學(xué)分析

G1ix=G1i·bix=-G1i2xbi,

G1iy=G1i·biy=-G1i2ybi,

G1iz=G1i·biz=O,

圖5　上連桿受力分析

圖6是對(duì)半圓盤幾何中心的分析。根據(jù)矢量分析的方法[7],建立上連桿BiCi的力和力矩平衡方程

∑Mx(F)=MBicosγi+MCicosγi+

∑My(F)=MBicosβi+MCicosβi=0,

∑Mz(F)=MBicosφi+MCicosφi-

式中:MBi——主動(dòng)關(guān)節(jié)在被動(dòng)關(guān)節(jié)產(chǎn)生的力矩;

MCi——?jiǎng)悠脚_(tái)在在被動(dòng)關(guān)節(jié)產(chǎn)生的力矩;

γi、βi、φi——MCi(MBi)分別和Bi-xBiyBizBi坐標(biāo)系中各軸的夾角。

G2ix=G2i·aix=-G2i2xai,

G2iy=G2i·aiy=-G2i2yai,

G2iz=G2i·aiz=-G2i2zai,

(-FCiy-G1iy)·(2xai2ybi+3xai3ybi)。

根據(jù)力矩平衡原理,并考慮半圓盤幾何中心的分布如圖7所示,可得zai軸方向的力矩表達(dá)式

|AiNi|cos(∠BiAiNi)-G2iy·

|AiNi|sin(∠BiAiNi)=0，

(6)

式中：∠BiAiNi=33°；

|AiNi|=0.026 9 m。

圖6　下連桿受力分析

圖7　半圓盤幾何中心分析

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

圖8為沿x軸方向的運(yùn)動(dòng)軌跡。圖8a參數(shù)的運(yùn)動(dòng)軌跡為沿x軸方向，范圍為-50～50 mm(y=25 mm,z=200 mm),F=(-4.9 N,-4.9 N,-4.9 N)。圖8b、c分別給出了每個(gè)驅(qū)動(dòng)軸的力矩M1、M2和M3的ADMAS仿真結(jié)果和力學(xué)模型理論值。通過(guò)兩種方法下的誤差對(duì)比可以得出,兩種方法結(jié)果基本一致,力矩誤差Me對(duì)比如圖8d所示，誤差對(duì)比如圖8d所示。

圖8　x軸方向運(yùn)動(dòng)軌跡

5　結(jié)束語(yǔ)

筆者對(duì)自主研制的可用于外科手術(shù)的主操作手重力補(bǔ)償算法進(jìn)行了研究。通過(guò)矢量分析的方法建立系統(tǒng)動(dòng)力學(xué)模型,計(jì)算各部分重心在關(guān)節(jié)空間中所產(chǎn)生的轉(zhuǎn)矩,并利用力矩電機(jī)輸出相應(yīng)的力矩進(jìn)行重力補(bǔ)償。通過(guò)仿真實(shí)驗(yàn)表明,重力補(bǔ)償方法控制效果令人滿意,主操作手可以實(shí)現(xiàn)任意位置靜平衡,提高了操作性能,能夠保證手術(shù)的安全性。

[1]NATHAN R R. A constant force generation mechanism[J]. ASME Journal of Mechanism, Transmissions and Automation in Design, 1985, 107(4): 508-512.

[2]HERVE J M. Device for counter-balancing the forces due to gravity in a robot arm[J].United States Patent, 1986, 14(3): 620-623.

[3]STREIT D A, GILMORE B J. Perfect spring equilibrator for rotatable bodies[J]. ASME Journal of Mechanisms, Transmissions and Automation in Design, 1989, 11(4): 451-458.

[4]ULRICH N, KUMAR V. Passive mechanical gravity compensation for robot manipulators[C]//Proceedings of the IEEE International Conference on Robotics and Automation. California: Sacramento, 1991: 1536-1541.

[5]KAZEROONI H, KIM S. A new architecture for direct drive robot[C]//Proceedings of the ASME Mechanism Conference. Chicago:[s.n.], 1990: 21-28.

[6]TOMONORI YAMAMOTO. Applying tissue models in teleported robot-assisted surgery[D]. Baltimore: The Johns Hopkins University, 2011.

[7]趙枝凱. 機(jī)器人輔助遠(yuǎn)程骨科手術(shù)時(shí)延控制研究[D]. 哈爾濱: 哈爾濱工業(yè)大學(xué), 2013.

[8]HU B, YU JJ, LU Y, et al. Statics and stiffness model of serial-parallel manipulator formed by k parallel manipulators connected in series[J]. Journal of Mechanisms and Robotics-Transactions of The ASME, 2012, 4(3): 123-136.

[9]YU L, SONG H, WANG T, et al. A new asymmetrical mass distribution method on the analysis of universal “force-sensing” model for 3-DOF translational parallel manipulator[J]. Industrial Robot: An International Journal, 2014, 41(1): 56-69.

(編輯徐巖)

Time delay control with gravity compensation for master manipulator

WANGHongmin1,LüXiongfei1,WANGMaosheng2,SUFengwu1,KANGHongming1

(1.School of Electrical & Control Engineering, Heilongjiang University of Science & Technology, Harbin 150022, China; 2.College of Mechanical & Electrical Engineering Technology, Heilongjiang College of Construction, Harbin 150025, China)

This paper proposes a gravity compensation algorithm for three-dimensional force feedback manipulator, a self-designed approach used in surgical operation. The paper provides an in-depth analysis of time delay occurring in master operation control system and introduces the greatest reduction in time delay occurring master control system using multi-thread and multi-event technology and a solution to the motor output torque for balancing the master gravity using vector analysis method. The simulation analysis conducted in ADMAS and MATLAB environment verifies the viability of system dynamics model and the method is adequate for gravity balance.

master manipulator; gravity compensation; time delay; surgical operation

2013-12-11

黑龍江省教育廳科學(xué)技術(shù)研究項(xiàng)目(12541713;12541723)

王宏民(1978-),男,河北省承德人,副教授,碩士,研究方向:自動(dòng)控制、機(jī)器人控制,E-mail:wang-hongmin@163.com。

10.3969/j.issn.2095-7262.2014.03.011

TN953

2095-7262(2014)03-0272-05

A