重離子放療輔助醫(yī)用機(jī)械臂避撞路徑規(guī)劃研究

喬宇，姚運(yùn)萍，馬利強(qiáng)，楊小龍，陳繼鵬，陳惠賢

蘭州理工大學(xué) 機(jī)電工程學(xué)院，甘肅 蘭州 730050

引言

隨著醫(yī)療技術(shù)和工業(yè)機(jī)器人技術(shù)的不斷發(fā)展，醫(yī)用機(jī)械臂應(yīng)用技術(shù)已成為醫(yī)科技術(shù)的重要研究方向[1-3]。醫(yī)用機(jī)械臂在放療過(guò)程中將病患運(yùn)送至治療位置，其高精度、高可靠性的特點(diǎn)使相關(guān)應(yīng)用技術(shù)得到國(guó)內(nèi)外專家的重視[4-7]。在放射治療環(huán)境下，輔助治療設(shè)備的存在使得擺位難度增加，因此醫(yī)用機(jī)械臂路徑規(guī)劃的研究變得極為重要。目前，針對(duì)醫(yī)用機(jī)械臂的典型路徑規(guī)劃方法包括自由空間法和人工勢(shì)場(chǎng)法兩種[8-10]。該文針對(duì)醫(yī)用機(jī)械臂的特殊結(jié)構(gòu)，將醫(yī)用機(jī)械臂和障礙模型進(jìn)行簡(jiǎn)化；將障礙域投影到關(guān)節(jié)空間，得到安全擺位空間；通過(guò)A*算法在安全擺位空間內(nèi)搜索擺位路徑；最后，通過(guò)仿真驗(yàn)證該方法的安全可行性。

1 碰撞模型

1.1 醫(yī)用機(jī)械臂模型

該文研究的重離子放療輔助醫(yī)用機(jī)械臂為4自由度機(jī)械臂，由于其結(jié)構(gòu)不規(guī)則，因此其模型采用圓柱模型，將各不規(guī)則突出部分包含在各簡(jiǎn)化的圓柱包絡(luò)模型內(nèi)。醫(yī)用機(jī)械臂實(shí)物圖，見圖1a；其D-H坐標(biāo)系，見圖1b。圖1b中各坐標(biāo)系為關(guān)節(jié)子坐標(biāo)系，其中桿1為關(guān)節(jié)1與關(guān)節(jié)2軸線間實(shí)體部分簡(jiǎn)化模型，直徑為0.38 m，桿2為關(guān)節(jié)2與關(guān)節(jié)3軸線間實(shí)體部分簡(jiǎn)化模型，直徑為0.56 m，桿3為關(guān)節(jié)3與關(guān)節(jié)4軸線間實(shí)體部分簡(jiǎn)化模型，直徑為0.85 m，桿4為末端碳纖維板簡(jiǎn)化后模型，直徑為0.48 m。

圖1 醫(yī)用機(jī)械臂實(shí)物及D-H參數(shù)坐標(biāo)系圖

醫(yī)用機(jī)械臂D-H參數(shù)，見表1。表1中qi為關(guān)節(jié)變量，為關(guān)節(jié)角，di為關(guān)節(jié)平移量，ai為桿長(zhǎng)，為扭角。關(guān)節(jié)1為旋轉(zhuǎn)關(guān)節(jié)，關(guān)節(jié)變量為，關(guān)節(jié)平移量為0，桿長(zhǎng)a1=0.96 m，扭角為0；關(guān)節(jié)2為平移關(guān)節(jié)，關(guān)節(jié)變量為d2，關(guān)節(jié)角和桿長(zhǎng)為0，扭角為-90度；關(guān)節(jié)3為平移關(guān)節(jié)，關(guān)節(jié)變量為d3，關(guān)節(jié)角為-90度，桿長(zhǎng)a3=0.15 m，扭角為90度；關(guān)節(jié)4為平移關(guān)節(jié)，關(guān)節(jié)變量為d4，關(guān)節(jié)角、扭角均為0度。

表1 醫(yī)用機(jī)械臂D-H參數(shù)表

由醫(yī)用機(jī)械臂D-H參數(shù)可寫出相鄰桿件的位姿矩陣，將相鄰桿件的位姿矩陣依次相乘，則有：

由位姿矩陣通式可得該醫(yī)用機(jī)械臂的運(yùn)動(dòng)學(xué)方程：

1.2 障礙模型

重離子放療輔助醫(yī)用機(jī)械臂的工作環(huán)境為重離子放射治療室，治療室內(nèi)的障礙主要有擺位驗(yàn)證檢測(cè)機(jī)、治療頭、水平束流遮擋裝置。由于各障礙物幾何形狀不規(guī)則，文中采用各障礙物的圓柱體包絡(luò)來(lái)近似建立障礙模型，這種建模方法使得障礙域范圍有所擴(kuò)大，但使得障礙域形狀大為簡(jiǎn)化，醫(yī)用機(jī)械臂路徑規(guī)劃的效率得到提高，同時(shí)由于障礙域有所擴(kuò)大，間接提高了醫(yī)用機(jī)械臂路徑規(guī)劃的安全性[11-13]。

擺位驗(yàn)證障礙檢測(cè)機(jī)障礙域采用圓柱包絡(luò)描述為S1(P1,r1,h1)，其在水平面的投影形成的平面區(qū)域S1’(P1,r1)，如圖2a所示。其中P1為圓柱體障礙域在基坐標(biāo)系xoy平面投影圓心的坐標(biāo)，r1為障礙域的半徑，h1為障礙域的高度，由于圖中為投影平面區(qū)域，無(wú)法提現(xiàn)高度，因此圖中無(wú)標(biāo)注。r1=1.02 m，h1=1.76 m。

治療頭障礙域采用圓柱包絡(luò)描述為S2(P2,r2,h2)，由于治療頭緊靠墻壁，所以S2”(P2,r2)為有效障礙域在水平面內(nèi)的投影，其在水平面投影形成的平面區(qū)域S2’(P2,r2)及有效障礙域S2”(P2,r2)，如圖2b所示。其中P2為圓柱障礙域在基坐標(biāo)系xoy平面投影圓心的坐標(biāo)，r2為障礙域半徑，h2為障礙域高度，由于圖中為投影平面區(qū)域，無(wú)法提現(xiàn)高度，因此圖中無(wú)標(biāo)注。r2=1.46 m，h2=2.20 m。

水平束流遮擋裝置障礙域采用圓柱包絡(luò)描述為S3(P3,r3,h3)，其在基坐標(biāo)系水平面的投影形成的平面區(qū)域S3’(P3,r3)，如圖2c所示。其中r3為障礙域半徑，h3為障礙域高度，由于圖中為投影平面區(qū)域，無(wú)法提現(xiàn)高度，因此圖中無(wú)標(biāo)注。r3=0.35 m，h3=1.52 m。

圖2 障礙域在水平面的投影

2 笛卡爾空間障礙向構(gòu)型空間的轉(zhuǎn)化

2.1 總體思路

在醫(yī)用機(jī)械臂擺位過(guò)程中，多數(shù)情況下擺位驗(yàn)證檢測(cè)機(jī)處于靜止?fàn)顟B(tài)，擺位完成后，其進(jìn)入工作狀態(tài)，同時(shí)為了便于闡述該文提出的避障規(guī)劃算法，以下均針對(duì)醫(yī)用機(jī)械臂擺位過(guò)程中，擺位驗(yàn)證檢測(cè)機(jī)處于靜止的情況進(jìn)行討論。在擺位過(guò)程中，其相對(duì)于醫(yī)用機(jī)械臂存在運(yùn)動(dòng)的情況下，醫(yī)用機(jī)械臂的安全擺位空間通過(guò)多次求解即可得到，具體求解方法與兩者相對(duì)靜止情況相同。本節(jié)著重說(shuō)明求解醫(yī)用機(jī)械臂安全擺位空間的方法。

由于各障礙物形成的障礙域?yàn)閳A柱形包絡(luò)，因此除升降關(guān)節(jié)的運(yùn)動(dòng)外，其余關(guān)節(jié)的運(yùn)動(dòng)都有可能導(dǎo)致醫(yī)用機(jī)械臂與障礙物之間發(fā)生碰撞。為得到醫(yī)用機(jī)械臂各部分與障礙物碰撞的條件，采用分級(jí)討論的方法求解。

總體思路為：① 求解旋轉(zhuǎn)關(guān)節(jié)1的運(yùn)動(dòng)可能導(dǎo)致醫(yī)用機(jī)械臂初始位姿狀態(tài)與障礙物發(fā)生碰撞的運(yùn)動(dòng)范圍，后續(xù)各移動(dòng)關(guān)節(jié)的運(yùn)動(dòng)導(dǎo)致碰撞發(fā)生的情況均在該范圍基礎(chǔ)上分析討論；② 由于各障礙域?yàn)閳A柱包絡(luò)模型，所以升降平移關(guān)節(jié)2的單獨(dú)運(yùn)動(dòng)不會(huì)導(dǎo)致碰撞的發(fā)生，在第一步得到范圍基礎(chǔ)上推導(dǎo)關(guān)節(jié)3運(yùn)動(dòng)與障礙發(fā)生碰撞的條件；③ 在關(guān)節(jié)3運(yùn)動(dòng)與障礙物不發(fā)生碰撞的移動(dòng)范圍內(nèi)，分析關(guān)節(jié)4的運(yùn)動(dòng)導(dǎo)致與障礙發(fā)生碰撞的條件。

2.2 醫(yī)用機(jī)械臂各部分簡(jiǎn)化為桿件后與障礙碰撞條件分析

重離子放療輔助醫(yī)用機(jī)械臂簡(jiǎn)化后的桿件模型中，由于桿3的長(zhǎng)度0.15 m小于桿3圓柱模型半徑0.425 m，因此桿3與障礙發(fā)生碰撞的情況已包含在其關(guān)節(jié)變量與障礙發(fā)生碰撞的討論過(guò)程中。以下針對(duì)桿1、桿3、桿4與障礙發(fā)生碰撞的條件進(jìn)行討論。

2.2.1 導(dǎo)致碰撞發(fā)生的關(guān)節(jié)1運(yùn)動(dòng)范圍

為了分析醫(yī)用機(jī)械臂與障礙發(fā)生碰撞的條件，首先求解由關(guān)節(jié)1的運(yùn)動(dòng)可能導(dǎo)致碰撞發(fā)生的運(yùn)動(dòng)范圍。將醫(yī)用機(jī)械臂簡(jiǎn)化后的桿件模型向xoy平面投影，則關(guān)節(jié)1運(yùn)動(dòng)導(dǎo)致碰撞發(fā)生的4個(gè)極限位置如圖3所示。

圖3 旋轉(zhuǎn)關(guān)節(jié)1運(yùn)動(dòng)極限位置圖

由圖3可求出障礙域1中心在平面投影與坐標(biāo)原點(diǎn)的連線和x軸夾角：

求解醫(yī)用機(jī)械臂運(yùn)動(dòng)至障礙域1附近的兩個(gè)極限位置與障礙域1中心投影與坐標(biāo)原點(diǎn)連線的夾角：

式中，d11為障礙域1中心在xoy平面的投影到坐標(biāo)原點(diǎn)的距離，d12和d13分別為障礙域中心投影到醫(yī)用機(jī)械臂兩極限位置中軸線的距離。

同理可得醫(yī)用機(jī)械臂擺位運(yùn)動(dòng)過(guò)程中與障礙域2可能發(fā)生碰撞的關(guān)節(jié)1運(yùn)動(dòng)范圍為：，其中

2.2.2 關(guān)節(jié)3的運(yùn)動(dòng)與障礙發(fā)生碰撞的條件

圖4 關(guān)節(jié)3運(yùn)動(dòng)范圍示意圖

（1）計(jì)算d31：

圖5 關(guān)節(jié)4運(yùn)動(dòng)范圍示意圖

以上討論過(guò)程采用解析法進(jìn)行，可以將笛卡爾空間的各障礙域模型實(shí)時(shí)轉(zhuǎn)化至關(guān)節(jié)空間中，進(jìn)而得到重離子放療輔助醫(yī)用機(jī)械臂的安全擺位空間。

3 基于A＊算法的無(wú)碰路徑規(guī)劃

A*算法是在Dijkstra算法基礎(chǔ)上加了約束條件的啟發(fā)性搜索算法，其具有可納性、最優(yōu)性，用于搜索狀態(tài)空間的最好優(yōu)先路徑[14-15]。

3.1 算法思想

A*算法是應(yīng)用于人工智能中的典型啟發(fā)式搜索方法之一，這些方法都使用了啟發(fā)函數(shù)，但節(jié)點(diǎn)搜索策略有所不同。

A*算法估價(jià)函數(shù)的設(shè)計(jì)是其算法思想的核心[16]。估價(jià)函數(shù)定義為：

3.2 基于A＊算法的路徑規(guī)劃方法

為了方便使用A*算法對(duì)重離子放療輔助醫(yī)用機(jī)械臂的擺位路徑進(jìn)行規(guī)劃，定義醫(yī)用機(jī)械臂的四個(gè)關(guān)節(jié)變量為一個(gè)四維數(shù)組，各起始關(guān)節(jié)變量為qfirst[4]；關(guān)節(jié)變量搜索步長(zhǎng)為dstep[4]；第i步最佳關(guān)節(jié)變量為qbest_i[4]；后繼節(jié)點(diǎn)關(guān)節(jié)變量為qfol_i[4]；期望關(guān)節(jié)變量為qexp[4]。

（1） 生成初始關(guān)節(jié)位置搜索圖，將qfirst[4]放入OPEN表。

（2）生成CLOSE表，初始值為空。

（3）若OPEN表為空，失敗。

（4）將OPEN表中f值最小的點(diǎn)作為qbest_i[4]放入CLOSE表，并將其從OPEN表中刪除。

（5）判斷qbest_i[4]是否是qexp[4]，若是，成功，否則進(jìn)行下一步。

（6）根據(jù)dstep[4]生成后繼節(jié)點(diǎn)qfol_i[4]。

（7）對(duì)qfol_i[4]進(jìn)行如下過(guò)程：① 判斷qfol_i[4]是否在醫(yī)用機(jī)械臂安全擺位空間內(nèi)，若在，建立返回qbest_i[4]的指針，若不在，返回（6）；② 計(jì)算③ 判斷qfol_i[4]是否在OPEN表中，若在，進(jìn)入下一步，否則進(jìn)入⑤；④ 定義qnew_i[4]，令，將qnew_i[4]放入qbest_i[4]后繼節(jié)點(diǎn)表，并判g(shù)(qnew_i[4])＞g(qbest_i[4])是否成立，若成立，將qnew_i[4]上一節(jié)點(diǎn)作為qbest_i[4]，令qfol_i[4]=qnew_i[4]，進(jìn)入⑦，否則q(qnew_i[4]=q(qfol_i[4])，直接進(jìn)入⑦；⑤ 判斷qfol_i[4]是否在CLOSE表中，若在進(jìn)入④，否則進(jìn)入下一步；⑥ 將qfol_i[4]放入OPEN表，并放入qbest_i[4]后繼節(jié)點(diǎn)表；⑦ 計(jì)算f(qfol_i[4])，進(jìn)入（4）。

循環(huán)結(jié)束，CLOSE表中數(shù)據(jù)即為醫(yī)用機(jī)械臂擺位最優(yōu)路徑。其算法流程圖如圖6所示。

4 試驗(yàn)與仿真

對(duì)以上提出的醫(yī)用機(jī)械臂路徑規(guī)劃方法進(jìn)行仿真驗(yàn)證。經(jīng)Solidworks軟件建立醫(yī)用機(jī)械臂擬真工作環(huán)境，并將其導(dǎo)入Adams軟件得到完整的醫(yī)用機(jī)械臂仿真虛擬樣機(jī)模型如圖7所示。

圖6 算法流程圖

為了驗(yàn)證避障的有效性，設(shè)計(jì)了運(yùn)動(dòng)仿真試驗(yàn)。試驗(yàn)中醫(yī)用機(jī)械臂的初始位姿均為{0，0，0，0}。試驗(yàn)期望關(guān)節(jié)變量為{168.4，86.5，53.7，237.3}。在軟件中設(shè)置仿真環(huán)境，仿真結(jié)果如圖8所示。

圖7 醫(yī)用機(jī)械臂仿真模型圖

圖8 仿真結(jié)果圖

圖8中三幅圖分別為仿真過(guò)程圖、等中心回轉(zhuǎn)運(yùn)動(dòng)角速度和角加速度曲線圖、X和Y向運(yùn)動(dòng)速度和加速度曲線圖。由圖8可知，醫(yī)用機(jī)械臂擺位過(guò)程中，由于自重較大，等中心回轉(zhuǎn)運(yùn)動(dòng)過(guò)程中有較小的速度突變，會(huì)導(dǎo)致擺位過(guò)程存在輕微沖擊振動(dòng)現(xiàn)象，但影響較小，X和Y向運(yùn)動(dòng)過(guò)程平穩(wěn)，基本滿足擺位過(guò)程穩(wěn)定性要求。仿真過(guò)程中無(wú)碰撞的情況發(fā)生，該路徑規(guī)劃方法能滿足醫(yī)用機(jī)械臂擺位避障要求。

5 結(jié)論

文中針對(duì)醫(yī)用機(jī)械臂在放射治療環(huán)境下擺位過(guò)程中可能與輔助設(shè)備發(fā)生碰撞的情況，對(duì)醫(yī)用機(jī)械臂路徑規(guī)劃方法進(jìn)行研究。首先將障礙模型進(jìn)行合理簡(jiǎn)化，并將其轉(zhuǎn)化至關(guān)節(jié)空間，從而求解出醫(yī)用機(jī)械臂的安全擺位空間，并分析得到其各部分與障礙域碰撞條件；在此基礎(chǔ)上，基于A*算法于安全擺位空間內(nèi)進(jìn)行路徑搜索，得到醫(yī)用機(jī)械臂擺位最優(yōu)路徑。仿真試驗(yàn)表明，提出的應(yīng)用于醫(yī)用機(jī)械臂擺位過(guò)程的路徑規(guī)劃方法是安全可行的。