# Dynamic Path Planning for Bevel-Tip Flexible Needle Insertion into Soft Tissue Based on a Real-Time Finite Element Model

Authors: Murong Li; Dedong Gao; Yong Lei; Tian Xu
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

This paper presents a novel dynamic path planning methodology for needle steering into the soft tissue. A real-time finite element model is used to simulate the procedure of a flexible needle into the homogeneous soft tissue, which provides the dynamic deformation information for the path planning. The relationship between needle base and tip is formulated as the transformations of homogeneous matrix with quasi-static assumptions. Based on the reachability of the flexible needle, the real-time motions of obstacles and target are considered through the dynamic needle-tissue interactions. A testbed including a XY linear stage, one rotator, and a CCD camera is constructed, and the experiments are designed to validate the proposed method. The23G PTC needle was inserted into the PVA phantom with markers, and the CCD camera was utilized to record the needle trajectories and motions of target and obstacles. The targeting errors between the experimental and planned paths are less than 1.20mm, and the distance from the obstacle to needle is not smaller than 1.16mm. The results demonstrate that the proposed algorithm is effective for online planning the paths in the needle-tissue interactive environment.


---

## Body

## 1. Introduction

The needle insertion procedures are one of the most common minimally invasive medical procedures, which are widely used in biopsy, anaesthesia, and radiotherapy [1–4]. Needles are inserted deep into the focus area of different organs such as lung, liver, or prostate for diagnosis, localized therapeutic drug delivery, and sample removal from tissues. Sensitive parts such as the vessels should avoid contact with needles at the same time. During the interaction process, the interaction force between the needle and tissue would lead to the movement of obstacles and goals. And the needle can deviate from its intended path due to the unbalanced resistance force acting on the needle tip. Hence, this research proposed a dynamic path planning methodology that can steer and control the needle to get to the focus area while avoiding the vessels or other sensitive parts inside the organ.The needles used in the interventional procedures are classified into three types, that is, rigid needle, flexible needle, and soft needle [5]. In many diagnoses and treatment procedures, the soft and flexible needles are required to avoid obstacles or reach the targets in one insertion [6], which requires the needle possessing good flexibilities and steerability [7]. However, the needle-tissue interactions, which cause the obstacles and targets to move from their original position, contribute to the greater targeting challenges of the needle as the practice scenarios of needle insertions are more complicated. In the complicated scenarios, the percutaneous needles are required to hit the targets hidden by the anatomical obstacles or to get to multiple targets in different locations. Therefore, it is essential to research the needle steerability and path feasibility for improving the accuracy and successful rate of needle insertion operations.To improve the accuracy of needle insertions, the needle trajectory should be planned and updated according to the dynamic needle-tissue interactions. Needle path planning is to control the insertion parameters and guide the tip for avoiding the obstacles and reaching the targets [8–10], which reflects the dynamic procedure of needle-tissue interactions [11]. DiMaio et al. first addressed the concept of needle steering, modeled the domain of target and obstacles with the artificial potential field (APF), and planned the effective trajectories [8, 9]. Jiang et al. improved the APF algorithm with the conjugate gradient method, which can escape from a local minimal point [12], and the discrete potential field was used for path planning of surgical needles in 3D anatomical environments [13]. The virtual spring was employed to model the tissue properties by Glozman, and the path planning problem is converted into the optimization of lateral force [14]. However, the artificial potential field and virtual spring method did not account for the steerability of flexible needles.During insertion of the flexible needle, the tissue around the needle tip is compressed and the asymmetric resistance force against the compression due to the asymmetric needle tip can deflect the flexible needle, as shown in Figure1. The direction of bending is determined by the direction of the bevel-tip. Rotating the needle base would change the direction of the bevel-tip, which would change the direction of the asymmetric force and tip bending direction; hence, by rotating the needle base, different needle trajectories would be generated.Figure 1
Asymmetric resistance force against needle tip: (a) translation of needle base; (b) rotation of needle base.
(a)(b)Many groups have studied the steerability of bevel-tip needle. Webster’s experiments showed that the trajectory of flexible needles follows a circular arc under the ideal conditions [15]. Based on the phenomena, the nonholonomic unicycle and bicycle models were presented for describing the kinematics of the bevel-tip needles using the Lie group [16]. Reed et al. discussed the instant angle discrepancy between the base and the tip for the soft needle [17], but the time lag is better for flexible needles.Many path planning algorithms based on nonholonomic kinematic model have been proposed to obtain feasible needle trajectories [18–23]. Park et al. considered the probability of needle tip pose, which was approximated as the probability density function [18, 19]. The path planning problem is to solve the maximum probability of needle path [20]. Alterovitz et al. considered the uncertainties caused by the needle-tissue interactions [23]. The Markov decision model was used to depict the needle motion, and the dynamic programming is employed to obtain an optimal control sequence [22]. Alterovitz et al. also provided a targeting error model with Euclidean distance for optimizing the needle trajectories [10]. Duindam et al. presented the forward and inverse kinematic equations of flexible needle based on the nonholonomic model, and the null motion was employed to a three-dimensional needle path [21]. Wang et al. simplied Duindam’s work and proposed the stop-and-turn strategy to control the needle, which could decrease the number of needle rotations [24]. The computer searching methods were also applied in the needle path planning. The exhaustive method, Nelder–Mead downhill simplex method, and Haystack were used in the path planning by Hamzé, and the comparisons showed that Haystack is faster than others [25]. Abayazid et al. proposed a path planning algorithm with rapid exploring random trees (RRTs) to guide the needle tip in real time [26]. Vrooijnk et al. studied needle path planning problem in the nonstatic environment with two-dimensional ultrasound images, but the obstacles and target moved with constant speed [27]. Morimoto et al. reported that the image-based path planning was employed to design and fabricate the concentric tube robots [28]. Korayem et al. proposed the dynamic modeling of flexible arms which simultaneously take large motions and small deflections into account [29, 30], and based on the precise model of the general flexible mobile manipulator, the path planning approach of mobile elastic robotic arms is proposed, which points out the importance of dynamic effect in path planning [31]. In these studies, the motions of obstacles and targets caused by needle-tissue interactions were not considered for adjusting the needle trajectories.As it can be seen from the literature, various path planning algorithms have been developed for optimizing the needle trajectories. The traditional planning algorithm such as genetic, RRT, and annealing algorithm is in essence an optimization algorithm, which can only generate the optimal needle path in a rigid space. It is not sufficient and applicable to apply in the deformable environment because the tissue deformation cannot be considered. First, the targeting error involves two parts in the complicated environment with the moving targets and obstacles. One is the tip-target error, and the other is the needle-obstacle collision error. The two errors are affected by the needle-tissue interactions, which can lead to the offsets from their original locations of obstacles and targets inside the tissue. Second, the targeting error and the collisions with the obstacles cannot be predicted and corrected without a model-based control, even though there exist imaging devices in the needle insertion procedure. It is necessary to correct and compensate the insertion errors based on a predictive model-based control to ensure the convergence of a feasible and effective path in complicated circumstances.The aim of this paper is to develop a novel algorithm for planning a feasible trajectory in the dynamic needle-tissue interactive environment. Unlike the existing algorithms, the dynamic information of the targets and obstacles is predicted and applied to the path planning procedure, and they can facilitate to correct the needle insertion errors. The advantages of the proposed planning algorithm are as follows. The targeting error and obstacle collision error are both considered in the path planning algorithm. The dynamic needle-tissue interactions are as the feedback to guide and control the needle to avoid the obstacles and get to the target. The experiments are designed to validate the effectiveness of the proposed algorithm. This work can be used for developing virtual training system and assisting the surgeon to plan pre- and intraoperative paths.The rest of this paper is organized as follows: the problem of dynamic path planning is presented inSection 2; Section 3 provides the dynamic path planning with inverse kinematics, finite element prediction model, and general insertion error, followed by the algorithm for the dynamic path generation introduced in Section 4; the testbed setup is introduced; the experimental results are discussed in Section 5; and finally, the conclusions and future work are provided.


## 2. Problem Definition and Assumptions

In this paper, obstacles represent the sensitive parts such as the vessels that should avoid contact with needles. Goals represent the focus area for tissue extraction or drug delivery. The soft tissue is a deformable medium. Under the interaction of needles, the internal structure would move dynamically, and the needle would bend at the same time, which would lead to needle targeting errors and mitigate the effectiveness of diagnosis and therapy. The objective of the path planning for needle insertion is to find feasible paths between given insertion location and the moving target and obstacles, whose movement is caused by needle insertion actions. A complicated case for needle insertion is illustrated in Figure2.Figure 2
The complicated scenarios of flexible needle into soft tissue.In order to reach the targets inside the human body, the needle is deflected to avoid the obstacles (organs inside the body), such as the bones and vessels. It is necessary to plan a feasible path from the outside of the body to target inside the body.The path planning problem is described as follows. Given the initial pose of needle tipqstart, an initial pose of the target qtarget, and the initial poses of the obstacles qobstacle. Since the motions of target and obstacles need to be considered, the parameters of needle-tissue interaction are also provided. The procedure of needle insertion is divided into n time sequences. The outputs of the algorithm are the adjusting angles of all the needle segments, as shown in equation (2):(1)Φ=ϕ1,ϕ2,…,ϕn.The solution ofgnΦ satisfies the pose of changed target, as follows:(2)gnΦ=gqtargettn,and the paths should also avoid the pose of moved obstacles gqobstacletn.The assumption of this work is as follows. The procedure of needle insertion is composed of a series of quasi-static processes. The flexible needle moves in the dynamic environment, and the obstacles and the target move in the homogeneous, linear-elastic material of soft tissue. The motion of the needle with bevel angle tip is fully determined by the motion of needle tip, and the motion of needle tip is steered by adjusting the needle base. The experimental settings in our group for needle insertion into the phantom are at 1 mm/s, and there would be pause of 10 seconds after the rotation of the needle base. Hence, the lag between the needle base and needle tip can be compensated, and the angle of needle tip is fully controlled by the rotating the needle base.


## 3. Proposed Methodology

The schematic diagram of the proposed path planning algorithm is shown in Figure3. In clinical surgery, obstacles represent the sensitive parts such as the vessels that should avoid contact with needles. Goals represent the focus area for tissue extraction or drug delivery. The initial positions of obstacles and target can be predetermined by preoperative medical image. The dynamic path planning is converted as an optimal problem: the optimization objectives are to minimize the distance between the needle tip and the target and maximize the distance between the obstacle and needle trajectory. These two terms compose the general insertion error in artificial potential field (APF). The kinetic model that described the relationship between the needle base and needle tip and the needle-tissue interaction model that predicts the obstacles and goals’ movement are both required: the Denavit–Hartenberg method is used to describe the motion from needle base to the tip and formulate the forward and inverse kinematic equations of bevel-tip flexible needle. A real-time finite element (FE) method is employed to estimate the needle-tissue interactions, which are used to predict the displacements of the obstacles and target inside the tissue when planning the needle paths. The feasible paths from the current position of needle tip to moved target can be generated by the inverse kinematics. After each adjusting action, the new locations of target and obstacles are calculated by the FE model. Then, according to the initial location of tip and updated location of target, the sets of adjusting angle are generated by the inverse kinematic equation. The feasible paths provided by the kinematic equation are used for calculating the general insertion error. If the tip reaches the target, the optimal path is found. If not, calculate the new locations and repeat the above steps.Figure 3
Flowchart for dynamic path planning algorithm. denotes the direction of data flow, and denotes the model or method employed.


### 3.1. Kinematics Model of Flexible Needle

In this paper, we take into account the path planning of flexible needles in the dynamic environment, and the obstacles and target move in the linear-elastic material of soft tissue. It is assumed that the motion of the needle is fully determined by the motion of needle tip, and the motion of needle tip is steered by adjusting the needle base [32]. The lag between the needle base and needle tip is neglected, and the angle of needle tip is fully controlled by rotating the needle base [17, 21]. Under the previous assumptions, the motion of needle is determined by two control inputs: the insertion depth d and the rotation angle θ. The motion of needle insertion includes one translation and one rotation of needle base, which is shown in Figure 1.The translation of needle base transmitted to the needle tip is transformed into a combination of the translation along thez direction and the rotation about the y axis. Neglecting the needle torsion, the rotation of needle base is transformed into the rotation of needle tip, and ωtip≡ωbase [21].By analyzing the deflection of flexible, the needle deflection is independent of the depth of insertion. When the same type of needles is inserted into the same material of soft tissue, the needle deflection is a constant over the small insertion depth [32]. The two control inputs are reduced to one input, i.e., the angle of rotation ϕ. According to the quasi-static thinking, the needle can be decomposed into finite segments, and the insertion procedure can be taken as the combination of the segments’ insertions, shown in Figure 4.Figure 4
The motion from theith segment to the i+1th segment. At Oi, the needle base is rotated ϕi,i+1 angle about zi first and then is translated along zi.The generalized coordinate is chosen to describe the pose of needle segment, written as(3)q=q1T,q2T=x,y,z,ϕ,θ,ψT.The homogeneous matrixgq of body frame fixed at q is written as(4)giq=RqPq0T1∈SE3,where Pq=x,y,zT∈ℝ3 represents the relative position of the ith needle segment; Rq∈SO3 represents the orientation of the ith needle segment; and ϕ, θ, and ψ are the angles about X, Y, and Z axes in the reference frame. The initial body frame Ox0y0z0 is created on the needle base, and the end body frame Oxnynzn is created on the needle tip. Following the Denavit–Hartenberg method, the motion of flexible needles can be described by the transform of homogeneous matrix from the needle base to needle tip [32], written as follows:(5)gnΦ=g0,1ϕ1g1,2ϕ2⋯gi,i+1⋯gn−1,nϕn,where gi,i+1 denotes the homogeneous transform from the ith segment to the i+1th. Φ denotes the adjusting angle set of needle base. In the path planning algorithm, we formulate an expression to find an efficient adjusting angle set and construct a path from insertion location to the target.


### 3.2. Prediction Model for Needle-Tissue Interactions

The key to dynamic path planning is the real-time information of needle-tissue interactions, especially the movements of the target and obstacles. The finite element method can adopt the complex geometries and boundaries of linear and nonlinear soft tissue materials, and it is used for simulating the soft tissue deformation. In this paper, we consider the case that the flexible needle is inserted into the homogeneous, linear, and elastic soft tissue, which is modeled in the literature [33].In the FE model, the needle-tissue interactive procedure is decomposed into four different processes [33]. The stiffness force of soft tissue is acted on the needle tip in process 1. The needle enters into the tissue at a certain depth in process 2, and the needle’s displacement is constrained by the cutting and friction forces. The needle gets to the target and keeps its position in process 3. Finally, the needle is retreated from the tissue. After meshing the tissue model and initializing the boundaries and properties of needle and tissue, the calculation is implemented in Matlab. The iteration time for 1061×1061 2D grid mesh is 10ms on an Intel i7-3.5 GHz dual-core machine running Linux OS. The simulation can provide real-time information for path planning in the dynamic environment.The yield displacements of target and obstacles inside soft tissue are provided for updating the trajectories during a needle insertion procedure. For the sake of exposition, the displacement of tissue node from the FE model is defined as(6)u=ux2+uy2,where ux and uy are the components in the x and y directions. Through the FE model, the displacements of tissue node can be related to the parameters of needle insertion, and it is formulated as(7)u=fx0,y0,z0,mt,mn,v,α,where x0, y0, and z0 are the original coordination of the tissue node, for the two-dimensional case, z0=0. mt is the parameter of soft tissue, including its dimension, Young’s modulus, Poisson ratio, and areal density. mn is the parameter of needle material, including its Young’s modulus and moment of inertia. v is the insertion velocity, and α is the angle of needle tip. For illustrating the validation of FE simulation, 10 markers were embedded inside the phantom, as shown in Figure 5.Figure 5
Comparisons between the FE simulation and experimental results.∗ denotes the positions of the markers, and ○ denotes the positions of the markers.The errors between the simulation and experiment are calculated as the absolute error, written as(8)e=uSim−uExp,in which uSim and uExp are the displacements of simulation and experiment, respectively. The tissue nodes in Figure 5 are classified into three types, including the nodes constrained by the needle (nodes 1 and 3), the nodes close to the boundary (nodes 5 and 10), and the nodes far away from the needle (nodes 8 and 9). The errors of ten nodes in Figure 5 are calculated and listed in Table 1. In the table, e¯x and e¯y are the mean values of errors between the FE model and experimental results in the x and y directions, respectively. From the table, the errors of the nodes close to the needle are bigger than that of nodes far away from the needle, but the relative errors of the nodes close to the needle are smaller. The maximum value of the mean error between the simulated and experimental results is less than 0.50mm. The simulation results agree well with the experimental results, and the finite element mode can reflect the interactions between flexible needle and linear-elastic soft tissue. The prediction results through the FE model can provide the real-time information of targets and obstacles inside the tissue for the path planning algorithm.Table 1
Errors between the FE model and experimental results.
Markerse¯xe¯ye¯10.17mm0.37mm0.34mm20.23mm0.37mm0.35mm30.11mm0.31mm0.29mm40.21mm0.30mm0.27mm50.10mm0.19mm0.18mm60.08mm0.19mm0.19mm70.22mm0.17mm0.34mm80.27mm0.10mm0.22mm90.07mm0.10mm0.10mm100.08mm0.10mm0.10mm


### 3.3. General Insertion Error

The objective of path planning is to avoid the obstacle and reach the target. The Euclidean distance is employed to describe the error of needle insertion into soft tissue. Figure6 depicts the distances from needle body to the obstacle and from needle tip to the target.Figure 6
The errors for needle-tissue interactive procedure.In this figure,dont is the Euclidean distance between needle body and the obstacle at time t and dgtt is the Euclidean distance between needle tip and the target at time t. The distances dont and dgtt are formulated as(9)dgtt=xtt−xgtdonit=minxnt−xoit,where donit denotes the distance between flexible needle body and the ith obstacle at time t. xtt, xgt, and xoit are the positions of tip, target, and ith obstacle at time t. dgtt can be calculated by(10)dgt=xt−xg=xt−xgi+yt−ygj+zt−zgk.Since the turning points on the path can be obtained using the inverse kinematic equation, the interpolation is employed to smooth the needle trajectorySxn. The distance doni is calculated with Sxn and xoit.The artificial potential field is used to define the general insertion error as follows [34]:(11)εgtt=12κg⋅dgt2t,εont=12κo⋅∑i=1Noρi2t,where εgt is the targeting error and εon is the obstacle error. ρi is calculated with doni and the security distance d0, written as(12)ρi=1doni−1d0,doni≤d0,0,doni>d0.Equation (11) creates the attractive field with the target and the repulsive field with the obstacles. κgandκo are the factors of attractive and repulsive fields. No is the number of obstacles in the needle-tissue environment. The general insertion error εt is written as follows:(13)εt=εgtt+εont.The needle tip can move along the direction that the potential energy is the minimum; meanwhile, the needle body can avoid the obstacles.


### 3.4. Dynamic Path Planning with Inverse Kinematics

Planning the feasible paths is to solve the inverse kinematics of flexible needles. The initial insertion location needs to be determined considering the reachability. The initial insertion pose can be derived from equation (5) with the targeting pose, which is written as(14)g0,1=gnΦgn−1,n−1gn−2,n−1−1,…,g1,2−1.The needle insertion is an inverse procedure, and the reachable zone for the tip can help the surgeon to determine the insertion location when the target is given by the medical images. The feasible insertion locations can be calculated with the inverse kinematic equation [32]. Considering the anatomical structures around the target, the surgeon can determine an insertion location which can avoid the obstacles as much as possible.Considering the displacements of the target and obstacles during the procedure of needle insertion, we employ the finite element model to describe the needle-tissue interactions and predict their displacements in real time. The general insertion error is defined with concept of artificial potential field (APF), and the dynamic path planning is converted as an optimal problem of general insertion error.It is necessary to understand the real-time information of target and obstacles for the dynamic path planning. The needle-tissue procedure is divided inton continuous time sequence, and the corresponding adjusting angle is written as follows:(15)t=t0,t1,t2,…,tn,Φ=ϕ0,ϕ1,ϕ2,…,ϕnϕ0=0°.Substituting the adjusting setΦ into the kinematic model, the position of needle tip can be calculated in the time sequence. The proposed real-time FE model is used to calculate the dynamic positions of target and each obstacle in the time sequence t, which is written as(16)xg=xgt1,xgt2,…,xgtn,xoj=xojt1,xojt2,…,xojtn,j=1,2,…,No.The general insertion errorε in the time sequence t can be obtained with the dynamic information of needle tip, target, obstacles, and needle trajectory. Since there exist many or no feasible paths to target with the avoidance of obstacles, an optimal trajectory is the one with a minimal insertion error. The dynamic path planning is converted as an optimal problem, which is written as(17)minεt1,εt2,…,εtnS.t.gb,nΦ−gx⟶gtn≤εΦ=ϕ1,ϕ2,…,ϕn−π≤ϕi≤π,i=1,2,…,n,where ϵ is an allowable small offset. According to the flowchart in Figure 3, the parameters are initialized firstly, including the properties of needle and tissue. The parameters of needle involve needle length L, bevel angle of needle tip α, needle deflection ω, and rotation angle θ. The insertion information is required, including initial insertion location x⟶tt0, the target position x⟶gt0, and locations of obstacles xot0 at t0. The security distance between needle and obstacles d0, the attractive factor κg, and repulsive factor κo should also be initialized. If equation (17) is satisfied, the planned program follows YES or turns to NO path.


#### 3.4.1. Online Rolling Prediction

In the optimization of feasible paths with equation (17), the information of target and obstacles should be updated in each iterative steps. The dynamic information of target and obstacles is used to calculate the general insertion error in the current iterative step, and the updated information predicted by the FE model is used for the next iterative step. The online rolling method is used to update the prediction information, as shown in Figure 7.Figure 7
Iterative scheme of dynamic path generation.The adjusting angleϕk−1 in the previous step is the input of the current kth iterative step. ϕk−1 is substituted into the kinematic equation, and the location of needle tip xttk at the k step is calculated. The set of adjusting angles Φk is obtained with the future location of the target xgtk+1 and xttk based on the inverse kinematics. The future locations of the obstacles xotk+1 are employed to update the general insertion error εtk. The candidate adjusting angle for the k step is selected by minimizing the general insertion errors. Repeating the above steps, the current adjusting angle ϕk is the input of next planning step until the needle tip gets to the target.The detail planning process is shown as follows: first, run the FE simulation algorithm to calculate the position of targetxgt1 and the locations of obstacles xot1 at time t1. According to the initial location of needle tip xtt0 and xgt1, the M feasible paths can be calculated with the static path planning algorithm and the candidate set of adjusting angle is Φ1=ϕ11,ϕ12,…,ϕ1M. Choosing the minimal general insertion error is to obtain an optimal adjusting angle ϕ1. The specific update procedure of the needle tip is shown in Figure 8.Figure 8
Diagram of dynamic update procedure of optimal needle trajectories.In the figure,Φ1=ϕ11,ϕ12,…,ϕ1nt1 is the set of adjusting angle, by which the needle tip gets to the position xgt1 at t1. The green curve denotes the feasible trajectories to the new location xtt1 of needle tip at t1, and green sphere denotes the location of needle tip at t1. The purple curve denotes the feasible trajectories from xtt1 to xtt2 at t2, and purple sphere is the location of needle tip at t2.The new location of needle tipxtt1 is calculated with the planned adjusting angle ϕ1. Then, the new locations of target xgt2 and locations xot2 are updated with the FE model. The M feasible trajectories from new tip location xtt1 to next target position xgt2 are updated with static path planning algorithm. The planned needle is decomposed into nt2 segments, and the planned set of adjusting angle is calculated as Φ2=ϕ21,ϕ22,…,ϕ2M. The optimal set of adjusting angle is obtained with equation (17), and the second set of adjusting angle is obtained as ϕ2. For the time ti, the candidate set Φi of adjusting angle is calculated with the tip location xtti at ti and the target location xgti+1 at ti+1, and the adjusting angle is planned with the optimal general insertion error as ϕi∈Φi. Repeat the above steps, the optimal set adjusting angle and its corresponding set of segment length are written as(18)Φ=ϕ1,ϕ2,…,ϕn,Lseg=l1,l2,…,ln.


#### 3.4.2. Updating the Number of Needle Segment

In the path planning, the length of needle segment determined the target reachability and obstacle avoidance of needle tip. For the given initial insertion location and target position, the longer needle segment can get to the target with few rotations, which facilitates the operations of physicians and alleviates patient’s suffering. However, the long needle segment cannot construct the complicated trajectories with the obstacle avoidance. Therefore, we select the longer segments to generate the feasible paths. If the longer segments fail, we select the shorter segments to generate the feasible paths. For thekth planning step, the planned needle length is written as(19)Lk=L−∑i=1k−1li.First, we selectntk0 as the number of needle segments. If the planned paths can satisfy the general insertion error, the number of needle segments is chosen as ntk=ntk0. If not, the number of needle segments is chosen as ntk=ntkj=2j×ntk0 through verifying the general insertion error.Because the needle path is limited to the radius and depth of needle insertion, it is possible to fail to plan the feasible paths. Increasing the number of needle segments infinitely will lead to the endless loop of iterative calculations. Therefore, the maximum number of needle segments is set in the algorithm; if the number of needle segments is bigger than 50, the program will stop calculating. For the clinical practice, the surgeons or robot cannot operate too many segments. In the path planning algorithm, the segment’s length of initial planning step is bigger than that of later planning step, i.e.,lk−1≥lk. At the beginning of path planning algorithm, the initial segment’s length is given. The segment’s length of previous step lk−1 is selected as the initial value of segment’s length for the k middle step. The number of needle segments is dynamic, not static for the path planning algorithm.


#### 3.4.3. Convergence and Error Analysis

In the optimization of feasible paths, finding the minimum general insertion error is to search the zone with minimum potential energy. The local minimum of APF may lead to the failure of optimization and reaching the target [12]. For avoiding the local minimum of APF, finite initial values are given to calculate the sets of adjusting angles, in which the optimized path is searched from the finite feasible paths. The finite sets of inverse kinematic solution guarantees the convergence of optimization procedure. Because we do not search the optimized paths in the whole space, the optimized path is locally an optimal solution.In order to obtain the optimal solution in the whole space, we select the uniformly distributed initial values to generate the feasible paths, as follows:(20)ϕ0j=0°,j−1×Δϕ,…,180°T,j=1,2,…,M.Figure9 shows an example of feasible paths filling the whole space.Figure 9
Feasible paths filling the whole space with the uniformly distributed initial values. Starting point is0cm,0cm,0cm, the goal is 0.8cm,1.2cm,9.8cmΔϕ=18°, and the number of segments is M=21.From the figure, the uniformly distributed initial value can generate feasible paths filling the whole space, from which the optimal path is searched to guarantee the global optimization. If more optimal solution is required, the smaller intervalΔϕ is needed to generate the finer paths. In the path planning algorithm, we select M=21 as the number of needle segments, which can search the optimal path with less computation.The proposed algorithm does not include the feedback from the real-time information, which could lead to the error accumulation from the prediction models. According to the kinematics of flexible, the prediction error of needle tip is about±0.70mm. The prediction error of target inside tissue with the finite element model is about ±1.00mm. Therefore, the needle tip may displace off the moved target, and the error is approximated as ±1.70mm. It is inevitable to eliminate the errors originated from the prediction models, so the real-time imaging information should be used for adjusting the paths online.


## 3.1. Kinematics Model of Flexible Needle

In this paper, we take into account the path planning of flexible needles in the dynamic environment, and the obstacles and target move in the linear-elastic material of soft tissue. It is assumed that the motion of the needle is fully determined by the motion of needle tip, and the motion of needle tip is steered by adjusting the needle base [32]. The lag between the needle base and needle tip is neglected, and the angle of needle tip is fully controlled by rotating the needle base [17, 21]. Under the previous assumptions, the motion of needle is determined by two control inputs: the insertion depth d and the rotation angle θ. The motion of needle insertion includes one translation and one rotation of needle base, which is shown in Figure 1.The translation of needle base transmitted to the needle tip is transformed into a combination of the translation along thez direction and the rotation about the y axis. Neglecting the needle torsion, the rotation of needle base is transformed into the rotation of needle tip, and ωtip≡ωbase [21].By analyzing the deflection of flexible, the needle deflection is independent of the depth of insertion. When the same type of needles is inserted into the same material of soft tissue, the needle deflection is a constant over the small insertion depth [32]. The two control inputs are reduced to one input, i.e., the angle of rotation ϕ. According to the quasi-static thinking, the needle can be decomposed into finite segments, and the insertion procedure can be taken as the combination of the segments’ insertions, shown in Figure 4.Figure 4
The motion from theith segment to the i+1th segment. At Oi, the needle base is rotated ϕi,i+1 angle about zi first and then is translated along zi.The generalized coordinate is chosen to describe the pose of needle segment, written as(3)q=q1T,q2T=x,y,z,ϕ,θ,ψT.The homogeneous matrixgq of body frame fixed at q is written as(4)giq=RqPq0T1∈SE3,where Pq=x,y,zT∈ℝ3 represents the relative position of the ith needle segment; Rq∈SO3 represents the orientation of the ith needle segment; and ϕ, θ, and ψ are the angles about X, Y, and Z axes in the reference frame. The initial body frame Ox0y0z0 is created on the needle base, and the end body frame Oxnynzn is created on the needle tip. Following the Denavit–Hartenberg method, the motion of flexible needles can be described by the transform of homogeneous matrix from the needle base to needle tip [32], written as follows:(5)gnΦ=g0,1ϕ1g1,2ϕ2⋯gi,i+1⋯gn−1,nϕn,where gi,i+1 denotes the homogeneous transform from the ith segment to the i+1th. Φ denotes the adjusting angle set of needle base. In the path planning algorithm, we formulate an expression to find an efficient adjusting angle set and construct a path from insertion location to the target.


## 3.2. Prediction Model for Needle-Tissue Interactions

The key to dynamic path planning is the real-time information of needle-tissue interactions, especially the movements of the target and obstacles. The finite element method can adopt the complex geometries and boundaries of linear and nonlinear soft tissue materials, and it is used for simulating the soft tissue deformation. In this paper, we consider the case that the flexible needle is inserted into the homogeneous, linear, and elastic soft tissue, which is modeled in the literature [33].In the FE model, the needle-tissue interactive procedure is decomposed into four different processes [33]. The stiffness force of soft tissue is acted on the needle tip in process 1. The needle enters into the tissue at a certain depth in process 2, and the needle’s displacement is constrained by the cutting and friction forces. The needle gets to the target and keeps its position in process 3. Finally, the needle is retreated from the tissue. After meshing the tissue model and initializing the boundaries and properties of needle and tissue, the calculation is implemented in Matlab. The iteration time for 1061×1061 2D grid mesh is 10ms on an Intel i7-3.5 GHz dual-core machine running Linux OS. The simulation can provide real-time information for path planning in the dynamic environment.The yield displacements of target and obstacles inside soft tissue are provided for updating the trajectories during a needle insertion procedure. For the sake of exposition, the displacement of tissue node from the FE model is defined as(6)u=ux2+uy2,where ux and uy are the components in the x and y directions. Through the FE model, the displacements of tissue node can be related to the parameters of needle insertion, and it is formulated as(7)u=fx0,y0,z0,mt,mn,v,α,where x0, y0, and z0 are the original coordination of the tissue node, for the two-dimensional case, z0=0. mt is the parameter of soft tissue, including its dimension, Young’s modulus, Poisson ratio, and areal density. mn is the parameter of needle material, including its Young’s modulus and moment of inertia. v is the insertion velocity, and α is the angle of needle tip. For illustrating the validation of FE simulation, 10 markers were embedded inside the phantom, as shown in Figure 5.Figure 5
Comparisons between the FE simulation and experimental results.∗ denotes the positions of the markers, and ○ denotes the positions of the markers.The errors between the simulation and experiment are calculated as the absolute error, written as(8)e=uSim−uExp,in which uSim and uExp are the displacements of simulation and experiment, respectively. The tissue nodes in Figure 5 are classified into three types, including the nodes constrained by the needle (nodes 1 and 3), the nodes close to the boundary (nodes 5 and 10), and the nodes far away from the needle (nodes 8 and 9). The errors of ten nodes in Figure 5 are calculated and listed in Table 1. In the table, e¯x and e¯y are the mean values of errors between the FE model and experimental results in the x and y directions, respectively. From the table, the errors of the nodes close to the needle are bigger than that of nodes far away from the needle, but the relative errors of the nodes close to the needle are smaller. The maximum value of the mean error between the simulated and experimental results is less than 0.50mm. The simulation results agree well with the experimental results, and the finite element mode can reflect the interactions between flexible needle and linear-elastic soft tissue. The prediction results through the FE model can provide the real-time information of targets and obstacles inside the tissue for the path planning algorithm.Table 1
Errors between the FE model and experimental results.
Markerse¯xe¯ye¯10.17mm0.37mm0.34mm20.23mm0.37mm0.35mm30.11mm0.31mm0.29mm40.21mm0.30mm0.27mm50.10mm0.19mm0.18mm60.08mm0.19mm0.19mm70.22mm0.17mm0.34mm80.27mm0.10mm0.22mm90.07mm0.10mm0.10mm100.08mm0.10mm0.10mm


## 3.3. General Insertion Error

The objective of path planning is to avoid the obstacle and reach the target. The Euclidean distance is employed to describe the error of needle insertion into soft tissue. Figure6 depicts the distances from needle body to the obstacle and from needle tip to the target.Figure 6
The errors for needle-tissue interactive procedure.In this figure,dont is the Euclidean distance between needle body and the obstacle at time t and dgtt is the Euclidean distance between needle tip and the target at time t. The distances dont and dgtt are formulated as(9)dgtt=xtt−xgtdonit=minxnt−xoit,where donit denotes the distance between flexible needle body and the ith obstacle at time t. xtt, xgt, and xoit are the positions of tip, target, and ith obstacle at time t. dgtt can be calculated by(10)dgt=xt−xg=xt−xgi+yt−ygj+zt−zgk.Since the turning points on the path can be obtained using the inverse kinematic equation, the interpolation is employed to smooth the needle trajectorySxn. The distance doni is calculated with Sxn and xoit.The artificial potential field is used to define the general insertion error as follows [34]:(11)εgtt=12κg⋅dgt2t,εont=12κo⋅∑i=1Noρi2t,where εgt is the targeting error and εon is the obstacle error. ρi is calculated with doni and the security distance d0, written as(12)ρi=1doni−1d0,doni≤d0,0,doni>d0.Equation (11) creates the attractive field with the target and the repulsive field with the obstacles. κgandκo are the factors of attractive and repulsive fields. No is the number of obstacles in the needle-tissue environment. The general insertion error εt is written as follows:(13)εt=εgtt+εont.The needle tip can move along the direction that the potential energy is the minimum; meanwhile, the needle body can avoid the obstacles.


## 3.4. Dynamic Path Planning with Inverse Kinematics

Planning the feasible paths is to solve the inverse kinematics of flexible needles. The initial insertion location needs to be determined considering the reachability. The initial insertion pose can be derived from equation (5) with the targeting pose, which is written as(14)g0,1=gnΦgn−1,n−1gn−2,n−1−1,…,g1,2−1.The needle insertion is an inverse procedure, and the reachable zone for the tip can help the surgeon to determine the insertion location when the target is given by the medical images. The feasible insertion locations can be calculated with the inverse kinematic equation [32]. Considering the anatomical structures around the target, the surgeon can determine an insertion location which can avoid the obstacles as much as possible.Considering the displacements of the target and obstacles during the procedure of needle insertion, we employ the finite element model to describe the needle-tissue interactions and predict their displacements in real time. The general insertion error is defined with concept of artificial potential field (APF), and the dynamic path planning is converted as an optimal problem of general insertion error.It is necessary to understand the real-time information of target and obstacles for the dynamic path planning. The needle-tissue procedure is divided inton continuous time sequence, and the corresponding adjusting angle is written as follows:(15)t=t0,t1,t2,…,tn,Φ=ϕ0,ϕ1,ϕ2,…,ϕnϕ0=0°.Substituting the adjusting setΦ into the kinematic model, the position of needle tip can be calculated in the time sequence. The proposed real-time FE model is used to calculate the dynamic positions of target and each obstacle in the time sequence t, which is written as(16)xg=xgt1,xgt2,…,xgtn,xoj=xojt1,xojt2,…,xojtn,j=1,2,…,No.The general insertion errorε in the time sequence t can be obtained with the dynamic information of needle tip, target, obstacles, and needle trajectory. Since there exist many or no feasible paths to target with the avoidance of obstacles, an optimal trajectory is the one with a minimal insertion error. The dynamic path planning is converted as an optimal problem, which is written as(17)minεt1,εt2,…,εtnS.t.gb,nΦ−gx⟶gtn≤εΦ=ϕ1,ϕ2,…,ϕn−π≤ϕi≤π,i=1,2,…,n,where ϵ is an allowable small offset. According to the flowchart in Figure 3, the parameters are initialized firstly, including the properties of needle and tissue. The parameters of needle involve needle length L, bevel angle of needle tip α, needle deflection ω, and rotation angle θ. The insertion information is required, including initial insertion location x⟶tt0, the target position x⟶gt0, and locations of obstacles xot0 at t0. The security distance between needle and obstacles d0, the attractive factor κg, and repulsive factor κo should also be initialized. If equation (17) is satisfied, the planned program follows YES or turns to NO path.


### 3.4.1. Online Rolling Prediction

In the optimization of feasible paths with equation (17), the information of target and obstacles should be updated in each iterative steps. The dynamic information of target and obstacles is used to calculate the general insertion error in the current iterative step, and the updated information predicted by the FE model is used for the next iterative step. The online rolling method is used to update the prediction information, as shown in Figure 7.Figure 7
Iterative scheme of dynamic path generation.The adjusting angleϕk−1 in the previous step is the input of the current kth iterative step. ϕk−1 is substituted into the kinematic equation, and the location of needle tip xttk at the k step is calculated. The set of adjusting angles Φk is obtained with the future location of the target xgtk+1 and xttk based on the inverse kinematics. The future locations of the obstacles xotk+1 are employed to update the general insertion error εtk. The candidate adjusting angle for the k step is selected by minimizing the general insertion errors. Repeating the above steps, the current adjusting angle ϕk is the input of next planning step until the needle tip gets to the target.The detail planning process is shown as follows: first, run the FE simulation algorithm to calculate the position of targetxgt1 and the locations of obstacles xot1 at time t1. According to the initial location of needle tip xtt0 and xgt1, the M feasible paths can be calculated with the static path planning algorithm and the candidate set of adjusting angle is Φ1=ϕ11,ϕ12,…,ϕ1M. Choosing the minimal general insertion error is to obtain an optimal adjusting angle ϕ1. The specific update procedure of the needle tip is shown in Figure 8.Figure 8
Diagram of dynamic update procedure of optimal needle trajectories.In the figure,Φ1=ϕ11,ϕ12,…,ϕ1nt1 is the set of adjusting angle, by which the needle tip gets to the position xgt1 at t1. The green curve denotes the feasible trajectories to the new location xtt1 of needle tip at t1, and green sphere denotes the location of needle tip at t1. The purple curve denotes the feasible trajectories from xtt1 to xtt2 at t2, and purple sphere is the location of needle tip at t2.The new location of needle tipxtt1 is calculated with the planned adjusting angle ϕ1. Then, the new locations of target xgt2 and locations xot2 are updated with the FE model. The M feasible trajectories from new tip location xtt1 to next target position xgt2 are updated with static path planning algorithm. The planned needle is decomposed into nt2 segments, and the planned set of adjusting angle is calculated as Φ2=ϕ21,ϕ22,…,ϕ2M. The optimal set of adjusting angle is obtained with equation (17), and the second set of adjusting angle is obtained as ϕ2. For the time ti, the candidate set Φi of adjusting angle is calculated with the tip location xtti at ti and the target location xgti+1 at ti+1, and the adjusting angle is planned with the optimal general insertion error as ϕi∈Φi. Repeat the above steps, the optimal set adjusting angle and its corresponding set of segment length are written as(18)Φ=ϕ1,ϕ2,…,ϕn,Lseg=l1,l2,…,ln.


### 3.4.2. Updating the Number of Needle Segment

In the path planning, the length of needle segment determined the target reachability and obstacle avoidance of needle tip. For the given initial insertion location and target position, the longer needle segment can get to the target with few rotations, which facilitates the operations of physicians and alleviates patient’s suffering. However, the long needle segment cannot construct the complicated trajectories with the obstacle avoidance. Therefore, we select the longer segments to generate the feasible paths. If the longer segments fail, we select the shorter segments to generate the feasible paths. For thekth planning step, the planned needle length is written as(19)Lk=L−∑i=1k−1li.First, we selectntk0 as the number of needle segments. If the planned paths can satisfy the general insertion error, the number of needle segments is chosen as ntk=ntk0. If not, the number of needle segments is chosen as ntk=ntkj=2j×ntk0 through verifying the general insertion error.Because the needle path is limited to the radius and depth of needle insertion, it is possible to fail to plan the feasible paths. Increasing the number of needle segments infinitely will lead to the endless loop of iterative calculations. Therefore, the maximum number of needle segments is set in the algorithm; if the number of needle segments is bigger than 50, the program will stop calculating. For the clinical practice, the surgeons or robot cannot operate too many segments. In the path planning algorithm, the segment’s length of initial planning step is bigger than that of later planning step, i.e.,lk−1≥lk. At the beginning of path planning algorithm, the initial segment’s length is given. The segment’s length of previous step lk−1 is selected as the initial value of segment’s length for the k middle step. The number of needle segments is dynamic, not static for the path planning algorithm.


### 3.4.3. Convergence and Error Analysis

In the optimization of feasible paths, finding the minimum general insertion error is to search the zone with minimum potential energy. The local minimum of APF may lead to the failure of optimization and reaching the target [12]. For avoiding the local minimum of APF, finite initial values are given to calculate the sets of adjusting angles, in which the optimized path is searched from the finite feasible paths. The finite sets of inverse kinematic solution guarantees the convergence of optimization procedure. Because we do not search the optimized paths in the whole space, the optimized path is locally an optimal solution.In order to obtain the optimal solution in the whole space, we select the uniformly distributed initial values to generate the feasible paths, as follows:(20)ϕ0j=0°,j−1×Δϕ,…,180°T,j=1,2,…,M.Figure9 shows an example of feasible paths filling the whole space.Figure 9
Feasible paths filling the whole space with the uniformly distributed initial values. Starting point is0cm,0cm,0cm, the goal is 0.8cm,1.2cm,9.8cmΔϕ=18°, and the number of segments is M=21.From the figure, the uniformly distributed initial value can generate feasible paths filling the whole space, from which the optimal path is searched to guarantee the global optimization. If more optimal solution is required, the smaller intervalΔϕ is needed to generate the finer paths. In the path planning algorithm, we select M=21 as the number of needle segments, which can search the optimal path with less computation.The proposed algorithm does not include the feedback from the real-time information, which could lead to the error accumulation from the prediction models. According to the kinematics of flexible, the prediction error of needle tip is about±0.70mm. The prediction error of target inside tissue with the finite element model is about ±1.00mm. Therefore, the needle tip may displace off the moved target, and the error is approximated as ±1.70mm. It is inevitable to eliminate the errors originated from the prediction models, so the real-time imaging information should be used for adjusting the paths online.


## 3.4.1. Online Rolling Prediction

In the optimization of feasible paths with equation (17), the information of target and obstacles should be updated in each iterative steps. The dynamic information of target and obstacles is used to calculate the general insertion error in the current iterative step, and the updated information predicted by the FE model is used for the next iterative step. The online rolling method is used to update the prediction information, as shown in Figure 7.Figure 7
Iterative scheme of dynamic path generation.The adjusting angleϕk−1 in the previous step is the input of the current kth iterative step. ϕk−1 is substituted into the kinematic equation, and the location of needle tip xttk at the k step is calculated. The set of adjusting angles Φk is obtained with the future location of the target xgtk+1 and xttk based on the inverse kinematics. The future locations of the obstacles xotk+1 are employed to update the general insertion error εtk. The candidate adjusting angle for the k step is selected by minimizing the general insertion errors. Repeating the above steps, the current adjusting angle ϕk is the input of next planning step until the needle tip gets to the target.The detail planning process is shown as follows: first, run the FE simulation algorithm to calculate the position of targetxgt1 and the locations of obstacles xot1 at time t1. According to the initial location of needle tip xtt0 and xgt1, the M feasible paths can be calculated with the static path planning algorithm and the candidate set of adjusting angle is Φ1=ϕ11,ϕ12,…,ϕ1M. Choosing the minimal general insertion error is to obtain an optimal adjusting angle ϕ1. The specific update procedure of the needle tip is shown in Figure 8.Figure 8
Diagram of dynamic update procedure of optimal needle trajectories.In the figure,Φ1=ϕ11,ϕ12,…,ϕ1nt1 is the set of adjusting angle, by which the needle tip gets to the position xgt1 at t1. The green curve denotes the feasible trajectories to the new location xtt1 of needle tip at t1, and green sphere denotes the location of needle tip at t1. The purple curve denotes the feasible trajectories from xtt1 to xtt2 at t2, and purple sphere is the location of needle tip at t2.The new location of needle tipxtt1 is calculated with the planned adjusting angle ϕ1. Then, the new locations of target xgt2 and locations xot2 are updated with the FE model. The M feasible trajectories from new tip location xtt1 to next target position xgt2 are updated with static path planning algorithm. The planned needle is decomposed into nt2 segments, and the planned set of adjusting angle is calculated as Φ2=ϕ21,ϕ22,…,ϕ2M. The optimal set of adjusting angle is obtained with equation (17), and the second set of adjusting angle is obtained as ϕ2. For the time ti, the candidate set Φi of adjusting angle is calculated with the tip location xtti at ti and the target location xgti+1 at ti+1, and the adjusting angle is planned with the optimal general insertion error as ϕi∈Φi. Repeat the above steps, the optimal set adjusting angle and its corresponding set of segment length are written as(18)Φ=ϕ1,ϕ2,…,ϕn,Lseg=l1,l2,…,ln.


## 3.4.2. Updating the Number of Needle Segment

In the path planning, the length of needle segment determined the target reachability and obstacle avoidance of needle tip. For the given initial insertion location and target position, the longer needle segment can get to the target with few rotations, which facilitates the operations of physicians and alleviates patient’s suffering. However, the long needle segment cannot construct the complicated trajectories with the obstacle avoidance. Therefore, we select the longer segments to generate the feasible paths. If the longer segments fail, we select the shorter segments to generate the feasible paths. For thekth planning step, the planned needle length is written as(19)Lk=L−∑i=1k−1li.First, we selectntk0 as the number of needle segments. If the planned paths can satisfy the general insertion error, the number of needle segments is chosen as ntk=ntk0. If not, the number of needle segments is chosen as ntk=ntkj=2j×ntk0 through verifying the general insertion error.Because the needle path is limited to the radius and depth of needle insertion, it is possible to fail to plan the feasible paths. Increasing the number of needle segments infinitely will lead to the endless loop of iterative calculations. Therefore, the maximum number of needle segments is set in the algorithm; if the number of needle segments is bigger than 50, the program will stop calculating. For the clinical practice, the surgeons or robot cannot operate too many segments. In the path planning algorithm, the segment’s length of initial planning step is bigger than that of later planning step, i.e.,lk−1≥lk. At the beginning of path planning algorithm, the initial segment’s length is given. The segment’s length of previous step lk−1 is selected as the initial value of segment’s length for the k middle step. The number of needle segments is dynamic, not static for the path planning algorithm.


## 3.4.3. Convergence and Error Analysis

In the optimization of feasible paths, finding the minimum general insertion error is to search the zone with minimum potential energy. The local minimum of APF may lead to the failure of optimization and reaching the target [12]. For avoiding the local minimum of APF, finite initial values are given to calculate the sets of adjusting angles, in which the optimized path is searched from the finite feasible paths. The finite sets of inverse kinematic solution guarantees the convergence of optimization procedure. Because we do not search the optimized paths in the whole space, the optimized path is locally an optimal solution.In order to obtain the optimal solution in the whole space, we select the uniformly distributed initial values to generate the feasible paths, as follows:(20)ϕ0j=0°,j−1×Δϕ,…,180°T,j=1,2,…,M.Figure9 shows an example of feasible paths filling the whole space.Figure 9
Feasible paths filling the whole space with the uniformly distributed initial values. Starting point is0cm,0cm,0cm, the goal is 0.8cm,1.2cm,9.8cmΔϕ=18°, and the number of segments is M=21.From the figure, the uniformly distributed initial value can generate feasible paths filling the whole space, from which the optimal path is searched to guarantee the global optimization. If more optimal solution is required, the smaller intervalΔϕ is needed to generate the finer paths. In the path planning algorithm, we select M=21 as the number of needle segments, which can search the optimal path with less computation.The proposed algorithm does not include the feedback from the real-time information, which could lead to the error accumulation from the prediction models. According to the kinematics of flexible, the prediction error of needle tip is about±0.70mm. The prediction error of target inside tissue with the finite element model is about ±1.00mm. Therefore, the needle tip may displace off the moved target, and the error is approximated as ±1.70mm. It is inevitable to eliminate the errors originated from the prediction models, so the real-time imaging information should be used for adjusting the paths online.


## 4. Experiments and Results

### 4.1. Experimental Setup

The experimental setup consists of a CCD camera, a tissue container, a needle clamper, a light source, a needle, two linear actuators, and one rotator, as shown in Figure10.Figure 10
Needle insertion experimental setup. It consists of a CCD camera, a tissue container, a needle clamper, a light source, a needle, two linear actuators, and one rotator.The linear actuators (the resolution is0.25μm) are provided by Physik Instrumente (PI Shanghai) Co. Ltd. The rotator (the resolution is 0.225°) is provided by HWHR Instruments Co. Ltd. A 22G×150mm PTC needle (the diameter of solid needle is 0.58 mm, and the tip is with a bevel angle of 30°) with a plastic hub (provided by Bard Peripheral Vascular, Inc.) is fixed by the clamper. The sample of artificial tissue (phantom) is made of polyvinyl alcohol (PVA) (60%) and dimethyl sulfoxide (6%), its size is 20mm×80mm×150mm, and its Young’s modulus is 10kPa–50kPa. The CCD camera (the maximum resolution is 1296×966) is provided to record the trajectory of flexible needles by the Baumer group. The frequency of image acquisition 12Hz is used to record the needle trajectory and the motions of target and obstacles.For investigating and recording the tissue’s motion and needle trajectories, the markers are embedded inside the PVA phantom as the target and obstacles, shown in Figure11.Figure 11
The markers of target and obstacles in the dynamic path planning experiments.In this figure, the markers 1–5 denote the obstacles, and the marker 6 denotes the target inside the phantom.In the figure, the markers 1–5 obstacle and goal locations are shown in Table2.Table 2
Locations of obstacles 1–5 and the goal.
Obstacles and goalxmmymmzmmO120.8338.184.98O255.3839.875.13O319.2565.005.20O420.7568.004.89O553.6070.115.15G52.6697.645.08


### 4.2. Experiment Design for Dynamic Path Planning

Two sets of experiments are designed to illustrate the effectiveness of the dynamic path planning algorithm. Three and four rotations of needle base are required to adjust the direction of bevel-tip angle, and the detail of experiments is shown as follows.The needle is decomposed into 3 and 4 segments, and the continuous time sequences aret3=t1,t2,t3 and t4=t1,t2,t3,t4 correspondingly. The motions of target and obstacles are predicted with the proposed FE model, the dynamic target locations are calculated as xGn=x⟶G1,x⟶G2,…,x⟶Gn, and the updated locations of obstacles are xOjn=x⟶Oj1,x⟶Oj2,…,x⟶Ojnj=1,2,3,4,5.The turning points are calculated with the inverse and forward kinematics, and the spline curve is used to smooth the planned trajectory. The location of needle tipxt in the time sequence is calculated with the planned adjusting angle. The CCD camera recorded the procedure of needle insertion into the phantom with a frequency of 12HZ. The insertion time is about 20s, in which 240–280 photos were stored for analyzing needle trajectory. Therefore, the time sequence is taken as t=t1,t2,…,t250, and the smoothed needle path in the time sequence is obtained as xn=x⟶t1,x⟶t2,…,x⟶t250 with the spline curve. The general insertion error εt1,εt2,…,εt250 is calculated with the dynamic needle-tissue information in the time sequence tn.For the insertion procedure with three and four rotations, the needle segments were taken as33.33mm and 25.00mm. The attractive and repulsive factors are taken as κg=1 and κo=10, and the security distance d0=2.00mm. The initial insertion location of needle tip lied at xt0=53.00,0.00,5.00, which is above the target. With the dynamic path planning algorithm, the planned adjusting angle is Φ3D=0.20°,178.86°,−0.24° and Φ4D=0.00°,175.70°,−0.18°,0.12° for the insertions with three and four rotations, respectively.In the experiments, the directions of bevel-tip angle were adjusted with the plannedΦ3D and its insertion depths were 34.00mm, 33.00mm, and 33.00mm. With the set of adjusting angle Φ4D, the insertion depth was 25.00mm uniformly. The insertion velocity was 5mm/s.


### 4.3. Experimental Results and Analysis

In the experiments, the22G PTC needle bypassed the 2 and 5 obstacles and inserted into the PVA phantom. The CCD camera recorded the insertions with 3 and 4 rotations, shown in Figure 12.Figure 12
Experimental trajectories of flexible needle into PVA phantom.From the figure, the needle was far away from 2 obstacles and approached 5 obstacles.The obstacles and target moved from their original positions because of the needle-tissue interactions. When the needle base is rotated 3 times, the displacements inX and Y directions are shown in Figure 13.Figure 13
The displacements of obstacles and target with 3 rotations.From the figure, all the obstacles and the target displaces from their original location. The obstacles 1 and 4 (marker 3 is similar to marker 4, and it was not plotted in Figure13) are away from the needle, and their displacements are smaller. Obstacle 2 has bigger displacement in the Y direction, the maximum is 1.55mm and has smaller displacement in X direction, and the maximum is 0.07mm. The maximum of resultant displacement is 1.56mm, and its final location is 55.34,40.91. The enlarged view shows the displacement of the obstacle 5 and demonstrates the trajectory off its original location 53.60,70.11. Obstacle 5 has bigger displacement in the X direction, and the maximum is 0.54mm. The Y displacement is smaller than obstacle 2, and the maximum is 1.30mm. The resultant displacement of obstacle 5 is 1.41mm, and its final location is 53.18,70.91. The target’s maximal displacement is 0.05mm in the X direction, and its maximum is 1.42mm in the Y direction. The final location of the target is 52.61,98.68.For the insertion with 4 rotations, the motions of obstacles and target are shown in Figure14. The enlarged view shows the displacements of the obstacle 2. Obstacle 5 has the bigger displacement than obstacle 2. The maximal displacements in X and Y directions are 2.15mm and 0.60mm for obstacle 5. The maximal resultant displacement is 2.23mm, and the final location of obstacle 5 is 53.07,71.88. For obstacle 2, the maximum is 0.02mm in the X direction and 1.93mm in the Y direction. Its maximal resultant displacement is 1.94mm, and its final location is 55.37,41.21. The target’s maximal displacements are 0.14mm and 1.76mm in the X and Y directions. The final location of target is 52.58,99.00 in the XOY plane.Figure 14
The displacements of obstacles and target with 4 rotations.With the planned adjusting angles, the needle base was steered by the actuators to change the direction of bevel-tip’s angle. The flexible needle inserts into the PVA phantom, and the experimental trajectory is shown in Figure15.Figure 15
Comparison of the planned path and experimental path. Obstacles 2 and 5 and target denote their final positions of needle insertion.The needle tip moves towards the target from the initial insertion location53.00,0.00 along the Y direction. With the 3 and 4 rotations, the tip’s locations of planned path are 52.03,98.23 and 52.52,98.42, respectively. The tip’s locations of experimental path are 52.03,97.73 and 52.46,97.92, respectively. There exists the difference between the planned and experimental paths, and the difference lies between 0.1mm and 0.5mm.For illustrating the effectiveness of the planned path, the errorεG between needle tip and target is described as(21)εG=xg−xt2+yg−yt2+zg−zt2.Because obstacles 1, 3, and 4 are far away from the needle, only the errorsεO2 and εO5 of obstacles 2 and 5 are listed in Table 3. The distance between the needle and obstacles is bigger than 1.16mm, and the distance between needle tip and target is less than 1.20mm. The planned needle trajectory can avoid the dynamic obstacles and reach the moved target effectively.Table 3
Errors of target and obstacles for dynamic path planning.
Planned segmentsεO2εO5εG3 segments2.05mm1.16mm1.13mm4 segments2.15mm1.20mm1.09mmFrom the experimental results, the proposed dynamic path planning algorithm facilitates to avoid obstacles and reaching target and searching the trajectory with the minimal general insertion error. The algorithm accounts for the reachability and steerability of flexible needles, which can be realized with the experiments.


## 4.1. Experimental Setup

The experimental setup consists of a CCD camera, a tissue container, a needle clamper, a light source, a needle, two linear actuators, and one rotator, as shown in Figure10.Figure 10
Needle insertion experimental setup. It consists of a CCD camera, a tissue container, a needle clamper, a light source, a needle, two linear actuators, and one rotator.The linear actuators (the resolution is0.25μm) are provided by Physik Instrumente (PI Shanghai) Co. Ltd. The rotator (the resolution is 0.225°) is provided by HWHR Instruments Co. Ltd. A 22G×150mm PTC needle (the diameter of solid needle is 0.58 mm, and the tip is with a bevel angle of 30°) with a plastic hub (provided by Bard Peripheral Vascular, Inc.) is fixed by the clamper. The sample of artificial tissue (phantom) is made of polyvinyl alcohol (PVA) (60%) and dimethyl sulfoxide (6%), its size is 20mm×80mm×150mm, and its Young’s modulus is 10kPa–50kPa. The CCD camera (the maximum resolution is 1296×966) is provided to record the trajectory of flexible needles by the Baumer group. The frequency of image acquisition 12Hz is used to record the needle trajectory and the motions of target and obstacles.For investigating and recording the tissue’s motion and needle trajectories, the markers are embedded inside the PVA phantom as the target and obstacles, shown in Figure11.Figure 11
The markers of target and obstacles in the dynamic path planning experiments.In this figure, the markers 1–5 denote the obstacles, and the marker 6 denotes the target inside the phantom.In the figure, the markers 1–5 obstacle and goal locations are shown in Table2.Table 2
Locations of obstacles 1–5 and the goal.
Obstacles and goalxmmymmzmmO120.8338.184.98O255.3839.875.13O319.2565.005.20O420.7568.004.89O553.6070.115.15G52.6697.645.08


## 4.2. Experiment Design for Dynamic Path Planning

Two sets of experiments are designed to illustrate the effectiveness of the dynamic path planning algorithm. Three and four rotations of needle base are required to adjust the direction of bevel-tip angle, and the detail of experiments is shown as follows.The needle is decomposed into 3 and 4 segments, and the continuous time sequences aret3=t1,t2,t3 and t4=t1,t2,t3,t4 correspondingly. The motions of target and obstacles are predicted with the proposed FE model, the dynamic target locations are calculated as xGn=x⟶G1,x⟶G2,…,x⟶Gn, and the updated locations of obstacles are xOjn=x⟶Oj1,x⟶Oj2,…,x⟶Ojnj=1,2,3,4,5.The turning points are calculated with the inverse and forward kinematics, and the spline curve is used to smooth the planned trajectory. The location of needle tipxt in the time sequence is calculated with the planned adjusting angle. The CCD camera recorded the procedure of needle insertion into the phantom with a frequency of 12HZ. The insertion time is about 20s, in which 240–280 photos were stored for analyzing needle trajectory. Therefore, the time sequence is taken as t=t1,t2,…,t250, and the smoothed needle path in the time sequence is obtained as xn=x⟶t1,x⟶t2,…,x⟶t250 with the spline curve. The general insertion error εt1,εt2,…,εt250 is calculated with the dynamic needle-tissue information in the time sequence tn.For the insertion procedure with three and four rotations, the needle segments were taken as33.33mm and 25.00mm. The attractive and repulsive factors are taken as κg=1 and κo=10, and the security distance d0=2.00mm. The initial insertion location of needle tip lied at xt0=53.00,0.00,5.00, which is above the target. With the dynamic path planning algorithm, the planned adjusting angle is Φ3D=0.20°,178.86°,−0.24° and Φ4D=0.00°,175.70°,−0.18°,0.12° for the insertions with three and four rotations, respectively.In the experiments, the directions of bevel-tip angle were adjusted with the plannedΦ3D and its insertion depths were 34.00mm, 33.00mm, and 33.00mm. With the set of adjusting angle Φ4D, the insertion depth was 25.00mm uniformly. The insertion velocity was 5mm/s.


## 4.3. Experimental Results and Analysis

In the experiments, the22G PTC needle bypassed the 2 and 5 obstacles and inserted into the PVA phantom. The CCD camera recorded the insertions with 3 and 4 rotations, shown in Figure 12.Figure 12
Experimental trajectories of flexible needle into PVA phantom.From the figure, the needle was far away from 2 obstacles and approached 5 obstacles.The obstacles and target moved from their original positions because of the needle-tissue interactions. When the needle base is rotated 3 times, the displacements inX and Y directions are shown in Figure 13.Figure 13
The displacements of obstacles and target with 3 rotations.From the figure, all the obstacles and the target displaces from their original location. The obstacles 1 and 4 (marker 3 is similar to marker 4, and it was not plotted in Figure13) are away from the needle, and their displacements are smaller. Obstacle 2 has bigger displacement in the Y direction, the maximum is 1.55mm and has smaller displacement in X direction, and the maximum is 0.07mm. The maximum of resultant displacement is 1.56mm, and its final location is 55.34,40.91. The enlarged view shows the displacement of the obstacle 5 and demonstrates the trajectory off its original location 53.60,70.11. Obstacle 5 has bigger displacement in the X direction, and the maximum is 0.54mm. The Y displacement is smaller than obstacle 2, and the maximum is 1.30mm. The resultant displacement of obstacle 5 is 1.41mm, and its final location is 53.18,70.91. The target’s maximal displacement is 0.05mm in the X direction, and its maximum is 1.42mm in the Y direction. The final location of the target is 52.61,98.68.For the insertion with 4 rotations, the motions of obstacles and target are shown in Figure14. The enlarged view shows the displacements of the obstacle 2. Obstacle 5 has the bigger displacement than obstacle 2. The maximal displacements in X and Y directions are 2.15mm and 0.60mm for obstacle 5. The maximal resultant displacement is 2.23mm, and the final location of obstacle 5 is 53.07,71.88. For obstacle 2, the maximum is 0.02mm in the X direction and 1.93mm in the Y direction. Its maximal resultant displacement is 1.94mm, and its final location is 55.37,41.21. The target’s maximal displacements are 0.14mm and 1.76mm in the X and Y directions. The final location of target is 52.58,99.00 in the XOY plane.Figure 14
The displacements of obstacles and target with 4 rotations.With the planned adjusting angles, the needle base was steered by the actuators to change the direction of bevel-tip’s angle. The flexible needle inserts into the PVA phantom, and the experimental trajectory is shown in Figure15.Figure 15
Comparison of the planned path and experimental path. Obstacles 2 and 5 and target denote their final positions of needle insertion.The needle tip moves towards the target from the initial insertion location53.00,0.00 along the Y direction. With the 3 and 4 rotations, the tip’s locations of planned path are 52.03,98.23 and 52.52,98.42, respectively. The tip’s locations of experimental path are 52.03,97.73 and 52.46,97.92, respectively. There exists the difference between the planned and experimental paths, and the difference lies between 0.1mm and 0.5mm.For illustrating the effectiveness of the planned path, the errorεG between needle tip and target is described as(21)εG=xg−xt2+yg−yt2+zg−zt2.Because obstacles 1, 3, and 4 are far away from the needle, only the errorsεO2 and εO5 of obstacles 2 and 5 are listed in Table 3. The distance between the needle and obstacles is bigger than 1.16mm, and the distance between needle tip and target is less than 1.20mm. The planned needle trajectory can avoid the dynamic obstacles and reach the moved target effectively.Table 3
Errors of target and obstacles for dynamic path planning.
Planned segmentsεO2εO5εG3 segments2.05mm1.16mm1.13mm4 segments2.15mm1.20mm1.09mmFrom the experimental results, the proposed dynamic path planning algorithm facilitates to avoid obstacles and reaching target and searching the trajectory with the minimal general insertion error. The algorithm accounts for the reachability and steerability of flexible needles, which can be realized with the experiments.


## 5. Conclusions and Future Work

In this paper, a novel dynamic path planning algorithm is developed for needle insertion into soft tissue considering the dynamic motions of target and obstacles. The needle-tissue interaction model is based on a FE model with the modified local constraint method, which can provide real-time feedback for path planner. The forward and inverse kinematics of flexible needles are modeled based on the quasi-static approach and D-H method, in which the needle is steered by rotating the needle base to generate the desired trajectory. The existence of inverse kinematic solution is investigated to determine the reachability of flexible needle. An iterative approach is developed to generate the feasible path by minimizing the general insertion error defined by artificial potential field. The experimental results showed that the actual paths using proposed algorithm agree well with the simulation results. Future work includes developing a better FE model that can simulate the inhomogeneous and nonlinear tissue properties that are close to the realistic soft tissue. In addition, a robust control strategy will be studied to consider the stability and steering ability of the flexible needle.



---

*Source: 4512409-2020-07-27.xml*