經(jīng)顱磁六軸機(jī)械臂運(yùn)動(dòng)學(xué)分析與路徑規(guī)劃

嵇長(zhǎng)委，尚會(huì)超，韓鑫磊，付曉莉，李志強(qiáng)

(1.中原工學(xué)院機(jī)械工程學(xué)院，河南鄭州 450007；2.河南翔宇醫(yī)療設(shè)備股份有限公司，河南安陽(yáng) 455000)

0 前言

據(jù)古文獻(xiàn)記載，在中國(guó)很早就有將磁石應(yīng)用于治療頭痛、關(guān)節(jié)痛等疾病的例子。近年來(lái)，國(guó)內(nèi)外學(xué)者對(duì)磁場(chǎng)的生物醫(yī)學(xué)效應(yīng)進(jìn)行了深入研究。大量數(shù)據(jù)表明，磁場(chǎng)可以通過(guò)經(jīng)絡(luò)和穴位的作用改善微循環(huán)，促進(jìn)細(xì)胞代謝，加速細(xì)胞內(nèi)廢物和有害物質(zhì)的排泄，平衡內(nèi)分泌失調(diào)，從而實(shí)現(xiàn)對(duì)某些疾病的輔助治療[1]。

經(jīng)顱磁刺激(Transcranial Magnetic Stimulation，TMS)是一種電磁療法，作為一種無(wú)創(chuàng)、非侵入、無(wú)輻射的物理靶向治療，已成為治療精神和神經(jīng)系統(tǒng)疾病等多種臨床難治性疾病的有效手段。例如，腦卒中是近年來(lái)死亡率和致殘率非常高的老年人十大疾病之一，嚴(yán)重危及老年人的生命和健康安全，且患病年齡趨于年輕化。根據(jù)世界衛(wèi)生組織的數(shù)據(jù)，在接受康復(fù)治療的情況下仍會(huì)有60%的患者會(huì)有不同程度的肢體功能障礙[2]，嚴(yán)重影響患者的日常生活質(zhì)量。隨著經(jīng)顱磁刺激治療技術(shù)的發(fā)展，現(xiàn)階段腦卒中的救治率獲得了顯著提高，這大大促進(jìn)了患者的康復(fù)[3]。但是，經(jīng)顱磁刺激技術(shù)在臨床應(yīng)用中存在諸多問(wèn)題，其中精準(zhǔn)度、穩(wěn)定性、操作人員的誤差制約了這項(xiàng)技術(shù)的臨床推廣和進(jìn)一步發(fā)展[4-7]。本文作者旨在設(shè)計(jì)一款輔助經(jīng)顱磁治療的機(jī)械臂，通過(guò)建模設(shè)計(jì)、運(yùn)動(dòng)學(xué)分析工作空間建立、運(yùn)動(dòng)路徑規(guī)劃等一系列研究，使經(jīng)顱磁刺激治療過(guò)程更加智能化，減輕醫(yī)療人員的工作強(qiáng)度。

1 機(jī)械臂尺寸與建模

人體的頭部呈現(xiàn)不規(guī)則球體，頭顱的尺寸也因人而異。在設(shè)計(jì)六軸機(jī)械臂的尺寸時(shí)，本著實(shí)用最大化、均衡化的原則，根據(jù)GB 2428—1998《成年人頭面部尺寸》進(jìn)行尺寸設(shè)計(jì)，并以此作為研究目標(biāo)。經(jīng)顱磁六軸機(jī)械臂主要由基座、旋轉(zhuǎn)關(guān)節(jié)、連接件、末端執(zhí)行器構(gòu)成，相鄰的關(guān)節(jié)與軸兩兩垂直，整體SolidWorks三維模型如圖1所示。

圖1 三維模型Fig.1 3D model

1986年KHALIL、KLEINFINGER提出一種建立連桿坐標(biāo)系的改進(jìn)D-H參數(shù)法[8]，在建立關(guān)節(jié)坐標(biāo)系時(shí)將坐標(biāo)系固結(jié)于該連桿的近端，而非遠(yuǎn)端，更符合直觀理解，方便產(chǎn)品實(shí)現(xiàn)以及工程人員使用。本文作者采用改進(jìn)D-H參數(shù)法建立連桿坐標(biāo)系，簡(jiǎn)化后的機(jī)械臂數(shù)學(xué)模型如圖2所示：坐標(biāo)系C0固定在關(guān)節(jié)1基座，坐標(biāo)系C1、C2固定在關(guān)節(jié)2肩部，坐標(biāo)系C3固定在關(guān)節(jié)3肘部，坐標(biāo)系C4、C5固定在關(guān)節(jié)4、5腕部，目標(biāo)坐標(biāo)系C6固定在末端執(zhí)行器的中心[9]。改進(jìn)D-H參數(shù)表如表1所示。其中d1=0.158 m，d4=0.105 5 m，d5=0.105 5 m，d6=0.080 m，a2=0.268 m，a3=0.256 m。

表1 機(jī)械臂D-H參數(shù)Tab.1 Mechanical arm D-H parameters

圖2 連桿坐標(biāo)系分布Fig.2 Distribution of connecting rod coordinate system

2 正運(yùn)動(dòng)學(xué)求解

機(jī)械臂是由一系列運(yùn)動(dòng)關(guān)節(jié)連接的運(yùn)動(dòng)鏈。機(jī)械臂的運(yùn)動(dòng)學(xué)分析分為正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)。正運(yùn)動(dòng)學(xué)可以簡(jiǎn)單地理解為給定一組關(guān)節(jié)角度，正運(yùn)動(dòng)學(xué)的解總是唯一的。相反，在逆運(yùn)動(dòng)學(xué)求解的過(guò)程中，將會(huì)出現(xiàn)沒(méi)有解、唯一解或多解的情況。因此，國(guó)內(nèi)外學(xué)者在求逆解的過(guò)程中提出了不同的方法。

根據(jù)圖1、圖2建立的D-H連桿坐標(biāo)系和表1機(jī)械臂D-H參數(shù)，得到關(guān)節(jié)6末端執(zhí)行器相對(duì)于基坐標(biāo)系的總位姿變換矩陣，即式(1):

(1)

(2)

計(jì)算過(guò)程簡(jiǎn)化后可得經(jīng)顱磁機(jī)械臂末端執(zhí)行器的空間位置向量為

其中：ci=cosθi，si=sinθi，c23=cos(θ2+θ3)，s23=sin(θ2+θ3)。

3 逆運(yùn)動(dòng)學(xué)求解

當(dāng)待治療點(diǎn)的三維坐標(biāo)已知時(shí)，末端執(zhí)行器的位置可通過(guò)待治療點(diǎn)的位置坐標(biāo)獲得，末端執(zhí)行器的方向可由與待治療中心點(diǎn)垂直的法線得到，最終獲得末端執(zhí)行器的位置和方向，即(n，o，a，p)已知。而求解機(jī)械臂逆運(yùn)動(dòng)學(xué)問(wèn)題直接轉(zhuǎn)化為計(jì)算各關(guān)節(jié)變量θi。逆運(yùn)動(dòng)學(xué)求解方法主要有幾何法[10-11]、解析法[12]、數(shù)值法[13-14]、智能算法[15]等。本文作者在綜合對(duì)比之后采用逆變換求解各關(guān)節(jié)變量。由于關(guān)節(jié)2、3、4旋轉(zhuǎn)軸方向相同，因此可以表示為

(3)

(4)

求解逆解需要選擇合適的方程有序地求解，這將會(huì)很大程度上降低求解難度。對(duì)比式(3)(4)得到式(5)：

(5)

經(jīng)三角函數(shù)、萬(wàn)能公式計(jì)算可得各關(guān)節(jié)變量θi。

其中A1、A2為根據(jù)矩陣對(duì)比所得：

4 經(jīng)顱磁機(jī)械臂仿真

為了在仿真中驗(yàn)證正、逆運(yùn)動(dòng)學(xué)方程推導(dǎo)的正確性，根據(jù)D-H參數(shù)使用MATLAB中的機(jī)器人工具箱建立了經(jīng)顱磁機(jī)械臂運(yùn)動(dòng)學(xué)模型。通過(guò)調(diào)用相關(guān)程序函數(shù)建模，設(shè)置一組關(guān)節(jié)角度θ=[0，0，0，0，0]，獲得了圖3中的機(jī)器人示教姿態(tài)和圖4中的初始姿態(tài)模型。

圖3 調(diào)試位姿Fig.3 Debug pose

經(jīng)顱磁六軸機(jī)械臂的工作空間是指六軸機(jī)械臂的末端執(zhí)行器所能達(dá)到的最大空間區(qū)域，它是衡量機(jī)械臂性能的重要指標(biāo)[16]。根據(jù)每個(gè)連桿的尺寸和末端執(zhí)行器的位置矢量，在分析軟件MATLAB中使用蒙特卡羅方法求解機(jī)械臂的工作空間，仿真結(jié)果如圖5、6所示。

圖5 左視機(jī)械臂工作空間Fig.5 Left view robotic arm workspace

圖6 上視機(jī)械臂工作空間Fig.6 Upview robotic arm workspace

據(jù)圖5—6所得，經(jīng)顱磁機(jī)械臂的工作空間可近似為直徑1 000 mm的球體。對(duì)比人體男女生頭圍以及高度，該機(jī)械臂的工作空間能夠覆蓋頭顱上治療目標(biāo)范圍，滿足實(shí)際治療的工作空間要求。

5 經(jīng)顱磁機(jī)械臂的路徑規(guī)劃

5.1 經(jīng)顱磁機(jī)械臂的工作過(guò)程

首先，根據(jù)治療師的治療策略，對(duì)人體頭部主要治療穴位進(jìn)行顏色標(biāo)記，通過(guò)設(shè)置的單目攝像頭在頭顱上方、左方、右方、后方對(duì)標(biāo)記點(diǎn)進(jìn)行圖像數(shù)據(jù)采集，獲得治療穴位的三維坐標(biāo)信息。

其次，將收集的治療穴位標(biāo)記點(diǎn)按照科學(xué)治療方法進(jìn)行排序，求解機(jī)械臂工作范圍內(nèi)每個(gè)治療穴位對(duì)應(yīng)的機(jī)械臂逆解，根據(jù)穴位的治療角度和機(jī)械臂的逆解，在機(jī)械臂的工作空間范圍內(nèi)設(shè)置預(yù)治療點(diǎn)或最終治療點(diǎn)。

最終，控制器控制機(jī)械臂末端執(zhí)行器從初始位置移動(dòng)到首個(gè)治療穴位點(diǎn)，待完成一段有效時(shí)間治療后，控制機(jī)械臂移動(dòng)到下一個(gè)治療穴位點(diǎn)進(jìn)行治療。以此類推，直到最后一個(gè)標(biāo)記穴位治療完成后，機(jī)械臂回到初始位置，等待下一個(gè)患者進(jìn)行治療。

通過(guò)對(duì)以上工作過(guò)程進(jìn)行分析，在整個(gè)治療過(guò)程中經(jīng)顱磁機(jī)械臂的路徑規(guī)劃涉及兩個(gè)階段：階段一是對(duì)穴位順序進(jìn)行規(guī)劃，階段二是對(duì)人體頭顱整個(gè)治療路徑進(jìn)行軌跡規(guī)劃[17-18]。

5.2 穴位順序的規(guī)劃

根據(jù)國(guó)家經(jīng)穴部位標(biāo)準(zhǔn)可知，頭部的穴位非常多，經(jīng)顱磁治療中最常用的穴位包括頭頂中間部位的百會(huì)、風(fēng)府、啞門(mén)、神庭等主穴，以及四周的攢竹、天柱、四神聰?shù)入蜓?輔穴)，涉及的治療范圍也比較大，這就需要經(jīng)顱磁機(jī)械臂在工作過(guò)程中對(duì)治療的穴位進(jìn)行優(yōu)先順序的路徑規(guī)劃。

經(jīng)顱磁機(jī)械臂在治療穴位的優(yōu)先順序上秉承中醫(yī)治療策略中的先針刺患側(cè)穴或健側(cè)穴、后針刺病側(cè)穴的所謂“補(bǔ)健側(cè)、瀉患側(cè)”原則?；诖酥委熢瓌t，將得到一系列的治療穴位點(diǎn)Qi(i=1，2，…，n)。 其中，在這些治療穴位點(diǎn)的Qi集合中，將主穴Ri(i=1，2，…，n)與輔穴Gi(i=1，2，…，n)進(jìn)行區(qū)分。在治療路徑劃分中，一條路徑會(huì)由一個(gè)主穴或一個(gè)主穴與多個(gè)輔穴組成，如存在多個(gè)輔穴將對(duì)輔穴進(jìn)行排序。排序方法如下：通過(guò)單目攝像頭掃描治療穴位點(diǎn)Ri、Gi(i=1，2，…，n)獲取其三維空間位置坐標(biāo)(Xi，Yi，Zi)(i=1，2，…，n)。因?yàn)檠ㄎ稽c(diǎn)的位置常常分布于頭顱頂部或者頭顱后腦區(qū)域，所以需要將人體頭顱由上到下、由外到里(以放置機(jī)械臂方位為里)進(jìn)行劃分，如圖7所示。設(shè)穴位點(diǎn)中的主穴在平面中的坐標(biāo)為(x0，y0)，輔穴的坐標(biāo)(xi，yi)，分別計(jì)算需要治療的輔穴與主穴兩穴位之間在投影平面上的距離di(i=1，2，…，n)由公式(6)計(jì)算得出：

圖7 由上到下(a)、由外向里(b)劃分Fig.7 Divided from top to bottom (a)and from outside to inside (b)

(6)

最后按照輔穴與主穴距離由小到大原則對(duì)輔穴進(jìn)行排序，輸出新的治療點(diǎn)集Pi(i=1，2，…，n)， 即求得頭顱治療穴位的治療點(diǎn)集。

5.3 路徑軌跡規(guī)劃

獲取一條路徑中所治療穴位的點(diǎn)集之后，下面對(duì)所經(jīng)過(guò)點(diǎn)進(jìn)行路徑規(guī)化。機(jī)械臂的路徑規(guī)劃有多方法，基于搜索的路徑規(guī)劃可分為Dijkstra、A*等，基于采樣的路徑規(guī)劃可分為RRT、RRT*、Informd RRT*等，基于智能算法的路徑規(guī)劃可分為遺傳算法、蟻群算法等。

無(wú)碰撞路徑規(guī)劃是治療過(guò)程中需要解決的問(wèn)題之一。在整個(gè)操作過(guò)程中，人體頭部呈現(xiàn)不規(guī)則球體，目標(biāo)穴位點(diǎn)位于人體頭部表面，人的頭部將成為障礙物。為了將目標(biāo)穴位快速擴(kuò)展到機(jī)械臂的方向，需要對(duì)機(jī)械臂進(jìn)行碰撞檢測(cè)，以便于快速搜索出到達(dá)目標(biāo)穴位點(diǎn)的最佳擴(kuò)展路徑。經(jīng)顱磁機(jī)械臂的碰撞檢測(cè)方法是將頭顱規(guī)則化，并將頭顱包括在長(zhǎng)方體或圓柱體中。類似地，機(jī)械臂的連桿也簡(jiǎn)化為圓柱體[19-20]。為了方便計(jì)算，機(jī)械臂等效為一條線段。此后包絡(luò)后的頭顱進(jìn)行膨脹展開(kāi)。判定頭顱與機(jī)械臂碰撞的情況是當(dāng)簡(jiǎn)化機(jī)械臂的線段與簡(jiǎn)化頭顱圓柱題的面相交時(shí)，認(rèn)為發(fā)生了碰撞。

經(jīng)過(guò)研究對(duì)比，本文作者采用了改進(jìn)RRT算法中的RRT-connect算法，該算法具有收斂速度塊、時(shí)間短、成本低、接近最優(yōu)、常適用高維空間等優(yōu)點(diǎn)。算法的偽代碼如下：

T1←qstart；T2←qgoal；

Whilei

qrand←Sanple()；

qnearest←Nearest(T1，qrand)；

qnew←Steer(qnearest，qrand，ε)；

if Obstacle Free(qnew)then

if Obstacle Free(qnearest，qnew)then

T1←qnew；

else break；

return Path (T1，T2)；

else Swap(T1，T2)；

其中：Sample()表示通過(guò)隨機(jī)采樣獲得采樣點(diǎn)qrand；Steer(qnearest，qrand，ε)是指在連接采樣點(diǎn)和最近節(jié)點(diǎn)的線上選擇新點(diǎn)qnew，使得新點(diǎn)與最近節(jié)點(diǎn)之間的距離等于搜索步長(zhǎng)ε；Nearest(T，qrand)是指搜索隨機(jī)樹(shù)中最接近采樣點(diǎn)的節(jié)點(diǎn)qnearest；Obstacle Free(qnew)指生成的新點(diǎn)不與障礙物碰撞；Obstacle Free (qnearest，qnew)是指新點(diǎn)和最近節(jié)點(diǎn)之間的線不與障礙物碰撞；Path(T1，T2)指隨機(jī)樹(shù)T1和隨機(jī)樹(shù)T2相遇以獲得無(wú)沖突路徑；Swap(T1，T2)表示切換隨機(jī)樹(shù)T1和隨機(jī)樹(shù)T2。

6 試驗(yàn)與結(jié)果

為了驗(yàn)證經(jīng)顱磁性六軸機(jī)械臂的可行性，利用MATLAB建立了經(jīng)顱磁六軸機(jī)械臂的半仿真平臺(tái)，對(duì)經(jīng)顱磁六軸機(jī)械臂的路徑規(guī)劃進(jìn)行仿真測(cè)試。測(cè)試在主頻率為1.8 GHz、內(nèi)存為8 GB的計(jì)算機(jī)上進(jìn)行。

首先，所搭建仿真平臺(tái)如圖8所示，在人體頭顱模型上，用紅色進(jìn)行穴位模擬標(biāo)記，單目攝像頭可在頭顱上方、左方、右方、后方對(duì)標(biāo)記點(diǎn)進(jìn)行圖像數(shù)據(jù)采集，圖8所示攝像頭位置在平臺(tái)右后方，經(jīng)顱磁機(jī)械臂采用D-H參數(shù)相同的MATLAB Robotics Toolbox機(jī)器人工具箱代替。

圖8 仿真平臺(tái)搭建Fig.8 Simulation platform

其次，因?yàn)椴煌颊?、不同穴位的治療時(shí)間存在差異，所以治療穴位時(shí)間根據(jù)醫(yī)師治療策略進(jìn)行設(shè)置。當(dāng)機(jī)械臂從初始位置移動(dòng)到第一個(gè)治療主穴時(shí)，記錄為第一段移動(dòng)軌跡；當(dāng)治療完主穴后，將對(duì)此治療路徑中的輔穴進(jìn)行依次治療，記錄主穴到第一個(gè)輔穴的治療路徑為第二段移動(dòng)軌跡。因?yàn)榻?jīng)顱磁機(jī)械臂治療輔穴過(guò)程中需要設(shè)置一定的高度，避免對(duì)人體組織器官造成損傷，記錄貼近第一個(gè)輔穴治療下潛的過(guò)程為第三段軌跡。以此類推。

最后，將3段路徑進(jìn)行擬合優(yōu)化，在編程中采用了多項(xiàng)式插值函數(shù)對(duì)3段路徑軌跡進(jìn)行處理，得出完整的經(jīng)顱磁治療路徑，如圖9所示。并且輸出3段路徑軌跡的速度、角速度、角加速度隨時(shí)間變化的信息，如圖10—12所示。仿真試驗(yàn)結(jié)果表明：所設(shè)計(jì)的六軸機(jī)械臂能夠依次完成從初始點(diǎn)-主穴-輔穴-初始點(diǎn)的軌跡規(guī)劃，滿足經(jīng)顱磁治療的基本工作要求。

圖9 運(yùn)行軌跡Fig.9 Running trajectory

圖10 第一軌跡信息圖Fig.10 The first trajectory information map：(a)joint angle； (b)angular velocity；(c)angular acceleration

圖11 第二軌跡信息圖Fig.11 The second trajectory information map：(a)joint angle； (b)angular velocity；(c)angular acceleration

圖12 第三軌跡信息圖Fig.12 The third trajectory information map：(a)joint angle； (b)angular velocity；(c)angular acceleration

7 結(jié)論

(1)根據(jù)中國(guó)成年人頭型系列尺寸設(shè)計(jì)一款適用于經(jīng)顱磁治療儀的六軸機(jī)械臂，可以代替人工手持工作。機(jī)械臂整體由SolidWorks三維軟件建模，該機(jī)械臂的末端可以?shī)A取不同形狀的磁療、電療器具，具有精確、高效、靈活、穩(wěn)定、通用性高的特點(diǎn)。

(2)經(jīng)顱磁機(jī)械臂采用改進(jìn)D-H參數(shù)法，結(jié)合MATLAB軟件中機(jī)器人工具箱，建立機(jī)械臂的運(yùn)動(dòng)學(xué)模型，推導(dǎo)機(jī)械臂正、逆運(yùn)動(dòng)學(xué)方程并求解，得到機(jī)械臂末端執(zhí)行器與各大關(guān)節(jié)變量間的邏輯關(guān)系。在MATLAB仿真中驗(yàn)證了邏輯的正確性，采用蒙特卡羅方法驗(yàn)證機(jī)械臂的性能，仿真結(jié)果表明：該機(jī)械臂能滿足實(shí)際治療的工作空間要求。

(3)提出的方法解決了一條路徑中多目標(biāo)治療穴位路徑規(guī)劃問(wèn)題，應(yīng)用雙向RRT算法實(shí)現(xiàn)了無(wú)碰撞路徑軌跡。編程中采用了多項(xiàng)式插值函數(shù)對(duì)軌跡進(jìn)行處理，得出完整的路徑軌跡。搭建半仿真試驗(yàn)平臺(tái)，結(jié)果表明：所設(shè)計(jì)的六軸機(jī)械臂能夠依次完成從初始點(diǎn)-主穴-輔穴-初始點(diǎn)的軌跡規(guī)劃，基本滿足經(jīng)顱磁治療的工作要求。