周 麗，康劍靈，詹 潔，馮金陽

(東華大學(xué) 理學(xué)院，上海 201620)

柔性針在有障礙環(huán)境中的穿刺路徑規(guī)劃

周 麗，康劍靈，詹 潔，馮金陽

(東華大學(xué) 理學(xué)院，上海 201620)

斜尖柔性針相對組織有足夠的柔性，能夠穿刺出弧線軌跡，且能到達傳統(tǒng)鋼針不能到達的敏感區(qū)域. 在二維有障礙環(huán)境中，通過作圖找到能夠避開障礙的最優(yōu)路徑；在三維有障礙環(huán)境中，利用快速搜索隨機樹法找到所有可行路徑，再利用優(yōu)化目標(biāo)函數(shù)找到能避開障礙的最優(yōu)路徑. 仿真結(jié)果表明這兩種路徑規(guī)劃均能找到最優(yōu)路徑，驗證了規(guī)劃方法的有效性.

柔性針；路徑規(guī)劃；快速搜索隨機樹

微創(chuàng)醫(yī)療是近年來外科臨床醫(yī)學(xué)迅速發(fā)展起來的一門新技術(shù)，被廣泛應(yīng)用于活體組織病理檢查、局部組織藥物投送等手術(shù).與傳統(tǒng)手術(shù)的穿刺鋼針不同，斜尖柔性針[1](簡稱“柔性針”)相對組織具有足夠的柔性，可利用針軸的彎曲刺出不同的軌跡，同時能避開神經(jīng)、血管等障礙，靈活準(zhǔn)確地到達傳統(tǒng)鋼針達不到的靶點位置. 美國約翰斯霍普金斯大學(xué)的Webster等人在2004年最早提出柔性針概念，由于柔性針的路徑規(guī)劃研究是運動控制的基礎(chǔ)，對于準(zhǔn)確穿刺具有重要意義，因而成為當(dāng)前國內(nèi)外研究的熱點.

按照應(yīng)用場合的不同，柔性針路徑規(guī)劃可分為二維路徑規(guī)劃和三維路徑規(guī)劃；按算法柔性針路徑規(guī)劃大體可分為數(shù)值法、搜索法和反解法，其中，數(shù)值法是通過計算目標(biāo)函數(shù)的最值得到最優(yōu)路徑的方法，搜索法是根據(jù)路徑形式特點，利用計算機的人工智能搜索算法來搜索可行路徑，反解法是利用運動學(xué)反解來計算從入針點到靶點的可行路徑.

文獻[2]首次對柔性針進行二維路徑規(guī)劃，而文獻[3]首次把柔性針路徑規(guī)劃研究擴展到三維空間，后來又擴展到三維有障礙環(huán)境，文獻[4]采用DR(不同曲率的圓弧)混合型路徑對三維障礙環(huán)境進行規(guī)劃，文獻[5]采用R/S+L(多段定曲率的圓弧及直線)型混合路徑規(guī)劃二維障礙環(huán)境的路徑，文獻[6]利用穿刺針、靶向目標(biāo)與障礙物間的切線提出一種S(多段圓弧)型雙弧線障礙物規(guī)避路徑規(guī)劃算法，文獻[7]在三維有障礙環(huán)境中對柔性針穿刺進行螺旋形路徑規(guī)劃.

本文在剛性組織假設(shè)前提下，不考慮組織形變、針的變形等因素，首先討論柔性針在二維有障礙環(huán)境下的路徑規(guī)劃問題，利用入針點、障礙物及目標(biāo)點之間的位置關(guān)系，分別過入針點、目標(biāo)點做障礙圓的切線，提出一種R(弧線)型路徑規(guī)劃算法. 其次討論在三維有障礙環(huán)境下的路徑規(guī)劃問題，利用快速搜索隨機樹法[8]搜索可行路徑，考慮針尖偏差與路徑長度這兩個因素，建立優(yōu)化目標(biāo)函數(shù). 討論了優(yōu)化目標(biāo)函數(shù)中不同的權(quán)重值對應(yīng)的最優(yōu)路徑情況，如考慮手術(shù)中對組織的破壞性小這一因素，可選取針尖偏差盡可能大，從而減少穿刺時發(fā)生的碰撞造成對組織的破壞；若考慮減輕患者疼痛，可選取穿刺路徑長度盡可能短這一因素作為主要影響因素，通過選取合適的權(quán)重值找到最優(yōu)路徑. 最后通過增大障礙球的半徑進行障礙物規(guī)避.

1 柔性針運動學(xué)模型及穿刺路徑特點

1.1 運動學(xué)模型

柔性針相對組織具有足夠的柔性，穿刺時針軸受到組織的側(cè)向力會發(fā)生彎曲，穿刺出弧線軌跡. 柔性針一般有兩個自由度：一個是進給運動，實現(xiàn)針的穿刺；一個是自轉(zhuǎn)運動，改變針尖穿刺和針軸彎曲的方向.柔性針的運動形式近似看成一臺單輪車，如圖1所示，其中,u1為穿刺時的進給速度，u2為穿刺的自轉(zhuǎn)速度,r為穿刺時的圓弧半徑，ωx為穿刺時針尖沿x軸正向產(chǎn)生的角速度驅(qū)動，且ωx=u1/r.

圖1 單輪車運動學(xué)模型Fig.1 Unicycle kinematic model

在世界坐標(biāo)系A(chǔ)下，單輪車軸與針尖固聯(lián)，坐標(biāo)系B與單輪車軸固聯(lián),z軸沿軸線指向運動方向,y軸指向彎曲圓弧的外法線方向，由右手定則可判斷x軸方向.

1.2 穿刺路徑特點

柔性針在進給運動和自轉(zhuǎn)運動的協(xié)調(diào)控制下可以實現(xiàn)一定的空間位置和姿態(tài)，穿刺出獨特的路徑. 本文的路徑規(guī)劃問題研究中，假定組織是剛性的，忽略由于柔性針的刺入對組織環(huán)境的影響. 在解決二維路徑規(guī)劃之前，為了說明問題，首先給出控制度的概念.

控制度N：整個路徑規(guī)劃中需要控制不同路徑形式的次數(shù)，即包含的直線或圓弧的段數(shù).表1給出不同控制度下可能的路徑形式.

表1 不同控制度下的路徑形式Table 1 Different path forms under different control degrees

本文在有障礙二維環(huán)境中僅討論控制度為1的路徑規(guī)劃方法，即臨床上最易實現(xiàn)的R型路徑規(guī)劃.

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

2.1 二維有障礙環(huán)境的路徑規(guī)劃

在二維有障礙環(huán)境中，首先確定初始入針點和目標(biāo)點的位置，設(shè)定障礙，路徑規(guī)劃如圖2所示,P1和P2兩點分別是從入針點和目標(biāo)點向障礙圓所作切線的交點，求出過入針點、P1(或P2)和目標(biāo)點的圓的軌跡，此軌跡從入針點出發(fā)能夠到達目標(biāo)點且能避開障礙，從而得出兩條R型軌跡作為最終可行路徑.

圖2 路徑規(guī)劃簡圖

由于實際穿刺手術(shù)中障礙組織更復(fù)雜，所以這里設(shè)定兩個圓形障礙，障礙1是以坐標(biāo)(7,9)為圓心、2為半徑的圓，障礙2是以坐標(biāo)(14,10)為圓心、1.6為半徑的圓，入針點坐標(biāo)為(0,0)，目標(biāo)點坐標(biāo)為(20,22). 按照上述路徑規(guī)劃思想，利用Matlab求解計算并作圖，主要步驟如下：

(1) 在平面直角坐標(biāo)系中畫出障礙圓1和障礙圓2的圖像以及入針點和目標(biāo)點的位置；

(2) 分別計算過入針點到障礙圓1的兩條切線方程并在坐標(biāo)系中作出切線圖，計算目標(biāo)點到障礙圓1的兩條切線方程并在坐標(biāo)系中作出切線圖，記過入針點與障礙圓1的切線和過目標(biāo)點與障礙圓1的切線交點分別為P1和P2，如圖3所示；

(3) 計算P1和P2的坐標(biāo)，分別求出過入針點、P1(或P2)、目標(biāo)點的劣弧長度；

(4) 按照步驟(2)和(3)，同理作出過入針點到障礙圓2的兩條切線，以及目標(biāo)點到障礙圓2的兩條切線并作圖，記過入針點與障礙圓2的切線和過目標(biāo)點與障礙圓2的切線交點分別為P3和P4，如圖4所示；

(5) 計算P3和P4的坐標(biāo)，分別求出過入針點、P3(或P4)、目標(biāo)點的劣弧長度.

通過計算整理得出表2所示的過入針點坐標(biāo)(0,0)、切線交點Pi(i=1,2,3,4)、目標(biāo)點坐標(biāo)(20,22)的軌跡.

表2 過入針點和目標(biāo)點的4條可行軌跡Table 2 Four feasible paths connecting initial point and target point

由于柔性針在R型路徑規(guī)劃中有最小半徑的限制，這里假定R型軌跡最小半徑為50，則表2中滿足條件的只有軌跡R2和R3，又通過弧長判斷，弧長稍短的軌跡R2為控制度為1時的最優(yōu)路徑.圖5是4條可行路徑與障礙物的位置關(guān)系圖.

2.2 基于快速搜索隨機樹法的三維路徑規(guī)劃

在三維有障礙環(huán)境中，假定入針點坐標(biāo)為(0,0,0)，目標(biāo)點坐標(biāo)為(0,0,10)，障礙分別以坐標(biāo)(-0.5,0.5,2)、(0,0.5,5)、(0.7,0.1,7)為球心，半徑均為1的球體. 柔性針在三維環(huán)境中的路徑規(guī)劃問題，即為找到從入針點到目標(biāo)點且能避開障礙的可行路徑. 在三維環(huán)境中利用快速搜索隨機樹法找到可行路徑，快速搜索隨機樹法(RRTs)的思想：

給定初始位形qinit和目標(biāo)區(qū)域Qgoal(包含目標(biāo))，在初始位形qinit的周圍取隨機位形qrand，記其中距離Qgoal最近的qrand將其表示為qnear，若qnear與障礙物發(fā)生碰撞，則放棄生長，重新選取qnear，否則將qnear加入樹，再從qnear周圍選取新的隨機位形qrand，如此重復(fù)至最新的qnear在Qgoal的范圍內(nèi)，即找到可行路徑. 通過Matlab仿真得到如圖6所示的三維有障礙環(huán)境中的可行路徑.

為了在可行路徑中找到最優(yōu)路徑，一方面使得穿刺路徑最短(即對組織破壞性小、減少疼痛)，另一方面針尖距離障礙的間隙盡可能的大(防止因針的變形使得針與障礙發(fā)生碰撞)，建立如下優(yōu)化目標(biāo)函數(shù)[4]：

其中：Jο為針尖與最近障礙之間的距離；αl和α0為權(quán)重；T為柔性針穿刺時間. 通過選取適當(dāng)?shù)臋?quán)重αl和α0，使得上述優(yōu)化目標(biāo)函數(shù)值達到最小，即達到最優(yōu)路徑.

通過RRTs搜索到一系列可行路徑，隨機選取可行路徑當(dāng)中的5條路徑，計算出路徑長度∑li和針尖與最近障礙的距離和∑Jο的結(jié)果如表3所示.

選取不同的權(quán)重值αl和α0，計算相應(yīng)5條路徑的目標(biāo)函數(shù)值，如表4所示，其中Ji(i=1,2,3,4,5)為5種不同路徑的目標(biāo)函數(shù)值.

表4 不同權(quán)重值對應(yīng)的目標(biāo)函數(shù)值Table 4 The goal function value under different weights

由表4可知，若取αl=1.0,α0=0，即只考慮最短路徑，不考慮路徑與障礙之間的距離，此時第5條路徑對應(yīng)的目標(biāo)函數(shù)值最小，即第5條路徑為最優(yōu)路徑；若取αl=0,α0=1.0，即只考慮針尖與障礙之間的距離，不考慮最短路徑，此時第3條路徑對應(yīng)的目標(biāo)函數(shù)值最小，即第3條路徑為最優(yōu)路徑；分析上述5種路徑在不同權(quán)重值下的目標(biāo)函數(shù)值，只有第3條路徑在前面9種不同權(quán)重值的條件下，對應(yīng)的目標(biāo)函數(shù)值均最小，即總體而言第3條路徑較優(yōu).

在某些實際問題中，考慮到柔性針在穿刺過程中可能會發(fā)生變形等情況，這樣易使針尖與障礙物發(fā)生碰撞.為了避免這種情況的發(fā)生，在利用RRTs搜索可行路徑時，把上述半徑為1的小球，用半徑為1.5的球來回避(在實際情況下，可以根據(jù)各個組織不同的敏感性，設(shè)定不同的回避距離)，這樣可使可行路徑能夠遠離障礙物避免與其發(fā)生碰撞.

同樣如上述的三維有障礙路徑規(guī)劃實例，入針點與目標(biāo)點坐標(biāo)不變，只是將球形障礙的半徑變?yōu)?.5，其他條件相同，利用Matlab仿真搜索一系列可行路徑如圖7所示.

圖7 三維有障礙環(huán)境中考慮障礙規(guī)避的可行路徑Fig.7 Feasible paths in 3D environment while considering the avoidance of obstacles

從可行路徑中隨機選取5條路徑，計算出路徑長度∑li和針尖與最近障礙的距離和∑Jο的結(jié)果如表5所示. 將球形障礙的半徑增大后即已經(jīng)考慮遠離障礙物的情況，只需考慮路徑長度，由表5可知，路徑5對應(yīng)的路徑長度最短，即路徑5為最優(yōu)路徑.

表5 可行軌跡的路徑長度及與障礙之間的距離 Table 5 The distance of the feasible paths and the clearance from the obstacles

3 結(jié) 語

本文研究了柔性針在有障礙二維和三維環(huán)境中的路徑規(guī)劃問題.二維環(huán)境中通過入針點和目標(biāo)點向障礙圓作切線并計算切線交點，找到能夠避開障礙的最優(yōu)路徑；三維環(huán)境中利用快速搜索隨機樹法找到從入針點到目標(biāo)點且能避開障礙的可行路徑，再建立優(yōu)化目標(biāo)函數(shù)，考慮針尖偏差與路徑長度這兩個因素，通過權(quán)重值分析找到符合要求的最優(yōu)路徑. 仿真結(jié)果表明，這兩種規(guī)劃方法有效可行. 今后的研究中可以考慮組織形變等因素，以及在優(yōu)化目標(biāo)函數(shù)的建立時增加更多影響因素，從而提高穿刺精度.

[1] WEBSTER ш R J,KIM J S,COWAN N J,et al. Nonholonomic modeling of needle steering [J]. The International Journal of Robotics Research,2006,25(5/6): 509-525.

[2] ALTEROVITZ R,GOLDBERG K， OKAMURA A. Planning for steering bevel-tip needle insertion through 2D soft tissue with obstacles [C] // IEEE International Conference on Robotics and Automation. Piscataway,NJ,USA: IEEE,2005: 1652-1657.

[3] PARK W,KIM J S,ZHOU Y,et al. Diffusion-based motion planning for a nonholonomic flexible needle model [C] // IEEE International Conference on Robotics and Automation. Piscataway,NJ,USA: IEEE,2005: 4600-4605.

[4] PATIL S,ALTEROVITZ R. Interactivemotion planning for steerable needles in 3D environments with obstacles [C] // IEEE International Conference on Robotics and Automation. Piscataway,NJ,USA: IEEE,2010: 893-899.

[5] 張永德，趙燕江，陳浩.斜尖柔性針在軟組織中的二維路徑規(guī)劃[J].機器人,2011,33(6):750-757.

[6] 胡曉彤，曾超.靶向穿刺自動進針障礙物規(guī)避路徑規(guī)劃的研究[C] // 圖像圖形技術(shù)研究與應(yīng)用.2010:181-186.

[7] DUINDAM V,ALTEROVITZ R,SASTRY S,et al. Screw-baed motion planning for bevel-tip needles in 3D environments with obstacles [C] // IEEE International Conference on Robotics and Automation. Piscataway,NJ USA: IEEE,2008: 2483-2488.

[8] KUFFNER J J,LAVALLE S M. RRT-connect: An efficient approach to single-query path planning [C] // IEEE International Conference on Robotics and Automation. San Francisco,CA,2000: 995-1001.

Path Planning of Flexible Needles in the Environment with Obstacles

ZHOULi,KANGJian-ling,ZHANJie,FENGJin-yang

(College of Science,Donghua University,Shanghai 201620,China)

Flexible bevel-tip needles could move along curved trajectories within tissue,and could reach previously inaccessible targets while maneuvering around sensitive or impenetrable areas. In 2D environment with obstacles,an optimal path while avoiding obstacles was proposed by analysing and figuring. In 3D environment with obstacles,based on the Rapidly-Exploring Random Trees (RRTs) method,a set of feasible paths were found,then an optimal path which could avoid obstacles by minimizing the optimization criterion was given. Simulation results show that both of the two path planning methods could find the optimal paths,which validate the effectiveness of the path planning methods.

flexible needles； path planning； rapidly-exploring random trees

1671-0444(2015)01-0130-05

2013-10-29

國家自然科學(xué)基金資助項目(61104125)；上海曙光計劃資助項目(07SG38)；中央高?？蒲袠I(yè)務(wù)經(jīng)費資助項目(11D10911)

周 麗(1989—)，女，安徽安慶人，碩士研究生，研究方向為幾何控制理論.E-mail: lz7572@163.com

康劍靈(聯(lián)系人)，女，副教授，E-mail: kangjl@dhu.edu.cn

O 221.3

A