基于旋量理論的主動(dòng)介入導(dǎo)管運(yùn)動(dòng)學(xué)研究

， ， ，，

(1.南京航空航天大學(xué)機(jī)電學(xué)院，江蘇 南京 210016；2.上海交通大學(xué)醫(yī)學(xué)院附屬新華醫(yī)院,上海 200092；2.沈陽自動(dòng)化研究所，遼寧 沈陽 110016)

基于旋量理論的主動(dòng)介入導(dǎo)管運(yùn)動(dòng)學(xué)研究

張健1，陳柏1，陳筍2，耿令波3，吳洪濤1

(1.南京航空航天大學(xué)機(jī)電學(xué)院，江蘇 南京 210016；2.上海交通大學(xué)醫(yī)學(xué)院附屬新華醫(yī)院,上海 200092；2.沈陽自動(dòng)化研究所，遼寧 沈陽 110016)

針對(duì)介入手術(shù)操作環(huán)境幾何特征各異的特點(diǎn)，為提高介入設(shè)備的通用性，提出一種由記憶合金絲SMA(shape memory alloy)與繩索混合驅(qū)動(dòng)的模塊化主動(dòng)介入導(dǎo)管機(jī)器人。采用旋量理論和指數(shù)積POE(product of exponentials formula)方法推導(dǎo)建立了3關(guān)節(jié)主動(dòng)介入導(dǎo)管運(yùn)動(dòng)數(shù)學(xué)模型，對(duì)介入導(dǎo)管機(jī)器人的工作空間及運(yùn)動(dòng)仿真分析，結(jié)果驗(yàn)證了所建立運(yùn)動(dòng)學(xué)模型的正確性。所采用方法大大降低了多節(jié)主動(dòng)介入導(dǎo)管運(yùn)動(dòng)學(xué)模型的復(fù)雜性，為介入導(dǎo)管機(jī)器人的實(shí)際應(yīng)用奠定了理論基礎(chǔ)。

主動(dòng)介入導(dǎo)管；指數(shù)積方法；運(yùn)動(dòng)學(xué)；運(yùn)動(dòng)仿真；工作空間

0 引言

近年來，微創(chuàng)介入手術(shù)MIS(minimally invasive surgery)發(fā)展迅速。微創(chuàng)介入手術(shù)是指在醫(yī)學(xué)影像的引導(dǎo)下，將導(dǎo)管導(dǎo)絲從病人穿刺部位送至靶血管，進(jìn)行相應(yīng)的手術(shù)治療[1]。在介入手術(shù)過程中，操作環(huán)境的幾何特征各異，導(dǎo)管式介入裝置通常需要因時(shí)因地定制為不同的尺寸、形狀，通用性欠佳。為此，一種能隨環(huán)境管腔主動(dòng)變形的新型機(jī)器人化的介入導(dǎo)管裝置的研發(fā)顯得重要。Komatsubaral M等人提出了由SMA彈簧驅(qū)動(dòng)的一種新型主動(dòng)微導(dǎo)管，直徑為3.5 mm，總長度為60mm。導(dǎo)管通過控制SMA彈簧,可以實(shí)現(xiàn)導(dǎo)管尖端至少8個(gè)方向的運(yùn)動(dòng)[2-3]。Jayender等人研制SMA驅(qū)動(dòng)導(dǎo)管直徑約1.5 mm，采用了3條并行SMA導(dǎo)線結(jié)構(gòu)，并針對(duì)SMA主動(dòng)導(dǎo)管的控制進(jìn)行了大量的研究[4-6]。此外，近年來迅速發(fā)展的繩索驅(qū)動(dòng)技術(shù)， 基于繩索驅(qū)動(dòng)的優(yōu)勢(shì)，William W Cimino博士研制出了用于心臟治療的主動(dòng)導(dǎo)管頭端[7]。

目前，絕大多數(shù)主動(dòng)彎曲導(dǎo)管的數(shù)學(xué)模型是基于對(duì)導(dǎo)管彎曲軸線[8-9]的分析，建立彎曲及扭轉(zhuǎn)角度與圓弧曲率的函數(shù)關(guān)系。隨著介入導(dǎo)管機(jī)器人節(jié)數(shù)的增加，這種方法建立的數(shù)學(xué)模型將會(huì)變得十分復(fù)雜，其運(yùn)動(dòng)學(xué)的求解也將十分困難。

提出了一種基于記憶合金絲與繩索混合驅(qū)動(dòng)技術(shù)的新型介入設(shè)備。采用旋量和指數(shù)積的方法建立了多節(jié)導(dǎo)管的運(yùn)動(dòng)學(xué)模型，并通過仿真分析驗(yàn)證了其準(zhǔn)確性，在一定程度上完善介入導(dǎo)管機(jī)器人的設(shè)計(jì)理論。

1 混合驅(qū)動(dòng)介入導(dǎo)管系統(tǒng)

系統(tǒng)的核心部分是模塊化的介入導(dǎo)管，由骨架彈簧、連接件及SMA驅(qū)動(dòng)器、繩索組成。SMA驅(qū)動(dòng)器間隔120°分布在骨架彈簧的周圍，每根SMA驅(qū)動(dòng)器都連有導(dǎo)線，用于供電。同樣，繩索驅(qū)動(dòng)關(guān)節(jié)是由4根驅(qū)動(dòng)繩索均勻布置在骨架彈簧的周圍。介入主動(dòng)導(dǎo)管具體結(jié)構(gòu)如圖1所示。

圖1 介入導(dǎo)管結(jié)構(gòu)

導(dǎo)管機(jī)器人具有模塊化、可擴(kuò)展和可重構(gòu)的特點(diǎn)。隨著導(dǎo)管自由度的擴(kuò)展，使用傳統(tǒng)方法建立的數(shù)學(xué)模型將變得十分復(fù)雜，其運(yùn)動(dòng)學(xué)的求解也將變得十分困難。

基于該介入導(dǎo)管機(jī)器人的運(yùn)動(dòng)特點(diǎn)，提出使用旋量和指數(shù)積的方法對(duì)介入導(dǎo)管機(jī)器人運(yùn)動(dòng)學(xué)進(jìn)行描述。

2 基于旋量理論的導(dǎo)管運(yùn)動(dòng)學(xué)建模

2.1 正向運(yùn)動(dòng)學(xué)

利用旋量理論求解機(jī)器人運(yùn)動(dòng)學(xué)無需建立各個(gè)連桿坐標(biāo)系，為方便描述機(jī)器人各關(guān)節(jié)單元的運(yùn)動(dòng)形態(tài)，將各單位旋量建立在每節(jié)導(dǎo)管的中點(diǎn)位置，θi、βi(i=1,2,3)分別表示各關(guān)節(jié)的轉(zhuǎn)動(dòng)和彎曲角度。圖2為建立的導(dǎo)管機(jī)器人的坐標(biāo)系。

圖2 3節(jié)介入導(dǎo)管機(jī)器人坐標(biāo)系

為了準(zhǔn)確描述多節(jié)介入導(dǎo)管機(jī)器人的運(yùn)動(dòng)形態(tài)，將每節(jié)導(dǎo)管的彎曲運(yùn)動(dòng)分解成3個(gè)轉(zhuǎn)動(dòng)，首先繞Z1轉(zhuǎn)動(dòng)β1，然后繞Z2轉(zhuǎn)動(dòng)θ1，最后繞Z3轉(zhuǎn)動(dòng)-β1。即單關(guān)節(jié)的正向運(yùn)動(dòng)學(xué)指數(shù)積方程為：

(1)

gST(θ,β)表示機(jī)器人的末端位姿；gST(0)表示機(jī)器人位于初始位姿時(shí)慣性坐標(biāo)系與工具坐標(biāo)系間的坐標(biāo)變換。所有的關(guān)節(jié)運(yùn)動(dòng)旋量都按基座坐標(biāo)系{S}描述。

(2)

依據(jù)圖2所示的坐標(biāo)系，可以得到各關(guān)節(jié)的旋量坐標(biāo)。為簡(jiǎn)便表示多節(jié)導(dǎo)管的運(yùn)動(dòng)學(xué)方程，這里將單個(gè)關(guān)節(jié)介入導(dǎo)管的運(yùn)動(dòng)學(xué)記為：

(3)

c=cos;s=sin

具有3節(jié)導(dǎo)管的介入導(dǎo)管導(dǎo)向機(jī)器人正向運(yùn)動(dòng)學(xué)方程可以表示為：

gST(θ,β)=T1·T2·T3·gST(0)

(4)

將式(4)展開可得：

(5)

若給定工具坐標(biāo)系的初始位姿以及各個(gè)關(guān)節(jié)的轉(zhuǎn)角，利用正向運(yùn)動(dòng)學(xué)方程式(5)即可直接求得介入導(dǎo)管機(jī)器人的運(yùn)動(dòng)學(xué)正解，獲得末端執(zhí)行器的位姿。

2.2 反向運(yùn)動(dòng)學(xué)

對(duì)機(jī)器人控制及其運(yùn)動(dòng)學(xué)軌跡規(guī)劃，獲得運(yùn)動(dòng)學(xué)的逆解至關(guān)重要。介入導(dǎo)管機(jī)器人的反向運(yùn)動(dòng)學(xué)是指在給定末端執(zhí)行器具體位姿時(shí)，求出與該位姿相對(duì)應(yīng)的各關(guān)節(jié)的轉(zhuǎn)角，計(jì)算出驅(qū)動(dòng)各個(gè)關(guān)節(jié)轉(zhuǎn)過相應(yīng)角度時(shí)SMA驅(qū)動(dòng)器及繩索長度的變化量。

為了求解導(dǎo)管機(jī)器人反向運(yùn)動(dòng)學(xué)方程，需要獲得指數(shù)積方程。由于該導(dǎo)管機(jī)器人只有繞某個(gè)運(yùn)動(dòng)旋量坐標(biāo)的純轉(zhuǎn)動(dòng)。因此，可以利用該特征消去耦合的關(guān)節(jié)變量，簡(jiǎn)化求解過程，簡(jiǎn)化依據(jù)如下：

由正向運(yùn)動(dòng)學(xué)方程式(5)可以得到導(dǎo)管機(jī)器人的POE公式為:

(6)

若給定介入導(dǎo)管末端工具的位姿矩陣，根據(jù)所建立3關(guān)節(jié)介入導(dǎo)管機(jī)器人的坐標(biāo)系如圖2所示，各關(guān)節(jié)變量求解步驟如下所述。

第1步，求θ2。針對(duì)點(diǎn)p1(0,0,l1+l2+l3/2,1)，依據(jù)式(7)，應(yīng)用位置保持不變?cè)瓌t，將后面2個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)變量從指數(shù)積公式中消掉，剩余指數(shù)積子鏈中僅含有4個(gè)未知變量。因此，式(6)可以化簡(jiǎn)為式(8)。

(7)

(8)

再針對(duì)p0(0,0,0,1)點(diǎn)，應(yīng)用距離保持不變?cè)瓌t，消去指數(shù)積公式中與轉(zhuǎn)動(dòng)副相對(duì)應(yīng)的角度變量，即式(8)進(jìn)一步化簡(jiǎn)為：

g(θ,β)·p1-p0

(9)

由此可以得：

(10)

式(10)中只有未知角度變量θ2，所以可以用各個(gè)關(guān)節(jié)的長度l1,l2,l3表示出關(guān)節(jié)變量θ2。

第2步,求β3。同樣應(yīng)用位置保持不變?cè)瓌t，利用點(diǎn)p2(1,0,l1/2+l2+l3,1)，p0(0,0,0,1)，化簡(jiǎn)式(6)可以得:：

(11)

第3步,求θ3，同樣利用點(diǎn)p3(0,0,l1+l2+l3,1)，p0(0,0,0,1)，應(yīng)用位置不變與距離不變?cè)瓌t，可將方程(6)化簡(jiǎn)為:

(12)

(13)

將式(13)左邊記為q2，則式(13)變?yōu)椋?/p>

(14)

式(14)表示點(diǎn)p4先繞Z2轉(zhuǎn)動(dòng)θ1到點(diǎn)q1，然后繞Z1轉(zhuǎn)動(dòng)β1，最后到達(dá)q2點(diǎn)，如圖3所示。

圖3 點(diǎn)p4與q2幾何關(guān)系

根據(jù)圖3所示幾何關(guān)系，利用向量q和向量r的數(shù)量積公式可以將θ1表示出來，即

再將式(14)進(jìn)一步簡(jiǎn)化得：

(15)

第5步,求β2。前4步中各個(gè)角度變量的表達(dá)式已獲得。此時(shí)，對(duì)機(jī)器人的正向運(yùn)動(dòng)學(xué)方程式(6)右邊進(jìn)行化簡(jiǎn)即得:

(16)

對(duì)上述指數(shù)積方程，利用遺傳算法可求得各個(gè)關(guān)節(jié)變量的數(shù)值解。

3 導(dǎo)管的工作空間分析與運(yùn)動(dòng)仿真

3.1 導(dǎo)管的工作空間

根據(jù)主動(dòng)介入導(dǎo)管機(jī)器人的結(jié)構(gòu)與工作原理，相鄰兩個(gè)關(guān)節(jié)之間都可以通過相應(yīng)的坐標(biāo)系變換來建立聯(lián)系，設(shè)定單個(gè)彎曲單元骨架彈簧的長度為l=30mm，可以對(duì)導(dǎo)管機(jī)器人的主動(dòng)彎曲頭端的工作空間進(jìn)行分析。

圖4是使用數(shù)學(xué)軟件Mathematica獲得的3個(gè)區(qū)域，從下向上依次為第1、2、3關(guān)節(jié)末端的工作空間。由此可知：該介入導(dǎo)管機(jī)器人了主動(dòng)彎曲頭端工作空間較大，有利于利于給介入導(dǎo)管機(jī)器人的運(yùn)動(dòng)過程規(guī)劃出一條相對(duì)理想的無碰撞路徑，實(shí)現(xiàn)靈活的主動(dòng)導(dǎo)向及避障功能。

圖4 導(dǎo)管關(guān)節(jié)末端工作空間

3.2 主動(dòng)導(dǎo)管運(yùn)動(dòng)仿真

基于上述介入導(dǎo)管機(jī)器人的運(yùn)動(dòng)學(xué)模型，為驗(yàn)證數(shù)學(xué)模型的準(zhǔn)確性，以Mathematica為平臺(tái)編程，對(duì)主動(dòng)導(dǎo)管運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃。設(shè)定軌跡規(guī)劃任務(wù)：導(dǎo)管導(dǎo)向機(jī)器人末端關(guān)節(jié)角θ6在0～π/6變化，其他關(guān)節(jié)角變量保持初始值，通過正向運(yùn)動(dòng)學(xué)方程式(8)，可以得到介入導(dǎo)管機(jī)器人在初始平面的彎曲運(yùn)動(dòng)軌跡，如圖5點(diǎn)劃線所示。然后，在獲得的運(yùn)動(dòng)軌跡中離散出15個(gè)位置點(diǎn)，使用上述反向運(yùn)動(dòng)學(xué)求解方法求出對(duì)應(yīng)離散點(diǎn)的運(yùn)動(dòng)學(xué)逆解，將得到結(jié)果再次代入運(yùn)動(dòng)學(xué)方程，就可以再次得到對(duì)應(yīng)離散點(diǎn)的位置，如圖5中離散點(diǎn)所示。

根據(jù)圖5所示，離散點(diǎn)與實(shí)線基本吻合，表明上述通過旋量和指數(shù)積法建立的主動(dòng)介入導(dǎo)管機(jī)器人的運(yùn)動(dòng)學(xué)模型是正確的。

圖5 機(jī)器人末端運(yùn)動(dòng)軌跡

4 結(jié)束語

相比傳統(tǒng)的機(jī)器人D-H建模方法，使用基于旋量理論的指數(shù)積建模方法更加適合多關(guān)節(jié)介入主動(dòng)導(dǎo)管的運(yùn)動(dòng)學(xué)建模，可以比較容易地獲得關(guān)于關(guān)節(jié)變量的方程。通過得到的運(yùn)動(dòng)學(xué)逆解對(duì)介入導(dǎo)管的運(yùn)動(dòng)進(jìn)行了規(guī)劃仿真，發(fā)現(xiàn)這種建模方法利于機(jī)器人運(yùn)動(dòng)學(xué)的求解，為后續(xù)多關(guān)節(jié)介入導(dǎo)管機(jī)器人的設(shè)計(jì)與控制提供了有效的理論依據(jù)。

[1] Gomes P.Surgical robotics:Reviewing the past, analysing the present,imagining the future[J].Robotics and Computer-Integrated Manufacturing,2011,27(2):261-266.

[2] Komatsubara M,Namazu T,Nagasawa H,et al.Development of the forward-looking active micro-catheter actuated by Ti-Ni shape memory alloy springs[A].IEEE 22nd International Conference on Micro Electro Mechanical Systems[C].2009.1055-1058.

[3] Namazu T,Tashiro Y,Inoue S.Ti-Ni shape memory alloy film-actuated microstructures for a MEMS probe card[J].Journal of Micromechanics and Microengineering,2007,17(1):154-155.

[4] Jayender J,Patel R V.Master-slave control of an active catheter instrumented with shape memory alloy actuators[A].IEEE International Conference on Intelligent Robots and Systems[C].2007.759-764.

[5] Jayender J,Azizian M,Patel R V.Autonomous image-guided robot-assisted active catheter insertion[J].IEEE Transactions on Robotics,2008,24(4):858-871.

[6] Jayender J,Patel R V,Nikumb S.Robot-assisted ac-tive catheter insertion: algorithms and experiments[J].The International Journal of Robotics Research,2009,28(9):1101-1117.

[7] Camino W W.Active tip control for small diameter catheters:Power transmission and transduction requirements[D].Salt Lake City:The University of Utah,1992.

[8] Webster R J,Jones B A.Design and kinematic modeling of constant curvature continuum robots:A review[J].The International Journal of Robotics Research,2010,29(13):1661-1683.

[9] Sears P,Dupont P.A steerable needle technology using curved concentric tubes[A].IEEE/RSJ International Conference on Intelligent Robots and Systems[C].2006.2850-2856.

Kinematics Modeling of Intervention Active Catheter Based on Screw Theory

ZHANGJian1，CHENBai1，CHENSun2，GENGLing-bo3,WUHong-tao1

(1.Mechanical and Electrical Engineering,Nanjing University of Aeronautics and Astronautics,Nanjing 210016,China;；2.Xinhua Hospital Affiliated to Shanghai Jiaotong University School of Medicine,Shanghai 200092,China;3.Shenyang Institute of Automation,Shenyang 110016,China)

Considering complex and changeable characteristics of the operating environment for the surgical intervention,the paper proposes a SMA(shape memory alloy) and the cable hybrid-driven modular catheter to improve the versatility of interventional devices.Screw and POE (product of exponentials formula) method were used to establish the kinematics model of a three-joint interventional active catheter.According to the mathematical model,the workspace and motion simulation of the active catheter were analyzed by the use of Mathematica.The simulation results verified the effectiveness and correctness of the kinematics model，and the approach greatly reduced the complexity of kinematics model for multi-section interventional catheter,which makes a theoretical foundation for the practical application of interventional catheter robot.

interventional active catheter;product of exponentials formula;kinematics;motion simulation;workspace

2013-12-11

國家“八六三”科技計(jì)劃項(xiàng)目(2013AA041004)；國家自然科學(xué)基金資助項(xiàng)目(51075209);江蘇省自然科學(xué)基金資助項(xiàng)目(BK2012798);江蘇省產(chǎn)學(xué)研聯(lián)合創(chuàng)新資金----前瞻性聯(lián)合研究資助項(xiàng)目(BY2012011);南京市科委產(chǎn)學(xué)研計(jì)劃資助項(xiàng)目(201204014);上海市科委科技計(jì)劃資助項(xiàng)目(124119a3900);上海交通大學(xué)醫(yī)工交叉資助項(xiàng)目(YG2011MS08)

TP242.3

A

1001-2257(2014)05-0008-04

張健(1988-)，男，山東寧陽人,碩士研究生，研究方向?yàn)獒t(yī)療機(jī)器人技術(shù);陳柏(1978-)，男，江蘇泰州人,教授,博士研究生導(dǎo)師，研究方向?yàn)榉律鷻C(jī)器人、醫(yī)療器械。