蔡 旻 薛 杰 高涵文 李華一 陶重犇

1(蘇州科技大學(xué)天平學(xué)院 江蘇 蘇州 215009)2(蘇州科技大學(xué)電子與信息工程學(xué)院 江蘇 蘇州 215009)

0 引 言

微創(chuàng)介入治療已成為外科醫(yī)學(xué)發(fā)展的必然趨勢。經(jīng)皮穿刺是最常用的外科技術(shù)之一，廣泛應(yīng)用于活體組織病理檢查、局部給藥和癌癥治療。與傳統(tǒng)的采用剛性直線運(yùn)動(dòng)的針插入不同，柔性針對(duì)組織具有足夠的柔性，可以實(shí)現(xiàn)曲線運(yùn)動(dòng)。它利用針的變形來避免重要器官如神經(jīng)和血管以及骨骼等障礙。而傳統(tǒng)的剛性針頭很難實(shí)現(xiàn)靈活、精確定位[1]。

路徑規(guī)劃的研究是柔性插針運(yùn)動(dòng)控制的基礎(chǔ)。研究人員相繼提出了柔性針插入路徑規(guī)劃的概率法、目標(biāo)函數(shù)法、搜索法和逆運(yùn)動(dòng)學(xué)法[2]。霍本巖等[3]提出了基于多目標(biāo)粒子群優(yōu)化算法的斜尖柔性針穿刺路徑規(guī)劃。胡海龍[4]對(duì)套管柔性針路徑規(guī)劃進(jìn)行了深入的研究，在此基礎(chǔ)上，利用改進(jìn)的快速探索隨機(jī)樹(RRT)算法，提出了一種改進(jìn)RRT算法的套管柔性針路徑規(guī)劃算法。張永德等[5]利用二維圖像反饋控制器和狀態(tài)觀測器來估計(jì)針的俯仰角，并進(jìn)一步利用RRT路徑規(guī)劃方法。石開銘[6]采用啟發(fā)式RRT方法，通過二維超聲圖像反饋，提高了在線柔性針頭規(guī)劃控制的路徑規(guī)劃速度，實(shí)現(xiàn)了在線滾動(dòng)軌跡規(guī)劃。然而，由于柔性針系統(tǒng)的隨機(jī)性算法生成的確定路徑可能會(huì)碰到障礙物，最終位置可能遠(yuǎn)離所需的針頭位置。通常都有可能導(dǎo)致發(fā)生一些意外結(jié)果。

本文在算法的優(yōu)化過程中定義了一個(gè)成本函數(shù)來量化目標(biāo)誤差以及障礙物的接近程度。目標(biāo)誤差定義為路徑樣本到目標(biāo)的均方根距離，接近度定義為撞擊障礙物的概率。路徑分布的概率密度函數(shù)近似于位移高斯分布[7]。優(yōu)化的算法不是用RRT算法生成一條可能的路徑，而是在確定可行路徑生成后進(jìn)行優(yōu)化，有效地解決了上述研究的不足，并且降低了路徑成本，提高了安全性。

1 柔性針運(yùn)動(dòng)模型

柔性針模型是包括三維空間中位置和方向的6自由度系統(tǒng)，由于其非完整約束和不對(duì)稱的斜角尖端，針插入組織時(shí)會(huì)經(jīng)歷一個(gè)近圓弧[8]。如前所述，使用圍繞針切線的角速度ω(t)和沿針切線方向的平移速度ν(t)，本文可以在配置空間中將針尖引導(dǎo)到任何所需狀態(tài)。假設(shè)針的其余部分由于其高度靈活而遵循針尖運(yùn)動(dòng)所形成的路徑。

彈性針的運(yùn)動(dòng)模型由式(1)給出：

式中：κ是曲率；ξ是針尖的體速度；ν和ω是圖1中定義的兩個(gè)控制輸入。

圖1 柔性針模型

一般剛體運(yùn)動(dòng)的表示由SE(3)的一個(gè)元素描述，該元素的半積為R3和特殊的正交群SO(3)。SE(3)的元素可以用4×4矩陣g表示為：

柔性針的非完整運(yùn)動(dòng)模型由下式給出：

式中:κ是軌跡的曲率；ν(t)和ω(t)分別是切線方向的平移速度和切線周圍的角速度。

最終，柔性針的運(yùn)動(dòng)模型可以推導(dǎo)成指數(shù)積的形式：

gi+1=gieξΔt

(3)

2 柔性針RRT路徑規(guī)劃

2.1 擴(kuò)展函數(shù)

算法1中顯示了RRT算法的偽代碼。輸入元素包括初始狀態(tài)xinit、目標(biāo)狀態(tài)xgoal、閾值半徑Rthreshold、障礙物配置Qobs、最大迭代次數(shù)κ和間隔距離Δt。

算法1RRT算法

1. init (xinit)

2.fork=1 to κ

3. xrand←RANDOM_STATE

4.if(CHECK_COLLISION(xrand,Qobs)==FALSE)

5.continue

6.endif

7. xnear←NEAREST_NEIGHBOR(xrand,τ)

8. u←SELECT_INPUT(xrand,xnear)

9. xnew←NEW_STATE(xnear,u,Δt)

10.if(CHECK_EDGE(xnear,xnew,u,Qobs)==FALSE)

11.continue

12.endif

13. τ.add_vertex(xnew)

14. τ.add_edge(xnear,xnew,u)

15.if(CHECK_GOAL(xnew,xgoal,Rthreshold)==TRUE)

16.returnPATH(xinit,xgoal,τ)

17.break

18.endif

19.returnFailure

在從xnear擴(kuò)展到xnew的過程中，幾個(gè)原始運(yùn)動(dòng)作為控制輸入。算法1中步驟8和步驟9中的擴(kuò)展算法替換為擴(kuò)展函數(shù)，如算法2所示。

算法2 擴(kuò)展函數(shù)

1. xcandidate←DYNAMICS_EQUATION(xnear,umotion,Δt)

2. udeterministic←MIN_DISTANCE(xcandidate,xrand)

3. xnew←DYNAMICS_EQUATION(xnew,udeterministic,Δt)

4. xnew←CONSTANT_ID(xnew)

5. xnew←PARENT_ID(xnear)

6.returnxnew

步驟1使用給定的參數(shù)xnear、xrand、umotion和Δt生成運(yùn)動(dòng)模型的幾個(gè)候選對(duì)象。生成xcandidate后，函數(shù)MIN-DISTANCE會(huì)在所有候選對(duì)象中查找最接近xrand點(diǎn)的頂點(diǎn)。

2.2 成本函數(shù)

2.2.1目標(biāo)誤差

在實(shí)踐中，當(dāng)一根針反復(fù)插入介質(zhì)中時(shí)，會(huì)產(chǎn)生一組稍有不同的軌跡。因?yàn)榭紤]到柔性針系統(tǒng)的電機(jī)誤差和扭轉(zhuǎn)效應(yīng)等不確定性時(shí)，所以對(duì)具有給定輸入的路徑終點(diǎn)將形成概率分布，其平均值可能與目標(biāo)點(diǎn)不同。可以通過在角速度中添加噪聲參數(shù)為λ和單位高斯白噪聲為ω(t)的噪聲項(xiàng)ω(t)=ω0(t)+λω(t)來模擬隨機(jī)現(xiàn)象。λ可以用來確定采樣軌跡中擬投射的人工噪聲量。

該噪聲模型是SE(3)上的隨機(jī)微分方程，因此可以將有噪聲的非完整柔性針模型寫成：

當(dāng)柔性針插入軟組織時(shí)，由于柔性針系統(tǒng)的不確定性。將目標(biāo)誤差定義為從大量樣本到所需目標(biāo)的均方根(RMS)距離：

L′=trace(Σ)+(μx-xgoal)2+(μy-ygoal)2+(μz-zgoal)2

(6)

2.2.2障礙物接近度

當(dāng)柔性針插入人體進(jìn)行醫(yī)療操作時(shí)，需要避免血管、骨骼和關(guān)鍵神經(jīng)等部位。如果柔性針的規(guī)劃路徑過于靠近關(guān)鍵神經(jīng)或任何其他敏感區(qū)域時(shí)，這些部位可能因柔性針的隨機(jī)性而受到傷害。如圖2所示，在柔性針路徑中靠近障礙物(藍(lán)色球體)的點(diǎn)上存在概率分布[9]。假設(shè)針尖位置的概率密度函數(shù)可以被建模為一個(gè)高斯函數(shù)。為了柔性針能夠靈活地插入組織內(nèi)避免障礙物，所以與障礙物的接近應(yīng)該從概率的角度最小化。因此，應(yīng)計(jì)算并最小化觸碰到障礙物的概率。本文假設(shè)障礙物為球，qobs代表球的中心，robs代表球的半徑。一般高斯概率密度函數(shù)為：

式中：q是高斯(μq,Σ)隨機(jī)向量。

圖2 最終點(diǎn)和障礙物周圍的概率分布路徑

高斯分布撞擊障礙球(qobs,γobs)的概率為通過給定球體上的PDF積分計(jì)算，即：

2.3 優(yōu)化算法

將式(3)的指數(shù)積與式(4)的隨機(jī)微分方程相結(jié)合在擴(kuò)展函數(shù)中來考慮柔性針的隨機(jī)性。所需參數(shù)選擇為噪聲系數(shù)λ=0.04和Δt=1 s。為了計(jì)算平均值μ和協(xié)方差Σ，生成1 000個(gè)采樣路徑。使用數(shù)值方法來獲得μ和Σ[10]。優(yōu)化算法的偽代碼如算法3所示。

算法3優(yōu)化算法

1. c0←COST_FUNCTION(xinitial,xgoal,Qobs,uRRT,Δt,N)

2.fori=1:NUMBER_SEGMENT(uRRT)

3. utemp,i←CONTROL_CHANCE(uRRT,i)

4. ctemp←COST_FUNCTION(xinitial,xgoal,Qobs,utemp,Δt,N)

5. unew,i←MINIMUM(ctemp,c0)

6.end

7.returnPATH(xinitial,QobsQobs,unew,Δt)

圖3 迭代優(yōu)化

轉(zhuǎn)換定義為:

式中:gchange和ginitial分別是SE(3)中路徑終點(diǎn)的狀態(tài)，在分段優(yōu)化的每個(gè)循環(huán)期間，控制輸入和初始控制輸入都發(fā)生了變化。當(dāng)轉(zhuǎn)換x′=Rx+p時(shí)，協(xié)方差為Σ′=RΣRT。

因此，成本函數(shù)1寫為：

由式(10)可以看出在每次輸入更新后，無須再進(jìn)行一次采樣來計(jì)算平均μ和Σ協(xié)方差。只要有一個(gè)初始抽樣，所有即將到來的均值和協(xié)方差都可以從之前的推導(dǎo)中獲得。按照這些步驟，可以減少優(yōu)化的計(jì)算時(shí)間。

3 實(shí)驗(yàn)結(jié)果與分析

本文在MATLAB中進(jìn)行算法的仿真對(duì)比實(shí)驗(yàn)。在三維的環(huán)境下將基本RRT算法和多目標(biāo)粒子群優(yōu)化RRT的算法(算法1)與本文改進(jìn)的RRT算法(算法2)進(jìn)行對(duì)比，驗(yàn)證了本文改進(jìn)算法的優(yōu)越性和準(zhǔn)確性。

在下面的模擬中，考慮到了障礙物的接近度。新的總成本函數(shù)定義為目標(biāo)誤差(成本函數(shù)1)和障礙物接近度(成本函數(shù)2)的總和。式(8)用于計(jì)算障礙物碰撞的概率，即球面上高斯分布的積分。由于總成本函數(shù)由兩種類型的成本函數(shù)組成，因此根據(jù)規(guī)劃目標(biāo)，需要兩種成本函數(shù)的不同權(quán)重。使用比例因子α和β，將總成本函數(shù)定義為：

cost=αcost1+βcost2

(11)

通過改變比例因子α和β，可以調(diào)整目標(biāo)誤差和障礙物接近度的權(quán)重，即使在初始RRT確定性結(jié)果相同的情況下，也能給出不同的最優(yōu)路徑。

如圖4所示，粗線表示得到的路徑，圖4(d)中的細(xì)線表示算法的樹狀圖。優(yōu)化結(jié)果隨比例因子α和β的選擇而變化。當(dāng)β值較高時(shí)，障礙物的封閉性在優(yōu)化過程中起著更為重要的作用，并給出了一條遠(yuǎn)離障礙物的路徑。α值越大，目標(biāo)誤差對(duì)總成本的影響越大，這將使道路的終點(diǎn)更接近理想的目標(biāo)。當(dāng)比例因子β較大時(shí)，成本函數(shù)2對(duì)總成本降低的作用更大。當(dāng)比例因子α較大時(shí)，優(yōu)化算法會(huì)犧牲成本函數(shù)2以獲得較低的總成本函數(shù)。

圖4 本文的RRT算法在三維動(dòng)態(tài)環(huán)境下的路徑規(guī)劃

為了驗(yàn)證本文提出的基于成本函數(shù)優(yōu)化的柔性針RRT路徑規(guī)劃算法，在三維環(huán)境中放置了三個(gè)球形障礙物來模擬人體內(nèi)的環(huán)境。在這個(gè)三維環(huán)境中，進(jìn)行了多種算法的仿真對(duì)比，如圖5所示?？梢钥闯鏊惴?的路徑長度全面低于算法1和基本RRT算法，而且α=0.1,β=0.9時(shí)路徑的長度最小。不同比例因子下的成本函數(shù)的數(shù)值如表1-表3所示。算法2在目標(biāo)誤差和障礙物接近度方面優(yōu)于算法1和基本RRT算法。改變障礙物位置，在圖4(b)的環(huán)境下進(jìn)行20次仿真實(shí)驗(yàn)。從路徑總成本這個(gè)指標(biāo)將算法2與算法1進(jìn)行對(duì)比。

圖5 障礙物位置變化以后的柔性針比較

表1 α=0.1,β=0.9時(shí)的成本

表2 α=0.5, β=0.5時(shí)的成本

表3 α=0.9,β=0.1時(shí)的成本

初始環(huán)境下各算法的路徑長度對(duì)比圖如圖6所示。算法1和算法2的路徑總成本比較如圖7所示，虛線表示可用性值。結(jié)果表明算法2生成路徑的總成本基本低于算法1。而且在相同情況下，算法1產(chǎn)生40條成功路徑的概率為0.54，而算法2的概率為0.78。

圖6 初始環(huán)境的路徑長度對(duì)比圖

(a) α=0.1,β=0.9時(shí)的路徑總成本

(b) α=0.5,β=0.5時(shí)的路徑總成本

(c) α=0.9,β=0.1時(shí)的路徑總成本圖7 20次仿真路徑總成本對(duì)比圖

4 結(jié) 語

本文使用RRT算法在空間中生成一個(gè)具有避障功能的確定性可行路徑，然后對(duì)確定可行路徑進(jìn)行優(yōu)化。在優(yōu)化過程中，定義了一個(gè)成本函數(shù)來量化目標(biāo)誤差以及障礙物的接近程度。通過數(shù)值計(jì)算將上述成本函數(shù)最小化，減輕了隨機(jī)性對(duì)柔性針的影響，解決了柔性針的路徑規(guī)劃問題。