3D Motion Planning Algoithms fo Steeable Needles Using Invese Kinematics Vincent Duindam 1, Jijie Xu 2, Ron Alteovitz 1,3, Shanka Sasty 1, and Ken Goldbeg 1,2 1 Depatment of EECS, Univesity of Califonia, Bekeley, {vincentd,sasty}@eecs.bekeley.edu 2 Depatment of IEOR, Univesity of Califonia, Bekeley, {jijie.xu,goldbeg}@bekeley.edu 3 Compehensive Cance Cente, Univesity of Califonia, San Fancisco, onalt@bekeley.edu Abstact: Steeable needles can be used in medical applications to each tagets behind sensitive o impenetable aeas. The kinematics of a steeable needle ae nonholonomic and, in 2D, equivalent to a Dubins ca with constant adius of cuvatue. In 3D, the needle can be intepeted as an aiplane with constant speed and pitch ate, zeo yaw, and contollable oll angle. We pesent a constant-time motion planning algoithm fo steeable needles based on explicit geometic invese kinematics simila to the classic Paden-Kahan subpoblems. Reachability and path competitivity ae analyzed using analytic compaisons with shotest path solutions fo the Dubins ca (fo 2D) and numeical simulations (fo 3D). We also pesent an algoithm fo local path adaptation using null-space esults fom edundant manipulato theoy. The invese kinematics algoithm can be used as a fast local planne fo global motion planning in envionments with obstacles, eithe fully autonomously o in a compute-assisted setting. 1 Intoduction Steeable needles [18] fom a subclass of flexible needles that povide steeability due to asymmetic foces acting at the needle tip, fo example due to a beveled suface [18] o a kink nea the end of the needle [7]. By otating the needle at the base, the oientation of the tip can be changed and hence the tajectoy of the needle can be contolled. Steeable needles diffe in this espect fom symmetic flexible needles, which can only be contolled by applying asymmetic foces at the base [4], not at the tip. The exta mobility of steeable needles ove igid needles can be hanessed in medical applications such as bachytheapy [2] and bain sugey [7] to each difficult tagets behind sensitive o impenetable aeas.
2 Duindam, Xu, Alteovitz, Sasty, Goldbeg needle Ψ s v obstacles z x Ψ n y Ψ n Ψ g ω (a) Steeable needle. (b) Equivalent aiplane. (c) Coodinate setup. Fig. 1. Model setup of a steeable needle and a kinematically equivalent aiplane with fixed speed and pitch ate, zeo yaw, and contollable oll ate. Expeimental studies [17] show that the motion of steeable needles can be appoximated as having a constant adius of cuvatue that is independent of insetion speed. The contol inputs fo the needle ae the insetion speed and otation (oll) angle, although fo motion planning (the topic of this pape) insetion speed is often not impotant. The otation angle is then the only eal contol input and tajectoies can be paameteized by insetion depth. A steeable needle is thus kinematically equivalent to an aiplane with fixed speed and pitch ate, zeo yaw, and contollable oll ate (Fig. 1b). Motion planning fo steeable needles is an impotant poblem and has been studied in seveal ways in liteatue. Most studies focus on plana motion, fo which the contol input educes to switching between cuve-left and cuveight. Alteovitz et al. [2, 1, 3] pesent a oadmap-based motion planning famewok that explicitly incopoates motion uncetainty and computes the path that is most likely to succeed. Minhas et al. [12] show planning based on fast duty cycle spinning of the needle, effectively emoving the limitation of a fixed-adius path but equiing continuous angula contol input. Kallem et al. [10] intoduce a contolle that stabilizes the needle motion to a plane, allowing pactical implementation of plana motion planning methods. The fist 3D motion planning algoithm was intoduced by Pak et al. [14, 15] and used diffusion of a stochastic diffeential equation to geneate a family of solution paths. The authos also descibe seveal extensions to avoid obstacles. Duindam et al. [6] pesented a second 3D motion planning algoithm that uses fast numeical optimization of a cost-function to compute feasible needle paths in 3D envionments with obstacles. This pape pesents a diffeent solution to the 3D motion planning poblem fo steeable needles, based on invese kinematics. We popose a new geomety-based algoithm inspied by the Paden-Kahan subpoblems in taditional invese kinematics algoithms [13]. Just as the Paden-Kahan subpoblems, ou algoithm (Section 3) can be fully descibed in geometic tems
3D Motion Planning Algoithms fo Steeable Needles 3 of intesecting lines, planes, and cicles, and computing the solution simply equies a few tigonometic functions. We analyze eachability and competitivity of the solution (Section 4) using analytic compaison to the Dubins ca solution and numeical simulations. We also pesent a method to locally adapt needle paths using null-motions (Section 5). The poposed motion planning algoithms find feasible needle paths vey quickly and povide some capability fo obstacle avoidance. As the cuent accuacy of medical data is limited, these algoithms may be sufficient by themselves, but we envision them being pat of moe geneal global motion planning systems, as discussed in Section 6. 2 Poblem statement and modeling assumptions 2.1 Model paametes and assumptions Thoughout this pape, we only conside the idealized kinematics of the needle in a static envionment. We assume that the motion of the needle is fully detemined by the motion of the needle tip, that the motion of the needle tip is instantaneously along a pefect cicle of constant adius, and that otations of the base ae instantly tansmitted to otations of the tip. Expeimental esults [17] show that needle mateials can be chosen such that the needle indeed moves along an ac of appoximately constant adius, but the effects of tissue inhomogeneity, fiction, and needle tosion can be significant and will equie compensation [10] in pactical applications. Unde these assumptions, the motion of the needle is detemined kinematically by two contol inputs: the insetion velocity, denoted v, and the tip otation velocity, denoted ω. We pesent the kinematics model fo geneal v(t) but emove this edundant degee of feedom in the next section. Fig. 1c illustates the model setup. We igidly attach a coodinate fame Ψ n to the tip of the needle, with axes aligned as in the figue, such that the z-axis is the diection of fowad motion v and needle oientation ω, and the beveled tip causes the needle to otate instantaneously aound the line paallel to the x-axis and passing though the point (0,,0). Following standad obotics liteatue [13], the position and oientation of the needle tip elative to a efeence fame Ψ s can be descibed compactly by a 4 4 matix g sn (t) SE(3) of the fom [ ] Rsn (t) p g sn (t) = sn (t) (1) 0 1 with R sn SO(3) the otation matix descibing the elative oientation, and p sn T(3) the vecto descibing the elative position of fames Ψ s and Ψ n. The instantaneous linea and angula velocities of the needle ae descibed by a twist V sn se(3) which in body coodinates Ψ n takes the convenient fom
4 Duindam, Xu, Alteovitz, Sasty, Goldbeg 0 0 0 ω(t) 0 0 Vsn(t) n = v(t) v(t)/ ˆV sn(t) n = ω(t) 0 v(t)/ 0 0 v(t)/ 0 v(t) (2) 0 0 0 0 0 ω(t) in equivalent vecto and matix notation. The twist elates to g sn as ġ sn (t) = g sn (t)ˆv n sn(t) (3) This kinematic model is the same as the unicycle model by Webste et al. [16]. When the twist is constant, (3) becomes a linea odinay diffeential equation (ODE) that can be integated as g sn (t) = g sn (0)exp(tˆV n sn) (4) fo which a elatively simple analytic expession exists [13]. In the path planning algoithms, we constuct paths fo which ˆV sn n is piecewise constant and compute the esulting tansfomation using this efficient analytic expession. 2.2 Poblem statement The objective of the motion planning algoithms in this pape is to find feasible paths between given stat and goal configuations in the absence of obstacles 1. Moe pecisely, the inputs to the algoithm ae an initial needle pose g stat SE(3) and a desied needle pose g goal SE(3). The outputs of the algoithm ae contol functions v( ) and ω( ) and a finite end time 0 T <, such that the solution g sn ( ) of the diffeential equation (3) with g sn (0) = g stat satisfies g sn (T) = g goal. If no feasible path can be found, the algoithm etuns failue. The kinematics equations (2) and (3) ae invaiant to time scaling, in the sense that the path taced out by the needle does not change if the contol inputs v(t) and ω(t) ae scaled by the same (possibly time-vaying) facto. Theefoe, we can simplify the motion planning poblem by assuming without loss of geneality that v(t) 1, which is equivalent to paameteization by insetion depth [10, 6]. The insetion time T thus epesents the total path length since T 0 v(t) dt = T dt = T. 0 Although motion planning is based on connecting geneal 3D poses (full position and oientation), solving a diffeence in initial o final oll angle is tivial as this degee of feedom is diectly contolled though ω( ). So although the inputs to the algoithm ae geneal elements of SE(3), we often mainly focus on path planning between given stat and goal position and diection of the needle, i.e. only consideing the z-axis of Ψ n. Given a solution fom the stat position and diection to the goal position and diection, the full motion plan fom a stat pose to a goal pose follows diectly by adding the equied oll-otations to the beginning and end of this solution. 1 Section 5 discusses a way to avoid obstacles using path adaptation.
3D Motion Planning Algoithms fo Steeable Needles 5 ω π π ω θ 3=π θ 4=π θ 2 0 t 1 t 2 t 3 t 0 t 1 t 2 t 3 t 4 t θ 1 (a) Plana (2D) path. (b) Spatial (3D) path. Fig. 2. Stuctue of the solution ω( ) fo the 2D and 3D motion planning poblems. 3 Path planning using invese kinematics We pesent two motion planning solutions based on invese kinematics, one fo the plana (2D) case and one fo the geneal spatial (3D) case. The motion planning poblem is consideed plana if the stat position p s, stat diection z s, goal position p g, and goal diection z g ae all in the same plane. In both invese kinematics solutions, we look fo a contol input function ω( ) of a vey specific fom, namely a function that is zeo eveywhee except fo a fixed numbe of Diac impulses (two in the plana case, fou in the spatial case, see Fig. 2). Geometically, this means we look fo tajectoies that ae concatenations of a fixed numbe of cicula segments with adius : the needle moves along a cicle when ω = 0, and instantaneously changes diection at the time instants that ω is a Diac impulse. Futhemoe, the magnitudes of the Diac impulses in the plana case ae constained to be exactly π, coesponding to a change in diection between cuve-left and cuve-ight. We also choose a spatial solution fo which the last two impulsive otations ae π, making the last thee segments of the spatial path co-plana as well. The specific choices in the stuctue of ω( ) esult in geometically intuitive solutions that ae elatively staight-fowad to compute. The simplicity of the poposed solutions comes at the cost of not necessaily being optimal in tems of path length o equied contol effot. Pactical implementations should clealy not use impulsive otational contol and constant insetion speed, but altenate between pue insetion until the desied depth t i is eached, and pue otation to the desied angle θ i. 3.1 Invese kinematics in 2D We fist conside the plana path planning poblem with ω( ) as in Fig. 2a. The elative position of the stat and goal ae descibed by two displacements x and y, thei elative oientation by a single angle θ. The pupose of the path planning algoithm is to find the thee insetion depths t 1,t 2,t 3 descibing a feasible needle path fom stat to goal. Note that the needle tavels a distance t i = α i when moving along a cicle of adius fo α i adians, and hence we can equivalently look fo the thee angles α i in Fig. 3. These should be
6 Duindam, Xu, Alteovitz, Sasty, Goldbeg z g θ z g θ p g p g α 3 y α 2 α 3 y α 1 α 2 z s p s x Fig. 3. Two geometic solutions fo the same plana invese kinematics poblem, both using sequential bevel-left, bevel-ight, bevel-left motions. α 1 z s p s x such that if we stat at p s in the diection z s heading left, move α 1 fowad, tun π, move α 2 fowad, tun π, and move α 3 fowad, we aive exactly at the desied goal pose. The mioed case stating with a ight tun can be computed similaly. We can solve fo the angles α i by looking at the setup in Fig. 3 and ealizing that the thee centes of otation (maked by in the figue) fom a tiangle with known edge lengths. Using the cosine ule fo this tiangle, we can wite cos(α 2 ) = 1 (x + cos(θ))2 + (y sin(θ)) 2 8 2 (5) This equation has two solutions fo α 2, which coespond to the two paths shown in Fig. 3. With α 2 known, the othe two angles follow uniquely as α 1 = atan2 (y sin(θ),x + cos(θ)) 1 2 (π α 2) (6) α 3 = θ α 1 + α 2 (7) with atan2 the invese tangent function solved ove all quadants. Since the needle can only move fowad, angles must be chosen as α i [0,2π). The equied insetion distances t i follow immediately as t i = α i. 3.2 Invese kinematics in 3D Now conside the 3D invese kinematics poblem of connecting two geneal poses in SE(3) by a valid needle path. We popose one solution using eight consecutive inset and tun motions as shown in Fig. 2b; an explicit geometic solution using fewe motions is still an open poblem. The geomety of this solution is illustated in Fig. 4. The poblem is split into two pats: fist, the needle is tuned and inseted such that its instantaneous line of motion (the instantaneous diection of the needle) intesects the
3D Motion Planning Algoithms fo Steeable Needles 7 z g β 3 z g y 1 β 1 z s y 2 p s β 2 y s β 1 p 2 z 2 q p g p s p 2 β 6 p g β 4 β 5 z 3 q y 3 (a) Rotate β 1 aound z s until the needle s (y, z)-plane contains q. Inset β 2 until q is on the line though z 2. (b) Rotate β 3 aound z 2 until the needle s (y, z)-plane contains p g. Solve the emaining plana poblem. Fig. 4. Geometic deivation of an invese kinematic solution on SE(3). line descibing the goal position and diection. Second, the emaining plana poblem is solved using the solution fom Section 3.1. Moe pecisely, we fist choose any point q on the line defined by p g and z g. This point q will be the intesection point of the two lines defining the emaining plana poblem. With q defined, the needle is fist otated by θ 1 = β 1 until its (y,z)-plane contains q. The equied angle β 1 satisfies tan(β 1 ) = xt s (q p s ) y T s (q p s ) which has two solutions β 1 that diffe by π. Second, the insetion distance t 1 = β 2 is solved such that the line though the needle tip in the diection z 2 contains the point q. If q is outside the cicle descibing the needle motion along β 2, two solutions exist, with z 2 eithe pointing towads (as in the figue) o away fom q. These solutions ae β 2 = atan2 ( z T s q v,y T 1 q v ) ± accos ( q v with q v := q p s + y 1. No solution exists if q is inside the cicle ( q v < ). Thid, the needle is otated by θ 2 = β 3 until p g (and hence the whole line though q and p g ) is contained in the needle s (y,z)-plane: tan(β 3 ) = xt 2 (p g p 2 ) y T 2 (p g p 2 ) ) (8) (9) (10) which again gives two solutions that diffe by π. The emaining angles β 4,β 5,β 6 (coesponding to the time segments t 2,t 3,t 4 ) can then be solved using the 2D planne fom Section 3.1.
8 Duindam, Xu, Alteovitz, Sasty, Goldbeg 4 θ b 4 θ e q θ e Dubins path θ d θ d θ a IK path θ c p (a) Left-staight-left Dubins path. (b) Left-staight-ight Dubins path. Fig. 5. Paths geneated by the 2D IK algoithm vs. optimal paths fo a Dubins ca. In this algoithm, we have the following degees of feedom to choose a solution. Fist is the choice of the point q: this can be anywhee on the line containing p g and z g, which means vaying q geneates a one-dimensional subspace of possible invese kinematics solutions. Second, we can choose one of two possible solutions β i fo each of the fou angles in equations (8 10) and (5), esulting in 2 4 possible combinations. Not all of these choices may give feasible paths fo a given stat and end pose, and it is also not diectly obvious which choice will esult in the best path between the two poses. Nevetheless, since the invese kinematics equations can be computed vey quickly, one can simply compute all combinations fo a numbe of choices of q and pick the best solution, with best defined fo example using a cost function [6]. 4 Reachability and competitivity To evaluate the quality of the paths geneated by the pesented invese kinematics (IK) solutions, we study the set of eachable needle poses and competitivity [9, 8] of the computed solutions. Competitivity in this case efes to the path length of the computed solution; it has no elation to competitivity in the sense of computational speed (the IK algoithm uns in constant time). 4.1 Reachability and competitivity in 2D Conside fist the solution to the plana poblem as descibed in Section 3.1. The algoithm will clealy only find a solution if the ight-hand side of (5) has nom less than o equal to one, o geometically, if the centes of the cicles tangent to the stat and goal poses ae no fathe than 4 apat. This condition defines the set of eachable elative needle poses. To descibe competitivity of the algoithm fo these eachable poses, we compae the IK solutions to the tajectoies geneated by allowing an infinite numbe of diection changes. In that case, a tajectoy can be geneated with
3D Motion Planning Algoithms fo Steeable Needles 9 an abitay adius of cuvatue lage than o equal to, by asymmetically cycling between heading-left and heading-ight and taking the limit of this cycling fequency to infinity. This means that in the limit, the needle can behave like a Dubins ca [5] with minimum adius of cuvatue. The optimal path fo a Dubins ca is known to consist of two cicula acs with adius connected by anothe cicula ac o a staight line [5]. Fo a given stat and end pose, the IK solution only diffes fom the Dubins path if the connecting segment fo the Dubins path is a staight line; the IK solution will still contain a (sub-optimal) cicula ac. Futhemoe, the Dubins path may stat and end with cicula acs in the same diection o in opposite diections (Fig. 5), wheeas the IK solution always stats and ends with a tun in the same diection. It is intuitively clea that the lagest diffeence in path length occus at the bode of the eachable space whee the thee cicles of the IK solution ae aligned. In the fist case (Fig. 5a), the maximum atio of the path lengths is sup IK path Dubins path = sup θ a + θ b + 2π θ i 0 θ a + θ b + 4 = π 2 (11) Fo the second case (Fig. 5b), we can compute the length of the staight-line segment q p as q p 2 = 4 2 (2 sin(θ d ) sin(θ e )) 2 + 4 2 (cos(θ d ) cos(θ e )) 2 (12) fom which we find that the maximum path length atio equals sup IK path Dubins path = sup θ c + 2π θ e θ i 0 θ c + 2θ d + θ e + q p 1.63 > π 2 (13) The degee of competitivity of the 2D IK solution is hence appoximately 1.63. Note that this is a bound on the competitivity that does not take into account the numbe of diection changes; fo medical applications, this numbe should be kept small to avoid excessive tissue damage. 4.2 Reachability and competitivity in 3D Continuing with the 3D IK solution fom Section 3.2, we pesent a eachability and competitivity analysis based on numeical simulation. Fomal geometic poofs and bounds of the algoithm ae subject of futue eseach; at this point we do not have a good appoximation fo the optimal path and simply compae the IK solutions to the Euclidean distance between the stat and goal positions. Futue wok could compae the pesented solution to the paths geneated by the method of Pak et al. [15]. We take q = p g thoughout the analysis; diffeent choices give qualitatively simila esults. Fist conside Fig. 6. This illustates the lengths of the IK tajectoies stating at the cente of the figue and ending at goal positions in the plane
10 Duindam, Xu, Alteovitz, Sasty, Goldbeg 6 6 4 4 2 2 0 0 2 2 4 4 6 6 4 2 0 2 4 6 (a) Visualization of elative path lengths (shote paths ae dake) as a function of the goal position. 6 6 4 2 0 2 4 6 (b) Seveal example paths coesponding to vaious points in the image ( delimits path segments). Fig. 6. Reachability and elative path lengths obtained using the invese kinematics algoithm. The algoithm ties to find a path fom the cente of the image to each pixel in the image, with both stat and goal diection aimed to the ight. of the figue, with stat and goal diections equal and pointing to the ight. The bightness of each pixel indicates the length of the IK path divided by the Euclidean distance between the stat and goal locations: dak colos epesent small atios (good paths) while light colos epesent lage atios (bad paths). Fig. 6b illustates seveal examples of solution tajectoies. A set of eachable states shows up in the figue as the figue-eight aound the stat location. Points outside this shape cannot be eached fo the given goal diection. The figue also shows a distinction between poses that can be eached with easonably shot cuves (dake egion) and poses that equie significantly longe paths (lighte egion). This distinction is shap in the aea in font of the needle (ight side of the figue) but is moe diffuse fo poses on the sides of the needle (top and bottom of the figue). If we conside competitivity in an infomal way, meaning whethe the algoithm can geneated paths of easonable lengths, we can say that the algoithm is competitive in the dake egion of the figue; elative poses that ae in the lighte egion may be eachable, but the paths ae so unwieldy that they ae of little pactical use in ou bachytheapy application. Fig. 7 shows additional plots geneated by vaying the two emaining degees of feedom in placing the goal pose: the in-plane yaw angle and out-ofplane pitch angle (the invese kinematics solution is invaiant to oll about the initial and final needle diections). The figue shows that as the goal diection is tuned away fom the staight-ahead case (change in yaw), the set of eachable and competitive paths otates in the same diection while maintaining a oughly simila shape. As the goal diection is otated out of plane (change in pitch), the compet-
yaw 0 o 3D Motion Planning Algoithms fo Steeable Needles 11 60 o 120 o 180 o pitch 0 o 30 o 60 o Fig. 7. Reachability and elative path lengths as in Fig. 6 fo vaious elative yaw and pitch angles fo the goal pose. Gid lines ae 2 units apat. itive paths nea the stating pose disappea until only poses at a significant distance (thee to five times the adius of cuvatue) ae competitive. In the thee-dimensional case, it emains a difficult task to pecisely chaacteize the set of poses that can be eached with a easonably shot path. Compaing the solutions to the Euclidean distance povides some insight but no global bound on the solutions: competitivity measues ae unbounded when compaing to the Euclidean distance, since fo infinitely close but non-collinea needle oientations the path length of the IK solution emains finite. To use the invese kinematics planne as a local planne in global oadmapbased planning methods such as the Pobabilistic Roadmap method [11], it is impotant to chaacteize the set of goal poses that ae eachable fom a given needle pose. Numeical simulations such as those shown in Fig. 7 can be used to constuct an appoximation of the set of goal poses that can be eached with a competitive path (with competitive in the infomal meaning of easonably shot path ). This set can be used as the definition of neighbohood, i.e. those needle poses that ae likely to be connectible and can become edges in the oadmap if they do not intesect obstacles. 5 Path adaptation using null-motions The pesented invese kinematics (IK) solution can be computed vey quickly but is geneally not the optimal solution in the sense of avoiding obstacles o minimizing path length o equied contol effot. Ealie wok consideed
12 Duindam, Xu, Alteovitz, Sasty, Goldbeg q 8 goal pose q 5 q 6 q 7 Ψ n family of solutions pull to hee oig. IK solution q 4 q 3 q 2 q 1 Ψ s stat pose (a) A needle path as an 8-joint obot. (b) Null-motions geneate a family of needle paths between stat and goal. Fig. 8. Repesentation of a needle path as a obot with eight joints, and use of its null motions to geneate a family of solution cuves by pulling in diffeent diections. path planning as a pue numeical optimization poblem [6], but in this section we show how sub-optimal paths such as those geneated by the IK planne can be locally optimized and adjusted using null-motions. Conside an IK solution between two geneal 3D needle poses. By constuction, this solution descibes a path fom stat to goal consisting of eight consecutive tuning and insetion contol actions. We can think of this path as a edundant seial obot manipulato am with eight joints (Fig. 8a). Since the elative pose of the goal is given by six paametes, standad obotics theoy [13] tells us that the obot has a two-dimensional space of null-motions, povided it is not at a singulaity. If the joints ae moved in this null-motion space, the shape of the obot (i.e. the shape of the needle path) will change without changing the pose of the end effecto (i.e. the needle tip). The set of null motions is descibed by the null space of the geometic Jacobian J(q) R 6 8 of the obot, which elates the spatial twist Vsn s to the joint velocities q as Vsn s = J(q) q [13]. Given the Jacobian, we can change the shape of the path by changing the joint angles in such a way that q Null(J(q)) at all times. Fig. 8b shows an example of how one invese kinematics solution can be locally tansfomed in this way into a family of solution cuves. Intuitively, this set of cuves was geneated by stating fom the IK solution indicated in the figue (the dashed line), and pulling on the cuve fom seveal points laid out in a cicle, while holding the stat and end pose of the needle fixed. Moe pecisely, we model the obot as a viscous system with damping in each joint that counteacts applied foces, and wite the govening equations as q = B(q)J T i (q)f i + B(q)J T (q)λ (14) 0 = J(q) q (15)
3D Motion Planning Algoithms fo Steeable Needles 13 with F i the wench [13] coesponding to the (known) extenally applied pulling foce, J i (q) the Jacobian of link i at which F i is applied, λ the equied constaint wench acting at the tip to constain its motion, and B(q) > 0 the symmetic positive-definite invese damping matix that elates the joint toques τ to the joint velocities as q = B(q)τ. The fist equation elates the total toque (due to extenal foces F i and λ) to the change in the joint angles, the second equation descibes the end point constaint that should be satisfied. Note that these equations do not elate to any actual physical needle motions and only epesent a mathematical pocedue. Substituting (14) into (15), solving fo λ, and substituting back into (14) esults in an unconstained equation fo q that no longe contains λ: q = ( I BJ T (JBJ T ) 1 J ) BJ T i F i (16) This ODE descibes the evolution of q unde the influence of an extenal wench F i and tip constaint J q = 0. It has a unique solution if B is invetible and J has full ank (no singulaity). Equation (16) pojects the velocity BJ T i F i due to the wench F i along the columns of BJ T onto the null space of J. The matix B(q) defines a metic on the space of toques that can be chosen in any appopiate way, e.g. as a function that dives the system away fom configuations that ae singula o contain negative-length path segments. Fo the example of Fig. 8b, we chose B diagonal with B jj (q) 0 as q j 0 fo all joints j descibing insetion path segments, thus avoiding negativelength path segments. We applied a linea foce in the middle of the kinematic chain (link 5), diected towad one of the dots, and integated (16) ove time to obtain the pod-shaped family of needle paths shown in the figue. This method of path adaptation can be used in fully automated motion planning (e.g. to pefom gadient descent on some cost function with penalty costs fo obstacle penetation) with changes in q constained to be null motions. Moe diectly, fo compute-assisted motion planning as descibed in Section 6, it can povide the use with an intuitive path adjustment tool simila to the contol points on a spline cuve that can be moved to change its local shape. Although thee is no guaantee this appoach will always wok, it povides the use with an additional tool to constuct suitable needle paths. 6 Application in compute-assisted motion planning The invese-kinematics based motion planning algoithm quickly computes feasible needle paths and allows the use to focus on specifying highe-level objectives in tems of stat and goal needle poses. Indeed, the main motivation and eason fo compute-assistance in this motion planning poblem is the degee of unde-actuation and nonholonomicity, which can be dealt with using the pesented appoach. Nevetheless, the algoithms povide no guaantees fo a solution o a stuctued incemental way to seach fo othe tajectoies in case of failue
14 Duindam, Xu, Alteovitz, Sasty, Goldbeg (a) Planning poblem with maked intemediate pose. (b) Fist motion plan using invese kinematics. (c) Second motion plan using invese kinematics. Fig. 9. Difficult motion planning poblem solved using semi-automated planning. due to obstacles o othe complications. The only possibilities ae to choose diffeent paamete settings o, fo the null-motion based method, to ty applying extenal foces at a diffeent point o in a diffeent diection. When it comes to global motion planning, computes ae seveely limited in cognitive abilities and can equie lage amounts of computation powe and time to solve poblems that ae easy fo humans (expeiments using Rapidly Exploing Random Tees equied up to half an hou of computation time [19]). Conside fo instance the motion planning poblem of Fig. 9a: fo the eade it is instantly clea that any feasible path should pass nea the intemediate point maked in the figue, but compute-based plannes such as those descibed in the pevious sections may not be able to find a feasible solution. One way to solve this poblem is to combine human cognitive abilities fo global planning with compute powe fo local planning. If a human path planne indicates the desied intemediate point as in Fig. 9a, the automatic motion planning algoithms can be applied to solve the esulting two subpaths. Finding a path fom the stat location to the intemediate location is tivial, as is finding a path fom the intemediate location to the goal. Figs. 9b and 9c show the esulting subpaths obtained using the invese kinematics planne; compaable esults ae obtained when using diect numeical optimization [6]. Fo this example, the algoithms ae not sensitive to the exact position and oientation of the intemediate pose, but the appoach could be extended to iteate ove seveal intemediate poses nea the one indicated. 7 Conclusions and futue wok This pape pesents constant-time geometically motivated motion planning algoithms fo steeable needles and aiplanes with constant speed and pitch ate, zeo yaw, and contollable oll. The fist algoithm uses invese kinematics (IK) to explicitly compute feasible paths in 3D, the second uses nullmotions to adapt paths to avoid obstacles o achieve othe objectives.
3D Motion Planning Algoithms fo Steeable Needles 15 As biefly discussed, these algoithms can be used as components in lage compute-assisted motion planning schemes that use limited use-input to guide automatic local planning. In futue wok, we also plan to use the IK algoithm as a local planne in (autonomous) oadmap-based algoithms such as PRM [11]. Recent esults using Rapidly-Exploing Random Tees [19] ae encouaging, although computation equiements ae seveal odes of magnitude lage than with diect optimization-based algoithms [6]. Anothe main futue diection of ou eseach is to find a systematic way to include uncetainty duing motion planning. Ou application of steeable needles contains seveal souces of uncetainty, including needle motion uncetainty, tissue flexibility and fiction, and sensing inaccuacies. These uncetainties should be taken into account in the motion planning stage, as discussed and implemented fo the 2D case in pevious wok [3]. The pesented fast local motion planning algoithm can be used to quickly test connectivity and iteatively study the effect of petubations. Finally, eachability and competitivity analyses wee pesented fo the 2D and 3D invese kinematics algoithms. In futue wok, we plan to extend the analysis of the 3D algoithm to povide bounds on the competitivity compaed to the optimal shotest-path solution. A pomising diection is the compaison with paths geneated by the appoach of Pak et al. [15]. Acknowledgments This wok is suppoted in pat by the National Institutes of Health unde gants R01 EB006435 and F32 CA124138 and by the Nethelands Oganization fo Scientific Reseach. We thank Pofessos Okamua, Chiikjian, and Cowan fom Johns Hopkins Univesity fo the helpful comments and suggestions, and Pof. Cowan fo the idea of using null-motions fo path adaptation. Refeences 1. R. Alteovitz, M. Banicky, and K. Goldbeg. Constant-cuvatue motion planning unde uncetainty with applications in image-guided medical needle steeing. In Poceedings of Wokshop on the Algoithmic Foundations of Robotics, July 2006. 2. R. Alteovitz, K. Goldbeg, and A. Okamua. Planning fo steeable bevel-tip needle insetion though 2D soft tissue with obstacles. In Poceedings of the IEEE Intenational Confeence on Robotics and Automation, pages 1640 1645, Apil 2005. 3. R. Alteovitz, T. Siméon, and K. Goldbeg. The stochastic motion oadmap: A sampling famewok fo planning with Makov motion uncetainty. In Poceedings of Robotics: Science and Systems, June 2007. 4. S. P. DiMaio and S. E. Salcudean. Needle steeing and motion planning in soft tissues. IEEE Tansactions on Biomedical Engineeing, 52(6):965 974, June 2005.
16 Duindam, Xu, Alteovitz, Sasty, Goldbeg 5. L. E. Dubins. On cuves of minimal length with a constaint on aveage cuvatue, and with pescibed initial and teminal positions and tangents. Ameican Jounal of Mathematics, 79(3):497 516, July 1957. 6. V. Duindam, R. Alteovitz, S. Sasty, and K. Goldbeg. Scew-based motion planning fo bevel-tip flexible needles in 3D envionments with obstacles. In Poceedings of the IEEE Intenational Confeence on Robotics and Automation, pages 2483 2488, May 2008. 7. J. A. Engh, G. Podna, D. Kondziolka, and C. N. Riviee. Towad effective needle steeing in bain tissue. In Poc. 28th Annu. Intl. Conf. IEEE Eng. Med. Biol. Soc, pages 559 562, 2006. 8. Y. Gabiely and E. Rimon. Algoithmic Foundations of Robotics VI, volume 17 of STAR, chapte Competitive Complexity of Mobile Robot On Line Motion Planning Poblems, pages 155 170. Spinge-Velag, 2005. 9. C. Icking and R. Klein. Competitive stategies fo autonomous systems. In Modelling and Planning fo Senso Based Intelligent Robot Systems, pages 23 40, 1995. 10. V. Kallem and N. J. Cowan. Image-guided contol of flexible bevel-tip needles. In Poceedings of the IEEE Intenational Confeence on Robotics and Automation, pages 3015 3020, Apil 2007. 11. L. E. Kavaki, P. Švestka, J. C. Latombe, and M. H. Ovemas. Pobabilistic oadmaps fo path planning in high-dimensional configuation spaces. IEEE Tansactions on Robotics and Automation, 12(4):566 580, August 1996. 12. D. S. Minhas, J. A. Engh, M. M. Fenske, and C. N. Riviee. Modeling of needle steeing via duty-cycled spinning. In Poceedings of the Intenational Confeence of the IEEE EMBS Cité Intenationale, pages 2756 2759, August 2007. 13. R. M. Muay, Z. Li, and S. S. Sasty. A Mathematical Intoduction to Robotic Manipulation. CRC Pess, 1994. 14. W. Pak, J. S. Kim, Y. Zhou, N. J. Cowan, A. M. Okamua, and G. S. Chiikjian. Diffusion-based motion planning fo a nonholonomic flexible needle model. In Poceedings of the IEEE Intenational Confeence on Robotics and Automation, pages 4611 4616, Apil 2005. 15. W. Pak, Y. Liu, Y. Zhou, M. Moses, and G. S. Chiikjian. Kinematic state estimation and motion planning fo stochastic nonholonomic systems using the exponential map. Robotica, 26:419 434, July 2008. 16. R. J. Webste III, J. S. Kim, N. J. Cowan, G. S. Chiikjian, and A. M. Okamua. Nonholonomic modeling of needle steeing. Intenational Jounal of Robotics Reseach, 5/6:509 525, May 2006. 17. R. J. Webste III, J. Memisevic, and A. M. Okamua. Design consideations fo obotic needle steeing. In Poceedings of the IEEE Intenational Confeence on Robotics and Automation, pages 3588 3594, Apil 2005. 18. R. J. Webste III, A. M. Okamua, N. J. Cowan, G. S. Chiikjian, K. Goldbeg, and R. Alteovitz. Distal bevel-tip needle contol device and algoithm. US patent pending 11/436,995, May 2006. 19. J. Xu, V. Duindam, R. Alteovitz, and K. Goldbeg. Motion planning fo steeable needles in 3D envionments with obstacles using apidly-exploing andom tees and backchaining. In Poceedings of the IEEE Confeence on Automation Science and Engineeing, pages 41 46, August 2008.