基于Multibody的穿刺手術(shù)機(jī)器人設(shè)計(jì)與仿真

鄒宇鵬， 張際平， 鄭杰仁， 張宗源， 顧學(xué)斌， 李萬哲

（中國石油大學(xué)（華東）機(jī)電工程學(xué)院，山東青島 266580）

0 引 言

惡性腫瘤是對人類生命具有嚴(yán)重危害性的重大疾病，來自國家癌癥中心最新的數(shù)據(jù)顯示，惡性腫瘤已成為我國居民第二大死亡原因［1-2］。患者及早進(jìn)行穿刺診斷可有效降低癌癥死亡率。在醫(yī)療實(shí)踐中，核磁共振成像（Magnetic Resonance Imaging，MRI）技術(shù)引導(dǎo)的微創(chuàng)穿刺手術(shù)是診斷和治療腫瘤的重要手段，與常規(guī)手術(shù)相比，微創(chuàng)穿刺手術(shù)具有手術(shù)創(chuàng)傷小、患者恢復(fù)快和相關(guān)并發(fā)癥少等優(yōu)點(diǎn)，但仍面臨著多次掃描時(shí)間長，穿刺精度低等問題［3］。

針對微創(chuàng)穿刺手術(shù)核磁兼容性和穿刺定位精度要求，本文設(shè)計(jì)了一款2R3T 并聯(lián)柔索驅(qū)動核磁兼容穿刺定位手術(shù)機(jī)器人，采用柔索牽引代替剛性驅(qū)動，具有靈活、高精度和重量輕等特點(diǎn)［4］。每根柔索都由電動機(jī)驅(qū)動，以實(shí)現(xiàn)穿刺針的運(yùn)動和姿態(tài)調(diào)整。并聯(lián)結(jié)構(gòu)能使機(jī)器人的運(yùn)動更加平滑，同時(shí)減少機(jī)器人的慣性負(fù)載，提高機(jī)器人的響應(yīng)速度和穩(wěn)定性［5-7］。

對穿刺手術(shù)機(jī)器人進(jìn)行總體結(jié)構(gòu)設(shè)計(jì)，并對其進(jìn)行正、逆運(yùn)動學(xué)分析，同時(shí)基于Multibody 建立機(jī)器人運(yùn)動學(xué)模型，對運(yùn)動學(xué)模型進(jìn)行仿真分析，將仿真結(jié)果與理論結(jié)果進(jìn)行比較，對比驗(yàn)證Multibody建模的可行性和分析設(shè)計(jì)的合理性。Multibody 模塊是Matlab／Simulink一個(gè)可視化多體動力學(xué)建模工具箱［8］。使用Multibody模塊可方便構(gòu)建機(jī)械系統(tǒng)的運(yùn)動學(xué)和動力學(xué)模型，進(jìn)行運(yùn)動仿真和控制分析［9-10］。基于Multibody系統(tǒng)建模分析方法使復(fù)雜的運(yùn)動學(xué)和動力學(xué)數(shù)據(jù)可視化，可驗(yàn)證設(shè)計(jì)的可行性與準(zhǔn)確性，對于機(jī)器人運(yùn)動學(xué)和動力學(xué)仿真分析實(shí)驗(yàn)教學(xué)具有重要意義［11-12］。

1 穿刺手術(shù)機(jī)器人結(jié)構(gòu)設(shè)計(jì)

2R3T并聯(lián)柔索驅(qū)動核磁兼容穿刺定位手術(shù)機(jī)器人總體結(jié)構(gòu)如圖1 所示，包括柔索、驅(qū)動模塊、傳動模塊、導(dǎo)向模塊、機(jī)架和定位模塊。柔索從驅(qū)動模塊輸出，經(jīng)傳動模塊、導(dǎo)向模塊輸入穿刺定位模塊，通過改變?nèi)崴鏖L度牽引末端穿刺針進(jìn)行穿刺運(yùn)動。

圖1 穿刺機(jī)器人整體結(jié)構(gòu)示意圖

整體尺寸約0.64 m×0.64 m×0.45 m，符合磁共振空間要求，采用驅(qū)動模塊外置，解決了鐵磁性驅(qū)動材料對核磁空間的干擾問題，符合磁共振材料兼容要求，穿刺機(jī)架在核磁床上可移動，能根據(jù)病患位置調(diào)節(jié)手術(shù)機(jī)器人的位置，盡可能地增大穿刺針工作空間，通過調(diào)整磁床末端的驅(qū)動模塊輸出長度變化的6 根柔索，實(shí)現(xiàn)穿刺針的5 自由度運(yùn)動，以此來實(shí)現(xiàn)穿刺手術(shù)運(yùn)動。

2 六柔索機(jī)構(gòu)工作空間對比

穿刺手術(shù)機(jī)器人作為一個(gè)6 柔索5 自由度的完全約束機(jī)構(gòu)，該機(jī)構(gòu)的柔索布局大體可分為A 型6 柔索機(jī)構(gòu)、B型6 柔索機(jī)構(gòu)、C 型6 柔索機(jī)構(gòu)3 種類型。A型6 柔索機(jī)構(gòu)的6 個(gè)出繩點(diǎn)對稱分布在機(jī)構(gòu)的上、下兩端；B型機(jī)構(gòu)的6 個(gè)出繩點(diǎn)分布在機(jī)構(gòu)中間水平面上，高低出繩點(diǎn)兩兩重合；C型機(jī)構(gòu)的6 個(gè)出繩點(diǎn)以正6 邊形位置均勻分布在機(jī)構(gòu)中間水平面上，如圖2所示。

圖2 3種6柔索完全約束機(jī)構(gòu)

根據(jù)穿刺手術(shù)要求，設(shè)計(jì)3 種6 柔索并聯(lián)機(jī)構(gòu)空間框架為圓柱形框架，得到A、B和C型6 柔索機(jī)構(gòu)姿態(tài)角為0°時(shí)的工作空間，如圖3 所示，具體的選型對比見表1。

表1 3 種柔索機(jī)構(gòu)工作空間對比

圖3 3種6柔索機(jī)構(gòu)工作空間示意圖

3 穿刺手術(shù)機(jī)器人運(yùn)動學(xué)分析

6 柔索并聯(lián)穿刺手術(shù)機(jī)器人簡化模型如圖4 所示。圖中：M1～M6為柔索驅(qū)動點(diǎn)；P1～P6為穿刺針動平臺牽引點(diǎn)。初始狀態(tài)下穿刺針中心位置為全局坐標(biāo)系基礎(chǔ)原點(diǎn)，局部坐標(biāo)系坐標(biāo)原點(diǎn)在穿刺針的中心位置上，在初始狀態(tài)下，局部坐標(biāo)系P-XYZ和全局坐標(biāo)系O-xyz重合，動平臺的位姿為P（xp，yp，zp，θx，θy，θz），其中（xp，yp，zp）為P點(diǎn)在全局坐標(biāo)系中的坐標(biāo)，θx、θy、θz分別為穿刺針繞全局坐標(biāo)系x軸、y軸、z軸的旋轉(zhuǎn)角，以右手螺旋為正。

圖4 6柔索并聯(lián)機(jī)器人模型

穿刺針牽引點(diǎn)Pi至驅(qū)動點(diǎn)Mi的向量即為柔索長度向量，其中向量的模為柔索的長度即。則

其中每條柔索長度

式中：（xMi，yMi，zMi）、（xPi，yPi，zPi）分別為驅(qū)動點(diǎn)Mi和牽引點(diǎn)Pi在全局坐標(biāo)系O-xyz中的坐標(biāo)；ui為第i根柔索方向的單位矢量；ri為穿刺針中心到繩索連接點(diǎn)的矢量。

3.1 穿刺手術(shù)機(jī)器人逆向運(yùn)動學(xué)分析

已知?jiǎng)悠脚_的位姿P（xp，yp，zp，θx，θy，θz），則穿刺針牽引點(diǎn)Pi（xPi，yPi，zPi）在全局坐標(biāo)系O-xyz中的坐標(biāo)滿足：

式中：（XPi，YPi，ZPi）為Pi點(diǎn)在局部坐標(biāo)系P-XYZ中的坐標(biāo)；R為方向余弦向量。

把富集培養(yǎng)液進(jìn)行梯度稀釋，并涂布于無機(jī)鹽培養(yǎng)基平板，挑取10株單菌落劃線純化，并接入無機(jī)鹽液體培養(yǎng)基中，30℃靜置培養(yǎng)15 d，0.2 μm濾膜過濾，取濾液進(jìn)行氣相色譜檢測[6]。

根據(jù)前文已知6 柔索并聯(lián)結(jié)構(gòu)不存在繞軸線的轉(zhuǎn)動，即θz為0，此時(shí)方向余弦向量為

聯(lián)立式（2）、（5）可求得各柔索長度。

3.2 穿刺手術(shù)機(jī)器人正向運(yùn)動學(xué)分析

利用柔索長度的變化來控制穿刺針的位置和姿態(tài)為穿刺手術(shù)機(jī)器人的正向運(yùn)動學(xué)分析，由于控制穿刺針的6 條柔索長度在求解過程中不存在求根公式，為方便快速求解，此處運(yùn)動學(xué)求解利用牛頓迭代法計(jì)算得到柔索長度的近似解。牛頓迭代法是利用假象解帶入，層層迭代，在有限次求解后得到一個(gè)理想解，該理想解在一定的誤差允許范圍內(nèi)即可。具體的求解方式如下：

給定穿刺針的初始位姿點(diǎn)P0和該位姿點(diǎn)下的各柔索長度L0，與運(yùn)動學(xué)逆解得到的初始位姿點(diǎn)P0的各條柔索長度Lc，將給定長度L0與逆解得到的Lc比較，二者差值如果在誤差允許范圍內(nèi)，則位姿點(diǎn)滿足條件，否則更換位姿點(diǎn)后重新求解進(jìn)行比較。

將給定柔索長度L0和求解得到的柔索長度Lc比較函數(shù)定義為

則6 柔索機(jī)構(gòu)

系統(tǒng)結(jié)構(gòu)矩陣

當(dāng)前誤差可計(jì)算求得：

式中：δ 為待求解的系數(shù)；e為系統(tǒng)允許的誤差范圍。滿足不等式（13）的點(diǎn)就是穿刺針當(dāng)前的位姿點(diǎn)，如果當(dāng)前的目標(biāo)點(diǎn)不滿足條件，則取下一個(gè)數(shù)據(jù)Lc＋1，直至有滿足在允許范圍誤差的點(diǎn)出現(xiàn)。

4 穿刺手術(shù)機(jī)器人Multibody建模

穿刺手術(shù)機(jī)器人Multibody 運(yùn)動學(xué)建模由基礎(chǔ)環(huán)境配置、寄存器、軌跡輸入、運(yùn)動學(xué)逆解和穿刺針和6柔索驅(qū)動等模塊組成［13-14］，如圖5 所示：

穿刺手術(shù)機(jī)器人運(yùn)動學(xué)建模過程如下：

（1）配置模型的基礎(chǔ)環(huán)境。對求解器，物理參數(shù)配置器和世界坐標(biāo)系進(jìn)行配置，根據(jù)仿真建模要求依次設(shè)置求解器類型和構(gòu)件的物理參數(shù)。

（2）建立穿刺針和6 柔索驅(qū)動模塊。構(gòu)建穿刺針模塊，上下連繩點(diǎn)間的距離為0.3 m，6 個(gè)柔索驅(qū)動模塊輸出的柔索依次交錯(cuò)連接在穿刺針的上下2 個(gè)端點(diǎn)。每個(gè)柔索驅(qū)動模塊如圖6 所示，柔索驅(qū)動模塊由柔索單元、出繩輪和轉(zhuǎn)動輪組成，其中轉(zhuǎn)動輪模塊為模型中電動機(jī)輸出軸，主要由坐標(biāo)轉(zhuǎn)換單元（Rigid Transform）、運(yùn)動實(shí)體單元（Solid）和轉(zhuǎn)動副（Revolution Joint）組成，在柔索單元中，柔索的屬性由Belt-Cable Spool設(shè)置，柔索經(jīng)Pulley 模塊A端輸入B 段輸出，輸出的柔索經(jīng)過Belt-Cable End與穿刺針模塊相連。

圖6 柔索驅(qū)動模塊Multibody建模

（3）設(shè)置寄存器模塊和軌跡輸入模塊。建立寄存器模塊，存儲軌跡輸入模塊數(shù)據(jù)和運(yùn)動學(xué)逆解得到的各柔索長度變量，建立軌跡輸入模塊，將穿刺針的運(yùn)動軌跡設(shè)置為圓軌跡。

（4）將輸入軌跡模塊的穿刺針位移量輸入運(yùn)動學(xué)逆解模塊。求解得到各柔索長度變量，將各柔索長度變量轉(zhuǎn)換成電動機(jī)輸出軸角位移變量輸入轉(zhuǎn)動輪模塊Revolute Joint，使轉(zhuǎn)動輪帶動柔索運(yùn)動控制穿刺針運(yùn)動［15-16］。

5 運(yùn)動學(xué)仿真結(jié)果分析

對運(yùn)動學(xué)仿真模型進(jìn)行編譯，得到可視化界面如圖7 所示。獲取驅(qū)動輪在運(yùn)動過程中的參數(shù)，將參數(shù)進(jìn)行運(yùn)動學(xué)求解得到柔索的長度變量。

圖7 可視化界面

對穿刺手術(shù)機(jī)器人運(yùn)動學(xué)模型進(jìn)行仿真，每隔2 s對仿真模型在xOy平面運(yùn)動并記錄仿真模型運(yùn)動軌跡如圖8 所示。經(jīng)驅(qū)動輪采集到的仿真柔索長度變量Lsi（i＝1，2，…，6）如圖9 所示。運(yùn)動學(xué)逆解得到的理論柔索長度變量L0i（i＝1，2，…，6），如圖10 所示。

圖8 仿真模型運(yùn)動軌跡

圖9 驅(qū)動輪采集的仿真柔索長度變量

圖10 運(yùn)動學(xué)逆解得到的理論柔索長度變量

將Lsi與L0i之間的差值定義為柔索長度誤差Lei（i＝1，2，…，6），如圖11 所示。

圖11 柔索長度誤差

仿真模型得到的柔索長度變量與運(yùn)動學(xué)理論計(jì)算得到的柔索長度變量之間差距小于0.3 mm，誤差較小，分析誤差產(chǎn)生的原因可能是出繩輪在仿真中仍需要用實(shí)體代替，雖然實(shí)體半徑較小，但不可簡單將其按出繩點(diǎn)分析，出繩位置的不準(zhǔn)確性會對運(yùn)動學(xué)建模產(chǎn)生一定影響；穿刺針在運(yùn)動過程中，纏繞在出繩輪上柔索的切點(diǎn)位置也會發(fā)生變化。

將Lsi利用運(yùn)動學(xué)正解求解得出穿刺針的運(yùn)動軌跡，如圖12 所示。

圖12 穿刺針運(yùn)動軌跡

穿刺針在xOy平面上的投影，以（－100，0）為圓心、100 mm為半徑的圓形軌跡，這表明Multibody運(yùn)動學(xué)建模的準(zhǔn)確性，同時(shí)證明了前文運(yùn)動學(xué)分析的正確性和設(shè)計(jì)的合理性。

6 結(jié) 語

本文以2R3T并聯(lián)柔索驅(qū)動核磁兼容穿刺定位手術(shù)機(jī)器人為研究對象，對機(jī)器人進(jìn)行結(jié)構(gòu)設(shè)計(jì)和運(yùn)動學(xué)分析，通過Multibody 構(gòu)建了機(jī)器人運(yùn)動學(xué)模型，基于軌跡規(guī)劃完成仿真分析。仿真結(jié)果表明，該穿刺機(jī)器人運(yùn)動學(xué)模型準(zhǔn)確，設(shè)計(jì)合理，能縮短手術(shù)時(shí)間，提高手術(shù)安全。同時(shí)，基于Multibody的仿真過程對于機(jī)器人學(xué)，機(jī)器人技術(shù)等相關(guān)課程實(shí)驗(yàn)教學(xué)具有重要意義。