Shape Estimation for ImageGuided Surgery with aHighly Articulated Snake Robot
Stephen Tully, George Kantor, Marco A. Zenati, and Howie Choset
Abstract
—In this paper, we present a ﬁltering 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 ﬁlter whose update step incorporatesmeasurements from a 5DOF electromagnetic tracking sensorsituated at the distal end of the robot. A single tracking sensoris sufﬁcient for estimating the shape of the system becausethe robot is inherently a followtheleader mechanism withwell deﬁned motion characteristics. We therefore show that,with appropriate steering motion, the state of the ﬁlter is fullyobservable. The goal of our shape estimation algorithm is tocreate a more accurate and representative 3D rendered visualization for imageguided 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 semiautonomously drove the robot alongthe epicardial surface of a porcine heart.
I. I
NTRODUCTION
With minimally invasive surgery (MIS), a physician typically performs diagnostic or interventional procedures witha surgical tool or robot through small portlike incisions inorder to reduce patient trauma. Unfortunately, with MIS, surgeons cannot view an operation with direct vision and insteadmust rely on indirect imaging for surgical guidance. It iscommon for a surgeon to use ﬂuoroscopy [1], ultrasound [2],MRI [3], or endoscopy [4] for this purpose. Unfortunately, allof these modalities have limitations. Another option is
imageguided 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 ﬁlteringmethod that estimates, with measurements from a magnetictracking sensor, the shape and conﬁguration 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 imageguidance.While it is possible to simply track the position of the distaltip of the robot, we instead believe that estimating the fullshape and conﬁguration 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 University, 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 conﬁguration, which can be usefulfor righting joystick inputs and rectifying video. An exampleof the imageguidance we achieve is shown in Fig. 2.To perform shape estimation for a cabledriven snakerobot, we use an extended Kalman ﬁlter (EKF) formulationwith newly deﬁned motion models and a forward kinematicmeasurement model that incorporates 5DOF pose measurements at the distal tip of the robot from an electromagnetictracking sensor. Full shape estimation is possible, in thiscontext, because the robot is inherently a followtheleaderdevice with explicitly deﬁnable motion models.The contributions of the work presented in this paper are1) the novel use of an EKF to estimate the shape of a surgical snake robot, 2) new motion models for a cabledrivensurgical snake robot, 3) an analysis of the observability of shape estimation with a single 5DOF 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 semiautonomously on theepicardial surface of a porcine heart.II. B
ACKGROUND
A. ImageGuided Surgery
Imageguided surgery is a term that is often used to describe a procedure that uses patientspeciﬁc medical imagesas a form of visual feedback during surgery. In many cases,this equates to using preoperative CT or MRI data to reconstruct a 3D surface model of anatomical structures, as in [6].With imageguided 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 imageguidance 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(BioSense 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 realtime, but we believe it would be moreinformative to view the entire conﬁguration 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 ﬂexible tool.For example, in [8], the authors use an optical ﬁber with FBGsensing to determine in realtime 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 strainsensors to measure theshape of a ﬂexible needle in the ﬁeld of an MRI. While thisis one of the more popular methods for computing the shapeof a tool, there are several issues: the ﬁrst 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 ﬂexible 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 threedimensional 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ﬁne the robot’s shape and conﬁguration during imageguidedMIS. In this section, we will deﬁne the state vector that weare estimating as well as the motion and measurement modelsthat we have developed for this ﬁltering problem.
A. Kalman State Deﬁnition
The state that we are estimating in a Kalman ﬁlter framework is deﬁned 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 imageguidance. This is a live snapshotfrom an experiment with the HARP navigating semiautonomously on theepicardial surface of a porcine heart.
where
(
x
0
,y
0
,z
0
)
is deﬁned to be the position of the mostproximally located link of the robot that we are interested intracking at timestep
k
. There typically will be links behindthis ﬁrst 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 ﬁrst 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 deﬁning theconﬁguration 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 deﬁne 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 simpliﬁed 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 deﬁnedby the state vector as follows: the elements
φ
i
and
θ
i
inthe Kalman state deﬁnition from Eq. 1 are offset anglesassociated with link
i
that deﬁne 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 represents the pose of link
i
, we deﬁne 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 sufﬁciently deﬁnesthe pose of all links and thus the shape and conﬁguration of the robot.
B. Advancing Motion
The HARP is a multilink robot that is, by design, afollowtheleader device. (see [11] for the mechanism designdetails). The robot maintains its shape in threedimensionalspace 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 followtheleader 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 deﬁned 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 threedimensional 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 timestep
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 separation 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 imageguidance 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 the6DOF pose of a sensor in threedimensional space. We insertthe tracker into one of the tool channels of the HARP.While the tracker is designed for 6DOF 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 ﬁve 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 welldeﬁned motion models and a forward kinematic measurement model, it is reasonable to formulate this ﬁltering task in the framework of a Kalman ﬁlter (speciﬁcally an extendedKalman ﬁlter because of nonlinear models). The purpose of using a ﬁlter to estimate the state is that the motion andmeasurements are subject to noise and disturbances.The ﬁrst 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 the5DOF pose of the ﬁrst 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 ﬁrst two steps of our initialization process for theKalman ﬁlter.
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 ﬁrst measurement, we advance the robot one stepand evolve the mean of the ﬁlter 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 simpliﬁesthe formulation of our ﬁltering method because our steeringmodel from Sec. IIID is deﬁned 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 deﬁned in thissection to predict and update the EKF in realtime 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 ﬁltering scheme and a conventional EKF implementationis that the prediction step that we perform at any given timestep 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