張忠林 熊 璟 張東文

(中國科學(xué)院深圳先進(jìn)技術(shù)研究院 深圳 518055)

主從同構(gòu)穿刺手術(shù)機(jī)器人遙操作控制策略研究

張忠林 熊 璟 張東文

(中國科學(xué)院深圳先進(jìn)技術(shù)研究院 深圳 518055)

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

穿刺手術(shù)機(jī)器人；主從控制；雅克比矩陣；PD 控制算法；抖動消除

1 引 言

遙操作穿刺手術(shù)是通過專用手術(shù)器械插入患者體內(nèi)，對患者體內(nèi)的病灶進(jìn)行手術(shù)操作的外科手術(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]是較具代表性的醫(yī)療遙操作系統(tǒng)。它們不僅為醫(yī)生提供了先進(jìn)的視覺反饋，而且讓手術(shù)醫(yī)生擁有了更加靈活的操作方式，在醫(yī)療手術(shù)機(jī)器人領(lǐng)域具有革命性的意義。

穿刺手術(shù)主從機(jī)器人系統(tǒng)是一個擁有主從控制器的遙操作裝置。近年來，在穿刺手術(shù)機(jī)器人遙操作研究中，許多研究學(xué)者都嘗試通過反饋信息(如速度、加速度、力和視頻等)來進(jìn)行主從系統(tǒng)的精確跟隨控制研究，從而建立高精度的遙操作系統(tǒng)[5]。雖然通過上述反饋信息構(gòu)建的反饋控制系統(tǒng)精度很高，但是系統(tǒng)較為復(fù)雜，而且響應(yīng)速度未達(dá)理想要求，如當(dāng)速度或加速度很快時，其控制性能就可能會惡化，甚至不穩(wěn)定。此外，潛在的可變時延問題也可能會導(dǎo)致系統(tǒng)的不穩(wěn)定。

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

2 遙操作機(jī)器人構(gòu)型

穿刺手術(shù)機(jī)器人系統(tǒng)采用主從控制結(jié)構(gòu)，外科醫(yī)生通過操縱主機(jī)器人(主手)來實(shí)現(xiàn)對從機(jī)器人(從手)的控制，從而完成相關(guān)手術(shù)任務(wù)。本系統(tǒng)機(jī)器人采用的設(shè)備是 SensAble 公司生產(chǎn)的PHANToM OMNI[5]，如圖 1 所示。PHANToM OMNI 是一款具有6 自由度的小型機(jī)器人，其所有關(guān)節(jié)都為旋轉(zhuǎn)關(guān)節(jié)，前三個關(guān)節(jié)控制機(jī)器人末端位置坐標(biāo)，后三關(guān)節(jié)采用三軸交匯的方式控制機(jī)器人姿態(tài)坐標(biāo)。主手工作空間即為點(diǎn) P 的活動空間。另外，本文中所有坐標(biāo)均指在笛卡爾空間下的坐標(biāo)。

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

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

本文穿刺手術(shù)機(jī)器人主從控制系統(tǒng)如圖 2 所示。其中，Xm和 Xs分別表示主手和從手末端執(zhí)行器在空間坐標(biāo)中的位置坐標(biāo)；和分別表示主從手末端執(zhí)行器在空間坐標(biāo)中的速度矢量；θm和 θs分別表示從手關(guān)節(jié)角度和關(guān)節(jié)角速度矢量；為從手逆雅克比矩陣；LPF 為低通數(shù)字濾波器。

圖 1 PHANToM OMNI 初始位置信息Fig. 1. PHANToM OMNI robot and its initial position

圖 2 主從控制系統(tǒng)框圖Fig. 2. Block diagram of master-slave robot control system

3.1 主從控制策略

本文通過笛卡爾空間控制來建立主從機(jī)器人之間的聯(lián)系，笛卡爾空間控制的重點(diǎn)在于逆雅克比矩陣的運(yùn)算。本文對雅克比矩陣的運(yùn)算采用微分變換思想，即利用微小時間段內(nèi)的位移代替瞬時速度的方法對其進(jìn)行運(yùn)算。通過逆雅克比矩陣的運(yùn)算可以得到逆運(yùn)動學(xué)唯一解，從而降低了運(yùn)動學(xué)計(jì)算的復(fù)雜程度，且系統(tǒng)的實(shí)時性也得到了提高。

雅克比矩陣是機(jī)器人關(guān)節(jié)空間速度向機(jī)器人末端笛卡爾空間速度的映射，也可以看成是將機(jī)器人關(guān)節(jié)速度與機(jī)器人末端在笛卡爾空間的速度聯(lián)系起來，其表達(dá)式為：

因?yàn)橛?jì)算機(jī)控制是一種采樣控制，只能根據(jù)采樣時刻的偏差值計(jì)算控制量，所以可利用機(jī)器人末端和關(guān)節(jié)角微小時間段內(nèi)的位移和分別代替瞬時末端速度和關(guān)節(jié)速度，從而式(1)運(yùn)動學(xué)問題還可表示為：

由式(2)得機(jī)器人逆運(yùn)動學(xué)問題求解關(guān)系：

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

PD 閉環(huán)控制可以較好地消除上述主從跟隨誤差，所以我們在此引入了閉環(huán) PD 控制算法，將末端執(zhí)行器的位姿坐標(biāo)與目標(biāo)位姿坐標(biāo)信息比較后的誤差送到 PD 控制器，進(jìn)而消除這種誤差。通過調(diào)節(jié)比例微分系數(shù) kp、kv來使系統(tǒng)能夠迅速地達(dá)到穩(wěn)狀態(tài)，最終使從手末端執(zhí)行器位姿能夠精確、迅速地跟隨主手末端執(zhí)行器位姿坐標(biāo)變化。PD 控制律如下：

3.2 關(guān)節(jié)速度求解與抖動消除

PHANToM OMNI 關(guān)節(jié)速度的求解過程如圖3 所示。通過對機(jī)器人的任何一個關(guān)節(jié)引入一個延遲模塊，可以得到當(dāng)前時刻關(guān)節(jié)角度的輸出與上一時刻輸出的差量，即可得到機(jī)器人此關(guān)節(jié)的速度；

圖 3 關(guān)節(jié)速度求解框圖Fig. 3. The block diagram of solving joint velocity

在手術(shù)過程中醫(yī)生手部難免會發(fā)生抖動，而在長時間的手術(shù)過程中醫(yī)生疲勞度增加，手部抖動會更加明顯。這些無關(guān)的抖動通過主從映射到從手的運(yùn)動上，最終會影響手術(shù)精度[7]。本系統(tǒng)首先對主手采樣數(shù)據(jù)進(jìn)行一次滑動均值濾波，來減小外科醫(yī)生手部抖動對從手的影響，然后再對通過主從映射得到的從手關(guān)節(jié)角度數(shù)據(jù)信息進(jìn)行二次濾波，最終有效消除抖動。

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

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

4 半實(shí)物仿真

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

4.1 反饋消除誤差仿真

圖 4 為引入 PD 反饋控制環(huán)節(jié)前后主從跟隨誤差效果。從圖中的半實(shí)物仿真結(jié)果可以看出，引入 PD 環(huán)節(jié)的控制系統(tǒng)能夠有效地消除由于逆雅克比矩陣運(yùn)算而產(chǎn)生的積累誤差，從而能夠滿足穿刺手術(shù)任務(wù)的要求。

圖 4 主從運(yùn)動跟隨誤差Fig. 4. Master-slave tracking error

4.2 抖動消除仿真

本系統(tǒng)進(jìn)行抖動濾波時，取 n＝15，也即所設(shè)計(jì)數(shù)字濾波器為 15 階濾波器。圖 5 所示為從手末端的抖動濾波效果圖。其中，圖 5(a)表示抖動消除前的從手末端運(yùn)動軌跡，結(jié)果顯示其有較多的抖動毛刺；圖 5(b)表示經(jīng)過平滑數(shù)字濾波器進(jìn)行抖動濾波后的運(yùn)動軌跡，可以看出其運(yùn)動曲線毛刺明顯減少，也即是外科醫(yī)生手部的無關(guān)抖動得到了顯著消除。

圖 5 從手末端抖動消除前后效果圖Fig. 5. Tremor elimination results of slave end-effector

4.3 主從跟隨控制仿真

在進(jìn)行主從跟隨半實(shí)物仿真實(shí)驗(yàn)時，從手是在無約束的條件下跟隨主手運(yùn)動。圖 6 和圖 7 為主從手末端執(zhí)行器的位姿跟隨效果圖。圖中分別給出了主從手在 X、Y 和 Z 方向的位置隨時間變化情況。在半實(shí)物仿真過程中的剛開始的幾秒內(nèi)，操作者比較快速度地操作主手運(yùn)動，直到20 秒左右時，再以較慢的速度操作主手運(yùn)動。

圖 6 主從位置跟隨Fig. 6. Master-slave position following

從圖 6 和圖 7 可以看出，無論主手是在低速運(yùn)動還是高速運(yùn)動，穿刺手術(shù)機(jī)器人從手末端在笛卡爾空間下的 X、Y 和 Z 方向上的位姿坐標(biāo)都能夠平穩(wěn)、迅速地跟隨主手末端的位姿坐標(biāo)變化，說明了本系統(tǒng)控制策略的合理性，基本能夠滿足穿刺手術(shù)操作的任務(wù)要求。

圖 7 主從姿態(tài)跟隨Fig. 7. Master-slave posture following

本文為主從同構(gòu)機(jī)器人的控制，其主從關(guān)節(jié)一一對應(yīng)，所以選擇簡單的關(guān)節(jié)—關(guān)節(jié)控制即可獲得較為理想的控制效果。但是，本文目的是為下一步進(jìn)行主從異構(gòu)機(jī)器人控制研究做理論準(zhǔn)備，而主從異構(gòu)機(jī)器人關(guān)節(jié)并非一一對應(yīng)關(guān)系，不適合采用簡單的關(guān)節(jié)控制方法，因此主從機(jī)器人需要通過運(yùn)動學(xué)正、逆運(yùn)算才能建立聯(lián)系。逆運(yùn)動學(xué)的求解方法有很多，比較典型的有 Pieper解法[9]、Lee 和 Ziegler 提出的幾何法[10]以及 Paul等[11]提出的反變換法等。然而，這些方法得到的關(guān)節(jié)變量表達(dá)式可能含有超越函數(shù)，使計(jì)算較為復(fù)雜且會產(chǎn)生多個解，從而需要對這些解進(jìn)行實(shí)時最優(yōu)選取，這嚴(yán)重影響了系統(tǒng)的實(shí)時性及控制精度。

因此，本文提出了逆雅克比矩陣運(yùn)算的方法，逆雅克比矩陣運(yùn)算能得到唯一解，提高了系統(tǒng)實(shí)時性；同時又引入 PD 閉環(huán)控制環(huán)節(jié)來消除了逆雅克比矩陣運(yùn)算引起的積累誤差。半實(shí)物仿真結(jié)果充分說明本文控制策略的合理性，從而為下一步主從異構(gòu)穿刺機(jī)器人遙操作控制的研究提供了理論基礎(chǔ)。

5 總 結(jié)

為了滿足穿刺手術(shù)機(jī)器人主從運(yùn)動控制的精確、快速以及穩(wěn)定性，本文提出了基于逆雅克比矩陣運(yùn)算法和 PD 閉環(huán)控制的主從控制策略。半實(shí)物仿真結(jié)果驗(yàn)證了算法的可行性，基本能夠滿足穿刺手術(shù)任務(wù)的要求。但本文研究只涉及運(yùn)動學(xué)方面，并沒有虛擬力反饋和動力學(xué)方面的研究，因此下一步工作不僅僅要在實(shí)物平臺驗(yàn)證算法的可行性，還要進(jìn)行虛擬向?qū)Ш蛣恿W(xué)方面的研究，最終為主從異構(gòu)穿刺手術(shù)機(jī)器人遙操作控制的研究做好理論準(zhǔn)備。

[1] Kazanzides P, Fichtinger G, Hager GD, et a1. Surgical and interventional robotics-core concepts, technology, and design [J]. IEEE Robotics and Automation Magazine, 2008, 15(2): 122-130.

[2] Shew SB, Ostlie DJ, Holcomb GW. Robotic telescopic assistance in pediatric laparoscopic surgery [J]. Pediatric Endosurgery and Innovative Techniques, 2003, 7(4): 371-376.

[3] Lanfranco AR, Castellanos AE, Desai JP, 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-392.

[5] Silva AJ, Ramirez OAD, Vega VP, et al. PHANToM OMNI haptic device: kinematic and manipulability [C] // Robotics and Automotive Mechanics Conference on Electronics, 2009: 193-198.

[6] Aliaga I, Rubio A, Sánchez E. Experimental quantitative comparison of different control architectures for master-slave teleoperation [J]. IEEE Transactions on Control Systems Technology, 2004, 12(1): 2-11.

[7] Gonzalez JG, Heredia EA, Rahman T, et al. Optimal digital fi ltering for tremor suppression [J]. IEEE Transactions on Biomedical Engineering, 2000, 47(5): 664-673.

[8] Mohammadi A, Tavakoli M, Jazayeri A. PHANTSIM: a simulink toolkitfor the sensable PHANToM haptic devices [C] // Proceedings of the 23rd CANCAM, 2011: 787-790.

[9] Pieper DL.The kinematics of manipulators under computer control [D]. Stanford: Stanford University, 1968.

[10] Lee CSG, Ziegler M. A geometric approach in solving the inverse kinematics of PUMA robots [J]. IEEE Transactions on Aerospace and Electronic Systems, 1984, 20(6): 695-706.

[11] Paul RP, Shimano B, Mayer GE. Kinematic control equations for simple manipulators [J]. IEEE Transactions on Systems, Man and Cybernetics, 1981, 11(6): 449-455.

Study on Teleoperation Control Strategy of Master-Slave Isomorphic Robots in Puncture Surgeries

ZHANG Zhonglin XIONG Jing ZHANG Dongwen

( Shenzhen Institutes of Advanced Technology, Chinese Academy of Sciences, Shenzhen 518055, China )

In this paper, a semi-physical simulation platform of the master-slave robot control simulation system was built based on the analysis on functional requirements of the PHANToM OMNI robot system. Based on differential transformation method, the real-time master-slave control was executed by using Jacobian matrix and proportionalderivative(PD) close-loop control algorithm, and the digital fi lter was designed to eliminate effects of surgeons’ hand lowfrequency tremors on the precision of the master-slave control. The simulation experimental results demonstrate that the slave robot can follow precisely and quickly movements of the master robot and eliminate hand tremors.

teleoperation; master-slave control; Jacobian matrix; PD control algorithm; tremor elimination

TP 242

A

2013-11-20

張忠林，碩士研究生，研究方向?yàn)槲?chuàng)手術(shù)機(jī)器人的主從控制；熊璟(通訊作者)，助理研究員，研究方向?yàn)獒t(yī)療機(jī)器人與機(jī)器人機(jī)構(gòu)學(xué)等，E-mail：jing.xiong@siat.ac.cn；張東文，工程師，研究方向?yàn)獒t(yī)療機(jī)器人等。