3D Dynamic Motion Planning for Robot-Assisted Cannula Flexible Needle Insertion into Soft Tissue

Abstract

In robot-assisted needle-based medical procedures, insertion motion planning is a crucial aspect. 3D dynamic motion planning for a cannula flexible needle is challenging with regard to the nonholonomic motion of the needle tip, the presence of anatomic obstacles or sensitive organs in the needle path, as well as uncertainties due to the dynamic environment caused by the movements and deformations of the organs. The kinematics of the cannula flexible needle is calculated in this paper. Based on a rapid and robust static motion planning algorithm, referred to as greedy heuristic and reachability-guided rapidly-exploring random trees, a 3D dynamic motion planner is developed by using replanning. Aiming at the large detour problem, the convergence problem and the accuracy problem that replanning encounters, three novel strategies are proposed and integrated into the conventional replanning algorithm. Comparisons are made between algorithms with and without the strategies to verify their validity. Simulations showed that the proposed algorithm can overcome the above-noted problems to realize real-time replanning in a 3D dynamic environment, which is appropriate for intraoperative planning.

Keywords

Dynamic Motion Planning Rapidly-exploring Random Tree Cannula Flexible Needle Robot-assisted Surgery

1. Introduction

In minimally invasive surgeries, needle insertion is likely one of the most popular procedures, applied in tissue biopsies and radioactive brachytherapies for cancers. However, targeting is a challenge when targets are surrounded by anatomic obstacles or sensitive organs that must be avoided. Traditional rigid needles are not useful for this task. Therefore, a bevel tip flexible needle is proposed for overcoming this problem [1]. The conventional flexible needle is supposed to be flexible to a degree that it can bend inside tissue, due to lateral force being applied on the bevel tip at insertion. However, it still has some drawbacks: firstly, the curvature of the trajectory for a needle is supposed to be fixed in a tissue and cannot rectify its path once it has deviated. Secondly, it is difficult to precisely control the orientation of the bevel tip at rotation, due to the torsional friction between the needle shaft and tissue. To overcome the first drawback, Minhas and Majewicz utilized the duty-cycling of the spinning motor to generate a series of curvatures [2, 3]. Datla et al. proposed an active, flexible needle that takes advantage of the characteristics of shape memory alloys (SMA), which can also generate different curvatures by supplying different electric currents to the SMA actuators [4 –6]. However, the second drawback remains in place. We have been developing a cannula flexible needle, composed of a flexible cannula and a flexible bevel-tip stylet, as shown in Fig.1. It can overcome both drawbacks of the conventional bevel tip flexible needle mentioned above. Firstly, it can generate a series of curvatures of trajectories by the different the length d of the stylet out of the cannula. Secondly, it can improve the rotation precision of the bevel tip, because the cannula separates the stylet and the tissue, thereby reducing torsional friction.

Figure 1.

Schematic of a cannula flexible needle

Motion planning of the cannula flexible needle in the soft tissue is challenging due to the nonholonomic motion of the needle and the presence of anatomic obstacles and sensitive organs [7]. It is even complicated in a dynamic environment, with uncertainties present due to errors in needle tip positioning and needle modelling, the inhomogeneity and deformation of tissue and the physiological movement of organs [8]. All of these disturbances and movements may cause the needle to deviate from its intended path, which can have fatal consequences.

Motion planning for the conventional bevel tip flexible needle has been extensively studied using different approaches, which can be used as references for the cannula flexible needle. Duindam et al. proposed motion planning using discretization of the control space in a 3D environment with obstacles and formulated the motion planning problem as a nonlinear optimization problem [7]. However, this planning is applied for a static environment. Alterovitz et al. depicted the motion planning problem as a Markov decision process, using a discretization of the state space to maximize the probability of successfully reaching the target in a 2D environment [8]. Park et al. proposed a path-of-probability algorithm to optimize the paths by computing the probability density function [9]. Both of the above-noted approaches considered the uncertainty regarding the needle's response to control using dynamic programming; however, these approaches are applied for a quasi-dynamic environment, i.e., there is no actual disturbance or movement of the environment and the uncertainty is not actually formulated or updated to the planning in question. All of the above approaches adopted a mathematical computation (MC) method, which configures the problem as an optimization problem with an objective function and then computes the numerical optimal solution. This method generally has a computational expense and may suffer from convergence; therefore, such methods are generally used for preoperative planning, but are not appropriate for intraoperative planning.

Another method for considering uncertainty, particularly in the case of tissue deformation is the finite element mesh (FEM) method. Alterovitz et al. used a FEM to compute soft tissue deformations and combined it with a MC method to find a locally optimal trajectory [10]. Patil et al. proposed motion planning for highly deformable environments, using the FEM method combined with a sampling-based algorithm [11]. However, the efficiency of the FEM method depends significantly on how accurately the mesh represents the real tissue. Moreover, both of these studies did not consider many other uncertainties caused by the surrounding environment. Moreover, the FEM method is also time-consuming. Therefore, it is generally used in preoperative motion planning. However, if we consider real-time dynamic motion planning, it may not be appropriate.

A third important method for flexible needle motion planning is a sampling-based method such as the probabilistic roadmaps (PRM) or the rapidly-exploring random trees (RRT) method. Some motion planning has been developed based on PRM in a static environment or a quasi-dynamic environment [12, 13]. Ever since Xu et al. first applied a RRT-based method to search for valid needle paths in a 3D environment with obstacles, the RRT algorithm has been commonly used in flexible needle motion planning [14]. However, these studies were only considered in a static environment. Patil et al. utilized improved RRTs called reachability-guided RRTs (RG-RRTs) and achieved orders of magnitude speed-ups compared to previous approaches, and relaxed the constraint of constant-curvature needle trajectories by relying on duty-cycling to realize bounded-curvature needle trajectories. This enabled the RRT method to be appropriate for dynamic intraoperative motion planning [15]. Caborni et al. proposed risk-based path planning for a steerable flexible probe based on the RG-RRTs [16]. In terms of path replanning, Patil et al. proposed a rapid replanning algorithm based on RG-RRTs and conducted experimental evaluations to show its validity [17]. Vrooijink et al. also developed a closed-loop replanning algorithm based on the RG-RRTs for a 3D non-static environment using 2D ultrasound images [18]. Both of these studies considered the uncertainty arising from tissue deformations, actuation errors and noisy sensing; however, they did not consider the significant displacement of targets and obstacles caused by physiological movement, which may cause the needle to fatally deviate from the planned path. Bernardes et al. presented a fast intraoperative replanning algorithm that considers disturbances including the movements of obstacles and a target based on RRTs in both 2D and 3D environments; however, target movement was limited in 2D, despite having the potential to move in 3D [19, 20].

In summary, the advantage of the RRT method is that it is very fast (in milliseconds), easy to implement and probabilistic to complete, which is suitable for intraoperative planning. However, for dynamic replanning, all replanning algorithms may suffer from the large detour problem, convergence problem and/or accuracy problem. The large detour problem may be caused by the unpredicted movement of the obstacles and/or target; the convergence problem may be caused by nonholonomic motion and/or the minimum bending radius of the needle in the tissue, and the potential for the path adjustment becomes weaker when the needle gets close to the target; the accuracy problem may be caused by the termination conditions when the convergence problem occurs.

In this paper, we firstly calculate the kinematics of the cannula flexible needle, then introduce a fast motion planning algorithm based on RRT for a static environment (preoperative operation) for the cannula flexible needle in our previous work. Based on the proposed static motion planning algorithm, we propose dynamic (intraoperative operation) motion planning using replanning, taking into account any uncertainty caused by errors in needle tip positioning, the approximation of needle modelling, the inhomogeneity and deformation of tissue and physiological movement of the organs. Aiming at the large detour problem, the convergence problem and the accuracy problem that replanning encounters, we propose three novel strategies integrated into the intraoperative path replanning algorithm for overcoming these problems.

2. Kinematic Analysis of the Cannula Flexible Needle

Different from conventional bevel tip needles (with two DOFs: insertion and rotation) [1], the proposed cannula flexible needle has three DOFs: two translations u₁, u₂ for the cannula and the stylet, respectively, and rotation u₃ for the stylet,u₁, u₂ and u₃ can be input separately or simultaneously (see Fig.1).

2.1. Trajectory form analysis

We assume that the cannula flexible needle is pliably flexible and torsionally stiff, i.e., the needle shaft follows the needle tip, performing an approximate circular arc in the tissue when bending, while needle tip rotates at the same angle as the base [1, 7]. Similar to the conventional bevel tip flexible needles, it will be bent against the bevelled side when inserted into the tissue, due to the lateral force. The rotation of the stylet decides the orientation of the needle bending, so that the needle can achieve various paths in 3D by combining the three inputs.

Arc-based path: the radius of the needle path can vary according to the length d of the stylet which remains out of the cannula (see Fig.1). The more the stylet is able to get out of the cannula, the smaller radius the trajectory will receive. Thus, the relative velocity between u₁ and u₂ changes the radius of the path, and u₃ changes the orientation of the bending. As u₁ and u₂ are generally input simultaneously and with the same velocity, we can denote them as u₁₂. If we input u₁₂ and u₃ in a time-sharing way, the needle will generate an arc-based path (see Fig.2a). This method does not generate all radii of the path. The path will be in a range with the minimum bending radius r_min and the maximum bending radius r_max (as shown in Fig.3a). Both r_min and r_max are not only related to d, but also to the mechanical properties of the needle and the tissue. For the paths with a bending radius larger than r_max, we can also apply the duty-cycling method [2, 3]. By using a combination of both methods, the control expense is saved to some extent. The entire workspace is shown in Fig.3b.

Helix-based path: if we input u₁₂ and u₃ simultaneously, and the speed of u₃ is much slower than that of u₁₂, the needle will generate a helix-based path (see Fig.2b).

Linear path: if we input u₁₂ and u₃ simultaneously, however, the speed of u₃ will be much faster than that of u₁₂, and the needle will generate a linear path (see Fig.2c).

Figure 2.

Forms of path

Figure 3.

Workspace of the needle

In this paper, we adopt the arc-based and linear paths rather than the helix-based path, because they are easier to control. The helix-based path, in contrast, is difficult to formulate and control and may also cause more trauma to biological organs, because its winding approach may lengthen the path.

2.2. Kinematic model

The kinematic model of the cannula flexible needle is formulated as shown in Fig.4. In the world frame Ψ_w, the configuration of the needle tip frame Ψ_n can be described compactly by a 4 × 4 homogeneous transformation matrix:

g_{w n} = [\begin{matrix} R_{w n} & p_{w n} \\ 0 & 1 \end{matrix}] \in S E (3)

(1)

where R _wn∈SO(3) and $p_{w n} \in ℝ^{3}$ are the rotation matrix and the position of frame Ψ_n relative to the frame Ψ_w, respectively.

Figure 4.

Kinematic model of the cannula flexible needle

In frame Ψ_n, the instantaneous linear velocity is $v = {[\begin{matrix} 0 & 0 & u_{12} \end{matrix}]}^{T}$ , angular velocity is $ω = {[\begin{matrix} u_{12} / r_{i} & 0 & u_{3} \end{matrix}]}^{T}$ , where r is the radius of the path and the twist $\hat{ξ} \in s e (3)$ is formulated as:

\hat{ξ} = {[\begin{array}{l} v \\ ω \end{array}]}^{\land} = [\begin{matrix} 0 & - u_{3} & 0 & 0 \\ u_{3} & 0 & - u_{12} / r & 0 \\ 0 & u_{12} / r & 0 & u_{12} \\ 0 & 0 & 0 & 0 \end{matrix}]

(2)

where ^ is the wedge operator for forming a matrix in se(3) out of a given vector in $ℝ^{6}$ .

Then, the homogeneous transformation matrix can be formulated in exponential form as:

g_{w n} (t) = g_{w n} (0) \exp (\hat{ξ} t)

(3)

where g _wn(0) is the initial configuration, which is the configuration of the needle (frame Ψ_n) in frame Ψ_w before insertion and t is the execution time. Additional details can be found in [1].

However, different to other existing kinematics of the flexible needle [1, 7, 9, 11 –20], because the planning algorithm considers the insertion pose of the needle, we have to add an insertion configuration to the kinematics:

g_{w n} (t) = g_{w n} (0) g_{i n} \exp (\hat{ξ} t)

(4)

where g _in is the insertion configuration.

The entire path can be discretized by a few segments. Let $g_{i} (t_{i}) = \exp (\hat{ξ} t_{i})$ represent the transformation matrix of the ith segment, then the forward kinematics after N segments of execution is formulated as:

g_{w n} (T) = g_{w n} (0) g_{i n} \prod_{i = 1}^{N} g_{i} (t_{i})

(5)

where t_i is the execution time of the ith segment, T is the total execution time of the whole path, $T = \sum_{i = 1}^{n} t_{i}$ ; g _wn(0) is the initial configuration and g _in is the insertion configuration.

To simplify the calculation, we make the Ψ_n frame coincide with the Ψ_w frame initially (see Fig.5). Thus, g _wn(0)=I₄, where I₄ is a unit matrix with 4 × 4. The insertion pose can be regarded as frame Ψ_n rotating α₀ around axis Z_n, then rotating β₀ around axis Y_n; hence, configurations for the insertion configuration can be obtained by:

g_{i n} = R o t (Z_{n}, α_{0}) R o t (Y_{n}, β_{0})

(6)

Figure 5.

Insertion configuration of the needle

3. Static Motion Planning Algorithm

We utilized the RRT method for the static motion planning, combined with the greedy heuristic method and reachable guided strategies, known as greedy heuristic and reachability guided rapidly-exploring random trees (GHRG-RRTs). We adopted variable but bounded curvatures for the needle paths and also took account of linear segments and relaxation of insertion orientations where the trajectories were concerned. The superiority of this algorithm is attested to in our previous work [21, 22].

3.1. Outline of GHRG-RRTs algorithm

The outline of the GHRG-RRTs algorithm is shown in Algorithm 1. Further details can be obtained in [22].

3.2. Optimization function

Among the candidate solutions, all of which have already met the required constraints, the optimal trajectory can be chosen based on a cost function. The objectives of the optimization take consideration of minimizing tissue trauma and the danger of the path, as well as the path shape evaluation.

\min F (u, T) = \min {α_{1} L + α_{2} D + α_{3} S}

(7)

where L is the length of path $L = \sum_{i = 1}^{N} l_{i}$ , D is the degree of danger in the path, D=min(D_i), where D_i is the distance between the path and the ith obstacle, S is the curve evaluation of the path and $S = \sum_{i = 1}^{N} \frac{r_{\min}}{r_{i}}$ ; α₁-α₃ are the weighted coefficients. The result of the function is the comprehensive evaluation of the optimal path.

4. Dynamic Motion Planning Algorithm

Prior to surgery, we first used the GHRG-RRTs in preoperative planning to seek out an optimal path based on medical imaging (e.g., ultrasound, CT or MRI). During the surgery, we used online medical imaging to update the configurations of both the needle and the environment. We then used a fast intraoperative replanning algorithm to reform and adjust the path in order to realize a real-time closed-loop control up to the point where the needle reaches the target.

We acquired the optimal path using the preoperative motion planning algorithm. However, disturbances and uncertainties like model inaccuracy, tissue deformation and inhomogeneity, needle tip positioning errors, physiological movement, etc., could still deviate the needle from its intended path. Moreover, movement on behalf of the target and obstacles can render the planned path infeasible, leading to a collision with obstacles or misplacement of the target. To overcome this, we adopted the replanning idea [17], here referred to as conventional motion replanning (CMR), the outline of which is depicted in Algorithm 2. We attained the current state by using a 3D medical imaging system for replanning and execution in each cycle, in order to systematically rectify the path until the needle tip reaches target region δ.

Algorithm 2: CMR (g_{tip_now}, q_{goal_now}, Q_now).

1: T ← InitTree(g_{tip_now}); P ← P_planned 2: while the distance between the needle tip and the goal d>δ 3: P ← Modified_GHRG (g_{tip_now}, q_{goal_now}, Q_now) 4: return P 5: end while

where the modified GHRG-RRTs is the routine of lines 6-40 of Algorithm 1. Once the needle has been inserted into the tissue, the orientation at the needle tip can no longer be changed. Therefore, the linear path acquisition routine (lines 1–5 of Algorithm 1) are no longer appropriate for replanning.

This algorithm appears reasonable; however, it generally encounters some intractable problems. Firstly, this algorithm makes each cycle of replanning independent and without learning from former cycles, because of the stochastic property of the RRT algorithm, the blind replanning may cause a large detour in the path when the environment changes (see Fig.6a). Secondly, it may cause the convergence problem, i.e., as the flexibility of the needle is minimized due to nonholonomic constraints, whenever the needle gets close to the target, it is likely that the target will be unreachable because of its movement or other uncertain errors. Furthermore, the replanning algorithm will replan a circle-like path to make the target reachable again and as a result, the needle will detour in a loop without ever reaching the target (see Fig.6b). Therefore, the CMR algorithm is not suitable in terms of the significant movement of obstacles and the target.

To overcome these problems, we propose an improved motion replanning (IMR) algorithm with two strategies, i.e., an old point tracking strategy (OPTS) and an extreme trend extension strategy (ETES). OPTS tracks the old point in the old path in order to learn from former planning in order to overcome the large detour problem. ETES bends in an extreme manner when the needle gets close enough to a target that is unreachable to overcome the convergence problem. The programming of the IMR-1 is depicted in Algorithm 3.

Figure 6.

Problems of the CMR

OPTS (line 3–6 of IMR-1) tracks and utilizes the old connecting points q_i in the former path to replan a new path. If it cannot generate a new path using the old points, it will replan a new path by using a modified GHRG-RRTs. In this way, after several cycles of iteration, it will find a relatively stable path that is immune to changes in the environment to some extent, because it has learned from previous planning. This strategy overcomes the large detour problem.

ETES (lines 14–21 of IMR-1) prevents the planner from generating a circle-like path due to the failure of reachability. If the needle gets close enough to a target that is unreachable (the radius of the last segment r is less than the minimum radius r_min), instead of replanning a new path, it will try its best to extend the needle to the target with the minimum radius until it gets to the closest position to the target. This strategy overcomes the convergence problem.

We performed different strategies according to different conditions. Here, s₁ is the switch between the proposed replanning strategies and Δs is the length of the insertion we executed in each cycle. If the path was longer than s₁, the OPTS was performed; if the length of the path was between s₁ and Δs, the ETES was performed up to the point where the needle reaches the nearest position to the target.

Although the problems noted above were well solved, the ETES introduces another problem to the planning. Since we force the needle to extend to the target in an extreme manner, it may cause a large error (sometimes above 10mm), which is intolerant for clinical surgery. To overcome the complication, we propose the hardest goal tracking strategy (HGTS) and integrated it to the IMR-1 to form IMR-2, as depicted in Algorithm 4.

We found that the failure of reachability was badly influenced by movement of the target. Among all positions that the target has undergone, there is the hardest one to reach, which is contained in the path with the smallest radius. We called this the hardest goal. Theoretically, if the needle can achieve the hardest goal, it will be less difficult to achieve other positions linked to the goal. We therefore used the HGTS (lines 7–18 of IMR-2) to track the hardest goal, regardless of the target movement, until the length of path was shorter than s₂.

5. Simulation and Discussion

5.1. Settings for simulation

We simulated the motion planner in MATLAB® (ver. 7.8.0, R2009a; MathWorks, Natick, MA) on a 2.5 GHz 4-core Intel® i5™ PC. We adopted an environment similar to [14], modelling it as a cubical region at 200mm along each axis. Six spherical obstacles, each with a radius of 20mm represent the pubic arch, the urethra and the penile bulb around the prostate. The goal is set to (0 0 195), in millimetre (see Fig.8a). We set the minimum radius r_min=50mm and the specific metric at ρ=10mm (in line 29 of Algorithm 1). The maximum number of the candidate paths was set to 100 and the maximum number of iterations to 10 000. In order to speed up computation, we assumed there was a relatively safe margin m (here we set m=3mm) around the obstacle; as long as the needle did not puncture the safe margin, it would never puncture the obstacle; thus, the surgery was safe. The radius of the obstacle should also be enlarged by safe margin m when planning to make it safe. As such, there was no need to care about the exact distance between the needle and the obstacles, as long as the surgery was sufficiently safe. Therefore, we could disregard the second term in (7) by setting α₂=0. Other weighted coefficients were set to α₁=α₃=1, as we equally considered both of the remaining sub-objective functions. These settings were applied to both preoperative planning and intraoperative replanning.

For replanning, we added some disturbances and movements. The disturbances such as model inaccuracy, tissue deformation, tissue inhomogeneity and needle tip positioning errors, were modelled as white noise and followed normal distribution N~(μ, σ). We let μ=r and σ=r/10 mm to the radii of the path, μ=0 and σ=1mm to the tip position and μ=0 and σ=0.01 rad to the orientation of the needle tip. Movement of the target and obstacles were modelled as periodic sinusoidal motions in 3D, both with an amplitude of 5mm and at a period of 60s and 5s, respectively.

As noted, since s_i(i=1,2) are the switch for the replanning strategies and affect the validity of the planning, we need to discuss them in detail. For IMR-1, s₁ is the switch between OPTS and ETES. Here we will track the old connecting points along the planned path. The convergence problem always occurs in the final segment, especially close to the target. We thus have to place s₁ before the convergence problem occurs in the final segment. In order to attain switch s₁, we have to study where the convergence problem generally occurs and how long the final segment of path is. We simulated for 20 times of the Algorithm 2 (CMR) in the specified environment. Among the 20 trials, the maximum length of path occurring in the convergence problem was 30.73 mm; the minimum length of the final segment was 92.01 mm. Thus, s₁ should be between 30.73 mm and 92.01 mm; to be safe, we let s₁= 40~80 mm. Moreover, the convergence problem is also related to the environment and the value of minimum radius r_min.

As for IMR-2, s₂ is the switch between the first two strategies (OPTS and HGTS) and the last one (ETES). The analysis for IMR-2 is similar to that of IMR-1. We will perform the first two strategies until the needle gets close to the target and then perform the final strategy. However, the difference from the IMR-1 is that we do not have to worry about the convergence problem prior to releasing the HGTS, since the hardest goal is fixed. The critical matter is when to release the HGTS. If we release it too early, it will track the target thereafter and the effectiveness will be no better than for IMR-1; if we release it too late, the planning will have limited opportunity to adjust the path to the current target. Thus, the value of s₂ for IMR-2 is neither too large nor too small. In order to attain switch s₂ for IMR-2, we simulated each different value 20 times to observe its performance, as is shown in Fig.7. From the figure, we can conclude that the best performance for IMR-2 is at s₂=10. Moreover, s₂ not only concerns the environment and r_min, but also target movement.

Therefore, empirically, we set s₁=50 mm and s₂=10 mm for IMR-1 and IMR-2, respectively. We also set the insertion distance per cycle as Δs = 1 mm.

Figure 7.

Average errors of different values of s₂

5.2. Simulation results and discussion

In order to verify the validity of each strategy, we compared Algorithm 2 (CMR), Algorithm 3 (IMR-1) and Algorithm 4 (IMR-2). We performed 20 trials using the same target for each algorithm. The validity of the OPTS and ETES is obvious in the comparison of CMR and IMR-1, as shown in Table 1. The validity of the HGTS is shown in the comparison of IMR-1 and IMR-2 (see Table 1). Table 1 shows that some of the results in the 20 trials have the formation “mean ± standard deviation”.

Table 1.

Comparison of the three algorithms

Algorithms	Large detour problem (times)	Convergence problem (times)	Error (mm)	Time for each cycle (ms)	Length of optimal path (mm)	Length of actual path (mm)
CMR	18	16	/	202.6±101.1	214.5±5.6	/
IMR-1	0	0	7.77±4.15	1.6±11.4		228.6±18.2
IMR-2	0	0	1.31±0.38	3.7±21.6		219.8±4.7

The results show that CMR suffers from the large detour problem, while the other algorithms avoid it. This reveals that OPTS is effective in terms of the large detour problem. CMR also suffers the convergence problem, while the other algorithms avoid this, which reveals the ETES is effective in terms of the convergence problem. Moreover, the time for each cycle for CMR is much longer than that for the other algorithms. This is because OPTS, which not only learns from former cycles of replanning and renders the path stable, but also enhances the cycling replanning speed.

On the other hand, from the comparison of IMR-1 and IMR-2, it is clear that the accuracy of IMR-2 is much better than that of IMR-1, which reveals the effectiveness of the HGTS. The error of IMR-2 is within 2mm, which meets the needs of clinical requirements. From the length of the actual path, we can see that IMR-2 is much more stable than IMR-1. Although the replanning speed of IMR-2 is slightly slower than that of IMR-1 due to adding the extra strategy of HGTS, it is still in milliseconds (see the column “Time for each cycle” in Table 1), which is fast enough for intraoperative replanning.

We also obtained one of the simulation results and compared it with the result of the simulation without the replanning. The course of simulation is shown in Fig.8. The original optimal path, the executed path, the ongoing/adjusted path and the path added with disturbances and without replanning or adjusting are depicted in the figure. The final errors were 0.75 mm and 43.15 mm for the replanning and without replanning paths, respectively.

In order to verify the robustness of the proposed algorithm (IMR-2), we also performed simulation for different goals. We randomly set two more goals as G1 (5 20 190) and G2 (10 −20 185), both in millimetre, and simulated each goal 20 times. The results are as shown in Table 2 and have the formation “mean ± standard deviation”.

Table 2.

The performance of the IMR-2 algorithm

IMR-2	Large detour problem (times)	Convergence problem (times)	Error (mm)	Time for each cycle (ms)	Length of optimal path (mm)	Length of actual path (mm)
G1	0	0	1.56±0.53	2.5±18.3	204.9±6.1	208.2±8.7
G2	0	0	1.47±0.37	3.3±22.1	197.1±5.2	199.0±4.2

From Table 2, we can see that the large detour or convergence problem did not occur, the error is within 2mm and speed remains high. As such, the conclusion can be drawn that the proposed algorithm has strong robustness.

Figure 8.

Simulation result of Algorithm 4

6. Conclusion

In this work, we firstly calculated the kinematics for the cannula flexible needle. Based on the proposed RGHG-RRTs algorithm for static motion planning in our previous work, we propose a 3D dynamic motion planning algorithm for intraoperative surgery. We also propose three novel strategies to be integrated in the conventional replanning algorithm in order to improve it. By comparing the CMR, the IMR-1 and the IMR-2 algorithms, we can conclude that the proposed OPTS, ETES and HGTS are extremely effective for addressing the large detour problem, the convergence problem and accuracy problem, respectively. Moreover, the OPTS also benefits for the replanning speed of a cycle. Finally, we performed simulations for different goals. The results revealed the validity and robustness of the proposed replanning algorithm.

In future, we will integrate our planning with a real-time feedback controller to carry out experiments on the phantom tissue.

Footnotes

7. Acknowledgements

This research is supported in part by the National Natural Science Foundation of China (Grant#51305107), by the Natural Science Foundation of Heilongjiang Province of China (Grant#E2015059 and #E201448) and by the Science and Technology Project of the Education Department of Heilongjiang Province of China (Grant#12531110 and #12531122).

References

Webster

III Kim

Cowan

Chirikjian

Okamura

. Nonholonomic modeling of needle steering. Int. J. Robotics Research. 2006; 25(5–6), pp. 509–525.

Minhas

Engh

Fenske

. Modeling of needle steering via duty-cycled spinning. Proc. Int. Conf. IEEE Eng. in Medicine and Biology Society; 2007; pp. 2756–2759.

Majewicz

Siegel

Stanley

Okamura

. Design and Evaluation of Duty-Cycling Steering Algorithms for Robotically-Driven Steerable Needles. IEEE International Conference on Robotics & 0Automation; 2014; pp. 5883–5888.

Datla

Honarvar

Nguyen

Konh

Darvish

, Toward a nitinol actuator for an active surgical needle,™ in ASME Conference on Smart Material, Adaptive Structures and Intelligent Systems, 2012; pp. 19–21.

Konh

Honarvar

Hutapea

Application of SMA wire for an active steerable cannula. ASME Conference on Smart Materials, Adaptive Structures and Intelligent Systems, 2013; pp. 265–269.

Honarvar

Datla

Konh

Podder

T K

Dicker

Hutapea

Study of unrecovered strain and critical Stresses in one-way shape memory nitinol. Journal of Materials Engineering and Performance, 2014; 23(8), pp. 2885–2893.

Duindam

Alterovitz

Sastry

Goldberg

Screw-based motion planning for bevel-tip flexible needles in 3D environments with obstacles. Proceedings of the IEEE International Conference on Robotics and Automation; 2008; pp. 2483–2488.

Alterovitz

Branicky

Goldberg

Motion Planning under Uncertainty for Image-guided Medical Needle Steering. The International Journal of Robotics Research, 2008; 27(11–12), pp. 1361–1374.

Park

Wang

Chirikjian

. The path-of-probability algorithm for steering and feedback control of flexible needles. International Journal of Robotics Research; 2010; 29(7), pp. 813–830.

10.

Alterovitz

Goldberg

Okamura

Planning for steerable bevel-tip needle insertion through 2D soft tissue with obstacles. Proceedings of the IEEE International Conference on Robotics and Automation; 2005; pp. 1640–1645.

11.

Patil

Berg

JVD

Alterovitz

Motion planning under uncertainty in highly deformable environments. Proc. Robotics: Science and Systems (RSS), June; 2011.

12.

Alterovitz

Patil

Derbakova

Rapidly-exploring Roadmaps: Weighing exploration vs. refinement in optimal motion planning. In: Proc. IEEE Int. Conf. on Robotics and Automation; 9–13 May 2011; Shanghai, China. 2011; pp. 3706–3716.

13.

Lobaton

Zhang

Patil

Alterovitz

Planning curvature-constrained paths to multiple goals using circle sampling. In: Proc. IEEE Int. Conf. on Robotics and Automation; 9–13 May 2011; Shanghai, China. 2011; pp. 1463–1469.

14.

Duindam

Goldberg

Motion planning for steerable needles in 3D environments with obstacles using rapidly exploring random trees and backchaining. In: Proc. IEEE Int. Conf. on Automation Science and Engineering; 2008 Aug 23–26; Arlington, VA, USA. 2008; pp. 41–46.

15.

Patil

Alterovitz

Interactive motion planning for steerable needles in 3D environments with obstacles. In: Proc. IEEE RAS/EMBS Int. Conf. Biomedical Robotics and Biomechatronics; 2010 Sep 26–29; Tokyo, Japan. 2010; pp. 893–899 (2010).

16.

Caborni

Momi

Ferrigno

Baena

FRY

. Risk-based path planning for a steerable flexible probe for neurosurgical intervention. In: IEEE RAS/EMBS Int. Conf. on Biomedical Robotics and Biomechatronics; 24–27 Jun 2012; Rome, Italy. 866–871 (2012).

17.

Patil

Burgner

Webster

III Alterovitz

Needle steering in 3-D via rapid replanning. IEEE Trans. on Robotics. 2014; 30(4), pp. 853–864.

18.

Vrooijink

Abayazid

Patil

Alterovitz

Misra

Needle path planning and steering in a three-dimensional non-static environment using two-dimensional ultrasound images. Int. Journal of Robotics Research. 2014; 33(10), pp. 1361–1374.

19.

Bernardes

Adorno

Poignet

Borges

. Robot-assisted automatic insertion of steerable needles with closed-loop imaging feedback and intraoperative trajectory replanning. Mechatronics. 2013; 23(6), pp. 630–645.

20.

Bernardes

Adorno

Borges

Poignet

3D robust online motion planning for steerable needles in dynamic workspaces using duty-cycled rotation. Journal of Control, Automation and Electrical Systems. 2014; 25(2), pp. 216–227.

21.

Zhao

Joseph

FOM

Yan

, Path Planning for Robot-Assisted Active Flexible Needle using Improved Rapidly-Exploring Random Trees. 36th Annual International Conference of the IEEE Engineering in Medicine and Biology Society (EMBC′14), Chicago, Illinois, USA, August 26–30, 2014; pp. 380–383.

22.

Zhao

Konh

Honarvar

, 3D Motion Planning for Robot-Assisted Active Flexible Needle Based on Rapidly-Exploring Random Trees. Journal of Automation and Control Engineering. 2015; 3(5), pp. 360–367.