Electromagnetic Positioning for Tip Tracking and Shape Sensing of Flexible Robots
Shuang Song,Zheng Li,Member,IEEE,Haoyong Yu,Member,IEEE,and Hongliang Ren,Member,IEEE
Abstract—Wire-drivenflexible robots are efficient devices for minimally invasive surgery,since they can work well in complex and confined environments.However,the real-time positional and shape information of the robot cannot be well estimated, especially when there is an unknown payload or force working on the end effector.In this paper,we propo a novel tip tracking and shape nsing method for wire-drivenflexible robots.The propod method is bad on the length of each ction of the robot as well as the positional and directional information of the distal end of each ction of the robot.For each ction,an electromagnetic nsor will be mounted at the distal end to estimate the positional and directional information.
A reconstruction algorithm,which is bad on a three-order Bézier curve,is carried out utilizing the positional and directional information along with the length information of the ction. This method provides the advantage of good tracking results and high shape reconstruction accuracy with limited modification to the robot.Compared with other reconstruction methods, no kinematic model is needed for reconstruction.Therefore,this method works well with an unknown payload that applied at the tip of the robot.The feasibility of the propod method is verified by simulation and experimental results.
Index Terms—Bézier curve,electromagnetic tracking, wire-drivenflexible robot,shape nsing.
I.I NTRODUCTION
F LEXIBLE robots,such as wire(or tendon or cable)driven
manipulators[1]–[4]and concentric tube robots[5],[6] have been widely studied for the u in minimally invasive surgery[7].During a surgical operation,flexible robots may interact with tissue.Tissue will affect the position and shape of the robots,which need to be detected in real time to avoid damage to the tissue.Additionally,real time position and shape information are necessary to provide feedback to the controller to perform accurate maneuvering.Therefore,it is very important to provide shape information of the robot.One drawback of theflexible robot is that the joints’rotations Manuscript received March15,2015;accepted April13,2015.Date of publication April17,2015;date of current version June23,2015.This work was supported by the Singapore Academic Rearch Fund under Grant R397000166112,Grant R397000157112,and Grant R397000156112.The associate editor coordinating the review of this paper and approving it for publication was Dr.Patrick Ruther.(Corresponding authors:Hongliang Ren and Haoyong Yu.)
S.Song,H.Yu,and H.Ren are with the Department of Biomedical Engineering,National University of Si
ngapore,Singapore117580(e-mail: biesshua@nus.edu.sg;bieyhy@nus.edu.sg;ren@nus.edu.sg).
Z.Li is with the Institute of Digestive Dia and Chow Yuk Ho Technology Centre for Innovative Medicine,Chine University of Hong Kong,Hong Kong(email:lizheng@cuhk.edu.hk).
Color versions of one or more of thefigures in this paper are available online at
Digital Object Identifier10.1109/JSEN.2015.2424228cannot be controlled independently,therefore the backbone deformation cannot be controlled as desired and conquently the actual joints’rotations are unknown.As a result,the tip position and shape information of the robot cannot be well estimated when there is an external payload or force working on the end effector.
Usually,backbone deformation is estimated by kinematic modeling with some assumptions,such as the piecewi constant curvature assumption[8],[9].This method can only estimate the shape of the backbone without a payload.
A more accurate shape estimation is to u the Cosrat Rod Theory[10]and incorporate the statics model.Xu and Simaan[11]propod a method using elliptical integrals to achieve the shape restorati
on with a known external load.Trivedi et al.[12]prents a new approach for modeling soft robotic manipulators which incorporates the effect of material nonlinearities and distributed weight and payload. The model is geometrically exact for the large curvature,shear, torsion,and extension that often occur in the manipulators. In[13],a model bad on a Rayleigh-Ritz formulation is propod.By using the transversal tip force and distributed load as inputs,the needle deflection can be predicted.The drawback of the model bad methods is that the forces or payload applied to the backbone needs be known in advance. In a real application,the forces or loads are usually unknown,therefore the model bad shape estimation methods are of limited u.
An alternate way is to u a nsor-bad method for tip tracking and shape nsing.Medical image bad methods are often ud,such as Ultrasound[14]and Magnetic Resonance Imaging(MRI)[15].Another popular technology that has been well studied is the Fiber Bragg Grating(FBG)bad method[16],[17].Usually,a number of FBG nsors are mounted in the robot.The FBG nsors can measure the axial strain of the placed position,which enables the computation of the needle curvature.The three-dimensional(3-D)robot shape can then be reconstructed from the curvature.Besides, the Electromagnetic Tracking(EMT)[18]is also studied to detected the bending characteristics of a multi-gment continuum robot in contact with the environment.Compared with ot
her nsor bad methods,the EMT method can directly provide positional and directional information.It is easy to tup and has no line-of-sight problems.
In this paper,a novel tip tracking and shape nsing method for a wire-drivenflexible robot is propod.The propod method is bad on the positional and directional informa-tion of the distal end of each ction of the robot along
1530-437X©2015IEEE.Personal u is permitted,but republication/redistribution requires IEEE permission.
See /publications_standards/publications/rights/index.html for more information.
Fig.1.Multi-ction wire-drivenflexible robot.The manipulator compris aflexible backbone and a number of controlling wires.For each ction, a EM nsor is mounted at the distal end.Each ction will be reconstructed with two nsors.
with the length of the robot.As shown in Fig.1,the tip tracking and shape nsing system includes two parts,the multi-ction wire-drivenflexible Robot and the EMT system. The manipulator compris multi ctions.Each ction can be controlled to bend independently and two basic shape
s, a“C”shape or a“S”shape,can be achieved.For each ction, an electromagnetic nsor will be mounted at the distal end to estimate the positional and directional information of the ction’s tip.A reconstruction algorithm,which is bad on a three order Bézier Curve,is carried out utilizing the positional and directional information along with the length information of the ction.This method provides the advantage of good tracking results and high shape nsing accuracy with limited modification to the robot.Compared with other shape nsing methods,no kinematic model is needed for reconstruction. Therefore,the method works well with an unknown payload. The feasibility of this method is shown by simulation and experimental results.
The primary contributions of this work are summarized as follows:
•A shape nsing method bad on Bézier Curvefitting and LM algorithm is propod.This method only needs the positional and directional information of the distal end of each ction.
•Compared with kinematic and statics bad models, no prior payload or force information is needed.•Compared with other nsor bad methods,only one nsor is needed for each ction,and thus very few modifications are required to the robot.
The rest of this paper is organized as follows.In Section II, the electromagnetic tracking method for t
he tip tracking will be introduced.In Section III,the shape reconstruction method will be prented in detail.In Section IV,the simulation and experimental results will be shown.Finally,conclusions will be made in Section V.
II.EMT M ETHOD FOR T IP T RACKING
As shown in Fig.1,an uniaxial EM nsor is mounted at the distal end tip of each ction of the robot.EMT will be ud to track the positional and directional information of
each Fig.2.Electromagnetic tracking system consisting of six uniaxial transmit-ting coils,which are stimulated quentially.Uniaxial nsing coil is ud to rve as the tracking target.Localization coordinate system is established bad on thefirst transmitting coil.
nsor and provide5Degree-of-Freedom(DoF)information: 3DoF position and2DoF direction.
Generally,a magnetic tracking technique us one or more permanent magnets or electromagnetic coils as the excitation source,which generates a magneticfield that can be measured by magnetic nsors and then the position and orientation can be estimated[19]–[22].Bad on the magnetic sources,there are two types of magnetic tracking,permanent magnet bad tracking[19],[23]and quasi-static electromagnetic bad tracking[20],[24]–[26]. In contrast to other state-of-the-art tracking technologies such as mechanical optical tracking or ultrasonic tracking, magnetic tracking is emerging to provide an occlusion-free tracking scheme[27]–[30].Compared with optical tracking techniques,this occlusion-free feature brings substantial benefits for intracorporeal applications,which typically lack direct line-of-sights between the ba frames and the tracked targets[31],[32].
The tracking method ud in this paper is the EMT method. Most EMT technologies are bad on ac
curate mapping of a3D magneticfield generated by transmitting coils and computing from thefield mapped the position and orientation parameters of the nsors relative to the source[20],[25], [26],[33]–[35].Compared to the permanent magnet bad method,EMT has the advantages of anti-interference and a larger working space.As shown in Fig.2,the uniaxial nsing coil is ud as the target and ns the magneticfield that is generated by the six transmitting coils.The transmitting coils are stimulated quentially.The position and orientation information of the nsing coil can then be estimated bad on the nsing signals.
As shown in Fig.2,assuming that the positional and direc-tion parameters of the nsing coil in the tracking coordinate system is(x,y,z,m,n,p),where(x,y,z)is the position information,(m,n,p)is the direction vector and
m2+n2+p2=1.
Therefore the degree of freedom of the direction is2.
SONG et al.:EM POSITIONING FOR TIP TRACKING AND SHAPE SENSING OF FLEXIBLE ROBOTS 4567
For the i -th transmitting coil,the nsing magnetic field is
V i =k (m B xi +nB yi +pB zi )
(1)
where k is a constant relating to the turns of transmitting coils and nsing coil.Bad on the magnetic dipole model,(B xi ,B yi ,B zi )is reprented as follows
⎧
⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎩B xi =Q i (x −a i )R 5i −m i
R 3i B yi =Q i (y −b i )R 5i −n i R 3i B zi =
Q i (z −c i )R 5i −p i R 3i (2)where
Q i =3[m i (x −a i )+n i (y −b i )+p i (z −c i )]
(3)
(a i ,b i ,c i )is the position of the i -th transmitting coil,
(m i ,n i ,p i )is the direction of the i -th transmitting coil and R i is the distance between the nsing coil and the i -th transmitting coil
R i =
(x −a i )2+(y −b i )2+(z −c i )2In our tup,for each transmitting coil,(m i ,n i ,p i )=(0,0,1).Therefore (2)can be simplified as follow
⎧
⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎩
B xi =3(x −a i )(z −c i )R 5i B yi =3(y −b i )(z −c i )R 5i B zi =3(z −c i )2R 5i −1R 3i (4)
We define the error evaluation function Err as follows
Err =N
i =1
(V i −k (m B xi +nB yi +pB zi ))2
(5)
where N is the number of transmitting coils.Therefore
(x ,y ,z ,m ,n ,p )can be estimated with an optimization algorithm if N ≥5.In our EMT system,N =6and the LM algorithm is ud to solve this least square problem.As shown in Fig.1,a nsor is mounted at the distal end of the last ction.Therefore it can provide the tip’s position and direction.Thus tip tracking is realized.Note that since the uniaxial nsor is ud for the tracking,we can have a 5DoF tracking result,3DoF position and 2DoF direction.The lf rotation information is misd.Therefore,torsion may not be detected.
III.C URVE S HAPE R ECONSTRUCTION M ETHOD By applying this tracking method in a robotic system,the positional and directional information of the distal end of each ction can be estimated.A curve fitting method can be utilized in order to reconstruct the shape of the robot with an estimated result.This curve fitting problem needs an appropriate curve equation.When there is a large payload,the backbone will bend,and the constant curvature
assumption
Fig.3.Cubic Bézier curve that ud to perform the shape nsing for the flexible robot.Two kinds of curves:(a)Bézier curve with C shape and (b)Bézier curve with S shape can work well for the reconstruction of the robot with unknown payload.In the both curves,P 0is the start point and P 3is the end point.P 1and P 2are the two control points,which provide directional information for the curve.S 01and S 23are the length of P 0P 1and P 2P 3.H 0is the direction vector from P 0to P 1and H 3is the direction vector that from P 2to P 3.The curve starts at P 0going toward P 1and arrives at P 3coming from the direction of P 2.
may not stand anymore.Bad on the known parameters,the Bézier curve will be ud in the curve fitting,for its good performance of modelling smooth curves.In the following part,the Bézier curve and the shape reconstruction method will be introduced.A.Bézier Curve
A Bézier curve is a parametric curve frequently ud to model smooth curves.Take a cubic Bézier curve as an example,which will be ud in the following part.As shown in Fig.3,the cubic Bézier curve can have two kinds of space curves:the C shape curve and the S shape curve.For each curve,P 0is the start point and P 3is the end point;P 1and P 2are the control points;the curve starts at P 0going toward P 1and arrives at P 3coming from the direction of P 2.
4568IEEE SENSORS JOURNAL,VOL.15,NO.8,AUGUST 2015
Usually,it will not pass through P 1or P 2;the two control points are only there to provide directional information.The distance between P 0and P 1determines how long the curve moves into direction P 2before turning towards P 3.The explicit form of the curve is
B (t )=(1−t )3P 0+3(1−t )2t P 1+3(1−t )t 2P 2+3t 3P 3
(6)Generally,the formula for the Bézier curve of order n can be expresd explicitly as follows:
B (t )=
n i =0
b i ,n (t )P i
(7)
where t ∈[0,1]and b i ,n is defined as follow:b i ,n = n i
t i
(1−t )(n −i )
(8)
From the EMT system,we can have the positional and directional information of the distal end of each ction.For each ction,the distal end can be en as the start joint of the next ction.Therefore,for each ction,the start and end joints’position and direction are known.
A Bézier Curve is determined by two kinds of points,the start and end points,and the control points.The control points are located on the tangent lines of both end joints.Therefore,for a three order Bézier curve,only two unknown parameters (S 01and S 23that shown in Fig.3)need to be solved.
B-spline curves are often ud to fit a curve.Compared to the B-spline curves,the Bézier curve has the advantages as follows:1)the positional information of the nsors can be directly ud as the start point and end point of the Bézier curve;2)the directional information of the nsors can be directly ud to express the control points with the two unknown parameters.The above advantages make the Bézier curve a very good choice for shape reconstruction for EMT bad shape nsing.Besides,there is no other known position or direction information of the points between the start point and end point,therefore B-spline curves may not be a good choice for this rearch.B.Shape Reconstruction Method
For each ction in the robot,the same shape reconstruction method will be carried out.Therefore,we will u the k -th ction as an example to introduce the method.The distal end of the (k −1)-th ction is ud as the start point of the curve;the distal end of the k -th ction will be ud as the end point of the curve.As shown in Fig.4,define the parameters as follows:
S 01=||P 0P 1||S 23=||P 2P 3||
H 0=
−−→P 0P 1
S 01H 3=
−−→P 2P 3
S
23
Fig.4.Bézier curve.P 0is the start point and P 3is the end point.P 1and P 2are the control points.S 01
and S 23are the length of P 0P 1and P 2P 3.H 0is the direction vector from P 0to P 1and H 3is the direction vector that from P 2to P 3.P 0,P 3,H 0and H 3are measured from the EM nsors.B i relates to the i -th joint of the robot.
where H 0and H 3are the tangent vectors of the curve at point P 0and P 3.Positional and directional parameters of the nsing coil mounted at the (k −1)-th ction can be estimated from the EMT method and the results are (x 1,y 1,z 1,m 1,n 1,p 1).Position and direction parameters of the nsing coil mounted at the distal end of the k -th ction are (x 2,y 2,z 2,m 2,n 2,p 2)bad on the EMT result.The relationship between the cubic Bézier curve and the position and direction parameters can be established as follows
P 0H 0P 3H 3 =⎛
⎝x 1m 1x 2m 2y 1n 1y 2n 2z 1p 1z 2p 2⎞⎠(9)
The relationship between P 0,P 3and P 1,P 2can be en as follows:
P 1=P 0+S 01H 0
P 2=P 3−S 23H 3(10)
Therefore,to find a cubic Bézier curve that is described with (6),the start point P 0,end point P 3and two control points P 1and P 2need to be identified.P 0and P 3can be obtained from (9).The relationship between (P 0,P 3)and (P 1,P 2)can be en in (10).The reconstruction problem then leads to solving the two length parameters S 01and S 23.
In order to solve the two length parameters S 01and S 23,an error evaluation function needs to be established.The flexible robot can only bend during the operation and no stretching and contraction movement will be achieved.Bad on this character,the length information of the robot can be ud to establish the objective function for the optimization.
SONG et al.:EM POSITIONING FOR TIP TRACKING AND SHAPE SENSING OF FLEXIBLE ROBOTS4569 Define L i as the length of the i-th vertebra and L ci as the
corresponding curve length.The objective function can then
be defined as
L i=L ci(11)
where L ci is estimated as follows:
L ci=||B i−B i−1||=||B(i
n
)−B(i−1
n
)||(12)
where n is the number of points that are ud to estimate the curve length on the cubic Bézier curve.Here we define n equal to the joint number of the reconstruction ction of the robot.
Define the error estimation function f as follows
f=
n
i=1
(L i−L ci)2(13)
The parameters S1and S2can then be estimated by minimizing the objective function f.Here the LM algorithm is ud to perform the optimization.
Bad on the curve reconstruction result,each joint’s position can then be estimated bad on(6)with the following equation
(x i,y i,z i)=B i=B(i
n
)(14) Therefore,each joint on the robot has an estimation value and the shape reconstruction of the robot is achieved.
IV.S IMULATION AND E XPERIMENTAL R ESULT
In this part,first we will show some simulation result for a multi-ction robot without external force to test the shape nsing method.After that,the simulation and experimental results for a single ction robot with external payload will be shown.
The reconstruction is carried out independently for each ction.Therefore,the reconstruction result o
f one ction will not affect the results of other ctions.The only factor that would affect the results is the position and orientation results from the EMT system.Therefore,in the simulation test with external force and the real experimental test with external payload,only a single ction robot will be ud.
A.Simulation For a Multi-Section Robot
Without External Force
First we will do some simulations for a multi-ction robot without external force to test the reconstruction method. Three kinds of robots,from single ction robot to a robot that has three ctions,have been simulated.The simulation data is bad on the kinematic model as shown in[36].
Fig.5shows the simulation result of the single ction curve.Fig.6shows the simulation result of the two-ction curve.Fig.7shows the simulation result of the three-ction curve.For each ction,it is deformed without external force or payload.From the simulation results,it can be en that the propod shape nsing method works well for the multi-ction
robot.Fig.5.Simulation results of the single ction curve.In the simulation,five curves are generated bad on the kinematic model.Blue lines and circles reprent the results that generated by simulation and red lines and squares reprent the reconstruction results.
TABLE I
P ARAMETERS S ETUP FOR S IMULATION
B.Simulation For a Single Section Robot
With External Force
In this simulation,a single ction robot with10joints is ud to perform the test.The length of the robot in the simulation is135mm.6different shapes under different external forces are generated bad on a kinematic model. The forces are all applied at the distal tip position of the robot,as from the beam theory,the deformation of the beam under distributed external load is equivalent to the deformation under a lumped force and a pure moment at the backbone distal end[37],[38].It is also the position for external tools to be mounted and the most contacted position.The parameters ttings are shown in Tab.I.In thefirst simulation,no external load is applied.The robot suffers only gra
vitational force and forces from the wires when there is no external payload.The payload model is bad on the kinematic model mentioned in[38].
The curve reconstruction algorithm is then ud tofit the simulation data bad on(10)(11).Fig.8shows the simulation results of each curve reconstruction,where blue lines reprent the results generated by the simulation and red lines reprent the reconstruction result.
From Fig.8we can e that reconstruction results are very good.The joints of the robot allfit well.Error evaluation will be drawn in the following ction.
4570IEEE SENSORS JOURNAL,VOL.15,NO.8,AUGUST
2015
Fig.6.Simulation results of the two-ction curves.Blue lines and circles reprent the results that generated by simulation.Red lines reprent the reconstruction results for the first ction and green lines reprent the results for the cond
ction.
Fig.7.Simulation results of the three-ction curves.Blue lines and circles reprent the results that generated by simulation.Red lines,green lines and purple lines reprent the reconstruction results for the first,cond and third ction,respectively.
C.Algorithm Performance Evaluation Method
From Fig.5,Fig.6,Fig.7and Fig.8we can e that the reconstruction method performs well.For the error evaluation,the distance between the joint on the robot and the related position on the curve is ud,which is
shown in (15).
dis =||(x r i ,y r i ,z r i )−B i ||
(15)
where is (x r i ,y r i ,z r i )is the position of the i -th joint on the
robot in the simulation and B i is shown in (14).
