具有震顫抑制的微創機器人主從控制系統設計

楊鋮浩，桑宏強，2，劉 芬，贠今天，陳 發

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

1.天津工業大學 機械工程學院，天津 300387

2.天津市現代機電裝備技術重點實驗室，天津 300387

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

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

1 引言

微創外科手術中，醫生通過患者腹部的微小切口將細長的手術器械及內窺鏡伸入病灶，借助監視器進行操作。較傳統開放手術，其具有出血少，創口小，恢復快，病人痛苦輕等優點[1]。然而，它存在手眼協調性差，手術工具靈活度低，容易放大手部生理震顫，長時間操作導致醫生疲勞等缺點。隨著計算機與機器人技術的發展，機器人輔助微創外科手術開始受到國內外學者的廣泛關注[2-3]。

1994年，美國Computer Motion公司研制的AESOP機器人是美國食品藥品管理局（FDA）批準的第一臺可用于手術室輔助手術的機器人[4]。1996年，該公司利用先前積累的關鍵技術，成功研制出了具備強大視覺功能的ZEUS機器人系統[5]。2001年，美國Intuitive Surgical公司研發了第一代Da Vinci外科手術機器人并通過了FDA認證，其被視為微創外科手術機器人研究的一個里程碑[6]。

國內方面，天津大學于2004年開發了主從遙操作顯微外科手術機器人“妙手”系統[7]，并于2010年進一步研制了“妙手A”腹腔微創手術機器人系統[8]。2013年，哈爾濱工業大學成功研制出了腹腔鏡外科手術機器人系統[9]。

圖1 微創手術機器人系統結構示意圖

圖2 微創手術機器人主從控制結構圖

目前，國內對于微創手術機器人系統研究，特別是在控制系統方面仍存在一些亟待解決的問題，例如：醫生在手術操作中的生理震顫無法有效抑制，從操作臂在低速狀態下由于其關節內部非線性摩擦表現出的“粘滑行為”等。面對上述情況，本文提出一種震顫抑制的主從控制系統，分別設計了為抑制生理震顫的新型零相位濾波震顫抑制算法和為克服“粘滑行為”導致震顫現象的前饋補償PD控制器。最后通過仿真及實驗，驗證了該方法具有良好的震顫抑制效果。

2 微創手術機器人系統

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

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

3 微創手術機器人主從控制

3.1 主/從操作臂運動學

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

圖3 主操作臂零位構型圖

圖4 從操作臂零位構型圖

因此，主操作臂的正運動學映射可以表示為：

從操作臂的正運動學映射可以表示為：

從操作臂的逆運動學映射可以表示為：

3.2 新型零相位濾波

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

圖5 生理震顫抑制實現原理

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

新型零相位濾波器（ZPF）由一個傳統高通濾波器及一個傳統低通濾波器串聯而組成。低通濾波器用于消除傳感器產生的高頻無關噪聲，高通濾波器用于過濾頻率較低的指令信號。此外，由高通濾波器所引起的相位超前可由低通濾波器所引起的相位滯后抵消。通過合理設計兩個濾波器，可將8～12 Hz頻帶內的相位失真控制到近乎于零。

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

表1 濾波器主要設計參數

3.3 前饋補償PD控制

除人手生理震顫外，從操作臂關節內非線性摩擦同樣會導致手術工具末端震顫現象，特別是在低速狀態下。當醫生進行精細的手術動作時，從操作臂各關節角速度一般不超過π/3 rad/s，其表現出“停滯—滑動—停滯—滑動”的跳躍現象，即“粘滑行為”[13-14]。

為消除這種現象，在傳統PD控制的基礎上，引入前饋補償，并且選取Stribeck模型對非線性摩擦進行建模及參數辨識。

各關節角的摩擦力矩可表示為[15]：

其中i=1,2,3，Fci為庫倫摩擦，Fsi為靜摩擦，Vsi為切換速率，αi表示粘性摩擦系數。

各關節伺服系統可表示為：

其中i=1,2,3，Ji為當前關節的轉動慣量，θi為當前時刻關節角，ui表示控制力矩。由式（5）可知，關節處于勻速運動狀態即當0時，ui=fi。因此，分別選取一組速度，可得到各個關節對應的控制力矩。運用遺傳算法對摩擦參數進行辨識[16]，各關節摩擦辨識參數如表2所示。

表2 摩擦模型參數

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

圖6 摩擦前饋PD控制結構框圖

由于微創手術機器人的特殊應用場合，在控制力矩命令發送至伺服電機及驅動器前，還需要對其進行判斷是否超出輸出力矩安全閾值。

4 主從控制仿真及實驗

為驗證以上主從控制系統的有效性和優越性，設計以下仿真及實驗。

4.1 仿真1

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

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

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

圖8 新型零相位濾波震顫對比效果

4.2 仿真2

前饋補償PD控制（FFPD）能有效地抑制由非線性摩擦引起的“粘滑行為”，現對1關節進行仿真，其指令軌跡為：

關節初始位置為0.8 rad，初始速度為0.1 rad/s，分別對前饋補償PD（FFPD）控制和傳統PD控制進行仿真，位置跟蹤及速度跟蹤結果分別如圖9及圖10所示。

圖9 位置跟蹤仿真圖

圖10 速度跟蹤仿真圖

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

4.3 實驗

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

圖11 實驗空間軌跡圖

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

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

表3X軸位置跟蹤誤差

5 結束語

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

[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]王樹新，丁杰男，贠今天，等.顯微外科手術機器人——“妙手”系統的研究[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]馬如奇.微創外科手術機器人執行系統研制及其控制算法研究[D].哈爾濱：哈爾濱工業大學，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，Fu 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.