Shape reconstruction for wire-driven

Shape reconstruction for wire-driven flexible robots based on Bézier curve and electromagnetic positioningqShuang Song a ,c ,Zheng Li b ,Haoyong Yu a ,⇑,Hongliang Ren a ,*aDepartment of Biomedical Engineering,National University of Singapore,SingaporebInstitute of Digestive Disease,Chow Yuk Ho Technology Centre for Innovative Medicine,The Chinese University of Hong Kong,Hong Kong,China cSchool of Mechanical Engineering and Automation,Harbin Institute of Technology Shenzhen Graduate School,Shenzhen,Chinaa r t i c l e i n f o Article history:Received 11November 2014Accepted 11May 2015Available online 29May 2015Keywords:Bézier curveElectromagnetic tracking Wire-driven flexible robot Shape reconstruction Optimizationa b s t r a c tWire-driven flexible robots are efficient devices for minimally invasive surgery,since they can work well in complex and confined environments.However,the real-time position and shape information of the robot cannot be well estimated,especially when there is a payload on the end effector.In this paper,a novel tracking and shape reconstruction method for a wire-driven flexible robot is proposed.The pro-posed method utilizes the length of the robot as well as the positional and directional information mea-sured by an electromagnetic sensor mounted at the distal end of the robot.A reconstruction algorithm,which is based on a three order Bézier curve,is then carried out utilizing the positional and directional information along with the length information of the robot.This method can provide good tracking results and high shape reconstruction accuracy with limited modification of the pared with other reconstruction methods,the proposed method does not rely on any mechanical model of the robot.Therefore,the method works well with an unknown payload.The feasibility of the proposed method is verified by both simulation and experimental results.Ó2015Elsevier Ltd.All rights reserved.1.IntroductionMinimally invasive surgery (MIS)brings patients significant benefits such as decreased intra-operative blood loss,less post-operative complication or morbidity,shorter length of hospi-talization and recovery period,and better postoperative cosmesis [1].Robots used in traditional MIS,such as the Da Vinci Robot are equipped with rigid slim arms [2].However,the coverage of the rigid arms is small and three or more keyholes are required in a surgery.As a result,robot systems with snake-like flexible manipulators are increasingly studied in MIS [3,4].There are two prevailing flexible manipulator designs [5].One is the wire (or ten-don or cable)driven manipulator [6–10]and the other is the con-centric tube robot [11,12].Different from traditional rigid industrial robots,these flexible robots are under-actuated,i.e.the number of motion inputs is less than the system degrees of free-dom (DOF).As an example,the wire-driven robot arm in [13]has 30spherical joints and is controlled by only 6motors.The joints’rotations are constrained by an elastic tube,thus the joints rotation follows the elastic tube bending.During surgical operation,the flexible manipulators typically need to fulfill two types of tasks.One is diagnosis,and the other is tissue operation.During these tasks,the robot may interact with tissue.Tissue will affect the position and shape of the robot,which need to be detected in real time to avoid damage on the tissue.Additionally,real time position and shape information are neces-sary to be provided as feedback to the controller to perform accu-rate maneuvering.One drawback of the flexible robot is that the joints’rotations cannot be controlled independently,therefore the backbone defor-mation cannot be controlled as desired and consequently the actual joints’rotations are unknown.The backbone deformation is estimated by kinematic modeling with some assumptions,such as the piecewise constant curvature assumption [14].This method can only estimate the shape of the backbone without payload.A more accurate shape estimation is to use the Cosserat Rod Theory [15]and incorporate with the statics model.Xu [16]pro-posed a method using elliptical integrals to achieve the shape restoration with a known external load.Trivedi et al.[17]pre-sented another approach for modeling soft robotic manipulators which incorporates the effect of material nonlinearities and dis-tributed weight and payload.The model is geometrically exact/10.1016/j.mechatronics.2015.05.0030957-4158/Ó2015Elsevier Ltd.All rights reserved.qThis work is supported by Singapore Academic Research Fund (AcRF),under Grants R397000166112,R397000157112,and R397000156112.⇑Corresponding authors.E-mail addresses:bieyhy@.sg (H.Yu),ren@.sg (H.Ren).for the large curvature,shear,torsion,and extension that often occur in these manipulators.In[18],a model based on a Rayleigh–Ritz formulation is proposed.By using the transversal tip force and distributed load as inputs,the needle deflection can be predicted.The drawback of these model-based methods is that the forces applied to the backbone needs be known in advance. However,in a real application these forces are usually unknown and thus these model-based shape estimation methods are of lim-ited use.An alternate way is to use sensor-based shape estimation and reconstruction.Many technologies have been studied and used for tracking and shape reconstruction.Medical image based meth-ods are often used,such as Ultrasound[19]and Magnetic Resonance Imaging(MRI)[20].Another popular technology that has been well studied is the Fiber Bragg Grating(FBG)[21,22] based method.In addition,the Electromagnetic Tracking(EMT) [23]is also studied to detected the bending characteristics of a multi-segment continuum robot in contact with the environment. Compared with other sensor based methods,the EMT method can directly provide the positional and directional information,it is easy to setup and has no line-of-sight problem.EMT has also been studied to perform shape detection of endoscopes[24].Multi coils have been mounted in the endoscope to realize shape detection.In this paper,we propose a novel tracking and shape recon-struction method for a wire-drivenflexible robot.The proposed method is based on the length of the robot as well as positionalA reconstruction method based on the Bézier curvefitting andLM algorithm.This method needs only the positional and direc-tional information of the distal end of the curved robot.Compared with the kinematic based model,no prior payload or force information is needed.Compared with other sensor based methods,only one sensor is required for a one-section robot,and thus very few modifica-tions are required on the robot.The rest of this paper is organized as follows.In Section2,the design of the robot and the wire-driven mechanism will be pre-sented.In Section3,the electromagnetic tracking method will be introduced.In Section4,the shape reconstruction method will be presented in detail.In Section5,the simulation and experimental results will be shown.Finally,conclusions will be made in Section6.2.Overview of the wire-drivenflexible manipulatorFig.1shows the design of the wire-driven manipulator with an EM sensor mounted on the distal end of the robot.The sensor pro-vides the position and direction of the tip,which are then used to perform the shape reconstruction.As shown in Fig.1,the manipulator is highly under actuated.It has10joints and is controlled by only a pair of wires.The number of actuators is much less than the number of DOFs.Therefore,the backbone curve cannot be controlled to achieve arbitrary shapes. Two basic shapes are C shape and S shape.When there is no pay-load,the backbone bends to a C shape.Assuming the length of each vertebrae is the same,which is defined as l,then the joint position can be modeled using the constant curvature assumption as shown in(1).x i¼lsin i h2sinðiþ1Þh2sin hyi¼lsin i h2cosðiþ1Þh2sin h8>>>><>>>>:ð1Þwhere i represents the i-th joint and h is the joint rotation;x axis is along the backbone resting position,and y is the lateral displace-ment of the distal end.These parameters can be seen in Fig.1. With this assumption,all the rotations in the joints are identical. When there is a large payload,the backbone may bend to an S shape,and the constant curvature does not stand anymore.The joints rotations can be solved through static analysis as introduced in[25]when the payload is known.Therefore,the EMT method is needed to provide the position and direction of the robot tip in the application that the external payload or force is unknown.In the next section,we will introduce the EMT method for the tip positioning.3.Electromagnetic tracking algorithmThe tip tracking method used in this paper is the EMT method. Most EMT technologies are based on accurate mapping of a3D magneticfield generated by transmitting coils and computing from thefield to estimate the position and orientation parameters of the sensors relative to the source[26–30].As shown in Fig.2,a uniaxial sensing coil is used as the target and senses the magneticfield that is generated by six transmitting coils.These transmitting coils are stimulated sequentially.The position and orientation information of the sensing coil can then be estimated based on the sensing signals.As shown in Fig.2,assuming the positional and direction parameters of the sensing coil in the tracking coordinate system1.Wire-drivenflexible manipulator.The manipulator comprises theflexiblebackbone and a number of controlling wires.A prototypicalflexible manipulatorbeen fabricated with10vertebrae and has10joints and is controlled by a pairwires.In thefigure,i represents the i-th joint and h is the joint rotation;l i islength of the i-th vertebra.x axis is along the backbone resting position,and y islateral displacement of the distal end.Two basic shapes,C shape and S shape,canachieved.An EM sensor is mounted at the distal end of the robot to detectposition and direction of the tip.S.Song et al./Mechatronics29(2015)28–3529are ðx ;y ;z ;m ;n ;p Þ,where ðx ;y ;z Þis the position information,ðm ;n ;p Þis the direction vector and m 2þn 2þp 2¼1.For the i -th transmitting coil,the sensing magnetic field is V i and the theoret-ical value isB i ¼k ðmB xi þnB yi þpB zi Þð2Þwhere k is a constant relates to the turns of transmitting coils and sensing coil,ðB xi ;B yi ;B zi Þis represented as follows based on the mag-netic dipole model.B xi ¼Q i ðx Àa i ÞR i Àm i R i B yi ¼Q i ðy Àb i ÞR 5i Àn iR 3i B zi ¼Q i ðz Àc i ÞR iÀp i R i 8>>>>>>>><>>>>>>>>:ð3ÞwhereQ i ¼3½m i ðx Àa i Þþn i ðy Àb i Þþp i ðz Àc i Þð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 sensing coil and the i -th transmitting coil.Define the error evaluation function f Err as follows,f Err¼X N i ¼1ðV i ÀB i Þ2ð4Þwhere N is the number of transmitting coils.ðx ;y ;z ;m ;n ;p Þcan be estimated with an optimization algorithm if N !5.In our EMT sys-tem,N ¼6and LM algorithm is used to solve this least square problem.After having the position and direction,the shape reconstruc-tion can be carried out.The detailed method will be described in the following parts.4.Curve shape reconstruction methodBy applying the tracking method in the robotic system,the posi-tional and directional information of the distal end of the robot can be estimated.In order to reconstruct the shape of the robot,a curve fitting method can be utilized based on the estimated result.Thiscurve fitting problem needs an appropriate curve equation.When there is a large payload,the backbone may bend to an S shape,and the constant curvature does not stand anymore.Based on the known parameters,the Bézier curve will be used in the curve fitting,for its good performance of modeling smooth curves.In the following part,the Bézier curve and the shape reconstruction method will be introduced.4.1.Bézier curveA Bézier curve is a parametric curve frequently used to modelsmooth curves.The cubic Bézier curve will be used in the recon-struction method.As shown in Fig.3,a cubic Bézier curve can have two kinds of shapes in the 3D space,the C shape as shown in Fig.3(a)and S shape which can be seen in Fig.3(b).For each curve,P 0is the start point and P 3is the end point;P 1or P 2are the control points;the curve starts at P 0going toward P 1and arrives at P 3coming from the direction of P ually,it will not pass through P 1or P 2;these two control points are only there to provide direc-tional 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 isB ðt Þ¼ð1Àt Þ3P 0þ3ð1Àt Þ2t P 1þ3ð1Àt Þt 2P 2þ3t 3P 3ð5ÞGenerally,the formula for the Bézier curve of order n can beexpressed explicitly as follows,B ðt Þ¼Xn i ¼0b i ;n ðt ÞP i ð6Þwhere t 2½0;1 and b i ;n is defined as follows,b i ;n ¼n it i ð1Àt Þðn Ài Þð7ÞFrom the above description,we can see that a Bézier curve is determined by two kinds of points,one is the start and end points,and the other is the control point.The control points are located on the tangent lines of both end joints.From the EMT system,we can have the positional and directional information of the distal end joint.The start joint’s position and direction is also known asthesesystem.It consists of 6uniaxial transmitting coils,which are stimulated sequentially.Uniaxial sensing coil system is build based on the first transmitting coil.information are constant due to the system setup.Therefore,for a three order Bézier curve,only two unknown parameters (S 01and S 23that shown in Fig.3)need to be solved.4.2.Shape reconstruction methodAs the EM sensor is mounted at the distal end of the robot,the positional and directional information can be used as P 3and H 3in the cubic Bézier curve.What needs to be estimated are the control points P 1and P 2.As shown in Fig.3,defining S 01and S 23as the length of P 0P 1and P 2P 3,we can haveP 1¼P 0þS 01H 0P 2¼P 3ÀS 23H 3ð8Þwhere P 0and H 0are determined by the system setup.Generally,P 0¼ð0;0;0Þand H 0¼ð1;0;0Þ.Positional and directional parame-ters of the sensing coil mounted on the distal end of the robot can be estimated from the EMT method and the results are ðx ;y ;z ;m ;n ;p Þ.Therefore,P 3¼ðx ;y ;z Þand H 3¼ðm ;n ;p Þ.The recon-struction 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.Based on this character,the length information of the robot can be used to establish the objective function for the optimization.Define L i as the i -th vertebrae length and L ci as the correspond-ing vertebrae length on the curve.L ci is estimated asL ci ¼jj B i ÀB i À1jj ¼Bi n ÀB i À1nð9Þwhere n is the number of points that used to estimate the curvelength on the cubic Bézier curve.Here we define n equals to the number of joints of the robot.Define the error estimation function f asf ¼X n i ¼1ðL i ÀL ci Þ2ð10ÞThe parameters S 01and S 23can then be estimated by minimize the objective function f .Here the LM algorithm is used to perform the optimization.Therefore,S 01and S 23can be estimated and then the Bézier curve can be solved and determined with (5)and (8).Based on the curve reconstruction result,each joint’s position can then be estimated based on (5)with the following equation.ðx i ;y i ;z i Þ¼B i ¼Binð11ÞTherefore,the shape reconstruction of the robot is achieved.5.Simulation and experimental resultAs shown in Fig.1,in this wire-driven flexible manipulator design the joints are revolute joints,i.e.the bending of the back-bone is in a plane.For manipulator with revolute joints,in most cases the external forces are in plane with the backbone,the defor-mation is also planar.Therefore,in the following simulation and experimental test,only the in plane deformation is considered.5.1.SimulationIn order to simulate the real application,external load will be added in the simulation during the curve generation.The equations for the model with external load are based on (1)and mechanics principle,and the final equations can be seen in (12),(13)and (15).x i ¼X N i ¼1l i sin ða i Þy i ¼XN i ¼1l i cos ða i Þa i ¼X j ¼i j ¼1h i 8>>>>>>>>>><>>>>>>>>>>:ð12ÞM i ¼EI z ða i Àa i À1Þ=h 0F ix ¼F 0ði þ1Þx þT 0i sin b i þT 0i þ1sin b i þ1F iy ¼F 0ði þ1Þy þG i ÀT 0i cos b i ÀT 0i þ1cos b i þ1M i ¼M 0i þ1þl i F gt8>>>><>>>>:ð13Þ(a)Cubic B´e zier curve with C shape (b)Cubic B´e zier curve with S shapeBézier curve.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.P 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 1the direction of P 2.S.Song et al./Mechatronics 29(2015)28–353132S.Song et al./Mechatronics29(2015)28–35Simulation results with C shape curve.In the simulation,6curvesgenerated based on(12).For each curve,there are10joints.Blue stars andrepresent the data generated by simulation and red circles and lines representreconstruction results.(For interpretation of the references to colour in thisthe reader is referred to the web version of this article.)the simulation with load added.(a)Backbone static analysis;(b)Tension analysis;(c)Joint angle relationship;that used in the simulation,which can be seen in following equation¼jjðx c i ;j ;y c i ;j ;z ci ;j ÞÀB i ;j jjð19where is ðx c i ;j ;y c i ;j ;z ci ;j Þis the position of the i -th joint of the j -th exper-iment from the camera and B i ;j is shown in (11).6.Mean error and the standard deviation of the simulation results.The wholeerror is 1.26mm.7.Flexible robot shape sensing system setup.An uniaxial electromagnetic sensing coil is mounted at the distal end of the robot.Two cameras are used capture the position of each joints.The EMT system contains a system board that generates the transmitting signals and samples the sensing signals.Six transmitting coils are used to be the sources and stimulated sequentially.8.Experimental results with load.In the experiments,6different shape generated and estimated.There are 10joints in the experimental robot.Blue lines represent the position results from the camera and red lines represent reconstruction results.(For interpretation of the references to colour in this figure legend,the reader is referred to the web version of this article.)Distance error with each joint between the camera result and the reconstruction result is used to evaluate the performance.whole mean error is3.02mm,which is evaluated withDiscussioncubic Bézier curve is determined by four points,the starting the ending point and two control point.As shown in curve is a space curve and these four points are not restricted in the same plane.Therefore,the method can still work section is deformed to different planes.Another thing needs to mention is that for a bending section,its better for the radian of the curve less than p.Otherwise,the method may be invalid.As shown in Fig.10,the shape reconstruction method works good when the radian of the arc less then p.In the real experiments, the maximum radian that a single section can bend is no more than p=2thanks to its structure.Therefore,the proposed method is expected to have a good reconstruction result no matter how the robot bends.From the experimental results,we can see that the method will work better if the robot still maintains the C shape after bent by the external force or payload.If the robot is forced to bend to an S shape,the position error of the joints may increase,but the results are still acceptable.6.ConclusionIn this paper,a novel shape reconstruction method for the wire-drivenflexible robots is proposed.An electromagnetic sensor is mounted on the distal end of the robot.The sensor can provide positional and directional information of the tip.Then the recon-struction algorithm,which is based on a three order Bézier curve, is carried out utilizing the position and direction information of the sensor,together with the length information of the robot.A few experiments have been performed under different payload conditions and their results have proved the feasibility of the method.The mean distance error of the joints between the recon-struction results and the camera results is3.02mm.This method can be used to track the tip and detect the shape of theflexible robot and provide the feedback information during a transoral or transnasal surgery.In the future,we will reduce the size of the robot and apply this method in real operating room to test feasibil-ity of the method for the minimally invasive surgery. References[1]Ren H,Lim CM,Wang J,Liu W,Song S,Li Z,et puter assisted transoralsurgery withflexible robotics and navigation technologies:a review of recent progress and research challenges.Crit Rev Biomed Eng2013;41(4–5):365–91.[2]da Vinci surgery,</>,2014-10-12.[3]Simaan N,Xu K,Wei W,Kapoor A,Kazanzides P,Taylor R,et al.Design andintegration of a telerobotic system for minimally invasive surgery of the throat.Int J Robot Res2009;28(9):1134–53.[4]Shang J,Payne CJ,Clark J,Noonan DP,Kwok K-W,Darzi A,et al.Design of amultitasking robotic platform withflexible arms and articulated head for minimally invasive surgery.In:2012IEEE/RSJ international conference on intelligent robots and systems(IROS).Vilamoura,Portugal:IEEE;2012.p.1988–93.[5]Webster RJ,Jones BA.Design and kinematic modeling of constant curvaturecontinuum robots:a review.Int J Robot Res2010;29(13):1661–83.[6]Li Z,Du R,Lei MC,Yuan SM.Design and analysis of a biomimetic wire-drivenrobot arm.In:ASME2011international mechanical engineering congress and exposition.Denver,USA:American Society of Mechanical Engineers;2011.p.191–8.[7]Palli G,Pirozzi S.A miniaturized optical force sensor for tendon-drivenmechatronic systems:design and experimental evaluation.Mechatronics 2012;22(8):1097–111.[8]Bajo A,Dharamsi LM,Netterville JL,Garrett G,Simaan N.Robotic-assistedmicro-surgery of the throat:the trans-nasal approach.In:2013IEEE international conference on robotics and automation(ICRA).Karlsruhe, Germany:IEEE;2013.p.232–8.[9]Palli G,Melchiorri C.Friction compensation techniques for tendon-drivenrobotic hands.Mechatronics2014;24(2):108–17.[10]Gouttefarde M,Daney D,Merlet J-P.Interval-analysis-based determination ofthe wrench-feasible workspace of parallel cable-driven robots.IEEE Trans Robot2011;27(1):1–13.[11]Dupont PE,Lock J,Itkowitz B,Butler E.Design and control of concentric-tuberobots.IEEE Trans Robot2010;26(2):209–25.[12]Rucker DC,Jones BA,Webster RJ.A geometrically exact model for externallyloaded concentric-tube continuum robots.IEEE Trans Robot 2010;26(5):769–80.[13]Li Z,Du R.Design and analysis of a bio-inspired wire-driven multi-sectionflexible robot.Int J Adv Robot Syst2013;10(209):1–11.[14]Rucker DC,Webster RJ,Chirikjian GS,Cowan NJ.Equilibrium conformations ofconcentric-tube continuum robots.Int J Robot Res2010;29(10):1263–80.Mean error and the standard deviation of the experiment results withwhole mean error is3.02mm.10.Reconstruction result for a circle arc using cubic Bézier curve.The blue linesthe arcs and the red lines represent the reconstruction result.(For interpretationthe references to colour in thisfigure legend,the reader is referred to the webversion of this article.)34S.Song et al./Mechatronics29(2015)28–35[15]Rucker DC,Webster RJ.Statics and dynamics of continuum robots with generaltendon routing and external loading.IEEE Trans Robot2011;27(6):1033–44.[16]Xu K,Simaan N.Analytic formulation for kinematics,statics,and shaperestoration of multibackbone continuum robots via elliptic integrals.J Mech Robot2010;2(1):011006.[17]Trivedi D,LotfiA,Rahn CD.Geometrically exact models for soft roboticmanipulators.IEEE Trans Robot2008;24(4):773–80.[18]Roesthuis RJ,Abayazid M,Misra S.Mechanics-based model for predicting in-plane needle deflection with multiple bends.In:20124th IEEE RAS&EMBS international conference on biomedical robotics and biomechatronics (BioRob).Rome,Italy:IEEE;2012.p.69–74.[19]Boctor EM,Choti MA,Burdette EC,Webster Iii RJ.Three-dimensionalultrasound-guided robotic needle placement:an experimental evaluation.Int J Med Robot Comput Assist Surgery2008;4(2):180–91.[20]Park Y-L,Elayaperumal S,Daniel B,Ryu SC,Shin M,Savall J,et al.Real-timeestimation of3-d needle shape and deflection for mri-guided interventions.IEEE/ASME Trans Mechatron2010;15(6):906–15.[21]Henken K,Van Gerwen D,Dankelman J,Van Den Dobbelsteen J.Accuracy ofneedle position measurements usingfiber bragg gratings.Minimally Invasive Therapy Allied Technol2012;21(6):408–14.[22]Kim B,Ha J,Park FC,Dupont PE.Optimizing curvature sensor placement forfast,accurate shape sensing of continuum robots.In:2014IEEE international conference on robotics and automation(ICRA).Hong Kong,China:IEEE;2014.p.5374–9.[23]Bajo A,Simaan N.Kinematics-based detection and localization of contactsalong multisegment continuum robots.IEEE Trans Robot2012;28(2):291–302.[24]Taniguchi A,Aizawa C,Yoshizawa Y,Onoda F,Toriyama S,Kawabata T,et al.Endoscope shape detecting apparatus wherein form detecting processing is controlled according to connection state of magneticfield generating means.US Patent6,432,041,August13;2002.[25]Li Z,Du R,Yu H,Ren H.Statics modeling of an underactuated wire-driven robotarm.In:IEEE international conference on biomedical robotics and biomechatronics(BioRob).Sao Paulo,Brazil:IEEE;2014.p.326–31.[26]Hu C,Song S,Wang X,Meng M-H,Li B.A novel positioning and orientationsystem based on three-axis magnetic coils.IEEE Trans Magnet 2012;48(7):2211–9.[27]Song S,Hu C,Li B,Li X,Meng M-H.An electromagnetic localization andorientation method based on rotating magnetic dipole.IEEE Trans Magnet 2013;49(3):1274–7.[28]Song S,Qiao W,Li B,Hu C,Ren H,Meng M-H.An efficient magnetic trackingmethod using uniaxial sensing coil.IEEE Trans Magnet2014;50(1):1–7. [29]Song S,Ren H,Yu H.An improved magnetic tracking method using rotatinguniaxial coil with sparse points and closed form analytic solution.IEEE Sens J 2014;14(10):3585–92.[30]Ren H,Kazanzides P.Investigation of attitude tracking using an integratedinertial and magnetic navigation system for hand-held surgical instruments.IEEE/ASME Trans Mechatron2012;17(2):210–7.S.Song et al./Mechatronics29(2015)28–3535。