IROS2019国际学术会议论文集 1099

Magnetic Needle Steering Model Identifi cation Using Expectation Maximization Richard L Pratt and Andrew J Petruska Abstract Deep Brain Stimulation is used to treat Parkin son s disease and other neurological disorders by implanting an electrode into the brain via a straight path needle insertion Enabling course correction and curved trajectories by using a steerable needle has the potential to improve operative outcomes In this work a physically motivated dynamic model for an actively steered magnetic tipped needle is derived Pro cess and measurement noise covariances and model parameter curvature gain are identifi ed using an expectation maximization EM algorithm Parameter convergence and accuracy are evaluated and an RMS position trajectory accuracy of 0 81mm is calculated for expected conditions The EM algorithm converged for expected parameter variations in simulation which supports the EM implementation s use in identifying the parameters of the model in a physical system I INTRODUCTION Deep Brain Stimulation DBS is an intervention applied to a wide range of central nervous system pathologies including Parkinson s disease 1 and Alzheimer s disease 2 In DBS electrode placement surgeries an electrode is inserted through a burr hole in the skull and directed along a straight path to the target by a rigid apparatus mounted on the head of the patient 3 Unfortunately this technique can lead to incorrect electrode placement When the target is missed additional electrode insertions are performed which increase both trauma to the brain and operative duration No method is clinically available to adjust or correct the electrode trajectory of a single insertion or to insert the electrode accurately along a curved trajectory Approaches to provide tip control to the surgeon fall into two categories of steering passive and active The passive methods rely on a bevel tip needle design such that the forces at the tip cause the needle to curve in a predictable manner 4 Researchers have explored the use of bevel tip needles with various design and actuation methods 5 6 Alternatively the active steering methods allow for external steering input and have a wide range of designs including tendon driven 7 shape memory alloy 8 adjustable paral lel segments 9 and magnetic tipped 10 Both passive and active approaches have limitations Pas sive bevel tip needles are constrained by the amount of asymmetric reaction force the brain can exert on the needle during insertion to generate a curved trajectory 11 12 Active methods must consider physical space and material requirements and add complexity and stiffness to the needle This research was supported by funds from the Boettcher Foundation s Webb Waring Biomedical Research Program TheauthorsarewiththeofMechanicalEngineering Colorado SchoolofMines 1500IllinoisSt Golden Colorado80401 rlpratt mines edu apetruska mines edu Fig 1 Magnetic tip needle diagram labeled with states green and derivation variables red 14 13 Magnetically steered needles have been proposed to limit complexity added to the needle as they only require a permanent magnet in the needle tip and add no complexity to the trailing wire Recently the design and actuation of a magnetic tip steerable needle for guiding a DBS electrode insertion was presented and its functionality was demonstrated in an agarose brain tissue phantom 14 The design followed curved trajectories under direct human operator control and executed trajectories with multiple targets The next step toward a clinically viable device is to demonstrate a controllable system which requires a kinematic model Webster et al showed that a bevel tip needle could be represented with a bicycle model with a fi xed steering angle 4 and Ko et al adapted the same model but with a steerable angle 9 However the bicycle model does not necessarily represent the physics of a magnetic tip needle It is subject to a no slip condition that constrains the velocity of the needle wire and tip in the direction perpendicular to their orientation but the authors have found that for needle insertion in agarose the tip violates this condition Thus the scope of the bicycle model may be limited when applied to magnetic needle steering This paper derives a physically motivated needle steering model for the application presented in 14 An expectation maximization EM technique is introduced to identify the model s unknown parameter as well as process and measure ment noise covariances Parameter convergence and accuracy for simulated data are analyzed results are discussed and conclusions are presented II DERIVATION OFNEEDLEMODEL The needle design incorporates a nonmagnetic fl exible trailing wire permanent magnet ball joint and permanent magnet tip as diagrammed in Fig 1 Using an external 2019 IEEE RSJ International Conference on Intelligent Robots and Systems IROS Macau China November 4 8 2019 978 1 7281 4003 2 19 31 00 2019 IEEE5432 magnetic fi eld B fi eld with full 3D orientation control the direction of the magnetic tip is actively steered The insertion of the needle is actuated using a motorized linear advancer at the proximal end of the trailing wire Due to an asymmetric stress distribution the tip penetrates through tissue in a preferential direction and the wire follows As shown in Fig 1 the heading of the wire is indicated by h the position of the ball is p and the direction of the B fi eld is B These variables represented with 3D Cartesian vectors B and h being specifi cally unit vectors form the states of the system x Figure 1 also indicates several additional derivation variables heading of the needle tip T the angle from h to T T and the angle from T to B B To derive x for a physically motivated state space model fi rst assume the needle progresses in the direction of the wire heading such that p hv where v is the input scalar insertion velocity Then B B B where Bis the angular rate of change of the B fi eld and the same is true for h and h as summarized in Eq 1 x p h B hv h h B B 1 Using Bas an input instead of B itself completes the equa tion for B and proves advantageous As the B fi eld cannot be changed instantaneously it has an associated time rate of change controlling Bis more physically representative than directly controlling B Furthermore saturating Bas a control input can directly mitigate the windshield wiping effect of rapidly moving the needle tip through the medium that could result in tissue damage during clinical application To derive an equation for h we assume a homogeneous medium such that the needle turns with a constant radius of curvature for a given T Then as summarized in Eq 2 1 h is inversely proportional to the radius of curvature r for a tighter curve the needle turns sharper 2 h is proportional to v if the needle is not inserting it cannot turn 3 h is proportional to kh Tk the more misaligned the tip and wire are the sharper the needle turns k hk v r kh Tk 2 As shown in Fig 1 the wire heading and the tip defi ne a plane Assume steady state dynamics such that B must also be in this plane Steady state dynamics are justifi ed by assuming that the tip tissue interaction dynamics are much faster than the control dynamics due to the saturation on B so that the non planar forces on the tip from the B fi eld are insignifi cant h must also be in plane because no forces exist to move it out of plane and perpendicular to h because as a unit vector h cannot change in its own direction Equation 3 puts h in the appropriate direction and removes the magnitude designations h v r h T h 3 At equilibrium T and B have an algebraic relationship so Eq 3 can be simplifi ed to remove the explicit dependence on T Due to the steady state planar assumption T and B are in plane Physically the torque from the B fi eld on the magnetic tip is counteracted by an equivalent restoring spring torque from the medium which causes T and B to differ by a nonzero B Assuming small tip defl ections which is accurate for clinically relevant trajectories allows for small angle approximation such that these torques become linearly related w r t B and it can be shown that h cv h B h 4 where unknown constant c curvature gain combines the un known ratio from the linear torque balance and the unknown radius of curvature inverse r 1 In addition to the physically motivated terms the nu merically implemented model adds the stabilizing terms h 1 khk2 and B 1 kBk2 in h and B respectively to regulate them to unit length and prevent the accumulation of numerical error Finally letting C be the identity for full state feedback results in the state space dynamic needle steering model x p h B hv cv h B h h 1 khk2 B B B 1 kBk2 5 y Cx x 6 III METHODS The unknown parameter c must be chosen to implement the derived needle model For optimal implementation it should refl ect the true properties of the system Petruska et al found diffi culty in determining an exact coeffi cient by directly measuring c from empirical data 14 so we implement an algorithmic approach by augmenting c to the state vector Eqs 7 and 8 and estimating it with an extended Kalman smoother EKS x p h B c hv cv h B h h 1 khk2 B B B 1 kBk2 0 7 y p h B Cax I000 0I00 00I0 x 8 A EKS The derived needle model can be written in the following generic discrete form for nonlinear state space systems xn 1 f xn un wn 9 yn h xn vn 10 with w N 0 Q and v N 0 R where Q is the unknown process noise covariance R is the unknown mea surement noise covariance f is the numerical integration of Eq 7 from time n to n 1 and h is the measurement model This system can be linearized using a fi rst order Taylor series expansion so that it can be approximated as a time varying linear system 5433 xn 1 Anxn Bnun wn 11 yn Cnxn vn 12 A regular Kalman smoother minimizes the variance of the output estimation error of a given dataset for a linear system model In other words given a batch data set of measurements YN y1 y2 yN and inputs UN u1 u2 uN where N is the total number of data the Kalman smoother results in an optimal trajectory estimate for state XN and covariance PN EKS applies regular Kalman smoothing to local linearizations of a nonlinear system We employ the Rauch Tung Striebel smoother type of the EKS as given in 15 on the nonlinear needle model Numerical integration of the state and sensitivity matrices are computed using the Dormand Prince 8th order Runge Kutta integrator 16 To implement the EKS Q and R must be presumed Conveniently accurately identifying Q and R is already necessary to optimally implement the derived needle model While an estimate of R can be calculated from sensors the best method to determine Q and ultimately validate R is by using empirical data We do so by implementing the EM algorithm B EM Algorithm The EM algorithm was developed to solve a maximum likelihood estimate problem where the data is incomplete or missing for directly evaluating the likelihood function 17 If the state variables XT are considered as missing data then the EM algorithm can be employed for simultaneous state and parameter estimation Thus the EM algorithm pro vides a method to optimize estimates for all three unknown parameters c Q and R The EM algorithm consists of two iterated steps expectation E Step and maximization M Step and is implemented as shown in Algorithm 1 Shumway and Stoffer found a closed form EM solution for the discrete linear state space model 18 Their solution can be adapted to the time varying linear system in Eqs 11 and 12 1 E Step The EKS is a well established E step method to estimate XNand PNin nonlinear applications 19 22 A full explanation is omitted for brevity but the backwards pass smoothed variables are summarized in Eq 13 Ln Pn nA n nP 1 n 1 n Pn N Pn n Ln Pn 1 N Pn 1 n L n xn N xn n Ln xn 1 N xn 1 N 13 Note that in this EM implementation the E step is re peated for reasons explained in Section III C 2 M Step Shumway and Stoffer derived equations for maximizing Q and R in linear state space 18 23 The maximization equations shown in Eqs 14 and 15 are adapted to the time varying linear model Eqs 11 and 12 using the smoothed EKS variables Eq 13 Algorithm 1 EM Algorithm with repeated E step Data YNand UN Result Converged parameters Q R and XN 1Guess parameters Q and R and initial state x0 and covariance P0 2do 3do 4E Step Find the smoothed distribution estimate p XN YN UN Qi Ri by using EKS to fi nd XNand PN 5x0 x0 N 6while k x0k between iterations tolE Step 7M Step Maximize the likelihood function L Q R independently for Q and R to obtain Qmax argmax Q L Q R Rmax argmax R L Q R 8Qi Qmaxand Ri Rmax 9while k Qk k Rk between iterations tolEM Qmax 1 N N 1 X n 0 x n 1 N f xn N x n 1 N f xn N An N Pn NA n N Pn 1 N Pn 1 NL nA n N An NLn Pn 1 N 14 Rmax 1 N 1 N X n 0 yn Cn xn N yn Cn xn N Cn Pn NC n 15 C Repeated E step to Converge Initial Conditions Because c cannot be directly measured its initial condition must be guessed If the guess is too inaccurate the EM algorithm may not recover the true c Thus a repeated E step is implemented within the EM algorithm to converge inaccurate initial state conditions We consider the E step converged when the difference between successive initial states is below a tolerance i e kx0 x0 Nk tolE Step Note that while the repeated E step is motivated by large error in unmeasured c it will minimize all initial state estimate error for both measured and unmeasured states IV SIMULATIONSETUP A Generating Data Generating the required batched data YNand UNwith a known Qtrueand Rtrueallows for comparison of the con verged values of Q and R identifi ed by the EM algorithm and their induced trajectory error to the true values and optimal trajectory error to evaluate the algorithm s performance We attempted to generate interesting and relevant data to provide the most confi dence in our results Interesting data was considered to be a curved trajectory which is affected by total time UN and c Total time was fi xed and unwas set constant except for BZ which was set to increase linearly so that all ctruevalues resulted in curved trajectories in the 5434 050100150200 0 50 100 150 200 250 300 Fig 2 Position trajectory examples demonstrating the effect of Qtrue N and ctrueare set to the defaults in Table I TABLE I VARIABLES FOR CONVERGENCE ENVELOPE VariableDefault Default Justifi cation ctrue10Best guessofphysicalsystem value cguess15 Suffi ciently far to provide interest ing convergence Qtrue15 10 7Worst case RtrueQtrueMakes both Q and R relevant Qguess0 1Qtrue Suffi ciently far to provide interest ing convergence Rguess10RtrueDifferent interesting convergence than Qguess Nsamples100Worst case 1 Qtrueis a diagonal matrix with entries of this value for the 9 non augmented states Section IV C and 0 for the c variance value since c is presumed constant x y plane Variations of an example trajectory are shown in Fig 2 Relevant data required realistic fi xed inputs and likely values of varying inputs see Section IV B For U v was limited below 3mm s which is the expected operating condition and B was set suffi ciently low to satisfy the planar dynamics assumption Section II Total time was set to 100s which allowed for traversal across the human brain space at an average insertion rate B Testing Conditions Table I shows the variables used to characterize the parameterization algorithm These variables were tested across their likely conditions for the presented needle model From Section II if the magnetic torque on the needle tip is signifi cantly larger than the restoring spring force of the medium then c approaches r 1 Thus c represents the inverse of the minimum turning radius of the needle and has units of m 1 The upper limit of c was set at 1000 minimum turning radius of 1mm which suffi ciently approximates an instantaneous turn The lower limit was set at 10 minimum turning radius of 100mm which is the lowest possible value to still allow a signifi cant turn within the human brain When using varying ctrue cguesswas set to 100m 1 and the initial c covariance was set to 104m 2 which were both empirically found to result in convergence across all ctrue An estimate for Rtruecan be found from the expected accuracy of the sensors The physical system in development expects to measure B fi eld heading with a 0 1 accuracy and position with 0 4mm RMS accuracy These combine for an average expected Rtrueof 5 10 7 which is on the same order of the default Qtrue For a model that exactly refl ects reality with no error Qtrue 0 We approximated this lower limit with 5 10 10 to represent process noise with a negligible effect on the system Fig 2 The upper limit was set at 5 10 7 because as seen in Fig 2 any larger process noise overtook the model dynamics for the desired trajectory space which would render the model invalid Qguessand Rguessare expected to be within three orders of magnitude of Q and R respectively for the physical system so they were varied by a relative scaling factor from true from 0 001 to 1000 under four different combinations each individually together and inversely C Presumed Conditions Conditions not expected to greatly impact results were not extensively tested We found that while the relative difference of Q and R decayed at a consistent rate Q and R percent error plateaued beyond 50 iterations Thus the EM loop limit was set at 50 and a convergence tolerance was not considered Q and R were constrained to diagonal matrices for sim plifi cation of analysis Physical Q and R are not expected to be diagonal which could impact results However they are expected to be heavily weighted diagonally so any effects are expected to be minor If Q and R are not equal in magnitude then noise added to the system from the larger will dominate that from the smaller Thus the larger becomes more important to parameterize accurately Correspondingly the EM algorithm will strongly optimize the larger of the two While this is an acceptable and useful result for the purposes of evaluating the performance