基于成本函數優化的柔性針RRT路徑規劃算法

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

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

0 引 言

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

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

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

1 柔性針運動模型

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

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

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

圖1 柔性針模型

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

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

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

最終，柔性針的運動模型可以推導成指數積的形式：

gi+1=gieξΔt

(3)

2 柔性針RRT路徑規劃

2.1 擴展函數

算法1中顯示了RRT算法的偽代碼。輸入元素包括初始狀態xinit、目標狀態xgoal、閾值半徑Rthreshold、障礙物配置Qobs、最大迭代次數κ和間隔距離Δ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中的擴展算法替換為擴展函數，如算法2所示。

算法2 擴展函數

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使用給定的參數xnear、xrand、umotion和Δt生成運動模型的幾個候選對象。生成xcandidate后，函數MIN-DISTANCE會在所有候選對象中查找最接近xrand點的頂點。

2.2 成本函數

2.2.1目標誤差

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

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

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

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

(6)

2.2.2障礙物接近度

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

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

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

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

2.3 優化算法

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

算法3優化算法

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 迭代優化

轉換定義為:

式中:gchange和ginitial分別是SE(3)中路徑終點的狀態，在分段優化的每個循環期間，控制輸入和初始控制輸入都發生了變化。當轉換x′=Rx+p時，協方差為Σ′=RΣRT。

因此，成本函數1寫為：

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

3 實驗結果與分析

本文在MATLAB中進行算法的仿真對比實驗。在三維的環境下將基本RRT算法和多目標粒子群優化RRT的算法(算法1)與本文改進的RRT算法(算法2)進行對比，驗證了本文改進算法的優越性和準確性。

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

cost=αcost1+βcost2

(11)

通過改變比例因子α和β，可以調整目標誤差和障礙物接近度的權重，即使在初始RRT確定性結果相同的情況下，也能給出不同的最優路徑。

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

圖4 本文的RRT算法在三維動態環境下的路徑規劃

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

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

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

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

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

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

圖6 初始環境的路徑長度對比圖

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

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

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

4 結 語

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