基于成本函數(shù)優(yōu)化的柔性針RRT路徑規(guī)劃算法

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

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

0 引 言

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

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

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

1 柔性針運動模型

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

彈性針的運動模型由式(1)給出：

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

圖1 柔性針模型

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

柔性針的非完整運動模型由下式給出：

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

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

gi+1=gieξΔt

(3)

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

2.1 擴展函數(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擴展到xnew的過程中，幾個原始運動作為控制輸入。算法1中步驟8和步驟9中的擴展算法替換為擴展函數(shù)，如算法2所示。

算法2 擴展函數(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生成運動模型的幾個候選對象。生成xcandidate后，函數(shù)MIN-DISTANCE會在所有候選對象中查找最接近xrand點的頂點。

2.2 成本函數(shù)

2.2.1目標(biāo)誤差

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

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

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

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

圖2 最終點和障礙物周圍的概率分布路徑

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

2.3 優(yōu)化算法

將式(3)的指數(shù)積與式(4)的隨機微分方程相結(jié)合在擴展函數(shù)中來考慮柔性針的隨機性。所需參數(shù)選擇為噪聲系數(shù)λ=0.04和Δt=1 s。為了計算平均值μ和協(xié)方差Σ，生成1 000個采樣路徑。使用數(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)中路徑終點的狀態(tài)，在分段優(yōu)化的每個循環(huán)期間，控制輸入和初始控制輸入都發(fā)生了變化。當(dāng)轉(zhuǎn)換x′=Rx+p時，協(xié)方差為Σ′=RΣRT。

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

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

3 實驗結(jié)果與分析

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

在下面的模擬中，考慮到了障礙物的接近度。新的總成本函數(shù)定義為目標(biāo)誤差(成本函數(shù)1)和障礙物接近度(成本函數(shù)2)的總和。式(8)用于計算障礙物碰撞的概率，即球面上高斯分布的積分。由于總成本函數(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)β值較高時，障礙物的封閉性在優(yōu)化過程中起著更為重要的作用，并給出了一條遠(yuǎn)離障礙物的路徑。α值越大，目標(biāo)誤差對總成本的影響越大，這將使道路的終點更接近理想的目標(biāo)。當(dāng)比例因子β較大時，成本函數(shù)2對總成本降低的作用更大。當(dāng)比例因子α較大時，優(yōu)化算法會犧牲成本函數(shù)2以獲得較低的總成本函數(shù)。

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

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

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

表1 α=0.1,β=0.9時的成本

表2 α=0.5, β=0.5時的成本

表3 α=0.9,β=0.1時的成本

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

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

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

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

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

4 結(jié) 語

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