力反饋主操作手附加力的補償策略研究

贠今天 劉陽陽 楊全利 桑宏強

1.天津工業(yè)大學機械工程學院，天津，300387 2.天津市現代機電裝備技術重點實驗室，天津，300387 3.天津職業(yè)技術師范大學，天津，300222

力反饋主操作手附加力的補償策略研究

贠今天1,2劉陽陽1楊全利3桑宏強1,2

1.天津工業(yè)大學機械工程學院，天津，300387 2.天津市現代機電裝備技術重點實驗室，天津，300387 3.天津職業(yè)技術師范大學，天津，300222

針對力反饋主操作手的連桿重力、關節(jié)摩擦力以及慣性力影響醫(yī)生真實感知反饋力信息的問題，建立了一套完整的動力學模型。基于力反饋主操作手動力學模型，分別建立了主操作手的重力補償模型、摩擦力補償模型以及慣性力補償模型。通過力補償控制策略來消除力反饋主操作手本身所具有的重力、關節(jié)摩擦力以及慣性力伴隨關節(jié)運動對反饋作用力造成的干擾，實現了醫(yī)生的真實力覺感知，從而提高了微創(chuàng)手術的精度。

力反饋；主操作手附加力；補償策略；動力學模型；微創(chuàng)手術

0 引言

遙操作機器人(teleoperation robotics)技術是當前具有實用價值的交互式機器人前沿技術，其中，臨場感(telepresence)一直是眾多學者研究工作的目標之一[1-4]。力反饋遙操作機器人系統(tǒng)中，從操作手末端執(zhí)行器上的力傳感器檢測到與外界環(huán)境相互作用的力信息,經控制系統(tǒng)及力反饋主操作手(下稱主手)傳遞給操作者，這一力信息即為期望的反饋力。這一力信息中不僅包含從操作手末端執(zhí)行器與外界環(huán)境的交互作用力，而且包含運動過程中力反饋主手關節(jié)處的摩擦力、桿件的重力和慣性力等，導致操作者很難分辨出從操作手與外界環(huán)境真實交互作用力的大小，使操作者缺乏力覺臨場感。微創(chuàng)外科手術機器人臨床應用操作中因力反饋主手動力學特性的干擾而缺乏力覺臨場感，使醫(yī)生失去了真實的力覺感知，從而降低了手術的精度。

國內外學者為增強操作者的力覺臨場感而展開了相關研究。FAN等[5]提出了一種使用配重平衡定位機構和彈簧位置機構來減少力反饋主手重力對遙操作透明性影響的方法，該方法雖然減小了主手重力對反饋力的影響，但卻增大了主手慣性力對反饋力的影響。VALLERY等[6]提出了利用保守力場來減小操作者對力反饋主操作手進行操作時產生的不必要的力/力矩，提高了力反饋遙操作的透明性。WANG等[7]為了研究摩擦力對力反饋遙操作系統(tǒng)的影響，提出了基于二端網絡的支持向量機離線估計法，對摩擦力進行在線前饋補償。MELINTE等[8]利用神經網絡法來補償摩擦力。謝琦等[9]基于等效微分變換法，提出了適合手術機器人的實時主從控制算法。黃勇[10]在分析現有遙操作系統(tǒng)控制策略局限性的基礎上，從位置約束空間的角度，提出了一種新型控制策略，以位置約束空間矩陣代替常數增益，結合主從關節(jié)空間誤差，來得到準確的反饋作用力。

目前，相關研究只針對減小重力或摩擦力對操作者感知反饋力信息造成的影響開展研究，并未實現全面消除主手重力、關節(jié)摩擦力及慣性力對操作者感知反饋力所造成的影響，使得操作者并不能感知真實的反饋力信息。

針對微創(chuàng)手術機器人力反饋主手因自身連桿重力、關節(jié)摩擦力以及慣性力對反饋作用力造成的干擾，本文以力反饋操作器PHANToM作為微創(chuàng)手術機器人的主手,建立計及關節(jié)摩擦力、連桿重力以及慣性力的力反饋主手多體系統(tǒng)動力學模型，分析慣性力、關節(jié)摩擦力以及連桿重力對主手動力學行為的影響規(guī)律,通過附加力補償策略來消除主手的連桿重力、關節(jié)摩擦力以及慣性力對反饋作用力產生的影響。

1 力反饋主手動力學模型

1.1 力反饋器PHANToM動力學模型

圖1 PHANToM結構示意圖Fig.1 Schematic of PHANToM structure

根據Lagrange方程

(1)

L=K-Pi=1,2,…，n

式中，Fi為廣義力；L為拉格朗日函數；K、P分別為系統(tǒng)的動能和勢能。

在不考慮摩擦和外界干擾的情況下，力反饋主手PHANToM的動力學方程可表示為

(2)

式(2)可整理成：

(3)

mdl1p4)cθ2sθ3]

M12=M13=M21=M31=0

M23=M32=(-mcl2p3-mdl1p4)sθ23

C22=C33=0

C21=-C12

C31=-C13

N1=0

N2=g(map1+mbp3+mdl1)cθ2

N3=g(-mbp2+mcl1+mdp4)sθ3

τ=(τ1，τ2，τ3)T

i,j=1,2,3

式(3)為不計關節(jié)摩擦力的力反饋主手機構的動力學模型。

1.2 計及關節(jié)摩擦的PHANToM動力學模型

為精確實現主手力反饋，使操作者感知真實的反饋力信息，必須建立更準確的主手動力學模型，將主手關節(jié)處摩擦力一并計入主手的動力學模型中。力反饋主手中的摩擦主要來源于關節(jié)摩擦及傳動引起的摩擦，本文忽略傳動摩擦，只考慮關節(jié)處摩擦力。因其他摩擦模型具有復雜性及不確定性的特點[14]，同時考慮到使摩擦模型利于線性化，本文選用經典的Coulomb+Viscous組合摩擦模型[15]來近似描述主手機構關節(jié)處的摩擦效應，Coulomb+Viscous摩擦模型可表示為

(4)

力反饋主手PHANToM關節(jié)處的摩擦效應可近似地描述為如下形式：

(5)

式中，Ffci(i=1,2,3)為關節(jié)i處的Coulomb摩擦因數；Ffvi(i=1,2,3)為關節(jié)i處的Viscous摩擦因數。

在考慮關節(jié)摩擦效應時，計及力反饋主手的連桿重力、慣性力及關節(jié)摩擦力的完整動力學模型可表示為

(6)

建立的主手動力學模型(式(6))中，慣性力項、摩擦力項、離心力和哥氏力項以及重力項都具有高度的非線性，不利于力反饋主手機器人對力反饋主手控制器的設計，所以將主手動力學模型為線性參數化的形式。本文選取14個動力學參數，則式(6)可化為如下形式：

(7)

Y3×14=[Yd3×8Yf3×6]

πf6×1=(Ffc1，Ffc2，Ffc3，Ffv1，Ffv2，Ffv3)T

其中，Y為3×14維回歸矩陣，表征關于力反饋主手的關節(jié)角、關節(jié)角速度以及關節(jié)角加速度的回歸矩陣；π為14×1維動力學參數向量，表征關于力反饋主手系統(tǒng)動力學的定常參數向量。

Yd3×8中，

πd8×1中,

根據文獻[16]中力反饋主手PHANToM的相關參數確定動力學參數，如表1所示。

2 力反饋主手附加力補償策略

2.1 力反饋主手附加力

在微創(chuàng)手術機器人系統(tǒng)中引入力反饋，可極大地提高遙操作系統(tǒng)的透明性，增強力覺臨場感，使醫(yī)生更好地實施力控制。從操作手末端執(zhí)行器與外界未知環(huán)境相互作用后會產生交互作用力，力反饋主手根據反饋作用力的大小在各個關節(jié)施加相應的驅動信號，使電機輸出相應的轉矩τf，經力反饋主手轉換使操作者感知這一反饋力信息。但是，由于受主手動力學特性的影響，主手在運動過程中關節(jié)處將產生動力學附加轉矩τi，因此，操作者感知到的力信息并不是真實的反饋力。在反饋力信息中不可避免地包含力反饋主手的桿件重力、關節(jié)摩擦力以及慣性力，將這些附加在反饋力信息中的力稱為主手附加力(induced master force)，如圖2所示。

表1 動力學參數

圖2 主手附加力Fig.2 Induced master force

圖2中，Hi為力反饋主手運動到附加力矩的傳遞函數，表征力反饋主手的動力學特性；θmu表示力反饋主手期望關節(jié)運動軌跡；τf為反饋力施加在主手電機上所產生的力矩；τi為力反饋主手附加力矩。操作者感受到的力可表示為

F=Ff+Fi

(8)

其中，τ經力反饋主手轉換后的力信息為反饋作用力Ff和附加力Fi，Fi包括主手桿件的重力、慣性力及關節(jié)摩擦力，二者一起被操作者感知。

在操作者操作力反饋主手使從操作手與外界環(huán)境相互作用時，計入附加力矩的力反饋主手完整動力學模型可寫成如下形式：

(9)

式(9)寫成線性化的形式為

(10)

為使操作者感知真實的反饋力，需使操作者感受到的力矩τu與反饋力矩τf實時平衡，則式(10)轉化成主手附加力形式為

(11)

結合人手對力反饋主手的實際操作，選取三個符合主動關節(jié)期望運動的驅動正弦函數：

根據式(11)經動力學反解計算可以得到主手的附加力矩-時間關系圖，如圖3所示。

(a)關節(jié)1

(b)關節(jié)2

由圖3可知，在操作者抓持力反饋主手末端操縱桿使其完成操作者所期望的運動時，隨著主手的運動在力反饋主手關節(jié)處會產生附加力矩。基于不計關節(jié)摩擦力的力反饋主手動力學模型，關節(jié)1、2、3處產生的附加力矩最大值約為0.1N·m；基于計及關節(jié)摩擦力的力反饋主手完整動力學模型，關節(jié)1處產生的最大附加力矩約為0.15N·m，關節(jié)2、3處產生的附加力矩約為0.12N·m。這表明操作者感知的實際力信息中包含的因力反饋主手自身產生的附加力Fi較大，會對操作者感知真實的反饋力信息造成較大的干擾。另外，對比計及關節(jié)摩擦與不計關節(jié)摩擦的主手附加力矩，力反饋主手關節(jié)處摩擦力對主手附加力Fi的貢獻較大，可進一步推斷出關節(jié)摩擦力對操作者感知反饋力信息的干擾也較大。因此，需要消除主手的連桿重力、關節(jié)摩擦力以及慣性力對操作者感知反饋力所造成的干擾。

2.2 附加力補償策略

在醫(yī)生進行微創(chuàng)手術的過程中，要使醫(yī)生感知真實的反饋力信息，使其不受主手自身動力學特性的影響，可以利用主手電機輸出相應的反向驅動力矩來平衡主手附加力矩。本文采用圖4所示的力反饋主手附加力補償策略來消除主手的連桿重力、關節(jié)摩擦力及慣性力對醫(yī)生感知反饋力的影響。

圖4 主手附加力補償策略Fig.4 Compensation strategy for induced master force

力反饋主手附加力補償策略為：在主手控制器中分別建立主手的重力補償模型、關節(jié)摩擦力補償模型以及慣性力補償模型。操作者抓持力反饋主手末端的操縱桿使主手沿期望運動軌跡θmu運動，使從操作手與外界環(huán)境發(fā)生交互，編碼器實時測量主手的各關節(jié)轉角信息并以脈沖的形式發(fā)送給解碼器，主手控制器將關節(jié)轉角信息輸入主手重力補償模型、摩擦補償模型以及慣性力補償模型中，通過求解計算得到各關節(jié)需要平衡主手重力、關節(jié)摩擦力、慣性力所需力矩的大小，然后將得到的力矩信息指令實時地發(fā)送給主手各關節(jié)處的電機，使電機輸出反向驅動力矩，來抵消主手因動力學特性而產生的附加力矩。此時，力反饋主手各主動關節(jié)電機的力控制為

τm=cJT(θ)Ff+τiG+τiF+τiI

(12)

其中，τm為力反饋主手各主動關節(jié)電機的驅動轉矩；J(θ)為力反饋主手的速度雅可比矩陣；τiG為補償主手重力電機輸出的力矩；τiF為補償主手關節(jié)摩擦力電機輸出的力矩；τiI為補償主手慣性力電機輸出的力矩；當力反饋主手與從操作手連接時，c=1；當力反饋主手與從操作手斷開時，c=0。

2.3 附加力補償模型

通過建立重力補償模型實現對主手重力的補償，消除連桿重力對反饋力的干擾。主手機構的重力補償模型為

(13)

主手運動過程中，編碼器將實時測量到的主手關節(jié)轉角信息發(fā)送給解碼器，主手控制器將其輸入重力補償模型來求解計算各關節(jié)要平衡主手連桿重力所需的力矩，然后實時地將力矩信息指令發(fā)送給主手電機，使其輸出反向驅動力矩，從而達到動態(tài)補償主手重力的目的。為補償主手重力，各電機需要輸出的反向力矩如圖5a所示。

通過建立主手摩擦力補償模型來實現對主手摩擦力的補償，消除關節(jié)摩擦力對反饋力的干擾。摩擦力補償模型為

(14)

主手控制器將編碼器發(fā)送的主手關節(jié)轉角信息輸入主手摩擦力補償模型，計算各關節(jié)要平衡主手關節(jié)摩擦力所需的力矩，然后將力矩信息指令發(fā)送給主手電機，使電機輸出反向驅動力矩，從而達到動態(tài)補償摩擦力的目的。為補償主手關節(jié)摩擦力，各電機需要輸出的反向力矩如圖5b所示。

對于力反饋主手慣性力的補償，由于在實際操作過程中，操作者不可能使主手操作桿始終保持勻速運動，因此，主手慣性力會隨操作者手的加速或減速而產生。然而，由于慣性力的二階非線性使其在實際中無法測量。本文通過設計的加速度觀測器來觀測力反饋主手的加速度，從而求得主手在運動過程產生的慣性力：

(15)

通過下式求解平衡慣性力所需力矩的大小：

(16)

將慣性力矩信息發(fā)送給主手電機，使電機輸出反向力矩，實現對力反饋主手慣性力的動態(tài)補償。各電機輸出的反向力矩如圖5c所示。

(a)補償主手重力

(b)補償關節(jié)摩擦力

(c)補償主手慣性力圖5 補償主手重力、摩擦力、慣性力時電機輸出的力矩Fig.5 Motor output torque compensating gravity,friction and inertia force of master manipulator

基于建立的主手附加力補償模型，使主手電機輸出反向驅動力矩來平衡抵消主手因自身動力學特性而產生的附加力矩，由此來消除因主手的連桿重力、關節(jié)摩擦力以及慣性力而產生的主手附加力矩，各電機輸出的補償力矩與各關節(jié)處的附加力矩如圖6所示。由圖6可知，在主手運動過程中，基于主手附加力補償模型，主手各電機為平衡抵消附加力矩而輸出的補償力矩與關節(jié)處的附加力矩大致相等。其中，電機2的補償效果欠佳，其原因為力反饋主手在運動過程中，主手機構自身產生的的離心力(自耦力)和哥氏力(互耦力)在關節(jié)2處產生的附加力矩較大，故在實際操作過程中需要緩慢操作力反饋主手運動，減小其對操作者感知反饋力的影響。

(a)電機1輸出的補償力矩與關節(jié)1處的附加力矩

(b)電機2輸出的補償力矩與關節(jié)2處的附加力矩

(c)電機3輸出的補償力矩與關節(jié)3處的附加力矩圖6 各電機輸出的補償力矩與各關節(jié)處的附加力矩Fig.6 Output torque of each motor and induced torque at each joint

在微創(chuàng)手術中，醫(yī)生在緩慢操作主手時，主手電機輸出的補償力矩與附加力矩可實時動態(tài)地平衡抵消，從而使醫(yī)生真實感知反饋力信息。通過建立主手附加力補償模型可消除主手重力、關節(jié)摩擦力以及慣性力給反饋力帶來的干擾，將反饋力信息無干擾地傳遞給醫(yī)生，使醫(yī)生獲得真實的力覺臨場感，從而實施恰當的力控制行為。

3 結束語

本文建立了計及力反饋主手PHANToM的連桿重力、關節(jié)摩擦力及慣性力的完整動力學模型，通過主手附加力補償策略，消除因力反饋主手動力學特性對醫(yī)生感知反饋力所造成的干擾，以實現醫(yī)生的真實力覺感知。仿真結果表明，在力反饋主手運動過程中，主手各電機輸出的補償力矩可動態(tài)地平衡主手因自身連桿重力、關節(jié)摩擦力以及慣性力而產生的附加力矩。

[1] 宋愛國.人機交互力覺臨場感遙操作機器人技術研究[J].科技導報，2015，23：100-109.SONGAiguo.ResearchonHuman-RobotInteractionTelerobotwithForceTelepresence[J].Science&TechnologyReview, 2015, 33(23): 100-109.

[2] 盛國棟，曹其新，潘鐵文，等.主從式機器人系統(tǒng)中力反饋的實現[J].中國機械工程，2015，26(9)：1157-1160.SHENGGuodong,CAOQixin,PANTiewen,etal.ImplementationofForceFeedbackinMaster-slaveRobotSystems[J].ChinaMechanicalEngineering, 2015, 26(9): 1157-1160.

[3]ARTIGASJ,HIRZINGERG.ABriefHistoryofDLR’sSpaceTeleroboticsandForce-feedbackTeleoperation[J].ActaPolytechnicaHungarica, 2016, 13(1): 239-249.

[4] 文廣，鞏明德，張紅彥.H∞控制的主從遙操作力覺雙向伺服控制系統(tǒng)研究[J].中國機械工程，2011，22(22)：2717-2722. WEN Guang, GONG Mingde, ZHANG Hongyan. Research on Master-slave Tele-operated Force Sense Bilateral Servo Control System byH∞Control[J]. China Mechanical Engineering, 2011, 22(22): 2717-2722.

[5] FAN R, ZHAO C, ZHAO H.Improvement of Dynamic Transparency of Haptic Devices by Using Spring Balance[C]//IEEE International Conference on Robotics and Biomimetics. Guangzhou, 2012: 1075-1080.

[6] VALLERY H, DUSCHAU-WICKE A, RIENER R. Optimized Passive Dynamics Improve Transparency of Haptic Devices[C]//IEEE International Conference on Robotics and Automation. Kobe, 2009:301-306.

[7] WANG G L, LI Y F, BI D X. Support Vector Network Enhanced Adaptive Friction Compensation[C]//IEEE International Conference on Robotics and Automation. Orlando, 2006: 3699-3704.

[8] MELINTE O, MUNTEANU R, GAL I A, et al. Compensating Dynamics of Impedance Haptic Devices Using Neural Networks[C]//8th International Symposium on Advanced Topics in Electrical Engineering (ATEE). Bucharest, 2013: 1-6.

[9] 謝琦，潘博，付宜利，等.基于腹腔微創(chuàng)手術機器人的主從控制技術研究[J]. 機器人，2011，33(1)：53-58. XIE Qi, PAN Bo, FU Yili, et al. Master-slave Control Technology Research Based on Abdominal Minimally Invasive Surgery Robot[J]. Robot, 2011,33(1): 53-58.

[10] 黃勇.主-從異構遙操作系統(tǒng)的雙向控制策略研究[D].長春：吉林大學，2014. HUANG Yong. Research on the Bilateral Control Strategy of Master-slave Isomerism Tele-operation System[D]. Changchun: Jilin University, 2014.

[11] CAVUSOGLU M C, FEYGIN D. Kinematics and Dynamics of Phantom (TM) Model 1.5 Haptic Interface[EB/OL]. University of California at Berkeley [2016-07-01]. http://citeseer.ist.psu.edu/viewdoc/download?doi=10.1.1.19.7619&rep=rep1&type=pdf.

[12] 蔡自興.機器人學[M].3版.北京：清華大學出版社，2015：79-84. CAI Zixing. Robotics[M].3rd ed. Beijing: Tsinghua University Press, 2015: 79-84.

[13] KHALIL W, KLEINFIGER J. A New Geometric Notation for Open and Closed-loop Robots[C]// IEEE International Conference on Robotics and Automation. San Francisco, 1986: 1174-1179.

[14] 丁千，翟紅梅.機械系統(tǒng)摩擦動力學研究進展[J].力學進展，2013，43(1)：112-131. DING Qian, ZHAI Hongmei. The Advance Researches of Friction Dynamic in Mechanics System[J]. Advances in Mechanics, 2013, 43(1): 112-131.

[15] THOMSEN J J, FIDIN A. Analytical Approximations for Stick-slip Vibration Amplitudes[J]. International Journal of Non-linear Mechanics, 2003, 38(3):389-403.

[16] TAHMASEBI A M, TAATI B, MOBASSER F, et al. Dynamic Parameter Identification and Analysis of a PHANToM Haptic Device[C]//Proceedings of 2005 IEEE Conference on Control Applications. Toronto, 2005: 1251-1256.

(編輯 王旻玥)

Research on Compensation Strategy for Induced Forces of Force Feedback Master Manipulators

YUN Jintian1,2LIU Yangyang1YANG Quanli3SANG Hongqiang1,2

1.School of Mechanical Engineering,Tianjin Polytechnic University,Tianjin,3003872.Advanced Mechatronics Equipment Technology Tianjin Area Major Laboratory,Tianjin,3003873.Tianjin University of Technology and Education,Tianjin,300222

Aiming at the problems that the connecting rod gravity, joint frictions and inertia forces of force feedback master manipulator affected doctor’s real perception of feedback force informations, a complete dynamics model was established. Based on the dynamics model of force feedback master manipulator, gravity compensation model, friction compensation model, and inertia force compensation model were established respectively. Force compensation strategies were used to eliminate the interferences to feedback forces from gravity, joint friction and inertia force of force feedback master manipulator in joint movements. Thus the doctor’s real force perception is realized and the accuracy of minimally invasive surgery is improved.

force feedback; induced force of master manipulator; compensation strategy; dynamics model; minimally invasive surgery

2016-07-04

國家自然科學基金資助項目(51205287)；天津市自然科學基金資助項目(16JCYBJC18400)

TP242.6

10.3969/j.issn.1004-132X.2017.10.004

贠今天，男，1970年生。天津工業(yè)大學機械工程學院教授、博士。主要研究方向為多體系統(tǒng)動力學與控制、機器視覺。發(fā)表論文30余篇。劉陽陽(通信作者)，男，1989年生。天津工業(yè)大學機械工程學院碩士研究生。E-mail: lyyvino@163.com。楊全利，男，1966年生。天津職業(yè)技術師范大學實驗師。桑宏強，男，1978年生。天津工業(yè)大學機械工程學院副教授、博士。