具有震顫抑制的微創(chuàng)機(jī)器人主從控制系統(tǒng)設(shè)計(jì)

楊鋮浩，桑宏強(qiáng)，2，劉 芬，贠今天，陳 發(fā)

YANG Chenghao1,SANG Hongqiang1，2,LIU Fen1,YUN Jintian1,CHEN Fa1

1.天津工業(yè)大學(xué) 機(jī)械工程學(xué)院，天津 300387

2.天津市現(xiàn)代機(jī)電裝備技術(shù)重點(diǎn)實(shí)驗(yàn)室，天津 300387

1.School of Mechanical Engineering,Tianjin Polytechnic University,Tianjin 300387,China

2.Tianjin Key Laboratory ofAdvanced Mechatronic Equipment Technology,Tianjin 300387,China

1 引言

微創(chuàng)外科手術(shù)中，醫(yī)生通過(guò)患者腹部的微小切口將細(xì)長(zhǎng)的手術(shù)器械及內(nèi)窺鏡伸入病灶，借助監(jiān)視器進(jìn)行操作。較傳統(tǒng)開(kāi)放手術(shù)，其具有出血少，創(chuàng)口小，恢復(fù)快，病人痛苦輕等優(yōu)點(diǎn)[1]。然而，它存在手眼協(xié)調(diào)性差，手術(shù)工具靈活度低，容易放大手部生理震顫，長(zhǎng)時(shí)間操作導(dǎo)致醫(yī)生疲勞等缺點(diǎn)。隨著計(jì)算機(jī)與機(jī)器人技術(shù)的發(fā)展，機(jī)器人輔助微創(chuàng)外科手術(shù)開(kāi)始受到國(guó)內(nèi)外學(xué)者的廣泛關(guān)注[2-3]。

1994年，美國(guó)Computer Motion公司研制的AESOP機(jī)器人是美國(guó)食品藥品管理局（FDA）批準(zhǔn)的第一臺(tái)可用于手術(shù)室輔助手術(shù)的機(jī)器人[4]。1996年，該公司利用先前積累的關(guān)鍵技術(shù)，成功研制出了具備強(qiáng)大視覺(jué)功能的ZEUS機(jī)器人系統(tǒng)[5]。2001年，美國(guó)Intuitive Surgical公司研發(fā)了第一代Da Vinci外科手術(shù)機(jī)器人并通過(guò)了FDA認(rèn)證，其被視為微創(chuàng)外科手術(shù)機(jī)器人研究的一個(gè)里程碑[6]。

國(guó)內(nèi)方面，天津大學(xué)于2004年開(kāi)發(fā)了主從遙操作顯微外科手術(shù)機(jī)器人“妙手”系統(tǒng)[7]，并于2010年進(jìn)一步研制了“妙手A”腹腔微創(chuàng)手術(shù)機(jī)器人系統(tǒng)[8]。2013年，哈爾濱工業(yè)大學(xué)成功研制出了腹腔鏡外科手術(shù)機(jī)器人系統(tǒng)[9]。

圖1 微創(chuàng)手術(shù)機(jī)器人系統(tǒng)結(jié)構(gòu)示意圖

圖2 微創(chuàng)手術(shù)機(jī)器人主從控制結(jié)構(gòu)圖

目前，國(guó)內(nèi)對(duì)于微創(chuàng)手術(shù)機(jī)器人系統(tǒng)研究，特別是在控制系統(tǒng)方面仍存在一些亟待解決的問(wèn)題，例如：醫(yī)生在手術(shù)操作中的生理震顫無(wú)法有效抑制，從操作臂在低速狀態(tài)下由于其關(guān)節(jié)內(nèi)部非線性摩擦表現(xiàn)出的“粘滑行為”等。面對(duì)上述情況，本文提出一種震顫抑制的主從控制系統(tǒng)，分別設(shè)計(jì)了為抑制生理震顫的新型零相位濾波震顫抑制算法和為克服“粘滑行為”導(dǎo)致震顫現(xiàn)象的前饋補(bǔ)償PD控制器。最后通過(guò)仿真及實(shí)驗(yàn)，驗(yàn)證了該方法具有良好的震顫抑制效果。

2 微創(chuàng)手術(shù)機(jī)器人系統(tǒng)

微創(chuàng)手術(shù)機(jī)器人樣機(jī)系統(tǒng)結(jié)構(gòu)如圖1所示，其主要由主操作臂、控制系統(tǒng)硬件、從操作臂3部分組成。主操作臂選用美國(guó)SensAble Technologies公司的觸覺(jué)設(shè)備Phantom Desktop。從操作臂包括：基于雙平行四邊形原理設(shè)計(jì)的遠(yuǎn)程運(yùn)動(dòng)中心機(jī)構(gòu)（RCM）以及3自由度手術(shù)工具。控制系統(tǒng)硬件結(jié)構(gòu)主要包括1臺(tái)具有數(shù)據(jù)采集卡的上位機(jī)（PC），1個(gè)可編程多軸控制器（PMAC）作下位機(jī)和7套伺服控制電機(jī)及驅(qū)動(dòng)器。此外，系統(tǒng)還配備了按鈕開(kāi)關(guān)、腳踏開(kāi)關(guān)、接近開(kāi)關(guān)等數(shù)字輸入和電位器等模擬輸入。為精確地獲取人手生理震顫信號(hào)，在主操作臂內(nèi)還集成了一個(gè)3軸慣性測(cè)量單元（IMU）。

主從控制的實(shí)現(xiàn)過(guò)程如圖2所示，本文僅關(guān)注3自由度無(wú)力反饋的情況。手術(shù)過(guò)程中，醫(yī)生手持主操作臂進(jìn)行縫合、打結(jié)等一系列動(dòng)作。主操作臂各關(guān)節(jié)的角位移以及此時(shí)主操作臂末端的加速度信息被實(shí)時(shí)采集至上位機(jī)內(nèi)。上位機(jī)通過(guò)主操作臂運(yùn)動(dòng)學(xué)計(jì)算和零相位濾波得出手術(shù)指令的期望軌跡，將該指令進(jìn)行比例縮放及增量后，發(fā)送至下位機(jī)PMAC中。在PMAC中進(jìn)行從操作臂運(yùn)動(dòng)學(xué)計(jì)算，得到各個(gè)關(guān)節(jié)角度指令，并判斷其是否超過(guò)安全范圍。前饋補(bǔ)償PD控制器保證各關(guān)節(jié)驅(qū)動(dòng)器對(duì)軌跡指令進(jìn)行高性能跟蹤，從而完成手術(shù)工具末端對(duì)醫(yī)生的手術(shù)操作進(jìn)行實(shí)時(shí)復(fù)現(xiàn)。

3 微創(chuàng)手術(shù)機(jī)器人主從控制

3.1 主/從操作臂運(yùn)動(dòng)學(xué)

主操作臂和從操作臂的零位構(gòu)型分別如圖3及圖4所示，圖中空間坐標(biāo)系S與工具坐標(biāo)系T重合。定義si=sin(θi)，ci=cos(θi)，li和θi(i=1,2,3)分別表示對(duì)應(yīng)操作臂的連桿長(zhǎng)度和關(guān)節(jié)角度，x、y、z表示末端點(diǎn)的空間位移。

圖3 主操作臂零位構(gòu)型圖

圖4 從操作臂零位構(gòu)型圖

因此，主操作臂的正運(yùn)動(dòng)學(xué)映射可以表示為：

從操作臂的正運(yùn)動(dòng)學(xué)映射可以表示為：

從操作臂的逆運(yùn)動(dòng)學(xué)映射可以表示為：

3.2 新型零相位濾波

為抑制醫(yī)生手部的生理震顫，克服傳統(tǒng)低通濾波器容易造成相位失真和有效信息丟失的缺點(diǎn)，避免傳統(tǒng)零相位濾波器無(wú)法在線使用的局限性，提出一種新型零相位濾波器，其震顫抑制方法如圖5所示。PC機(jī)對(duì)采集到的主操作臂角位移進(jìn)行運(yùn)動(dòng)學(xué)計(jì)算，得到含有震顫信號(hào)及期望信號(hào)的實(shí)際末端軌跡x、y、z。慣性測(cè)量單元（IMU）采集到的加速度信號(hào)經(jīng)過(guò)積分及零相位濾波后可提取出當(dāng)前生理震顫信號(hào)xt、yt、zt，將其以相同幅值相反相位疊加至實(shí)際位置信號(hào)中，得到期望信號(hào)x^、y^、z^。

圖5 生理震顫抑制實(shí)現(xiàn)原理

根據(jù)文獻(xiàn)所述，生理震顫是人手一種固有的、無(wú)意識(shí)的震顫信號(hào)，其頻率主要分布在8～12 Hz頻帶內(nèi)，幅值約為50 μm，可近似為正弦曲線[10-11]。而手術(shù)過(guò)程中，期望指令信號(hào)的頻率一般不超過(guò)0.5～1 Hz[12]。

新型零相位濾波器（ZPF）由一個(gè)傳統(tǒng)高通濾波器及一個(gè)傳統(tǒng)低通濾波器串聯(lián)而組成。低通濾波器用于消除傳感器產(chǎn)生的高頻無(wú)關(guān)噪聲，高通濾波器用于過(guò)濾頻率較低的指令信號(hào)。此外，由高通濾波器所引起的相位超前可由低通濾波器所引起的相位滯后抵消。通過(guò)合理設(shè)計(jì)兩個(gè)濾波器，可將8～12 Hz頻帶內(nèi)的相位失真控制到近乎于零。

設(shè)計(jì)高通濾波器及低通濾波器時(shí)，對(duì)比巴特沃斯濾波器，切比雪夫?yàn)V波器及橢圓濾波器，選擇三者中滿足設(shè)計(jì)條件下8～12 Hz頻帶內(nèi)相位失真最小且相位角變化率最小的橢圓濾波器。分別設(shè)計(jì)2階IIR橢圓高通濾波器和2階IIR橢圓低通濾波器，設(shè)計(jì)參數(shù)如表1所示。

表1 濾波器主要設(shè)計(jì)參數(shù)

3.3 前饋補(bǔ)償PD控制

除人手生理震顫外，從操作臂關(guān)節(jié)內(nèi)非線性摩擦同樣會(huì)導(dǎo)致手術(shù)工具末端震顫現(xiàn)象，特別是在低速狀態(tài)下。當(dāng)醫(yī)生進(jìn)行精細(xì)的手術(shù)動(dòng)作時(shí)，從操作臂各關(guān)節(jié)角速度一般不超過(guò)π/3 rad/s，其表現(xiàn)出“停滯—滑動(dòng)—停滯—滑動(dòng)”的跳躍現(xiàn)象，即“粘滑行為”[13-14]。

為消除這種現(xiàn)象，在傳統(tǒng)PD控制的基礎(chǔ)上，引入前饋補(bǔ)償，并且選取Stribeck模型對(duì)非線性摩擦進(jìn)行建模及參數(shù)辨識(shí)。

各關(guān)節(jié)角的摩擦力矩可表示為[15]：

其中i=1,2,3，F(xiàn)ci為庫(kù)倫摩擦，F(xiàn)si為靜摩擦，Vsi為切換速率，αi表示粘性摩擦系數(shù)。

各關(guān)節(jié)伺服系統(tǒng)可表示為：

其中i=1,2,3，Ji為當(dāng)前關(guān)節(jié)的轉(zhuǎn)動(dòng)慣量，θi為當(dāng)前時(shí)刻關(guān)節(jié)角，ui表示控制力矩。由式（5）可知，關(guān)節(jié)處于勻速運(yùn)動(dòng)狀態(tài)即當(dāng)0時(shí)，ui=fi。因此，分別選取一組速度，可得到各個(gè)關(guān)節(jié)對(duì)應(yīng)的控制力矩。運(yùn)用遺傳算法對(duì)摩擦參數(shù)進(jìn)行辨識(shí)[16]，各關(guān)節(jié)摩擦辨識(shí)參數(shù)如表2所示。

表2 摩擦模型參數(shù)

在獲得摩擦模型的基礎(chǔ)上，設(shè)計(jì)前饋補(bǔ)償PD（FFPD）控制，其控制結(jié)構(gòu)如圖6所示。定義跟蹤誤差為e=θd-θ，其中e、θd和θ均為3×1維列向量。定義比例系數(shù)KP，微分系數(shù)KD為3×3維對(duì)角矩陣。摩擦力矩僅與當(dāng)前關(guān)節(jié)角速度有關(guān)，因此摩擦前饋值可根據(jù)位置指令微分及Stribeck摩擦模型獲得。將位置差e乘以比例系數(shù)KP，速度差e˙乘以微分系數(shù)KD后，三者可得輸出控制力矩為：

圖6 摩擦前饋PD控制結(jié)構(gòu)框圖

由于微創(chuàng)手術(shù)機(jī)器人的特殊應(yīng)用場(chǎng)合，在控制力矩命令發(fā)送至伺服電機(jī)及驅(qū)動(dòng)器前，還需要對(duì)其進(jìn)行判斷是否超出輸出力矩安全閾值。

4 主從控制仿真及實(shí)驗(yàn)

為驗(yàn)證以上主從控制系統(tǒng)的有效性和優(yōu)越性，設(shè)計(jì)以下仿真及實(shí)驗(yàn)。

4.1 仿真1

新型零相位濾波器可無(wú)相位失真的獲取到人手真實(shí)的生理震顫信號(hào)，根據(jù)前文所述生理震顫信號(hào)n(t)和手術(shù)指令d(t)分布頻率不同，二者分別可用以下函數(shù)進(jìn)行表示：

如圖7所示，藍(lán)色線表示輸入信號(hào)，其包括期望信號(hào)d(t)，震顫信號(hào)n(t)以及一些高頻無(wú)關(guān)干擾，紅色線表示經(jīng)過(guò)新型零相位濾波器（ZPF）處理后的信號(hào)，圖中所示震顫信號(hào)被完整的分離出來(lái)。為更明顯地體現(xiàn)該方法震顫信號(hào)提取效果，圖8輸入信號(hào)僅為震顫信號(hào)n(t)，從圖中可以看出，經(jīng)過(guò)新型零相位濾波器（ZPF）處理后的震顫信號(hào)，在幅值和相位上均沒(méi)有變化。

圖7 新型零相位濾波震顫提取

圖8 新型零相位濾波震顫對(duì)比效果

4.2 仿真2

前饋補(bǔ)償PD控制（FFPD）能有效地抑制由非線性摩擦引起的“粘滑行為”，現(xiàn)對(duì)1關(guān)節(jié)進(jìn)行仿真，其指令軌跡為：

關(guān)節(jié)初始位置為0.8 rad，初始速度為0.1 rad/s，分別對(duì)前饋補(bǔ)償PD（FFPD）控制和傳統(tǒng)PD控制進(jìn)行仿真，位置跟蹤及速度跟蹤結(jié)果分別如圖9及圖10所示。

圖9 位置跟蹤仿真圖

圖10 速度跟蹤仿真圖

圖9和圖10中黑色線表示指令位置或指令速度，紅色線表示前饋補(bǔ)償PD控制律下的位置跟蹤或速度跟蹤，藍(lán)色線表示傳統(tǒng)PD控制律下的位置跟蹤或速度跟蹤。由9圖可知，傳統(tǒng)PD控制在3 s及6 s附近出現(xiàn)“平頂現(xiàn)象”影響了位置跟蹤精度，而前饋補(bǔ)償PD控制則較好的跟蹤了指令位置。圖10中，在角速度接近零附近時(shí)，傳統(tǒng)PD控制出現(xiàn)了“停滯—啟動(dòng)”的現(xiàn)象即為“粘滑行為”，該現(xiàn)象必將導(dǎo)致機(jī)械臂的震顫，而前饋補(bǔ)償PD控制則有效地避免了該現(xiàn)象的出現(xiàn)。

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

分別將新型零相位濾波算法和前饋補(bǔ)償PD控制律寫(xiě)入PC和PMAC中，進(jìn)行完整實(shí)驗(yàn)驗(yàn)證。實(shí)驗(yàn)采用主從1∶1比例控制，為直觀對(duì)比，主操作臂及從操作臂的初始位置均為零位。主從映射后，手持主操作臂在空間內(nèi)完成一段空間曲線，分別記錄經(jīng)過(guò)零相位濾波后的光滑指令，使用本文主從控制策略（ZPF+FFPD）和傳統(tǒng)PD控制律下的軌跡，其空間軌跡如圖11所示。

圖11 實(shí)驗(yàn)空間軌跡圖

圖11中黑色線表示經(jīng)過(guò)生理震顫濾波后的光滑指令，紅色線表示使用本文控制策略下的末端位置軌跡，藍(lán)色線表示傳統(tǒng)PD控制下的末端軌跡。從圖中可知，傳統(tǒng)PD控制律導(dǎo)致末端震顫現(xiàn)象嚴(yán)重，而使用本文提出的震顫抑制主從控制系統(tǒng)，震顫現(xiàn)象得到有效抑制。以光滑指令為理想值，則二者在X軸方向的誤差如圖12所示，由圖可知，本文提出的主從控制系統(tǒng)相比僅使用傳統(tǒng)PD控制器具有明顯震顫抑制效果。此外二者的標(biāo)準(zhǔn)差及均方根誤差如表3所示，從表中可知，本文提出的方法具有良好的位置跟蹤精度。

圖12X軸方向位置跟蹤誤差

表3X軸位置跟蹤誤差

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

針對(duì)現(xiàn)有微創(chuàng)手術(shù)機(jī)器人普遍存在的末端手術(shù)工具震顫現(xiàn)象，提出了一種震顫抑制主從控制系統(tǒng)。在分析了人手生理震顫以及手術(shù)過(guò)程中期望指令的特征前提下，提出新型零相位濾波算法，它將生理震顫信號(hào)無(wú)相位失真的提取出并反向補(bǔ)償至實(shí)際位置指令中，得到平滑的期望信號(hào)。此外，為克服從操作臂關(guān)節(jié)非線性摩擦導(dǎo)致的“粘滑行為”提出了前饋補(bǔ)償PD（FFPD）控制，根據(jù)Stribeck模型提前對(duì)摩擦力進(jìn)行補(bǔ)償。最后，通過(guò)對(duì)比仿真及實(shí)驗(yàn)，驗(yàn)證了該方法能有效地抑制微創(chuàng)手術(shù)機(jī)器人手術(shù)工具末端震顫現(xiàn)象。本文不僅對(duì)微創(chuàng)手術(shù)機(jī)器人震顫抑制提供了系統(tǒng)方法，同時(shí)該思想同樣適用于其他遙操作機(jī)器人設(shè)計(jì)。

[1]Ortmaier T J.Motion compensation in minimally invasive robotic surgery[D].Germany：Technical University of Munich，2003.

[2]Noonan D P，Elson D S，Mylonas G P，et al.Laser-induced fluorescence and reflected white light imaging for robotassisted MIS[J].IEEE Transactions on Biomedical Engineering，2009，56（3）：889-892.

[3]Vitiello V，Lee S L，Cundy T P，et al.Emerging robotic platforms for minimally invasive surgery[J].IEEE Reviews in Biomedical Engineering，2012，6：111-126.

[4]Mettler L，Ibrahim M，Jonat W.One year of experience working with the aid of a robotic assistant（the voicecontrolled optic holder AESOP）in gynaecological endoscopic surgery[J].Human Reproduction，1998，13（10）：2748-2750.

[5]Butner S E，Ghodoussi M.A real-time system for telesurgery[C]//Proceedings of the International Conference on Distributed Computing Systems，2001：236-243.

[6]Marescaux J，Leroy J，GagnerM，et al.Transatlantic robotassisted telesurgery[J].Nature，2001，413：379-380.

[7]王樹(shù)新，丁杰男，贠今天，等.顯微外科手術(shù)機(jī)器人——“妙手”系統(tǒng)的研究[J].機(jī)器人，2006，28（2）：130-135.

[8]Sang H Q，Wang S X，Li J M，et al.Control design and implementation of a novel master-slave surgery robot system，MicroHand A[J].Int J Med Robotics Comput Assis Surg，2011，7（3）：334-347.

[9]馬如奇.微創(chuàng)外科手術(shù)機(jī)器人執(zhí)行系統(tǒng)研制及其控制算法研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2013.

[10]Deuschl G，Raethjen J，Lindemann M，et al.The pathophysiology of tremor[J].Muscle&Nerve，2001，24（6）：716-735.

[11]Defazio G，Gigante A F，Abbruzzese G，et al.Tremor in primary adult-onset dystonia：prevalence and associated clinical features[J].Journal of Neurology Neurosurgery&Psychiatry，2013，84（4）：404-408.

[12]Veluvolu K C，Ang W T.Estimation of physiological tremor from accelerometers for real-time applications[J].Sensors，2011，11（3）：3020-3036.

[13]Zou S，Pan B，F(xiàn)u Y，et al.An adaptive fuzzy sliding mode control for minimally invasive surgical robot’s remote center mechanisms[C]//Proceedings of the IEEE International Conference on Information and Automation，July 28-30，2014：102-107.

[14]Armstrong-Hélouvry B.Stick slip and control in lowspeed motion[J].IEEE Trans on Automat Contr，1993，38（10）：1483-1496.

[15]Xu J，Qiao M，Wang W，et al.Fuzzy PID control for AC servo system based on Stribeck friction model[C]//Proceedings of the 6th International Forum on Strategic Technology，Aug 22-24，2011：706-711.

[16]Liu D.Parameter identification for Lugre Friction model using genetic algorithms[C]//Proceedings of the International Conference on Machine Learning and Cybernetics，Aug 13-16，2006：3419-3422.