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

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

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

?

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

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

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

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

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

0　引　言

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

1　主操作端時延特性分析

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

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

圖2　主操作手耗時

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

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

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

多線程技術(shù)使主端時延相比于100.63 ms的采樣周期略有減小,但時延依然很大。在保證重力補(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是對半圓盤幾何中心的分析。根據(jù)矢量分析的方法[7],建立上連桿BiCi的力和力矩平衡方程

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

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

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

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

MCi——動平臺在在被動關(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　實驗驗證

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

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

5　結(jié)束語

筆者對自主研制的可用于外科手術(shù)的主操作手重力補(bǔ)償算法進(jìn)行了研究。通過矢量分析的方法建立系統(tǒng)動力學(xué)模型,計算各部分重心在關(guān)節(jié)空間中所產(chǎn)生的轉(zhuǎn)矩,并利用力矩電機(jī)輸出相應(yīng)的力矩進(jìn)行重力補(bǔ)償。通過仿真實驗表明,重力補(bǔ)償方法控制效果令人滿意,主操作手可以實現(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ù)時延控制研究[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ù)研究項目(12541713;12541723)

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

10.3969/j.issn.2095-7262.2014.03.011

TN953

2095-7262(2014)03-0272-05

A