Documents

iros11snakeEstimation.pdf

Categories
Published
of 6
All materials on our website are shared by users. If you have any questions about copyright issues, please report us to resolve them. We are always happy to assist you.
Related Documents
Share
Description
Shape Estimation for Image-Guided Surgery with a Highly Articulated Snake Robot Stephen Tully, George Kantor, Marco A. Zenati, and Howie Choset Abstract—In this paper, we present a filtering method for estimating the shape and end effector pose of a highly articulated surgical snake robot. Our algorithm introduces new kinematic models that are used in the prediction step of an extended Kalman filter whose update step incorporates meas
Transcript
  Shape Estimation for Image-Guided Surgery with aHighly Articulated Snake Robot Stephen Tully, George Kantor, Marco A. Zenati, and Howie Choset  Abstract —In this paper, we present a filtering methodfor estimating the shape and end effector pose of a highlyarticulated surgical snake robot. Our algorithm introducesnew kinematic models that are used in the prediction stepof an extended Kalman filter whose update step incorporatesmeasurements from a 5-DOF electromagnetic tracking sensorsituated at the distal end of the robot. A single tracking sensoris sufficient for estimating the shape of the system becausethe robot is inherently a follow-the-leader mechanism withwell defined motion characteristics. We therefore show that,with appropriate steering motion, the state of the filter is fullyobservable. The goal of our shape estimation algorithm is tocreate a more accurate and representative 3D rendered visual-ization for image-guided surgery. We demonstrate the feasibilityof our method with results from an animal experiment inwhich our shape and pose estimate was used as feedback in acontrol scheme that semi-autonomously drove the robot alongthe epicardial surface of a porcine heart. I. I NTRODUCTION With minimally invasive surgery (MIS), a physician typ-ically performs diagnostic or interventional procedures witha surgical tool or robot through small port-like incisions inorder to reduce patient trauma. Unfortunately, with MIS, sur-geons cannot view an operation with direct vision and insteadmust rely on indirect imaging for surgical guidance. It iscommon for a surgeon to use fluoroscopy [1], ultrasound [2],MRI [3], or endoscopy [4] for this purpose. Unfortunately, allof these modalities have limitations. Another option is  image-guided surgery , which seeks to display a virtualized renderedview of an operation for guidance by fusing information froma tracking device with preoperative surface models [5].In this paper, we present a nonlinear stochastic filteringmethod that estimates, with measurements from a magnetictracking sensor, the shape and configuration of a high degreeof freedom surgical snake robot, see Fig. 1. The goal of thiswork is to display an accurate rendering of the snake robotalongside preoperative surface models for image-guidance.While it is possible to simply track the position of the distaltip of the robot, we instead believe that estimating the fullshape and configuration of the robot would provide moreinformative feedback to the surgeon: e.g., if a trajectory toan anatomical target fails due to an anatomical obstruction,viewing the full shape of the robot in relation to the anatomywould tell the surgeon how and why the intended path failed. S. Tully is with the Electrical and Computer Engineering Departmentat Carnegie Mellon University, Pittsburgh, PA 15213, USA. G. Kantorand H. Choset are with the Robotics Institute at Carnegie Mellon Uni-versity, Pittsburgh, PA 15213, USA,  { stully@ece, kantor@ri,choset@cs } .cmu.edu .Marco Zenati is with the Harvard Medical School at Harvard University,Boston, MA 02115, USA,  Marco Zenati@hms.harvard.edu .Fig. 1. The HARP surgical robot navigating on the surface of a phantomheart model. Another reason for estimating the full shape is that we caninfer twist in the robot’s configuration, which can be usefulfor righting joystick inputs and rectifying video. An exampleof the image-guidance we achieve is shown in Fig. 2.To perform shape estimation for a cable-driven snakerobot, we use an extended Kalman filter (EKF) formulationwith newly defined motion models and a forward kinematicmeasurement model that incorporates 5-DOF pose measure-ments at the distal tip of the robot from an electromagnetictracking sensor. Full shape estimation is possible, in thiscontext, because the robot is inherently a follow-the-leaderdevice with explicitly definable motion models.The contributions of the work presented in this paper are1) the novel use of an EKF to estimate the shape of a sur-gical snake robot, 2) new motion models for a cable-drivensurgical snake robot, 3) an analysis of the observability of shape estimation with a single 5-DOF tracking sensor atthe tip of the robot, and 4) a feasibility study of our shapeestimation method through the discussion of an experimentthat involved navigating a robot semi-autonomously on theepicardial surface of a porcine heart.II. B ACKGROUND  A. Image-Guided Surgery Image-guided surgery is a term that is often used to de-scribe a procedure that uses patient-specific medical imagesas a form of visual feedback during surgery. In many cases,this equates to using preoperative CT or MRI data to recon-struct a 3D surface model of anatomical structures, as in [6].With image-guided surgery, a tracking device is integratedwith a surgical tool and registered to the preoperative imagesso that the position of the tool can be overlayed on therendered models, as in Fig. 2.  An example of image-guidance is [5], in which Clearyet. al. use an electromagnetic (EM) tracker registered withpreoperative images. Also, in [7], an automatic registrationmethod is introduced to align EM tracker measurements withpreoperative images using an iterative closest point (ICP)method. Commercial examples include Ensite NavX (St JudeMedical, St Paul, MN, USA) and Carto XP/CartoMerge(Bio-Sense Webster, Diamond Bar, CA, USA), which havebeen applied successfully to electrical mapping for cardiacablation. The majority of existing methods track the tip of asurgical tool in real-time, but we believe it would be moreinformative to view the entire configuration of the tool, as inFig. 2. Tracking the full shape is the subject of this paper.  B. Shape Estimation The use of Fiber Bragg Grating (FBG) sensors is becominga popular method for estimating the shape of a flexible tool.For example, in [8], the authors use an optical fiber with FBGsensing to determine in real-time the shape of a colonoscope.Likewise, in [9], a novel slim FBG wire is inserted into thebiopsy channel of a colonoscope to determine shape. In [10],the authors use optical FBG strain-sensors to measure theshape of a flexible needle in the field of an MRI. While thisis one of the more popular methods for computing the shapeof a tool, there are several issues: the first is that the sensoris temperature dependent. The second issue is that, whilethe overall shape may be accurately detected, the Euclideanposition at the end effector may have accumulated error. Ourshape estimation algorithm presented in this paper avoidsthese two drawbacks. C. HARP Surgical Robot  The robot we are using for MIS is a highly articulatedrobotic probe (HARP), which is a surgical snake robotpresented in [11]–[13]. The advantage of the HARP isthat it has the stability of a rigid device as well as themaneuverability of a flexible tool (a photograph of the robotcan be seen in Fig. 1). This type of robot is unique, in thatit can navigate any curve in a three-dimensional space withonly six actuators. The HARP is made up of many rigidlinks which are actuated at the distal end by three cables. Aprototype version of the HARP has been used experimentallyto investigate epicardial ablation on porcine models [13].III. S NAKE  S HAPE  E STIMATION The objective of our snake shape estimation method is torecursively compute the most likely state parameters that de-fine the robot’s shape and configuration during image-guidedMIS. In this section, we will define the state vector that weare estimating as well as the motion and measurement modelsthat we have developed for this filtering problem.  A. Kalman State Definition The state that we are estimating in a Kalman filter frame-work is defined as follows, X  k  = [ x 0 , y 0 , z 0 , α 0 , β  0 , γ  0 , φ 1 , θ 1 , ... φ N  − 1 , θ N  − 1 ] T  ,  (1) Fig. 2. An example of overlaying a model of a surgical robot onpreoperative surface models for image-guidance. This is a live snapshotfrom an experiment with the HARP navigating semi-autonomously on theepicardial surface of a porcine heart. where  ( x 0 ,y 0 ,z 0 )  is defined to be the position of the mostproximally located link of the robot that we are interested intracking at time-step  k . There typically will be links behindthis first link that we do not care about until they advanceforward, see Fig. 3. Also,  ( α 0 ,β  0 ,γ  0 )  are the yaw, pitch, androll respectively of that first link. The terms  φ i  and  θ i  foreach  i  are angle offsets that we will discuss shortly. T  0 T  1 ...T  15 ...(x    , y    , z     ,   α    , β     , γ    ) 0 0 0 0 0 0   }    Fig. 3. A depiction of the state parameterization we use for defining theconfiguration of the HARP snake robot. Transformation matrices derivedfrom the state describe the pose of each link. To help formulate the pose of a rigid body in threedimensions, we define the following three rotation matrices, R z ( α )=  c α  − s α  0 s α  c α  00 0 1  ,R y ( β  )=  c β  0  s β 0 1 0 − s β  0  c β  ,R x ( γ  )=  1 0 00  c γ   − s γ  0  s γ   c γ   , where the trigonometric notation has been simplified forconvenience (i.e.,  s γ   = sin( γ  ) ). With these rotation matrices,we can describe the pose of the most proximally referencedlink as a function of the Kalman state with a transformationmatrix, T  0 ( X  k ) =   R z ( α 0 ) R y ( β  0 ) R x ( γ  0 )  p 0 0 1 × 3  1  ,  (2)where  p 0  = [ x 0 , y 0 , z 0 ] T  .The pose of more distally located links are also definedby the state vector as follows: the elements  φ i  and  θ i  inthe Kalman state definition from Eq. 1 are offset anglesassociated with link   i  that define link   i ’s orientation relativeto the preceding link. A visual interpretation of   φ i  and  θ i can be seen in Fig. 4.To compute the transformation matrix  T  i ( X  k )  that repre-sents the pose of link   i , we define the following recursive  θ  i φ i Fig. 4. The effect of the offset angles  φ i  and  θ i  on the pose of a robotlink relative to the preceding link. process that is initialized with the pose of the starting link, T  i,ang ( X  k ) =   R x ( θ i ) R y ( φ i ) R x ( − θ i ) 0 3 × 1 0 1 × 3  1  T  adv  =  1 0 0  L 0 1 0 00 0 1 00 0 0 1  T  i ( X  k ) =  T  i − 1 ( X  k ) T  i,ang ( X  k ) T  adv , where  L  is the length of a link. As seen in Fig. 3, each link   i has an associated transformation matrix that can be computedfrom the previous transformation matrix via the offsets  φ i and  θ i . Thus, the state vector from Eq. 1 sufficiently definesthe pose of all links and thus the shape and configuration of the robot.  B. Advancing Motion The HARP is a multi-link robot that is, by design, afollow-the-leader device. (see [11] for the mechanism designdetails). The robot maintains its shape in three-dimensionalspace and when commanded, advances one link length at atime: each link theoretically moves into the correspondingpose of the link in front of it. In this case, a link behind themost proximally referenced link will move into its place andassume the role of the starting link of the Kalman state withtransformation matrix  T  0 . The way the robot advances canbe seen in Fig. 5. T  0  T  1 T  15 ...T  0 T  1  ...T  16  T  15 ... Fig. 5. A depiction of the way that the HARP advances in a follow-the-leader fashion when commanded. When all of the links advance one step ahead, the statespace grows by two parameters (there is effectively one extralink in the state), as seen in Fig. 5. The motion model for thisadvancing step can be defined with the following function, f  a ( X  k ) =  X  T k  ,  0 ,  0  T  .  (3) C. Retracting Motion Like advancing, when commanded to retract, the HARPmaintains its shape in three-dimensional space. The mostproximally referenced link moves backwards into a positionthat is not tracked by the Kalman state vector while the link one step ahead moves into its place and assumes the roleof the starting link of the Kalman state with transformationmatrix  T  0 . The distal link also theoretically moves into theposition of the link preceding it. Assuming  M   is the lengthof the state vector at time-step  k , the motion model forretracting is, f  r ( X  k ) =  I  ( M  − 2) × ( M  − 2)  0 ( M  − 2) × 2  X  k . The length of the state is reduced by two because the numberof links tracked by the Kalman state is reduced by one.  D. Steering Motion When the HARP is in  steering mode , all of the linkspreceding the distal link in the state space are restrictedfrom moving because an inner mechanism is locked intoassuming the current shape (see [11] for details). This meansthat actuating the three cables that run through the entirety of the snake will theoretically control just the orientation of thedistal link. Thus, by pulling on these three cables in differentamounts with the proximally mounted motors, the pose of the distal link will change.We have formulated a steering model that determines theangle offsets  φ N  − 1  and  θ N  − 1  of the distal link relative to thelink preceding it based on the cable lengths, where  N   is thenumber of links we are tracking in the Kalman state vector, θ N  − 1  = arctan  √  3(2 c 2  +  c 1 )3 c 1   (4) φ N  − 1  =  arcsin   − c 1 C  R  cos( θ N  − 1 )  .  (5)For this model,  C  R  is a radius term that depends on the sepa-ration of the cables and ( c 1 ,  c 2 ) are the measured differentiallengths of each of two cables running down the robot, relativeto the positions that the cables were in after advancing. Thevalue  c 3  associated with the third steering cable in the robotdoes not appear in this model because it is geometrically afunction of   c 1  and  c 2  and is therefore redundant information.The derivation of this model is omitted for brevity but wenote that it is based completely on the geometry of the distallink of the robot. An interpretation of this steering model isas follows: 1) the angle at which the link will be orienteddepends on which cables you pull tighter and 2) the extentthat the link will be angled depends on the amount we pullon the cables. We again refer to Fig. 4 for a depiction of theangles that are affected by the actuation of the cables.From the measured cable lengths, which are obtained fromencoders on the actuated motors, we can obtain the new angle  offsets  φ N  − 1  and  θ N  − 1  of the distal link of the robot usingEqs. 4 and 5. We use these updated values to compute thechange in angles from the previous time step, stored as  ∆ φ and  ∆ θ , and then formulate the following motion model forsteering the HARP, f  s ( X  k ) =  X  k  +  0 T  ( M  − 2) × 1 ,  ∆ φ,  ∆ θ  T  .  E. Measurement Model The sensor we are using for image-guidance is a magnetictracking sensor situated at the distal end of the snake robot.The device we are using is the trakSTAR TM (AscensionTechnologies, Burlington, VT, USA), which can measure the6-DOF pose of a sensor in three-dimensional space. We insertthe tracker into one of the tool channels of the HARP.While the tracker is designed for 6-DOF pose estimation,only 5 degrees of freedom are useful in our application. Thisis because the tracker must be inserted into the HARP suchthat it can be removed easily for exchanging tools, and thusthe roll parameter of the tracker is free to rotate withinthe working channel. The measurement therefore directlyobserves five elements of the pose of the distal link of therobot, and we can formulate the measurement model as, h ( X  k ) =   p T N  − 1 , α N  − 1 , β  N  − 1  T  ,  (6)where  p N  − 1  is the position of the distal link, as in Eq. 2, and( α N  − 1 ,  β  N  − 1 ) are the yaw and pitch of the distal link. All of these parameters can be extracted from  T  N  − 1 ( X  k ) , which iscomputed from the state  X  k . F. EKF Formulation In this paper we are introducing a method to estimatethe state of the HARP given the measurements obtained atthe distal tip by a magnetic tracker. Because we have welldefined motion models and a forward kinematic measure-ment model, it is reasonable to formulate this filtering task in the framework of a Kalman filter (specifically an extendedKalman filter because of nonlinear models). The purpose of using a filter to estimate the state is that the motion andmeasurements are subject to noise and disturbances.The first step of our EKF formulation is to initialize theestimate of the state. To do this, we begin an experimentwith the snake robot completely retracted with the magnetictracker in the distal link, which also happens to be theonly link in the Kalman state. A depiction of the stateof the system is shown in Fig. 6-(a). In this situation, asingle magnetic tracker measurement directly measures the5-DOF pose of the first link in the Kalman state. We cantherefore initialize the mean and covariance matrix of ourEKF implementation as follows, ˆ X  0 | 0  =   z 0 0  , P  0 | 0  =   R  0 5 × 1 0 1 × 5  σ 2 γ   , where  z 0  is the initial sensor measurement which is modeledaccording to Eq. 6. The roll parameter in the initialized meanis set to zero because we do not yet have enough informationto set a value for this element and thus we must initialize T  0  T  1 T  0 a) b) Fig. 6. This shows the first two steps of our initialization process for theKalman filter. the roll arbitrarily. For the covariance initialization,  R  is theuncertainty in the sensor noise and  σ 2 γ   is a variance valuechosen by the user that models the large initial uncertaintyin the roll parameter of the state.After this first measurement, we advance the robot one stepand evolve the mean of the filter based on the motion modelin Eq. 3. As for the covariance, we add a small amount of noise to represent the fact that parameters may be disturbedthrough the actuation of the cables. The state of the robotafter advancing is depicted in Fig. 6-(b). The new estimatebecomes (  ˆ X  1 | 0 , P  1 | 0 ). The reason for advancing the robot anextra step before any steering takes place is that it simplifiesthe formulation of our filtering method because our steeringmodel from Sec. III-D is defined for at least two links.After the robot advances, another measurement is acquiredfrom the magnetic tracking sensor and the standard Kalmanmeasurement update is applied using the measurement modelin Eq. 6. The new estimate then becomes (  ˆ X  1 | 1 , P  1 | 1 ).After this initialization procedure, we can subsequentlyrely on the motion and measurement models defined in thissection to predict and update the EKF in real-time using thewell known Kalman prediction and update equations. Wenote that we add prediction noise (to the variances of the link angles) after each steering command. One difference betweenour filtering scheme and a conventional EKF implementationis that the prediction step that we perform at any given time-step will depend on the mode that the robot is in (steering,advancing, or retracting). The overall algorithm for our EKFimplementation is described in Alg. 1. Algorithm 1  Snake Shape Estimation Algorithm 1:  (  ˆ X  1 | 1 ,  P  1 | 1 )  ←  initializeStateEstimate() 2:  for  k ← 2  to  ∞  do 3:  if   mode  =  steer  then 4:  (  ˆ X  k | k − 1 , P  k | k − 1 )  ←  steer(  ˆ X  k − 1 | k − 1 ,  P  k − 1 | k − 1 ,  u k ) 5:  else if   mode  =  advance  then 6:  (  ˆ X  k | k − 1 , P  k | k − 1 )  ←  advance(  ˆ X  k − 1 | k − 1 ,  P  k − 1 | k − 1 ,  u k ) 7:  else 8:  (  ˆ X  k | k − 1 , P  k | k − 1 )  ←  retract(  ˆ X  k − 1 | k − 1 ,  P  k − 1 | k − 1 ,  u k ) 9:  end if  10:  (  ˆ X  k | k ,  P  k | k )  ←  correctionStep(  ˆ X  k | k − 1 ,  P  k | k − 1 ,  z k ) 11:  end for IV. O BSERVABILITY OF  S HAPE  E STIMATION To achieve shape estimation, we are estimating the jointangles of a high degree of freedom snake robot with onlya magnetic tracker that measures the pose at the distal tip
We Need Your Support
Thank you for visiting our website and your interest in our free products and services. We are nonprofit website to share and download documents. To the running of this website, we need your help to support us.

Thanks to everyone for your continued support.

No, Thanks