摘  要： 在分析穿刺機(jī)器人系統(tǒng)功能需求的基礎(chǔ)上,，搭建了主從遙操作系統(tǒng)的半實(shí)物仿真平臺,，并給出雅克比矩陣方法和PD控制律的聯(lián)合控制方法,。通過設(shè)計數(shù)字濾波器,，以消除外科醫(yī)生的手部低頻抖動對穿刺手術(shù)機(jī)器人精度的影響。實(shí)驗(yàn)結(jié)果表明，從機(jī)器人末端執(zhí)行器在笛卡爾空間坐標(biāo)下能夠精確,、快速,、安全地跟隨主機(jī)器人末端執(zhí)行器的位置變化，并且外科醫(yī)生的手部抖動能夠被有效消除,。

　　關(guān)鍵詞： 主從控制,；雅克比矩陣；PD控制算法,；抖動消除

　　遙操作穿刺手術(shù)是通過專用手術(shù)器械插入患者體內(nèi),，并在醫(yī)學(xué)圖像的引導(dǎo)下，對患者體內(nèi)的病灶進(jìn)行手術(shù)操作的外科手術(shù),，具有創(chuàng)傷小,、痛苦小、康復(fù)快,、術(shù)后并發(fā)癥少等優(yōu)點(diǎn),，是當(dāng)前主要發(fā)達(dá)國家競相研究的醫(yī)學(xué)領(lǐng)域之一[1]。目前已有很多較為成熟的醫(yī)療遙操作系統(tǒng),，如Computer Motion公司研發(fā)的AESOP腹腔鏡操作機(jī)器人[2],、Zeus微創(chuàng)手術(shù)機(jī)器人系統(tǒng)[3]以及Intuitive Surgical公司研發(fā)的Da Vinci微創(chuàng)手術(shù)機(jī)器人系統(tǒng)[4]。這些系統(tǒng)不僅為醫(yī)生提供了先進(jìn)的視覺反饋,，而且讓手術(shù)醫(yī)生擁有了更加靈活的操作方式,，在醫(yī)療手術(shù)機(jī)器人領(lǐng)域具有革命性的意義。

　　穿刺手術(shù)主從機(jī)器人系統(tǒng)作為一個擁有主從控制方式的遙操作裝置,，近年來,，在穿刺手術(shù)機(jī)器人遙操作研究中，其系統(tǒng)結(jié)構(gòu)也從主從同構(gòu)形式發(fā)展成了主從異構(gòu)形式,。主從異構(gòu)機(jī)器人實(shí)時控制需要系統(tǒng)能夠在短時間內(nèi)完成機(jī)器人的正逆運(yùn)動學(xué)計算,、誤差消除、抖動消除等,。

　　本文主要針對穿刺手術(shù)遙操作控制策略,、抖動消除等進(jìn)行了相關(guān)半實(shí)物仿真研究。本文方法的優(yōu)點(diǎn)在于利用較為簡單的控制系統(tǒng)結(jié)構(gòu),，獲得較好的控制精度和響應(yīng)速度,，并易于在實(shí)驗(yàn)中實(shí)現(xiàn)。

　　1 遙操作主從機(jī)器人構(gòu)型

　　穿刺手術(shù)機(jī)器人系統(tǒng)采用主從式控制結(jié)構(gòu),，外科醫(yī)生通過操縱主機(jī)器人（主手）來實(shí)現(xiàn)對從機(jī)器人（從手）的控制,。本系統(tǒng)的主機(jī)器人采用的是SensAble公司的Phantom Omni[5]，具有6自由度(DOF),，所有關(guān)節(jié)都為旋轉(zhuǎn)關(guān)節(jié),，前3個關(guān)節(jié)控制機(jī)器人末端位置,，后3個關(guān)節(jié)以三軸交匯的方式控制機(jī)器人姿態(tài)，如圖1(a)所示,。另外,，本文所有坐標(biāo)均是指在笛卡爾空間下的坐標(biāo)。從機(jī)器人作為穿刺手術(shù)系統(tǒng)的執(zhí)行部分負(fù)責(zé)完成手術(shù)操作,，擁有6DOF,，且都是旋轉(zhuǎn)關(guān)節(jié)，前3個關(guān)節(jié)配合實(shí)現(xiàn)從機(jī)器人末端定位,；后3個關(guān)節(jié)以三軸交匯的方式實(shí)現(xiàn)末端姿態(tài)調(diào)整,，如圖1(b)所示。

　　2 輔助穿刺遙操作系統(tǒng)

　　主從控制系統(tǒng)在醫(yī)療機(jī)器人遙操作系統(tǒng)中處于核心的地位,，起著協(xié)調(diào)主從手,、監(jiān)控手術(shù)對象、為操作者提供手術(shù)操作信息的作用,。手術(shù)醫(yī)生通過操作主手,、主手的位置和速度信息，通過主從映射傳遞給病灶端的從手,，從而實(shí)現(xiàn)主從手的快速,、精確跟隨。

　　主從控制系統(tǒng)框圖如圖2所示,，其中Xm,、Xg分別表示主手和從手末端執(zhí)行器在笛卡爾空間坐標(biāo)中的位置坐標(biāo)；分別為主從手末端執(zhí)行器在笛卡爾空間坐標(biāo)中的速度矢量,；分別表示從手關(guān)節(jié)角度和關(guān)節(jié)角速度矢量,；為從手逆雅克比矩陣；k為主從映射比例系數(shù),；LPF為低通數(shù)字濾波器,。

　　2.1 主從控制策略的選取

　　一般情況下，主從控制方案的選擇與主從手的結(jié)構(gòu)相關(guān),。主從同構(gòu)型的機(jī)器人,，主從手關(guān)節(jié)數(shù)目相同且一一對應(yīng)，利用簡單的關(guān)節(jié)-關(guān)節(jié)控制就能實(shí)現(xiàn)快速精確的主從控制,。然而對于主從異構(gòu)型的機(jī)器人,，由于主從手各關(guān)節(jié)不再一一對應(yīng)，不能利用關(guān)節(jié)控制來解決,，必須引入主從手的運(yùn)動學(xué)正,、逆運(yùn)算，在笛卡爾空間坐標(biāo)系中建立起映射關(guān)系,。

　　笛卡爾空間坐標(biāo)下的主從控制的關(guān)鍵在于逆運(yùn)動學(xué)的求解,，逆運(yùn)動學(xué)求解有多種方法[6-8],，但是這些方法得到的關(guān)節(jié)變量表達(dá)式可能含有超越函數(shù)，計算較為復(fù)雜,，且會產(chǎn)生多個解,，需要對這些解進(jìn)行實(shí)時最優(yōu)選取,，影響了系統(tǒng)的實(shí)時性,。本文基于微分變換的思想，利用逆雅克比矩陣進(jìn)行逆運(yùn)動學(xué)求解,，得到唯一解,，降低了運(yùn)動學(xué)計算量，且實(shí)時性也得到了提高,。

　　雅克比矩陣能將機(jī)器人關(guān)節(jié)速度與末端在笛卡爾空間的速度聯(lián)系起來,，也可稱為機(jī)器人關(guān)節(jié)空間速度向末端笛卡爾空間速度的映射，表示為：

　　將機(jī)器人末端和關(guān)節(jié)角在微小時間段內(nèi)的位移?駐X和?駐?茲分別代替瞬時末端速度和關(guān)節(jié)速度,，則式(1)又可表示為：

　　由式(2)可得到主手末端速度,，經(jīng)過主從映射后得到從手末端速度，再根據(jù)式(3)可得穿刺手術(shù)機(jī)器人關(guān)節(jié)速度,。然而,，由于逆雅克比矩陣是相對于局部空間位置的映射，隨著機(jī)器人在其工作空間的運(yùn)動,，主從跟隨誤差就會不斷積累,，從而降低跟隨精度，導(dǎo)致穿刺手術(shù)失敗,。為了消除這種積累誤差,，本系統(tǒng)引入了比例微分PD(Proportional_Derivative)反饋控制環(huán)節(jié)。

　　在PD控制環(huán)節(jié),，通過調(diào)節(jié)比例微分系數(shù)kp,、kd來使系統(tǒng)能夠迅速地達(dá)到穩(wěn)狀態(tài)，最終使得從手末端執(zhí)行器位姿能夠精確,、迅速地跟隨主手末端執(zhí)行器位姿坐標(biāo)變化,。PD控制律如下：

　　其中分別表示主、從手末端執(zhí)行器位姿速度,，Xm,、Xs分別表示主、從手末端執(zhí)行器位姿,。

　　2.2 主從控制中的抖動消除

　　在手術(shù)過程中醫(yī)生手部難免發(fā)生抖動,，特別是長時間手術(shù)時手部抖動會更加顯著。這些無關(guān)抖動通過主從映射會反映到從手的運(yùn)動上,，進(jìn)而會影響手術(shù)精度,。本系統(tǒng)首先對主手采樣數(shù)據(jù)進(jìn)行一次滑動均值濾波,，然后再對從手關(guān)節(jié)角度數(shù)據(jù)信息進(jìn)行二次濾波，這樣將有效濾除抖動,。

　　滑動均值算法對周期性的干擾具有較好的抑制作用,，當(dāng)系統(tǒng)在一定時間內(nèi)進(jìn)行連續(xù)采樣時，每計算一次測量數(shù)據(jù),，只需進(jìn)行一次采樣,，從而極大地提高了對測量數(shù)據(jù)的計算速度，滿足快速控制的實(shí)時性要求,。算法如下：

　　在第i次采樣周期中,，將采樣得到的每一個離散點(diǎn)在進(jìn)入下一步采樣之前，利用式(5)進(jìn)行計算,，得到均值采樣結(jié)果,。

　　3 半實(shí)物仿真

　　為了驗(yàn)證本文方法的有效性，利用上述主從控制方案分別進(jìn)行主從跟隨的半實(shí)物仿真實(shí)驗(yàn),，實(shí)驗(yàn)是在帶有Phantom 工具箱[9]的MATLAB Simulink實(shí)時仿真環(huán)境下進(jìn)行的,。

　　3.1 反饋消除誤差仿真

　　圖3所示為引入PD反饋控制環(huán)節(jié)前后主從跟隨誤差效果。從圖中可以看出,，引入PD環(huán)節(jié)后的控制系統(tǒng)能夠有效地消除系統(tǒng)積累誤差,。

　　3.2 抖動消除仿真

　　進(jìn)行抖動濾波時，取n=15,，即滑動均值濾波器為15階濾波器,。圖4所示為從手末端的抖動濾波效果圖，圖4(a)表示抖動消除前的從手末端在Z軸的位移,，可以看出其有較多的抖動毛刺,；圖4(b)表示經(jīng)過平滑數(shù)字濾波器進(jìn)行抖動濾波后的運(yùn)動軌跡，可以看出其運(yùn)動曲線毛刺明顯減少,，即外科醫(yī)生手部的無關(guān)抖動得到了顯著消除,。

　　3.3 主從跟隨控制仿真

　　在進(jìn)行主從跟隨半實(shí)物仿真時，設(shè)定主從映射比例k=1,，即從手運(yùn)動軌跡完全一樣,。通過對PD控制參數(shù)的調(diào)節(jié)，當(dāng)kp=0.3,、kd=0.001時,，能得到較好的跟隨效果，結(jié)果如圖5所示(由于在X,、Y,、Z方向的主從手情況類似，故只給出在X方向的跟隨情況),。此外在實(shí)驗(yàn)剛開始的幾秒鐘里,，先緩慢地操縱主手,，大概13 s后再加快主手運(yùn)動速度。

　　從圖5(a)可以看出,，無論主手運(yùn)動速度快慢,，從手都能夠平穩(wěn)、快速,、精確地跟隨主手運(yùn)動,。從圖5(b)可以看出，主從手跟隨誤差不超過0.5 mm,，說明了本系統(tǒng)控制算法的合理性,。

　　為了滿足穿刺手術(shù)機(jī)器人主從運(yùn)動控制的精確,、快速及穩(wěn)定性,，本文提出了基于雅克比矩陣和PD控制律的主從控制算法，半實(shí)物仿真結(jié)果驗(yàn)證了算法的可行性,，基本能夠滿足穿刺手術(shù)任務(wù)的要求,。本文研究只涉及運(yùn)動學(xué)方面，并沒有虛擬力反饋和動力學(xué)方面的研究,，下一步工作是進(jìn)行虛擬向?qū)Ш蛣恿W(xué)方面的研究,。

　　參考文獻(xiàn)

　　[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.