哈 樂，王東輝

基于D-H參數(shù)方法的微創(chuàng)手術(shù)機(jī)器人運(yùn)動(dòng)仿真研究

哈 樂，王東輝

目的：為了更好地研究外科微創(chuàng)手術(shù)機(jī)器人的工作性能，提出一種基于D-H參數(shù)方法的機(jī)器人運(yùn)動(dòng)仿真研究方法。方法：通過分析機(jī)器人各關(guān)節(jié)的幾何關(guān)系，建立其D-H參數(shù)模型，分別對(duì)機(jī)器人系統(tǒng)進(jìn)行正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)分析，利用Matlab軟件中的SimMechanics模塊對(duì)機(jī)器人機(jī)電系統(tǒng)進(jìn)行建模和仿真實(shí)驗(yàn)。結(jié)果：仿真實(shí)驗(yàn)結(jié)果表明，末端執(zhí)行機(jī)構(gòu)的期望軌跡與實(shí)際軌跡基本一致，運(yùn)動(dòng)誤差都在實(shí)際允許范圍內(nèi)，驗(yàn)證了機(jī)器人運(yùn)動(dòng)學(xué)仿真研究方法的正確性。結(jié)論：采用D-H參數(shù)方法對(duì)微創(chuàng)手術(shù)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)仿真分析具有很好的實(shí)用性和準(zhǔn)確性，對(duì)外科手術(shù)機(jī)器人的精密設(shè)計(jì)和系統(tǒng)改進(jìn)具有普遍的適用意義。

手術(shù)機(jī)器人；D-H方法；運(yùn)動(dòng)仿真；正運(yùn)動(dòng)學(xué)；逆運(yùn)動(dòng)學(xué)

0 引言

1985年，Payne和Wickham首次提出了微創(chuàng)外科（minimal invasive surgery，MIS）的概念，即外科醫(yī)生在患者身體上開2個(gè)或3個(gè)小孔，1個(gè)用于插入內(nèi)窺鏡，另外1個(gè)或2個(gè)用于插入細(xì)長(zhǎng)的手術(shù)工具，醫(yī)生通過監(jiān)視器觀察插入患者體內(nèi)的工具端部運(yùn)動(dòng)來進(jìn)行外科手術(shù)[1-2]。與傳統(tǒng)打開式手術(shù)相比，MIS能使手術(shù)對(duì)健康組織的創(chuàng)傷最小化，能夠很大程度減少患者的痛苦，降低傷口感染的風(fēng)險(xiǎn)，縮短術(shù)后恢復(fù)的時(shí)間，因此，很快被應(yīng)用到各種外科手術(shù)中。目前，MIS主要用于腹腔鏡外科、胸外科、關(guān)節(jié)外科、血管外科、骨盆鏡外科手術(shù)中等。

隨著科學(xué)技術(shù)的不斷發(fā)展，醫(yī)療機(jī)器人系統(tǒng)在醫(yī)學(xué)外科領(lǐng)域越發(fā)普及，微創(chuàng)手術(shù)機(jī)器人因其具有操作準(zhǔn)確、易操作、創(chuàng)傷小等優(yōu)點(diǎn)被廣泛應(yīng)用[3-5]。為了更好地了解其工作性能，需要對(duì)其運(yùn)動(dòng)學(xué)特性進(jìn)行研究和分析。1955年，Denavit和Hartenberg提出關(guān)于用齊次變換矩陣描述機(jī)器人各關(guān)節(jié)之間的幾何關(guān)系的方法[6]（D-H參數(shù)方法）。本研究利用D-H參數(shù)方法建立機(jī)器人系統(tǒng)的運(yùn)動(dòng)學(xué)模型，分別對(duì)其正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)方程進(jìn)行計(jì)算，通過系統(tǒng)仿真實(shí)驗(yàn)的方式驗(yàn)證D-H模型的正確性，從而證明該方法的可行性。

1 D-H參數(shù)模型

1.1 D-H參數(shù)方法

機(jī)器人的運(yùn)動(dòng)學(xué)研究的是機(jī)器人各個(gè)桿件之間的位移關(guān)系、速度關(guān)系和加速度關(guān)系。在進(jìn)行機(jī)器人運(yùn)動(dòng)學(xué)分析時(shí)，需要在空間中為機(jī)器人的每一個(gè)桿件設(shè)置絕對(duì)坐標(biāo)系或者對(duì)機(jī)器人基座設(shè)置相對(duì)坐標(biāo)系，從而獲得每一個(gè)桿件的位置及方向。D-H參數(shù)法用一個(gè)4×4的齊次變換矩陣描述相鄰2個(gè)連桿的空間關(guān)系，從而推導(dǎo)出“末端執(zhí)行器坐標(biāo)系”相對(duì)于“基坐標(biāo)系”的等價(jià)齊次坐標(biāo)變換矩陣，建立系統(tǒng)的運(yùn)動(dòng)方程。

選擇6-DOF微創(chuàng)手術(shù)機(jī)器人作為研究對(duì)象（如圖1所示）。該機(jī)器人包括3個(gè)主動(dòng)關(guān)節(jié)和3個(gè)被動(dòng)關(guān)節(jié)，通過電動(dòng)機(jī)驅(qū)動(dòng)主動(dòng)關(guān)節(jié)實(shí)現(xiàn)對(duì)末端手術(shù)器械的位置調(diào)整，被動(dòng)關(guān)節(jié)用來克服患者切口的約束作用，實(shí)現(xiàn)微調(diào)功能。

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

機(jī)器人運(yùn)動(dòng)學(xué)的研究存在正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)2類基本問題。正運(yùn)動(dòng)學(xué)，是指給定機(jī)器人各個(gè)關(guān)節(jié)的參數(shù)，計(jì)算機(jī)器人末端手爪的位置和姿態(tài)；逆運(yùn)動(dòng)學(xué)，是指已知機(jī)器人末端手爪的位姿，通過運(yùn)動(dòng)學(xué)方程求解機(jī)器人相應(yīng)的關(guān)節(jié)變量。通過對(duì)機(jī)器人運(yùn)動(dòng)學(xué)特性的研究，為機(jī)器人關(guān)節(jié)控制和軌跡規(guī)劃提供理論支撐。

1.2 正運(yùn)動(dòng)學(xué)模型

通過分析機(jī)器人各連桿及轉(zhuǎn)動(dòng)關(guān)節(jié)間的相互位置關(guān)系，利用D-H參數(shù)法建立機(jī)構(gòu)坐標(biāo)系，選擇機(jī)器人與大地相連的點(diǎn)O為基準(zhǔn)點(diǎn)，建立基準(zhǔn)坐標(biāo)系（如圖2所示）。

圖2 微創(chuàng)手術(shù)機(jī)器人的D-H參數(shù)模型

令T為位姿變換矩陣，n、o、a分別為x、y、z坐標(biāo)軸的單位方向矢量，即

其中，平移坐標(biāo)變換為

將機(jī)器人機(jī)器臂連桿參數(shù)帶入得

式中：ai為zi-1到zi沿xi方向的距離；αi為zi-1到zi繞xi旋轉(zhuǎn)的角度；di為xi-1到xi沿zi-1方向的距離；θi為xi-1到xi繞zi-1旋轉(zhuǎn)的角度。

機(jī)器人末端執(zhí)行器相對(duì)于基礎(chǔ)坐標(biāo)系的O0（x0，y0，z0）位姿的坐標(biāo)變換矩陣為

根據(jù)圖2，確定微創(chuàng)手術(shù)機(jī)器人的連桿參數(shù)（見表1）。

表1 微創(chuàng)手術(shù)機(jī)器人連桿參數(shù)

則機(jī)器人的坐標(biāo)系變換矩陣為

機(jī)器人運(yùn)動(dòng)學(xué)正解：

式中：si=sin θi；ci=cos θi；sij=sin（θi+θj）；cij=cos（θi+ θj）。

1.3 逆運(yùn)動(dòng)學(xué)模型

由于手術(shù)機(jī)器人具有3個(gè)被動(dòng)關(guān)節(jié)，其位姿信息可由傳感器直接測(cè)得。設(shè)手術(shù)切口點(diǎn)的坐標(biāo)系為q（qx，qy，qz），則機(jī)器人末端手術(shù)器械的空間矢量k為

k=（px-qx，py-qy，pz-qz）

在機(jī)器人基礎(chǔ)坐標(biāo)系中，末端手術(shù)器械與坐標(biāo)軸的方向角α、β、γ分別為

根據(jù)手術(shù)器械方位角與機(jī)器人關(guān)節(jié)轉(zhuǎn)角的位置關(guān)系，求得機(jī)器人姿態(tài)關(guān)節(jié)轉(zhuǎn)角為

根據(jù)機(jī)器人位姿模型反解機(jī)器人主動(dòng)關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3中的未知參數(shù)值分別為

式中

2 機(jī)器人運(yùn)動(dòng)控制系統(tǒng)建模仿真

機(jī)器人的控制系統(tǒng)是指機(jī)器人的運(yùn)動(dòng)輸出參數(shù)能夠按照控制輸入進(jìn)行調(diào)節(jié)的系統(tǒng)[7-8]。根據(jù)控制原理的不同，分為開環(huán)控制系統(tǒng)和閉環(huán)控制系統(tǒng)。在開環(huán)控制系統(tǒng)中，系統(tǒng)輸出受到輸入的控制，控制精度和抑制干擾的特性都比較差，往往在低精度運(yùn)動(dòng)條件下采用。閉環(huán)控制系統(tǒng)是建立在反饋原理的基礎(chǔ)之上，利用輸出量同期望值的偏差對(duì)系統(tǒng)進(jìn)行控制，因此能夠得到相對(duì)較好的控制性能[9-10]。

由于手術(shù)機(jī)器人對(duì)系統(tǒng)運(yùn)動(dòng)的精度要求較高，因此基本都采用閉環(huán)控制系統(tǒng)。本文采用Matlab中SimMechanics模塊對(duì)機(jī)器人的機(jī)電系統(tǒng)建模，根據(jù)手術(shù)機(jī)器人的機(jī)械結(jié)構(gòu)，建立SimMechanics模型。運(yùn)動(dòng)控制選用基于速度和位置的雙閉環(huán)控制方法實(shí)現(xiàn)對(duì)手術(shù)機(jī)器人運(yùn)動(dòng)的精確控制?？刂圃砣鐖D3、4所示。

上述控制系統(tǒng)的核心是數(shù)字控制器的選擇，考慮到所需控制器需對(duì)微創(chuàng)手術(shù)機(jī)器人的主動(dòng)關(guān)節(jié)進(jìn)行實(shí)時(shí)控制，為了保證控制參數(shù)調(diào)節(jié)簡(jiǎn)單，控制參數(shù)易于整定并且相互獨(dú)立，最終選用PID控制器。手術(shù)機(jī)器人各關(guān)節(jié)均采用電動(dòng)機(jī)驅(qū)動(dòng)，因此傳感器采用光電編碼器實(shí)現(xiàn)速度和位置的測(cè)量。

圖4 雙閉環(huán)控制系統(tǒng)

微創(chuàng)機(jī)器人的期望軌跡為執(zhí)行端手術(shù)器械實(shí)際需要的運(yùn)動(dòng)軌跡，選擇xy平面進(jìn)行末端軌跡仿真。將期望軌跡的各點(diǎn)位置通過運(yùn)動(dòng)學(xué)方程轉(zhuǎn)化為各個(gè)關(guān)節(jié)的運(yùn)動(dòng)變化輸出，通過與關(guān)節(jié)傳感器檢測(cè)到的數(shù)值反饋進(jìn)行比較，經(jīng)過PID控制器調(diào)節(jié)后輸送給電動(dòng)機(jī)控制模塊，從而驅(qū)動(dòng)各個(gè)關(guān)節(jié)到達(dá)運(yùn)動(dòng)位置。末端執(zhí)行機(jī)構(gòu)期望軌跡起點(diǎn)為（20，20）（單位：mm），終點(diǎn)為（60，30）（單位：mm），仿真結(jié)果如圖5所示。

圖5 末端執(zhí)行機(jī)構(gòu)運(yùn)動(dòng)軌跡

仿真結(jié)果表明，實(shí)際軌跡與期望軌跡基本重合?？梢?，手術(shù)機(jī)器人末端執(zhí)行機(jī)構(gòu)較為準(zhǔn)確地跟蹤了期望軌跡，在極限位置方向轉(zhuǎn)換時(shí)誤差稍大，但誤差最大不超過0.1 mm，在誤差允許范圍之內(nèi)，驗(yàn)證了運(yùn)動(dòng)模型的正確性。

3 結(jié)論

隨著微創(chuàng)外科的不斷發(fā)展，越來越多的手術(shù)機(jī)器人被應(yīng)用到外科手術(shù)中，本文提出一種基于D-H參數(shù)模型的機(jī)器人運(yùn)動(dòng)學(xué)研究方法，該方法能夠根據(jù)機(jī)器人各連桿及轉(zhuǎn)動(dòng)關(guān)節(jié)的位置關(guān)系得到機(jī)器人的運(yùn)動(dòng)學(xué)方程，進(jìn)而找到運(yùn)動(dòng)學(xué)規(guī)律。系統(tǒng)仿真實(shí)驗(yàn)結(jié)果表明，機(jī)器人末端執(zhí)行機(jī)構(gòu)的期望軌跡與實(shí)際軌跡基本一致，運(yùn)動(dòng)誤差都在實(shí)際允許范圍內(nèi)，驗(yàn)證了機(jī)器人運(yùn)動(dòng)學(xué)模型的正確性，從而證明了采用

（????）（????）D-H參數(shù)方法對(duì)微創(chuàng)手術(shù)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)仿真分析具有很好的實(shí)用性和準(zhǔn)確性，其對(duì)未來手術(shù)機(jī)器人的精密設(shè)計(jì)和系統(tǒng)改進(jìn)具有普遍的適用意義。

[1]劉少?gòu)?qiáng)，黃惟一，王愛民，等.用于微創(chuàng)外科手術(shù)的遙控機(jī)器人系統(tǒng)研究現(xiàn)狀及趨勢(shì)[J].機(jī)器人，2002，24（3）：283-288.

[2]Dario P，Guglielmelli E.Robotic for medical applications[J].IEEE Robotics and Automation Magazine，1996，9：44-56.

[3]王田苗，宗光華，張啟先．新應(yīng)用領(lǐng)域的機(jī)器人——醫(yī)療外科機(jī)器人[J].機(jī)器人技術(shù)與應(yīng)用，1997，18（2）：603-606.

[4]Daio P.Robotics in medicine[J].Internal Progress Report，ARTS Lab，1993，5：739-752.

[5]Chapman W H，Albrecht R J，Kim V B，et al.Computer-assisted laparoscopic splenectomy with the daVinci surgical robot[J].Journal of Laparoendoscopic& Advanced Surgical Techniques，2002，12（3）：155-159.

[6]Denavit J，Hartenberg R S.A kinematic notation for low-pair mechanisms based on matrices[J].Trans ASME J Appl Mech，1955，23：215-221.

[7]Loi-Wah Sun，Van Meer，Chung Kwong Yeung.Design and development of a DaVinci surgical system simulator[C]//Proceedings of the Mechatronics and Automation International Conference.New York：IEEE Press，2007：1 050-1 055.

[8]Taylor R H，Stoianovici D.Medical robotics in computer-integrated surgery[J].IEEE Transactions on Robotics and Automation，2003，19（5）：765-781.

[9]Dong-Soo Kwon，Ki Young Woo，Se Kyong Song，et al.Microsurgical telerobot system[C]//Proceedings of IEEE Int.Conf.on Intelligent Robots and Systems（IROS）.New York：IEEE Press，1998：945-950.

[10]Rovettta A，Sala R，XIA Wen，et al.Remote control in telerobotic surgery[J].IEEE Trans on SMC，1996，26：438-444.

（收稿：2014-02-28 修回：2014-05-14）

Research on kinematic simulation of minimally invasive surgical robot based on D-H methodology

HA Le,WANG Dong-hui
(Department of Medical Engineering,General Hospital of Shenyang Military Area Command,Shenyang 110016,China)

ObjectiveTo study the performance of minimally invasive surgical robot,and to propose a robot motion simulation method based on D-H methodology.MethodsBy analyzing the geometric relationship of each robot joints,DH model was built for the robot analysis of forward kinematics and inverse kinematics.At last,SimMechanics module was used for the robot electromechanical system modeling and simulation.ResultsThe desired trajectory of the actuator was consistent with the actual trajectory.With the actual motion errors within the acceptable range,the correctness of D-H model was well verified.ConclusionUsing D-H methodology for the robot kinematics study and simulation is useful and proper which has universal significance for the sophisticated design and system improvement of surgical robot.[Chinese Medical Equipment Journal，2014，35（11）：19-21，74]

surgical robot;D-H methodology;motion simulation;forward kinematics;inverse kinematics

R318.6；TP24；O311

A

1003-8868（2014）11-0019-04

10.7687/J.ISSN1003-8868.2014.11.019

哈 樂（1986—），男，博士，助理工程師，主要從事醫(yī)學(xué)工程和醫(yī)療機(jī)器人方面的研究工作，E-mail：halescut@126.com。

110016沈陽，沈陽軍區(qū)總醫(yī)院醫(yī)學(xué)工程科（哈 樂，王東輝）