摘 要： 在分析穿刺機器人系統(tǒng)功能需求的基礎上，搭建了主從遙操作系統(tǒng)的半實物仿真平臺，并給出雅克比矩陣方法和PD控制律的聯(lián)合控制方法。通過設計數(shù)字濾波器，以消除外科醫(yī)生的手部低頻抖動對穿刺手術機器人精度的影響。實驗結果表明，從機器人末端執(zhí)行器在笛卡爾空間坐標下能夠精確、快速、安全地跟隨主機器人末端執(zhí)行器的位置變化，并且外科醫(yī)生的手部抖動能夠被有效消除。
遙操作穿刺手術是通過專用手術器械插入患者體內，并在醫(yī)學圖像的引導下，對患者體內的病灶進行手術操作的外科手術，具有創(chuàng)傷小、痛苦小、康復快、術后并發(fā)癥少等優(yōu)點，是當前主要發(fā)達國家競相研究的醫(yī)學領域之一[1]。目前已有很多較為成熟的醫(yī)療遙操作系統(tǒng)，如Computer Motion公司研發(fā)的AESOP腹腔鏡操作機器人[2]、Zeus微創(chuàng)手術機器人系統(tǒng)[3]以及Intuitive Surgical公司研發(fā)的Da Vinci微創(chuàng)手術機器人系統(tǒng)[4]。這些系統(tǒng)不僅為醫(yī)生提供了先進的視覺反饋，而且讓手術醫(yī)生擁有了更加靈活的操作方式，在醫(yī)療手術機器人領域具有革命性的意義。
穿刺手術主從機器人系統(tǒng)作為一個擁有主從控制方式的遙操作裝置，近年來，在穿刺手術機器人遙操作研究中，其系統(tǒng)結構也從主從同構形式發(fā)展成了主從異構形式。主從異構機器人實時控制需要系統(tǒng)能夠在短時間內完成機器人的正逆運動學計算、誤差消除、抖動消除等。
本文主要針對穿刺手術遙操作控制策略、抖動消除等進行了相關半實物仿真研究。本文方法的優(yōu)點在于利用較為簡單的控制系統(tǒng)結構，獲得較好的控制精度和響應速度，并易于在實驗中實現(xiàn)。
1 遙操作主從機器人構型
穿刺手術機器人系統(tǒng)采用主從式控制結構，外科醫(yī)生通過操縱主機器人（主手）來實現(xiàn)對從機器人（從手）的控制。本系統(tǒng)的主機器人采用的是SensAble公司的Phantom Omni[5]，具有6自由度(DOF)，所有關節(jié)都為旋轉關節(jié)，前3個關節(jié)控制機器人末端位置，后3個關節(jié)以三軸交匯的方式控制機器人姿態(tài)，如圖1(a)所示。另外，本文所有坐標均是指在笛卡爾空間下的坐標。從機器人作為穿刺手術系統(tǒng)的執(zhí)行部分負責完成手術操作，擁有6DOF，且都是旋轉關節(jié)，前3個關節(jié)配合實現(xiàn)從機器人末端定位；后3個關節(jié)以三軸交匯的方式實現(xiàn)末端姿態(tài)調整，如圖1(b)所示。
2 輔助穿刺遙操作系統(tǒng)
主從控制系統(tǒng)在醫(yī)療機器人遙操作系統(tǒng)中處于核心的地位，起著協(xié)調主從手、監(jiān)控手術對象、為操作者提供手術操作信息的作用。手術醫(yī)生通過操作主手、主手的位置和速度信息，通過主從映射傳遞給病灶端的從手，從而實現(xiàn)主從手的快速、精確跟隨。
主從控制系統(tǒng)框圖如圖2所示，其中Xm、Xg分別表示主手和從手末端執(zhí)行器在笛卡爾空間坐標中的位置坐標；分別為主從手末端執(zhí)行器在笛卡爾空間坐標中的速度矢量；分別表示從手關節(jié)角度和關節(jié)角速度矢量；為從手逆雅克比矩陣；k為主從映射比例系數(shù)；LPF為低通數(shù)字濾波器。
2.1 主從控制策略的選取
一般情況下，主從控制方案的選擇與主從手的結構相關。主從同構型的機器人，主從手關節(jié)數(shù)目相同且一一對應，利用簡單的關節(jié)-關節(jié)控制就能實現(xiàn)快速精確的主從控制。然而對于主從異構型的機器人，由于主從手各關節(jié)不再一一對應，不能利用關節(jié)控制來解決，必須引入主從手的運動學正、逆運算，在笛卡爾空間坐標系中建立起映射關系。
笛卡爾空間坐標下的主從控制的關鍵在于逆運動學的求解，逆運動學求解有多種方法[6-8]，但是這些方法得到的關節(jié)變量表達式可能含有超越函數(shù)，計算較為復雜，且會產生多個解，需要對這些解進行實時最優(yōu)選取，影響了系統(tǒng)的實時性。本文基于微分變換的思想，利用逆雅克比矩陣進行逆運動學求解，得到唯一解，降低了運動學計算量，且實時性也得到了提高。
雅克比矩陣能將機器人關節(jié)速度與末端在笛卡爾空間的速度聯(lián)系起來，也可稱為機器人關節(jié)空間速度向末端笛卡爾空間速度的映射，表示為：
將機器人末端和關節(jié)角在微小時間段內的位移?駐X和?駐?茲分別代替瞬時末端速度和關節(jié)速度，則式(1)又可表示為：
由式(2)可得到主手末端速度，經過主從映射后得到從手末端速度，再根據(jù)式(3)可得穿刺手術機器人關節(jié)速度。然而，由于逆雅克比矩陣是相對于局部空間位置的映射，隨著機器人在其工作空間的運動，主從跟隨誤差就會不斷積累，從而降低跟隨精度，導致穿刺手術失敗。為了消除這種積累誤差，本系統(tǒng)引入了比例微分PD(Proportional_Derivative)反饋控制環(huán)節(jié)。
在PD控制環(huán)節(jié)，通過調節(jié)比例微分系數(shù)kp、kd來使系統(tǒng)能夠迅速地達到穩(wěn)狀態(tài)，最終使得從手末端執(zhí)行器位姿能夠精確、迅速地跟隨主手末端執(zhí)行器位姿坐標變化。PD控制律如下：
其中分別表示主、從手末端執(zhí)行器位姿速度，Xm、Xs分別表示主、從手末端執(zhí)行器位姿。
2.2 主從控制中的抖動消除
在手術過程中醫(yī)生手部難免發(fā)生抖動，特別是長時間手術時手部抖動會更加顯著。這些無關抖動通過主從映射會反映到從手的運動上，進而會影響手術精度。本系統(tǒng)首先對主手采樣數(shù)據(jù)進行一次滑動均值濾波，然后再對從手關節(jié)角度數(shù)據(jù)信息進行二次濾波，這樣將有效濾除抖動。
滑動均值算法對周期性的干擾具有較好的抑制作用，當系統(tǒng)在一定時間內進行連續(xù)采樣時，每計算一次測量數(shù)據(jù)，只需進行一次采樣，從而極大地提高了對測量數(shù)據(jù)的計算速度，滿足快速控制的實時性要求。算法如下：
在第i次采樣周期中，將采樣得到的每一個離散點在進入下一步采樣之前，利用式(5)進行計算，得到均值采樣結果。
3 半實物仿真
為了驗證本文方法的有效性，利用上述主從控制方案分別進行主從跟隨的半實物仿真實驗，實驗是在帶有Phantom 工具箱[9]的MATLAB Simulink實時仿真環(huán)境下進行的。
3.1 反饋消除誤差仿真
圖3所示為引入PD反饋控制環(huán)節(jié)前后主從跟隨誤差效果。從圖中可以看出，引入PD環(huán)節(jié)后的控制系統(tǒng)能夠有效地消除系統(tǒng)積累誤差。
3.2 抖動消除仿真
進行抖動濾波時，取n=15，即滑動均值濾波器為15階濾波器。圖4所示為從手末端的抖動濾波效果圖，圖4(a)表示抖動消除前的從手末端在Z軸的位移，可以看出其有較多的抖動毛刺；圖4(b)表示經過平滑數(shù)字濾波器進行抖動濾波后的運動軌跡，可以看出其運動曲線毛刺明顯減少，即外科醫(yī)生手部的無關抖動得到了顯著消除。
3.3 主從跟隨控制仿真
在進行主從跟隨半實物仿真時，設定主從映射比例k=1，即從手運動軌跡完全一樣。通過對PD控制參數(shù)的調節(jié)，當kp=0.3、kd=0.001時，能得到較好的跟隨效果，結果如圖5所示(由于在X、Y、Z方向的主從手情況類似，故只給出在X方向的跟隨情況)。此外在實驗剛開始的幾秒鐘里，先緩慢地操縱主手，大概13 s后再加快主手運動速度。
從圖5(a)可以看出，無論主手運動速度快慢，從手都能夠平穩(wěn)、快速、精確地跟隨主手運動。從圖5(b)可以看出，主從手跟隨誤差不超過0.5 mm，說明了本系統(tǒng)控制算法的合理性。
為了滿足穿刺手術機器人主從運動控制的精確、快速及穩(wěn)定性，本文提出了基于雅克比矩陣和PD控制律的主從控制算法，半實物仿真結果驗證了算法的可行性，基本能夠滿足穿刺手術任務的要求。本文研究只涉及運動學方面，并沒有虛擬力反饋和動力學方面的研究，下一步工作是進行虛擬向導和動力學方面的研究。
參考文獻
[1] KAZANZIDES P，F(xiàn)ICHTINGER G，HAGER G D，et a1.Surgical and interventional robotics-core concepts, technology，and design[J].IEEE Robotics and Automation Magazine，2008，15(2)：122-130.
[2] SHEW S B，OSTLIE D J，HOLCOMB G W.Robotic tele-scopic assistance in pediatric laparoscopic surgery[J].Pedia-tric Endosurgery and Innovative Techniques，2003，7(4)：371-376.
[3] LANFRANCO A R，CASTELLANOS A E，DESAI J P，et al.Robotic surgery-a current perspective[J].Annals of Surgery，2004，239(1)：14-21.
[4] BROEDERS I，RUURDA J.Robotics revolutionizing surgery：the intuitive surgical “Da Vinci” system[J].Industrial Robot，2001，28(5)：387-391.
[5] SANSANAYUTH J，NILKHAMHANG I，TUNGPIMOLRAT K.Teleoperation with inverse dynamics control for Phantom Omni haptic device[C].SICE Annual Conference，Akita University，Akita，Japan，2012：2121-2126.
[6] PIEPER D L.The kinematics of manipulators under computercontrol[D].Stanford，USA：Stanford University，1968.
[7] LEE C S G，ZIEGLER M.A geometric approach in solving the inverse kinematics of PUMA robots[J].IEEE Transac-tions on Aerospace and Electronic Systems，1984，20(6)：695-706.
[8] PAUL R P，SHIMANO B，MAYER G E.Kinematic control equations for simple manipulators[J].IEEE Transactions on Systems，Man，and Cybernet-ics，1981，11(6)：449-455.
[9] MOHAMMADI A，TACAKOLI M，JAZAYERIA.Phantsim：a simulink toolkit for the sens-able Phantom haptic devices[C].Proceedings of the 23rd CANCAM，Canada，2011：787-790.