Using Lie algebra for shape estimation of medical snake robots

Highly articulated robots have the potential to play a key role in minimally invasive surgeries by providing improved access to hard-to-reach anatomy. Estimating their shape inside the body and combining it with 3D preoperative scans of the anatomy enable the surgeon to visualize how the entire robot interacts with the internal organs. As the robot progresses inside the body, the position and orientation of every link comprising the robot, evolves over a coordinate-free Lie algebra, se(3). To capture the full motion and uncertainty of the system, we use an extended Kalman filter where the state vector is defined using elements of se(3). We show that this approach describes the shape of the robot more accurately, than the ones where the state vector is a conventional parametrization, such as Cartesian coordinates and Euler angles. We perform two experiments to demonstrate the effectiveness of this new filtering approach.


I. INTRODUCTION
When compared to conventional procedures, minimally invasive surgeries (MIS) have potential benefits including "reduced-pain", minimal "blood-loss", faster healing and reduced tissue disruption. To further improve upon MIS, our group, along with others [1]- [4] have been developing snakelike robots to provide deeper access to the anatomy with fewer incisions. However, robot-guided MIS introduces other drawbacks like lack of situational awareness and limited sensory feedback. This forces surgeons to use their expertise in finding relation between pre-operative images and the surgical reality. Although emerging technologies like fiber optics, MRI, ultrasound and CT have proved to be useful, they suffer from limited field of view and incompatibility with robot systems.
Prior efforts include generating 3D models of anatomical structures from pre-operative scans and combining them with model-based tool tracking, to create a virtual view of the operation for visual feedback. To successfully use this technique, the tool needs to be registered in the frame of reference of the 3D model. A number of algorithms have been developed to take input from electro-magnetic sensors [5], ultrasound [6], [7] etc., and help in the registration process.
When the tool is a highly articulated robot, estimation is more difficult because of the additional degrees of freedom (DOFs). Also complete knowledge of its shape configuration is necessary to avoid interfering with organs. In [8] a nonlinear stochastic filtering method is used to estimate the shape and configuration of a snake robot (see Fig. 1). It measures the pose of the tip and prescribes a motion model  based on the robot's kinematics and history of the previous inputs. The state of the snake robot, both its base pose and its internal shape, is described in terms of Cartesian coordinates and Euler angles. In presence of high uncertainties, this parametrization does a poor job of estimating the shape of the robot. However, recent work by Long et al [9] shows that modeling a system using exponential coordinates yields better estimation as opposed to using conventional parameters especially in the presence of large uncertainties.
Exploiting the underlying structure of the Lie group and algebra which defines motions for the robot, in this work we use an extended Kalman filter (EKF) framework to estimate the state of the system that is described by exponential coordinates. This approach provides better estimate of shape than using a conventional parametrization [10]. In general it is not trivial to obtain closed-form expressions for the filter's process and measurement models in terms of exponential coordinates as opposed to other parametrizations of SE(3); and hence prior work use Cartesian coordinates in combination with quaternions, Euler angles or Rodrigues parameters [11]. An important contribution of this paper is deriving closed form analytical expressions for the motion model to estimate the state of the robot defined in terms of exponential coordinates.
The rest of the paper is organized as follows: In Section 2, the motivation for this work is explained in detail. The description of elements of the state vector used in the EKF is provided in Section 3. Section 4 discusses the motion model and measurement model required for the EKF. In Section 5, experimental results are provided and finally conclusions are discussed in Section 6.

II. MOTIVATION
We start our discussion of by first estimating the position and orientation of a rigid body, which is typically described by a g ∈ SE(3). The special Euclidean group SE(3), is homeomorphic to R 3 × SO(3). The elements of SE(3) can be represented by R ∈ SO(3) and t ∈ R 3 in the form of the following 4 × 4 matrix, For a vector x = [v 1 , v 2 , v 3 , ω 1 , ω 2 , ω 3 ] T ∈ R 6 , an element x, of the Lie algebra se(3) can be expressed as, where and Ω is the skew-symmetric matrix formed from [ω 1 , ω 2 , ω 3 ] T : The vector x is referred to as the twist vector and the operator ∧ is used to map from R 6 to se(3). The twist vector parametrizes an element of the Lie algebra se(3) that belongs to the Lie group SE(3). The element T ∈ SE(3), corresponding tox can be obtained by using a matrix exponential, and so the twist vector x is also referred to as exponential coordinates. The SE(3) element can also be obtained using parameters such as Cartesian coordinates that parametrize t and Euler angles that parametrize R. However using the Lie algebra gives a coordinate-free way to represent the SE(3) element and hence provides more accurate estimation as shown in [12]. For example, Fig 2 shows the distribution over the Cartesian coordinates for a differentially driven robot moving along a straight line. Note how the probability density function (PDF) contours of Gaussian of best fit are not a good approximate to the distribution. On the other hand, representing the state vector in terms of exponential coordinates, captures the distribution better. Fig 3(a) shows how the PDF contours of Gaussian of best fit in exponential coordinates, approximates the distribution closely. When mapped back into the Cartesian space through the exponential map, the Gaussian contours appear to bend and take a banana-like shape as shown in Fig. 3(b), and approximate the distribution closely.
Exploiting the underlying structure of the Lie group and algebra that defines motions for the robot, we will now use exponential coordinates to define the state of every link of the snake-robot. III. DEFINITION OF STATE VECTOR The state vector for an N link snake robot [13] is described in [8] as T and e 0 = [α 0 , β 0 , γ 0 ] T define the position and orientation respectively of the most proximal link, while φ i , θ i give the rotation between successive links. In this work, the state vector is formulated in terms of exponential coordinates as, T ) T is the twist vector that is used to identify the element T 0 ∈ SE(3) of the most proximal link of the robot. The variables ω i 2 , ω i 3 for each link i are exponential coordinates, representative of the relative rotation between successive links with the subscripts indicating the axis along which they are defined.

A. Filter state
This section establishes the relation between the state vector in [8] and the one in Eq. (5). In doing so we define the notion of state for both the proximal and the remaining links.
1) Proximal link: Given the state vector of the proximal link, is the rotation matrix describing the rotation about axis k by an angle φ. Upon obtaining R, the matrix logarithm of R is evaluated to obtain the angular velocity ω 0 = [ω 0 1 , ω 0 2 , ω 0 3 ] T as shown in [14]. We obtain the following: where θ = arccos trace(R)−1 2 and R = I 3×3 . Note that, if R = I 3×3 , then θ = 2πk, where k is any integer and ω 0 can be chosen arbitrarily. Having obtained ω 0 , the linear velocity where 2) Distal links: The relative rotation between successive links is represented by φ i and θ i in [8]. Let the exponential coordinates describing the relative motion between successive links be The equivalent of this in the exponential coordinates can be obtained by following the procedure used in the case of the proximal link. The relative motion between links is purely rotational and so we have, v i Further since there is no roll motion between successive links, ω i 1 = 0. Thus ω i 2 , ω i 3 alone are sufficient to describe the relative motion. Taking advantage of the way φ i , θ i are defined, it is equivalent to stating that, in the axis-angle representation, i th link is oriented at an angle φ i about an axis: [0, cos θ i , sin θ i ] T , with respect to (i − 1) th link.
It is well know that given an axis k = [k x , k y , k z ] T and an angle of rotation φ about the axis, R = exp(Kφ), where exp(·) is the matrix exponential, K is the skew-symmetric matrix formed from the vector k and the corresponding exponential coordinates are [k x φ, k y φ, k z φ] T [11]. Hence the required exponential coordinates describing the relative motion between successive links can be obtained in terms of φ i , θ i as follows: Thus we establish that for an N −link snake, the state vector used in this paper is of the same dimensionality as that used in [8] and the two representations are equivalent.
3) Obtaining the SE(3) element: Given the state vector x k , the transformation matrix describing the pose of the most proximally located link is given by: where, To compute the transformation matrix T i (x k ) that represents the pose of i th link, the following recursive process is defined: where L is the length of a link. The Rodrigues parameters c 2 , c 3 are related to ω i 2 , ω i 3 as follows: where The rotation matrix R i can be obtained from c 2 , c 3 as follows:

IV. MOTION AND MEASUREMENT MODEL
The motion of the snake-robot has three distinct modes: advancing, retracting and steering. This section describes the motion models, f a (x k ), f r (x k ), f s (x k ) respectively for the same. An electro-magnetic sensor is used to measure the position of the tip of the robot. A forward kinematic measurement model that incorporates measurement from the sensor is also described in this section.

A. Advancing and Retracting motion
When the snake is advanced by one link, the state space grows by two parameters due to addition of ω N −1 During advancement there is no relative motion between the most distal link and the link before it, therefore φ N −1 = θ N −1 = 0, which implies ω N −1 2 = ω N −1 3 = 0. Thus the motion model for advancement, f a (x k ) remains the same as the one defined in [8]: Assuming M is the length of the state vector at time-step k, the motion model for retraction is also similar to the one defined by [8]: The length of the state is reduced by two because the EKF would track one link less.

B. Steering motion
In [8] the relation between the differential lengths of the cables (d 1 , d 2 , d 3 ) and the orientation of the most distal link relative to the link behind it, is given. Since there exists a typographical error in the equations listed there and a derivation of the relation is not provided, the correct relation has been derived and included in this paper for the sake of completeness. 1) Steering model: The kinematic representation of two adjacent units of the snake robot is shown in Fig. 4. The units can be approximated by equilateral triangles of circumradius r whose centers are separated by a fixed distance l c . The three cables pass through the three vertices of the equilateral triangles at the base and the top. Kinematically these can be modeled as prismatic actuators. The coordinates of the vertices of the equilateral triangle of the base link are given as: The coordinates of the vertices of the equilateral triangle of the top link are, given a relative rotation of R i as defined in Eq. (15): The length of the cables d i , i = 1, 2, 3 can be obtained geometrically as the distance between b i and p i : where k = 1, 2, 3. The design of the robot is such that, the ratio lc r ≈ 0. Hence, substituting this condition and simplifying yields the following constrain equations: Since there are only two unknowns, θ N −1 , φ N −1 and three equations, any two equations can be solved simultaneously to obtain expressions for θ N −1 , φ N −1 . Eliminating sin(φ N −1 /2) 2 from Eq. (23) and Eq. (24), one obtains: Eq. (26) is in terms of sin 2θ N −1 and cos 2θ N −1 , by using half-tangent substitution, it can be converted to an equation wholly in terms of tan θ N −1 . Upon solving for tan θ N −1 , one obtains: From Eq. (23), we obtain: are obtained using Eq. (8). Thus the motion model for steering the snake is: assuming M is the length of the state vector at that instant.

C. Measurement model
The tip of the highly articulated robot described in [13] is sensed by inserting an electromagnetic position sensor into one of the tool channels of the robot. The sensor used in this work is a trakSTAR TM (Ascension Technologies, Burlington, VT, USA) which has the capability to measure 6-DOF pose of the tip of the robot with respect to a world frame. Since the tracker might be removed from time to time for insertion of tools into the same channel, the roll parameter sensed, is set free. The sensor therefore observes five elements of the pose of the distal link, and the measurement model is given as: where p N −1 is the position of the distal link described in terms of Cartesian coordinates and α N −1 , β N −1 are the yaw and pitch of of the distal link as measured by the sensor. The parameters p T N −1 , α N −1 , β N −1 , can be obtained from T N −1 (x k ): (3,3) and T N −1 (x k ) i,j refers to the ij th term of T N −1 (x k ).

D. EKF formulation
In this paper the method to estimate the state of the snake given the measurements of the position and orientation of the distal tip, is similar to the one described in [8]. The only difference is that the elements of the state vector are now in terms of exponential coordinates. It is also worth noting that we still require an advancement of atleast one link before steering it for the system to be fully observable.

V. EVALUATION
Two bench-top tests were conducted to find out the effectiveness of this filtering approach. The snake robot as shown in Fig. 1, was driven using a joystick and the distal link was tracked using an electro-magnetic sensor. During the experiment time-stamped input values from the joystick as well as from the sensor were noted. After the end of the experiment, keeping the shape of the snake fixed, the sensor was pulled out of the tool-channel and a trail of data points was recorded that could be post processed and used as ground-truth.
Using the filtering approach discussed in Section. IV, the shape of the robot was estimated in both the tests. The length of each link was measured to to be 6.9 mm. The shape of . Blue-dotted curve is the ground-truth, green-thick curve is the estimated shape using proposed approach and red-thin curve is the shape estimate using the approach described in [8] the robot was also estimated using the approach described in [8] for the sake of comparison. The average error between the estimated shape and the ground-truth in both the experiments is tabulated in Table. I. Fig. 5 shows the comparison between the ground-truth, the shape estimated by the proposed work and the shape estimated by [8]. Note how the shape estimated by the proposed approach closely approximates the ground-truth. Fig. 6 shows the variation of the error between the groundtruth and the shape estimated for various links. Notice how the proposed method has low errors throughout, which is reflected in the low standard deviation as shown in the Table. I. The improvement in the results obtained is attributed entirely  6. Comparison of variation in error between estimated position and ground-truth across the links. Green line is the variation using proposed approach and red dotted line is the variation using approach described in [8] to changing the space of the state vector to exponential coordinates.

VI. CONCLUSION
The goal of this work is to present an algorithm to estimate in real-time, the shape of the snake robot in an accurate manner. For medical purposes this combined with preoperative data would provide a virtual visualization of the robot inside the body relative to the organs. Our algorithm makes use of sensing the tip position using an electromagnetic sensor and motions models to predict and update the shape of the entire robot. An important contribution of this work has been describing the state at every instant, analytically in terms of the exponential coordinates and using these as state vector for the filtering to obtain more accurate estimates. Promising results have been obtained using the work described in this paper demonstrating the capability of this approach to accurately filter the configuration of the robot in real-time.
The method described above can be extended to any system as long as the motion model can be obtained in closedform in terms of exponential coordinates. Future work would involve sensing the motor-encoder readings and using that in the measurement update step to account for uncertainties arising from inaccurate motor inputs. Also more advanced models that can capture the interaction of the robot with deformable bodies is a subject of future work. ACKNOWLEDGMENT