Stiffness Control of a Continuum Manipulator in Contact with a SoftEnvironment

Mohsen Mahvash and Pierre E. Dupont, Senior Member, IEEE

Abstract— Stiffness control of a continuum robot can pre-vent excessive contact forces during robot navigation insidedelicate, uncertain and confined environments. Furthermore, itenables the selection of tip stiffnesses that match varying taskrequirements. This paper introduces a computationally-efficientapproach to continuum-robot stiffness control that is based onwriting the forward kinematic model as the product of twotransformations. The first transformation calculates the non-contact kinematics of the robot and can be formulated basedon the specific type of continuum robot under consideration.The second transformation calculates the tip deflection due toapplied forces and is efficiently computed using the specialCosserat rod model. To implement a desired tip stiffness, thetwo transformations are used to solve for the actuator positionsthat deform the manipulator so as to generate the required tipforce at the measured tip position. The efficacy of the proposedcontroller is demonstrated experimentally on a concentric-tubecontinuum robot.

I. INTRODUCTION

Historically, the study of robot-environment interactionhas focused on industrial robots interacting with stiff en-vironments in assembly operations and other manufacturingtasks [1]. In these applications, robot position control is in-feasible since uncertainties in both the position and stiffnessof the environment lead to excessive contact forces as wellas to the jamming and wedging of parts. To avoid theseproblems, a variety of solutions have been introduced. Theseinclude the introduction of passive compliance in the robot,e.g., an RCC wrist [2], as well as hybrid force / motioncontrol [1] and stiffness control [3].

A continuum robot is a manipulator whose curvaturecan be controlled by adjusting the internal deformation ofmechanically coupled elastic components of the body ofrobot [4]. Continuum robots include steerable catheters [5],multi-backbone snake-like robots [6], [7] and concentric tuberobots [8], [9]. Two examples are shown in Figure 1. Sincerobot shape is controlled by the storage and release of elasticenergy in the robot’s component parts, continuum robots areflexible by design.

The resulting robot stiffness may, however, be largeenough to generate excessive and damaging contact forcesduring interactions with soft surgical environments. While itis possible to avoid this problem by building the robot fromextremely low stiffness components, e.g., as is done with

This work was supported by the National Institutes of Health under grantsR01HL073647 and R01HL087797.

M. Mahvash is with Harvard Medical School, Boston, MA emailmahvash@bu.edu. P. Dupont is with Cardiovascular Surgery, Chil-dren’s Hospital Boston, Harvard Medical School, Boston, MA 02115, USAemail Pierre.Dupont@childrens.harvard.edu.

(a)

(b)

Fig. 1. Schematics of two types of continuum robots. (a) concentric-tuberobot, (b) tendon-driven robot with flexible backbone.

catheters, such a robot cannot generate the forces requiredfor doing many surgical manipulation tasks such as tissuepenetration and suturing. An alternate approach that producesthe safety of low stiffness during robot navigation, but alsoenables high stiffness during tissue manipulation is to employstiffness control. In this approach, the robot operator canselect and modify the robot’s contact stiffness during aprocedure. The desired stiffness can be set lower than theinherent stiffness of the manipulator to safely navigate insideconfined surgical environments.

This paper introduces a stiffness controller for a continuumrobot in the context of the kinematic mapping rather thanthe force mapping generally used for rigid robots. Thekinematic model of the robot in contact is used to map thetip force and tip position to joint positions of the robot. Real-time measurement of tip position is used to calculate robotdeflection. The kinematic model is considered as the productof two transformations: the first transformation calculates theunloaded kinematics of the robot while the second calculatesthe tip deformation due to applied forces. It is assumedthat an unloaded forward kinematic model of the specificcontinuum robot is available, i.e., a model that calculates tipconfiguration from actuator positions assuming no externalforces are applied. These models are readily available in theliterature [6], [8], [9].

General applicability to all types of continuum robots isachieved by assuming that, in any given configuration, any

The 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems October 18-22, 2010, Taipei, Taiwan

978-1-4244-6676-4/10/$25.00 ©2010 IEEE 863

continuum robot can be approximated by a single Cosseratrod (of varying mechanical properties along its length)for the purpose of computing deformation due to externalloading [10], [11]. The special Cosserat rod model enablescomputationally efficient calculation of robot deflection dueto external forces [10]. Stiffness control is implementedusing an iterative method to solve for the actuator positionsthat achieve the desired tip force. Efficacy of the proposedcontroller is demonstrated experimentally for a 3 degree offreedom (DOF) concentric-tube robot.

The paper is arranged as follows. Related work is de-scribed in the following section. Section III presents thegeneral kinematic and force mappings of continuum robots.The deflection model using the special Cosserat rod modelis presented in Section IV. The stiffness controller is pre-sented in Section V and its experimental implementationis described in Section VI. Conclusions appear in the finalsection.

II. RELATED WORK

Unloaded kinematic models have been developed for avariety of continuum robots. Schematics of two types areshown in Figure 1. Figure 1(a) shows a concentric tube robotwhich is constructed by telescopically extending concentri-cally combined pre-curved superelastic tubes. The robot’sshape and tip location are controlled via the kinematic inputsconsisting of the relative rotations and translations of thetubes at their proximal ends (q1, . . . ,q8). The unloaded kine-matic models for this type of robot depend on the modelingassumptions. When the tubes are considered torsionally rigid,the kinematics are described by algebraic equations. Forlong or highly curved tubes, torsional compliance can beimportant and the kinematics take the form of differentialequations in arc length with split boundary conditions [9].

Figure 1(b) depicts a tendon-driven continuum robot ofthe type often used in steerable catheters and for distaldexterity enhancement in minimally invasive surgery. Theserobots possess a central flexible backbone that is deflectedby symmetrically arranged wires [6], [5] or tubes [12], [7].As shown in the schematic, spacer disks are attached to thecentral backbone. The wires or tubes slide through holes inall disks except for the most distal one to which they areattached. Unloaded kinematic models are developed by re-lating central backbone shape to tendon length, q1,q2,q3 [6],[5]. While wire tendons are limited to tensile forces, tubescan also be used in compression and their bending stiffnessmust be accounted for in the kinematics [12], [13]. For thistype of continuum robot, the mapping between forces in theactuating tubes and tip forces has also been derived [12].Using this model, tube forces were used to infer tip forces.

III. KINEMATIC AND FORCE MAPPINGS OF CONTINUUMROBOTS

All continuum robots can be modeled by a space curve,r(q,s) ∈ ℜ3, that is a function of the n kinematic inputvariables, q ∈ ℜn, and the arc length, s, together withcoordinate frames defined at the robot’s base (frame B)

and tip (frames T and T ). These are shown in Figure 2.Referring back to Figure 1, the space curve corresponds tothe common tube centerline of (a) and the curve of the centralbackbone in (b). In contrast to rigid robots, two tip frames,T and T , are defined. The former corresponds to the tipconfiguration when no external loads are applied while thelatter includes the deformation arising from a wrench F ∈ℜ6

applied externally at the tip. The first three components ofF correspond to the tip force while the latter three are thoseof the torque applied to the tip [14].

Fig. 2. Continuum robot represented as a space curve. W is worldcoordinate frame while B, P, T and T are robot body frames. T and T aretip frames without and with the application of tip wrench, F , respectively.

The configuration of frame C relative to frame B is a rigidbody transformation that can be written in homogeneouscoordinates as

gbc(q) =[

Rbc(q) pbc(q)0 1

](1)

where Rbc ∈ SO(3) is the rotation matrix, and pbc ∈ ℜ3 isthe translation vector between frames B and C [14]. Thisexpression also represents a mapping from kinematic inputspace q ∈ Q to the special Euclidean group SE(3).

For the continuum robot of Figure 2, the configuration ofthe frame T relative to the frame B, gbt , can be written asthe product of two transformations:

gbt(q,F) = gbt(q) gtt(q,F) (2)

where gbt is the configuration of frame T relative to Band gtt is the configuration of T relative to T . The firsttransformation, gbt(q), corresponds to the unloaded robotkinematic models cited in Section II. The second transfor-mation, gtt(q,F), represents the displacement associated withthe deformation of the robot due to the applied loading. Thus,the overall kinematic map, gbt(q,F), is a function of both thekinematic variables and the external loading.

The force mapping equation for continuum robots can bederived from the principle of virtual work as was done in [12]to obtain

τ = JTbtF +

∂E(q,F)

∂q(3)

864

Here, τ ∈ ℜn is the vector of actuator forces and torques,E(q,F) ∈ ℜ is the elastic energy of the manipulator, andJbt(q,F) is the Jacobian matrix mapping actuator velocitiesto tip velocity [14]. Of course, for rigid robots, the elasticenergy term drops out and this equation reduces to thestandard form, τ = JT

btF .To implement stiffness control in a rigid robot, the force

mapping equation must be used to relate desired tip wrenchesto actuator forces and torques. For continuum robots, how-ever, it is also possible to implement stiffness control usingthe kinematic mapping (2). In this approach, (2) is solvedfor the actuator positions corresponding to the desired tipwrench, Fd , and the actual measured tip configuration, gm

bt .Comparing the two approaches, stiffness control based on

the force mapping becomes an actuator force / torque controlproblem while stiffness control using the kinematic map is anactuator position control problem. Use of the force mappingrequires the use of joint force /torque sensors [12] or actuatorcurrents [15]. In contrast, actuator position control can beperformed accurately using the existing actuator encoders.

Thus, it may be advantageous to employ (2) if it can besolved efficiently. To explore this question, assume that it isdesired to apply a wrench Fd at the tip of a 6 DOF continuumrobot that has 6 independent joint variables while its tip isheld rigidly fixed at configuration gm

bt . Equation (2) can berewritten in terms of its inputs, measured tip configuration,gm

bt , and desired tip wrench, Fd , and its output, desiredactuator positions, qd

gmbt = gbt(q

d) gtt(qd ,Fd) (4)

Driving the actuator displacements to qd produces defor-mations of the continuum robot that produce the desiredtip wrench Fd at the measured tip configuration of therobot. Note that for the general case of contact with a softenvironment, producing a desired tip wrench will also causethe tip configuration to change.

The root finding problem of solving (4) for qd can beperformed efficiently if its right hand side can be computedquickly. The term gbt(·) is the unloaded forward kinematicmodel for which numerically efficient formulations are avail-able [6], [5], [12], [9]. The second term, gtt(·, ·), is the tipdisplacement produced by application of tip wrench on theunloaded robot.

As described below, given gbt(qd), an efficiently computedestimate of the product gbt(qd ,Fd) = gbt(qd) gtt(qd ,Fd) canbe obtained using the special Cosserat rod model.

IV. DEFLECTION MODEL

To obtain gbt(q,F) = gbt(q) gtt(q,F), we approximateany continuum manipulator as a single elastic rod whosecurvature and elastic properties match those of the robot foractuator values q and tip wrench F = 0. Thus, the curvatureis selected to match the robot’s backbone curve, r(q,s), andits stiffness is selected to match the composite stiffness ofall elastic components that comprise the robot.

Deflection of the robot is computed as deflection of therod in response to tip wrench, F . This model is approximate

in that it does not account for relative motions of therobot’s elastic components in response to the tip wrench.As demonstrated in our experiments, the error associatedwith this approximation is often negligible. It is also assumedthat the cross sectional shear and longitudinal extension ofthe rod are negligible. This assumption is often made indeveloping unloaded kinematic models for continuum robots;see, e.g., [9].

The special Cosserat rod model is well known in themechanics literature [11] and has also been employed in therobotics and computer graphics literatures [10], [16], [9].Here, a concise overview of the model and its numericalsolution are presented.

A. Strain and Curvature of a Rod

As shown in Figure 2, coordinate frames can be definedalong the backbone curve, r(q,s). In the following, actuatorvalues, q, are assumed constant and are omitted as argu-ments. Since these frames are intended to track materialdeformations of the rod’s cross sections, a natural choicefor frame orientation is to choose a base frame, B with oneaxis (i.e., the z axis) aligned with the curve’s tangent andthen to slide this frame without rotation about the local zaxis along r(s) [17].

The resulting Bishop frame P at arc length s can be writtenas gbp(s), s∈ [0, l] where l is the length of the robot. Its originlies on r(s) and experiences a rate of change with respect toarc length, v(s) ∈ℜ3, given by

v(s) =dr(s)

ds(5)

The rate of change of frame P’s coordinate axis unit vectors,{ex(s),ey(s),ez(s)}, with respect to arc length satisfy thebody-frame equations

dex(s)ds

= u(s)× ex(s),dey(s)

ds= u(s)× ey(s),

dez(s)ds

= u(s)× ez(s) (6)

in which u(s) ∈ℜ3.The vectors u(s) and v(s) are the angular and linear strains,

respectively, experienced by the cross section. Thus, u(s) hasthe units of curvature and its x and y components correspondto bending of the rod while its z component corresponds totwisting of the rod. Similarly, the x and y components of v arethe shear strain components of the cross section while the zcomponent is vz = 1+εz in which εz is the longitudinal strain.Given the assumptions of negligible shear and longitudinalstrain,

v(s) =[

0 0 1]T

, s ∈ [0, l] (7)

Angular and linear strains, u and v, provide body framedescriptions of the curved shape of the rod. It can be helpfulto note that u and v are analogous to body-frame angular andlinear velocities if time is substituted for arc length. Thus,

865

coordinate frames, gbp(s), are obtained as the solution to thedifferential equation

dds

(gbp(s)

)= gbp(s)

[[u(s)] v

0 0

](8)

in which the square bracket on the vector u indicates theskew-symmetric matrix form

[u(s)] =

0 −u3 u2u3 −u1−u2 u1 0

(9)

This differential equation can be integrated numerically frombase to tip or tip to base using a method that preserves thegroup structure of SE(3). A variety of numerical integrationmethods are available for this purpose [18], [19].

The initial curvature of the robot prior to the applicationof external loads is denoted by u(s). Given an unloadedkinematic model that computes gbp(s), the unloaded shapeof the rod used to model robot deflection can be computedas

[u(s)] = R−1bp (s) R−1

bp (s) (10)

in which Rbp(s) is the rotation matrix of gbp(s).

B. Rod Deformation Due to Applied Loads

To compute rod deformations, two equations are needed.The first is a constitutive model that relates cross-sectionstrains u and v to the bending moment, m ∈ℜ3, and force,n ∈ℜ3, acting on the cross section. Since shear of the crosssection and rod extension are neglected, only the relationshipbetween u and m is needed. The second equation relatescross-section bending moment and force to the externalloads. Both are described below.

When a rod with initial curvature u(s) is deformed to anew curvature u(s), a bending moment, m(s), is generated.Assuming linear elastic behavior, the bending moment at anypoint s along the rod is given by

m(s) = K(s)(u(s)− u(s)) (11)

where K(s) is the frame-invariant stiffness tensor. For a largeclass of rods, K(s) is given by

K(s) =

Ex(s)Ix(s) 0 00 Ey(s)Iy(s) 00 0 J(s)G(s)

(12)

and Ex(s), Ey(s) are moduli of elasticity, Ix(s) and Iy(s)are area moments of inertia, J(s) is the polar moment ofinertia and G(s) is the shear modulus. These values shouldbe selected so that the rod stiffness matches the overallcontinuum robot stiffness as a function of arc length.

To relate cross-section bending moment and force to theexternal loads, we employ the equilibrium equation of thespecial Cosserat rod model [11]. Written in the body framecoordinates of Figure 2, the differential equation governing

bending moment m and force n as a function of arc lengths is given by

dm(s)ds

= τ(s)− [u(s)]m(s)− [v(s)]n(s) (13)

dn(s)ds

= φ(s)− [u(s)]n(s)

where φ(s) and τ(s) are the applied force and torque perunit length of the rod and the bracket notation is as definedin (9).

For simplicity of exposition, it is assumed that only tiploads are applied to the robot and so τ(s) = φ(s) = 0, s ∈[0, l]. Combining (11) and (13) results in equations forcurvature u and force n,

du(s)ds

=du(s)

ds− [u(s)] (u(s)− u(s))+K−1(s)[v]n(s)

dn(s)ds

= −[u(s)]n(s) (14)

The boundary conditions for these equations, u(l), n(l), canbe specified from the body-frame wrench, Fb, applied at therobot’s tip [

nT (l) mT (l)]T

= Fb (15)

u(l) = K−1m(l)+ u(l)

and (14) can be solved numerically by integrating backwardin arc length from s = l → 0 [10]. Since (13) is written inbody coordinates, the body-frame twist velocity, [vT ,uT ]T

must be simultaneously integrated such that the equationsevolve on SE(3). As detailed in [10], a first-order Crouch-Grossman method [18] provides computational efficiency.The result of this integration is the desired estimate ofthe transformation between the base and tip of the robot,gbt = gbt gtt .

In stiffness control, the desired tip wrench is often cal-culated in a local frame at the tip of the robot whoseaxes always remain parallel to the axes of the world frame,Fd

tw [20]. In this case, the boundary condition is a functionof the rotation of the rod tip. Specifically, if the base of therod is positioned at the origin of the world frame then gwbin Figure 2 is the identity matrix and the desired body-frametip wrench, Fd

b , to be used in (15) is related to the desiredframe tip wrench by

Fdb =

[RT

bt 00 RT

bt

]Fd

tw (16)

where, following (1), Rbt is the rotation matrix of gbt(qd ,Fdb ).

While the integral must now be solved iteratively, conver-gence is rapid when the tip wrench varies smoothly andthe solution is seeded using the desired rod shape from theprevious time step.

V. STIFFNESS CONTROLLER USING A MODIFIEDPOSITION CONTROLLER

The task of the controller is to create a user-definedstiffness at the tip of the continuum manipulator. If thestiffness is specified for all degrees of freedom of the robot

866

tip, a pure stiffness controller can be implemented. In manymedical applications, however, it is desirable to controlthe stiffness in certain directions, e.g., tip position, and tocontrol the motion in other directions, e.g., tip orientation.Because of its practical importance, this latter case of hybridstiffness / motion control is considered here. The frameworkdescribed below can be easily adapted to other combinationsof stiffness- and motion-controlled coordinates.

The desired tip force, f d ∈ℜ3, is computed using the user-defined diagonal stiffness matrix, Kd ∈ ℜ3×3, based on thedifference between measured robot tip position, pm

bt ∈ ℜ3,and a reference tip position, pr

bt ∈ℜ3 as follows

f d = Kd(pmbt − pr

bt) (17)

The desired tip orientation of the robot is given as Rdbt ∈

SO(3).As shown in Figure 3, the stiffness equation (17) can be

pictured as a virtual spring of stiffness Kd that connectsthe reference position pr

bt to the current tip position of themanipulator.

Fig. 3. Controller implements a virtual linear spring at the tip of themanipulator. Desired actuator positions are such that when robot is deflectedfrom unloaded tip configuration, T , to configuration T , the desired tip springforce is generated and the desired tip orientation is achieved.

The desired force, f d , and desired tip orientation, Rdbt ,

must be mapped to the actuator positions, qd , such thatthe deflected robot will generate the force f d at the tipconfiguration

gdmbt =

[Rd

bt pmbt

0 1

](18)

The desired actuator positions are implicitly defined by (4),re-written for gdm

bt

gdmbt =

[Rd

bt pmbt

0 1

]= gbt(qd ,Fd

tw) = gbt(qd) gtt(q

d ,Fdtw) (19)

In this equation, pmbt is directly measured by a tip sensor while

gbt(qd ,Fdtw) is computed for values of qd and f d from the rod

deformation model (14)-(16) using the wrench boundarycondition

Fdtw =

[f d

[0 0 0]T

](20)

and the initial shape (10) obtained from the unloaded kine-matic model

[u(qd ,s)] = R−1bp (q

d ,s) R−1bp (q

d ,s) (21)

in which Rbp(qd ,s) is the rotation matrix of gbp(qd ,s).Using an efficient numerical implementation, e.g., Gauss-

Newton, (19) can be solved iteratively at each time step ofthe controller for the actuator positions, qd , that produce thedesired combination of tip force and orientation. By drivingthe actuator variables to qd , the manipulator is deformed soas to produce the desired tip force at Rd

bt . Position trackingcontrollers, e.g., PD, can be used to drive and maintain theactuators of the manipulator at their desired values. The gainsof the actuator controllers should be selected so that steady-state tip position error due to actuator error is small comparedto the deflection of the manipulator.

Since position controllers already exist for a variety ofcontinuum robots [6], [5], [9], it can be advantageous toleverage the existing controller implementation to achievestiffness control. This can be accomplished as follows.

A position controller solves the inverse kinematic problemand drives the actuators to the positions given by

qd = I (gdbt) (22)

where I (·) represents a multi-dimensional inverse kinematicfunction for the unloaded robot that calculates either numer-ically or analytically the actuator positions corresponding tothe user-defined configuration, gd

bt .To include the tip-applied wrench, the argument of I can

be rewritten using (19) to obtain

qd = I(gdm

bt g−1tt (qd ,Fd

tw))

(23)

where Cosserat deflection model is used to computegtt(·,Fd

w )).Since the deflection model computes gbt(qd ,Fd

tw) ratherthan gtt(qd ,Fd

tw), (23) can be rewritten using (2) to arrive at

qd = I(gdm

bt g−1bt (q

d ,Fdtw) gbt(q

d))

(24)

This equation can be solved for qd using fixed pointiteration [21],

qdi+1 = I

(gdm

bt g−1bt (q

di ,F

dtw) gbt(q

di )), i = 1,2, . . . (25)

where index i is the iteration number.In this way, the stiffness controller can be implemented

by the on-line iteration of (25). The controller computationsfor each iteration consist of a single evaluation of (i) the un-loaded kinematic model, gbt(qd

i ), (ii) the deformation model,gbt(qd

i ,Fd

tw), and (iii) the unloaded inverse kinematic model,I (·). Assuming that the pre-existing position controller runsat a sufficiently high rate, the changes in controller inputs(pr

bt and Rdbt ) and robot tip position (pm

bt ) are small from onecontroller cycle to the next and it is sufficient to evaluate theiteration equation (25) once per cycle.

VI. EXPERIMENTAL IMPLEMENTATION

The stiffness controller was implemented to produce adesired positional tip stiffness on a three DOF concentrictube robot. This is a case of pure stiffness control sincethe stiffness is specified for each degree of freedom. Therobot was comprised of the two 150 mm long tubes shown in

867

Figure 4. Relative rotation of the tubes varies their combinedcurvature, u(s), from the initial constant pre-curvature valueof u(s) = [1/240 0 0]T mm−1, s ∈ [0,150] mm, when thecurvatures are aligned (q1 = q2), to u(s) = [0 0 0]T , s ∈[0,150] mm (robot is straight) when the curvatures are anti-aligned (q1 = q2+π). The robot can also be translated in thez direction by actuator displacement q3. Varying {q1,q2} ∈SO(2)×SO(2) and q3 ∈ R produces a cylindrical workspaceat the robot’s tip of radius 48 mm and length 254 mm.

Fig. 4. Concentric tube manipulator and drive system. Electromagneticsensor is shown attached to robot tip. Actuator variables q1 and q2 controlrotation of tubes and q3 controls translation of tube pair. Curvature is variedby relative rotation of tubes. Environment motion is produced by manualloading of robot tip through cord attached to load cell.

An existing position control architecture [9] was modi-fied to implement stiffness control. The existing controllerconsists of a master-slave system in which the concentrictube manipulator is the slave arm and a PHANTOM Omnihaptic device (Sensable Technologies, Inc.) is employed asthe master arm. The position controller is implemented asa multithreaded process under Windows 2000. The processincludes two time-critical user mode threads running at 1 kHzthat implement the kinematic model and PD joint controllersand an application thread that updates a GUI.

The stiffness controller requires real-time measurement ofthe robot’s tip configuration. This was accomplished using anelectromagnetic tracking sensor (3D Guidance trakSTART M ,Ascension Technology Corporation). The 2 × 9.7 mm cylin-drical sensor (model 180) was attached to the robot’s tipas shown in Figure 4. Sensor accuracy is 1.4 mm RMS intranslation and 0.5 degree RMS in rotation with a resolutionof 0.5 mm and 0.1 degrees. The update rate of the sensorwas set to 100 Hz. The sensor’s electrical leads producednegligible deformation of the robot.

To calibrate the deflection model used in the stiffnesscontroller and to evaluate the controller’s performance, a22 N tension/compression load cell (Sensotec model 31)was used to measure environment force. The load cell wasconnected to the tip of the manipulator through a long thincord to prevent the metal components of the load cell fromdistorting the magnetic field of the tip tracking system. This

loading configuration is depicted in Figure 4.To evaluate the controller, experiments were performed

with a moving environment shown in Figure 4. The envi-ronment is produced by manually pulling on the robot tipin a desired direction through a cord attached to a load cell.During these tests, the desired reference tip position, pr

bt ,of (17) was held constant by fixing the position of the master.

Implementation of the proposed stiffness controller re-quires an unloaded kinematic model and a calibrated deflec-tion model. Each is described below followed by the resultsof the control experiments.

A. Unloaded Kinematic Model

To implement stiffness control by modification of a posi-tion controller as given by (25), it is assumed that forwardand inverse kinematic solutions are already implemented forthe non-contact case. Such models have been presented in [9]for concentric tube continuum robots. Modified versions ofthese models, appropriate to the pair of tubes used in theexperiments, are presented here.

While, in general, the combined curvature of two tubesof constant pre-curvature varies along their length due totorsional twisting of the tubes [9], this effect is negligiblefor the tubes used in the experiments. Thus, it is appropriateto model the combined curvature as a function of actuatorvalues, {q1,q2}, that is independent of arc length. It can bewritten in the world frame of Figure 4 as uw,

uw = Aκ (q1−q2)

cos((q1 +q2)/2+φκ(q1−q2))sin((q1 +q2)/2+φκ(q1−q2))

0

(26)

Here, Aκ(·) and φκ(·) compute the magnitude and phase ofcurvature as functions of the relative tube rotation angle,q1 − q2. For curve fitting, Aκ(·) and φκ(·) are interpretedas the magnitude and phase of a complex function κ(·).

The tip position, assuming no contact forces, is obtainedfrom the curvature, uw, as [9]

pbt(q) =

uw

y (1−cos(l|uw|)|uw|2

−uwx (1−cos(l|uw|)|uw|2

q3 +sin(l|uw|)|uw|

(27)

in which l is the arc length of the manipulator. To obtain themost accurate kinematic model, the complex function κ(·)was calculated from (26) and (27) as a truncated Fourierseries using position measurements obtained with the tiptracking sensor over two complete revolutions of the tubes.

B. Deflection Model Calibration

To calculate robot deflection due to tip loading, the deflec-tion model requires the unloaded body-frame curvature of therobot, u(s), as well its composite stiffness, K(s). While (10)provides a general expression for unloaded curvature, in thiscase, it can be directly obtained from (26). Due to the choiceof Bishop body frames and since the unloaded kinematicmodel is of constant curvature,

u(s) = uw (28)

868

The deflection model approximates the composite stiffnessof all elastic elements of the robot by the matrix of bendingand torsional stiffnesses, K(s), defined in (12). Since therobot is composed of NiTi tubes, K(s) reduces to

K(s) = K = diag(EcIc, EcIc, EcIc/(1+ν)) (29)

in which Ec and Ic are the composite values for elasticmodulus and area moment of inertia, respectively, and ν isPoisson’s ratio.

Using the value of ν = 0.3 that is appropriate for NiTi, EcIcwas estimated experimentally using the testing configurationof Figure 4 and the maximum possible value of initialcurvature u(s) = [1/240 0 0]T mm−1. An iterative methodwas used to solve for the stiffness matrix that minimizedthe error between the force-displacement response predictedby the model and that obtained by measurement in x and ydirections. The resulting calibrated stiffness of

K(s) = diag(0.049 0.049 0.038)N-m2 (30)

was used to compare the deflection model and experimentalrobot tip stiffnesses as shown in Figure 5. (See Figure 4for the coordinate directions.) The depicted experimentaldata was collected for cyclic displacements in the x, y andz coordinate directions while holding the robot actuatorsfixed. While the experimental data reveals a small amountof hysteresis, the deflection model provides a good fit withthe loop average in the x and y directions while the highstiffness in the z direction reduces the accuracy of the modelfit.

Fig. 5. Force versus displacement of the robot tip in coordinate directions{x,y,z}. Both experimental data and predictions from the calibrated deflec-tion model are shown for the maximum curvature configuration, labeled(max), and zero curvature configuration, labeled (zero).

C. Stiffness Controller

Stiffness control was implemented for the three DOF con-tinuum robot depicted in Figure 4 by modifying an existingposition controller. The position controller uses Newton’smethod to solve (26)-(27) at each time step for the actuatorpositions associated with the desired unloaded tip position,pd

bt ,

qd = I(

pdbt

)(31)

PD controllers are used to drive the actuators to the valuescomputed in (31).

To achieve stiffness control, (31) was replaced with theiteration equation (25) which, for this robot, reduces to anexpression involving only tip positions,

qdi+1 = I

(pm

bt − (pbt(qdi ,F

dtw)− pbt(q

di )))

(32)

Here, pmbt is the current tip position as measured by the

tip tracking sensor. The unloaded tip position, pbt(qdi ) is

computed using (26) and (27).The deflected tip position, pbt(qd

i ,Fd

tw) is calculated fromthe deflection model (14) using qd

i to compute the pre-deflected curvature, u(s), from (26) and (28). The boundaryconditions (15) are computed using the desired tip wrench,Fw, as defined by (17) and (20). This wrench is convertedto body coordinates using (16) with Rbt computed using thecurrent tip orientation as measured by the tip sensor. Theintegration was carried out with the discrete (in arc length)formulation detailed in [10] using ten nodes along the 150mm length of the robot.

D. Controller Evaluation

The testing configuration of Figure 4 was used to evaluatethe performance of the controller in the three coordinatedirections for various values of robot curvature and tipstiffness. During these tests, the master manipulator washeld fixed such that the reference tip position, pr

bt , wasconstant and lay in the y-z plane above the line definedby actuator axis, q3. (See Figure 4.) In each test, the robottip was displaced in one of the three coordinate directions.Each displacement started with the robot tip in the unloadedconfiguration and proceeded until an arbitrary maximumvalue was obtained. The displacement was then reversed.

Figure 6 depicts the measured tip force and displacementin the three coordinate directions of Figure 4 for an in-termediate value of non-contact robot curvature given byuw = [ 1

320mm 0 0]T . As is also shown, the desired stiffnessesof (17) were set to be equal in the three coordinate directions,Kd = diag(0.04 0.04 0.04) N/mm. The maximum applieddisplacements in the x and y directions were set to be around20 mm in order to demonstrate the range of forces overwhich the stiffness controller can be applied [20]. For mostsurgical applications, however, the forces and displacementsare expected to be smaller than the evaluated range.

It can be seen that the desired stiffness is accuratelyachieved in the x and y directions. Stiffness in the z directionis less accurate, especially at direction reversals where im-perfect cancellation of friction in the ball screw transmissionof actuator q3 leads to a large amount of hysteresis.

The most difficult configuration for stiffness control cor-responds to when the robot is straight, i.e., the non-contactcurvature is uw = [0 0 0]T . Tip force versus displacement datafor this configuration are shown in Figure 7 for a desiredstiffness of Kd = diag(0.02 0.08 0.2) N/mm. Recall that thenatural stiffness in the x and y directions for the straight robotshould be both equal to about 0.048 N/mm as depicted inFigure 5. The stiffness controller has succeeded in reducing

869

Fig. 6. Tip force versus displacement in the three coordinate directions fora non-contact robot curvature of uw = [ 1

320mm 0 0]T and desired tip stiffnessof Kd = diag(0.04 0.04 0.04) N/mm.

the natural robot stiffness by about a factor of two in they direction and in increasing the natural stiffness by abouta factor of two in the in the x direction. Not depicted,the stiffness controller as described is unable to control thestiffness of robot along the z axis when the robot has zerocurvature, i.e, is straight. A controller modification describedin [20] avoids this limitation and is applicable to backdrivablecontinuum manipulators.

Fig. 7. Tip force versus displacement in the x and y coordinate directionsfor a non-contact robot curvature of uw = [0 0 0]T and desired tip stiffnessof Kd = diag(0.02 0.08 0.2) N/mm.

VII. CONCLUSIONS

The contribution of this paper is to provide an approach forimplementing stiffness control on any continuum robot thatcan be modeled under loading as an elastic rod and for whichan unloaded kinematic model is available. Thus, the methodis broadly applicable to continuum robots including steerablecatheters, multi-backbone robots as well as concentric tuberobots.

The efficacy of the proposed stiffness controller wasdemonstrated on a 3 DOF concentric tube robot. It was foundthat desired tip stiffnesses could be achieved independent ofrobot configuration in the lateral or bending directions. Usingtip position sensing, stiffness control along the axis of the

robot is possible as long as the curvature of the entire robotis not close to zero. A future goal is to evaluate the stiffnesscontroller under surgical conditions.

REFERENCES

[1] J. Craig, Introduction to Robotics: Mechanics and Control, 2nd ed.Addison-Wesley, 1989.

[2] D. E. Whitney, “Quasi-static assembly of compliantly supported rigidparts,” J. Dyn. Sys., Meas., Control, vol. 104(1), March 1982.

[3] J. K. Salisbury, “Active stiffness control of a manipulator in cartesiancoordinates,” in Proc. IEEE Conf. Decision Control, 1980, pp. 95 –100.

[4] G. Robinson and J. Davies, “Continuum robots - a state of the art,” inIEEE International Conference on Robotics and Automation, vol. 4,1999,, pp. 2849–2854.

[5] D. Camarillo, C. Milne, C. Carlson, and K. J. Salisbury, “Mechanicsmodeling of tendon-driven continuum manipulators,” IEEE Transac-tions Robotics, vol. 24(6), pp. 1262–1273, 2008.

[6] B. Jones and I. Walker, “Kinematics for multi-section continuumrobots,” IEEE Transactions on Robotics, vol. 22, no. 1, pp. 45–53,2006.

[7] N. Simaan, R. Taylor, and P. Flint, “A dexterous system for laryngealsurgery,” in Proc. IEEE Intl. Conf. on Robotics and Automation, NewOrleans, April 2004, pp. 351–357.

[8] R. Webster, J. Romano, and N. Cowan, “Mechanics of precurved-tubecontinuum robots,” IEEE Transactions Robotics, vol. 25(1), pp. 67–78,2009.

[9] P. Dupont, J. Lock, B. Itkowitz, and E. Butler, “Design and controlof concentric tube robots,” IEEE Transactions Robotics, p. in press,2009.

[10] D. Pai, “Strands: Interactive simulation of thin solids using cosseratmodels,” in Proceedings of Eurographics’02, vol. 21, no. 3, 1990, pp.347–352.

[11] S. S. Antman, Nonlinear problems of elasticity. Springer Verlag, NewYork, 1995.

[12] K. Xu and N. Simaan, “An investigation of the intrinsic force sensingcapabilities of continuum robots,” IEEE Transactions on Robotics,vol. 24, no. 3, pp. 576–587, 2008.

[13] ——, “Analytic formulation for kinematics, statics and shape restora-tion of multi-backbone continuum robots via elliptic integrals,” ASMEJournal of Mechanisms and Robotics, vol. 2, no. 1, pp. 1–13, 2010.

[14] M. Murray, Z. Li, and S. S. Sastry, A Mathematical Introduction toRobotic Manipulation, 1st ed. CRC, 1994.

[15] M. Mahvash and A. Okamura, “Friction compensation for enhancingtransparency of a teleoperator with compliant transmission,” IEEETransactions on Robotics and Automation, vol. 23, no. 6, pp. 1240–1246, 2007.

[16] D. Trivedi, A. Lotfi, and C. Rahn, “Geometrically exact models forsoft robotic manipulators,” IEEE Transactions Robotics, vol. 24(4),pp. 773–780, 2008.

[17] R. Bishop, “There is more than one way to frame a curve,” TheAmerican Mathematical Monthly, vol. 82(3), pp. 246–251, 1975.

[18] P. E. Crouch and R. Grossman, “Numerical integration of ordinarydifferential equations on manifolds,” Journal of Nonlinear Science,vol. 3(1), pp. 1–33, 1993.

[19] J. Park, W. Chung, and Y. Youm, “Geometric numerical integrationalgorithms for articulated multi-body systems,” in IEEE/RSJ Int.Conference on Intelligent Robots and Systems, 2004, pp. 2803–3808.

[20] M. Mahvash and P. Dupont, “Stiffness control of surgical continuummanipulators,” IEEE Transactions Robotics, in press.

[21] K. Atkinson, An Introduction to Numerical Analysis, 2nd ed. Wiley,1989.

870