WO2013158978A1 - Method and system for compliant insertion of continuum robots - Google Patents

Abstract

Methods and systems for compliant insertion of a continuum robot into a cavity of unknown dimensions. The continuum robot includes a plurality of independently controlled segments along the length of the continuum robot. A plurality of forces acting on the continuum robot are determined including a force acting on each of the plurality of segments. Each determined force includes a magnitude and a direction. Each determined force is compared to a respective expected force for each of the plurality of segments. The position of each segment of the continuum robot is adjusted based on the difference between the determined force and the expected force for each segment. The segments are continually positioned to minimize the difference between the determined forces and the expected forces as the continuum robot is advanced into the cavity.

Description

METHOD AND SYSTEM FOR COMPLIANT INSERTION OF CONTINUUM ROBOTS

STATEMENT REGARDING FEDERALLY SPONSORED RESEARCH OR DEVELOPMENT

[0001] This invention was made with government support under grant IIS-1063750 awarded by the National Science Foundation. The government has certain rights in the invention.

RELATED APPLICATIONS

[0002] This application claims priority to U.S. Provisional Application No. 61/636,009, filed on April 20, 2012 and titled "METHOD AND SYSTEM FOR COMPLIANT INSERT AION OF CONTINUUM ROBOTS," the entire contents of which are incorporated herein by reference.

FIELD OF THE INVENTION

[0003] The present invention relates to systems and methods of controlling the position and pose of multi-segment continuum robots. More specifically, the present invention relates to systems and methods of compliant insertion of a continuum robot into a cavity of unknown size, dimensions, and structure.

BACKGROUND OF THE FNVENTION

[0004] Multi-segment continuum robots provide for snake-like movement and positioning by deformation of internal structures of the robotic mechanisms as opposed to the relative deformation of individual rigid links. As such, continuum robots can be used to extend deeper into cavities to perform functions including, for example, search and rescue in disaster relief, nuclear handling, and minimally invasive surgical procedures. However, adoption of continuum robots has been limited due to a lack of controls methods to prevent damage to the robotic structure and the environment when the device is inserted into a cavity of unknown dimensions. During insertion into a complex environment, such as a surgical or disaster site, continuum robots must interact safely while being subject to a multitude of unknown contact sites along the robot arm length. [0005] Passive compliance techniques have been proposed to achieve safe interaction without the need for additional sensing and complex control. Passively compliant structures contain flexible members which comply with environment interaction forces without explicit compliant control. Though these systems have shown promise in providing a measure of safety, they suffer from a number of significant disadvantages for exploration and intervention in unstructured environments. By the very nature of their design, passive compliant systems are limited in their accuracy and the magnitude of forces that can be applied during environment interaction. Conversely, the measure of safety provided by passive compliance is limited by the flexibility of the system. Additionally, the compliance adds uncertainty and modeling difficulties that degrade trajectory tracking performance. Thus, a need exists for active compliant instrumentation that can safely interact with the surrounding environment while maintaining manipulation precision and delivering adequate forces for executing manipulation tasks.

SUMMARY OF THE INVENTION

[0006] The systems and methods described below allow a continuum robot system to adapt to the shape of a multi-segmented continuum robot structure to thereby adapt to the previously unknown dimensions of a cavity. The shape and position of the individual segments of the continuum robot are continually adjusted as the continuum robot is advanced further into the cavity.

[0007] In one embodiment, the invention provides a method of compliant insertion of a continuum robot. The continuum robot includes a plurality of independently controlled segments along a length of the continuum robot. The continuum robot is inserted into a cavity of unknown dimensions. A plurality of forces acting on the continuum robot are determined. The plurality of forces includes a force acting on each of the plurality of segments along the length of the continuum robot. Each force of the plurality of forces includes a magnitude and a direction. These forces result in a generalized force that captures the statics of the robot as described in its configuration space (a space describing the shape of each segment by two configuration space angles per a segment). Each determined generalized force is compared to a respective expected generalized force for each of the plurality of segments. The expected force for each segment is determined based on the position of the continuum robot. The position of each segment of the continuum robot is adjusted based on a difference between the determined generalized force and the expected generalized force for the segment. In some embodiments, the method further includes repeating the acts of determining the plurality of forces, comparing each determined force to a respective expected force, and adjusting the position of each segment as the continuum robot is advanced further into the cavity.

[0008] In another embodiment, the invention provides a controller for compliant insertion of a continuum robot into a cavity of unknown dimensions. The continuum robot includes a plurality of independently controlled segments along the length of the continuum robot. The controller includes a processor and memory storing instructions that are executable by the processor. The controller is configured to determine a plurality of forces acting on the continuum robot. The plurality of forces includes a generalized force acting on each of the plurality of segments along the length of the continuum robot. Each generalized force includes a magnitude and a direction. Each determined generalized force is then compared to a respective expected generalized force for each of the plurality of segments. The expected generalized force is determined based on the position of the continuum robot. The controller adjusts the position of each segment based on the difference between the determined generalized force and the expected generalized force for each of the plurality of segments.

[0009] In another embodiment, the invention provides a robotic system including a continuum robot, a plurality of actuators, and a controller. The continuum robot includes a plurality of independently controlled segments along the length of the continuum robot and a plurality of back-bone structures extending through the continuum robot. Each actuator is connected to a different one of the plurality of back-bone structures and advances or retracts the back-bone structure to control the shape and end effector position of the continuum robot. The controller includes a processor and a memory. The controller determines a plurality of forces acting on the continuum robot based on forces exerted on the plurality of back-bone structures by the plurality of actuators. The determined forces include a generalized force acting on each of the plurality of segments along the length of the continuum robot. Each generalized force includes a magnitude and a direction. The controller compares each determined generalized force to a respective expected generalized force for each of the plurality of segments. The expected generalized force is determined based on the position of the continuum robot. The position of each segment is adjusted based on the difference between the determined generalized force and the expected generalized force for each segment. The acts of determining the plurality of forces, comparing the generalized forces to a respective expected generalized force, and adjusting the position of each segment are repeated as the continuum robot is advanced into the cavity.

[0010] In some embodiments, compliant motion control of the continuum robot is provided by mapping generalized forces into a configuration space of a robot. Support vector regression techniques are used to provide sparse interpolation to estimate and cancel out effects of uncertainty in the generalized forces due to friction and uncertainty in material properties.

[0011] In some embodiments, the compliant motion control methods are used to operate rapidly deployable handheld robotic devices that are inserted into the human anatomy for surgical operations such as trans-urethral bladder resection, trans-nasal surgery, and frontal sinus exploration/surgery. In some embodiments, the methods are also used to provide impedance control of continuum robots and to enable a method for bracing the continuum robot against the anatomical features inside the cavity once contact has been localized and detected. Bracing techniques provide increased stability and increased accuracy for operations performed by a tool positioned at the distal end of the continuum robot. Collaborative telemanipulation algorithms can be implemented to work with the compliant motion control methods to reduce the telemanipulation burden of high degree-of-freedom (DoF) surgical slave devices.

[0012] Continuum robots with compliant insertion functionality can be used, among other things, to perform a minimally invasive surgery (MIS) of the throat which typically requires full anesthesia and use of a laryngoscope. Additional examples of potential applications include frontal sinus exploration surgery and office-based sinus surgery treatment and monitoring of disease progression. This technology is can also be used, for example, for micro-surgical function restoration of paralyzed vocal cords, which requires control of the location and amount of material to inject inside the paralyzed vocal cord. Currently, surgeons guess the amount of material and adjust accordingly during a follow-up surgery (assuming they do not overfill the vocal cord). By avoiding the use of full anesthesia, cost of operation is reduced and surgeons can get feedback about surgical outcomes during the procedure. Relevance is also found in injection site targeting that currently pairs a trans-dermal injection with a microscope for visualizing the anatomy.

[0013] Other aspects of the invention will become apparent by consideration of the detailed description and accompanying drawings.

BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Fig. 1 is a perspective view of a multiple-segment continuum robot.

[0015] Fig. 2 is a perspective view of a single segment of the continuum robot of Fig. 1.

[0016] Fig. 3 is a block diagram of a controller for positioning the continuum robot of Fig. 1.

[0017] Fig. 4 is a perspective view of the continuum robot of Fig. 1 being inserted into the nasal passage of a human head.

[0018] Fig. 5 is a flowchart illustrating a method of adjusting the position of the continuum robot of Fig. 1 as the continuum robot is inserted into a cavity of unknown size and dimensions.

[0019] Fig. 6 is a flowchart illustrating a method of adjusting the position of a single segment of the multiple-segment continuum robot of Fig. 1 as the continuum robot is inserted into a cavity of unknown size and dimension.

[0020] Fig. 7 is a stiffness model for segment of the continuum robot of Fig. 1 with multiple back-bone structures.

[0021] Figs. 8 is a graph of sample training and estimation data for the uncertainty parameters in a multi-segment robot.

[0022] Fig. 9 is a graph of training and estimation data similar to the data provided in figure 8.

[0023] Fig. 10 is a functional block diagram of the operation of a controller providing for compliant insertion of the continuum robot of Fig. 1. [0024] Fig. 11 is a perspective view of an experimental system for analyzing the sensitivity of a continuum robot utilizing compliant motion control.

[0025] Fig. 12 is a graph illustrating poses of the continuum robot during sensitivity analysis of the compliant motion controller using the experimental system of Fig. 11.

[0026] Fig. 13 is a table providing experimental data recording during sensitivity analysis of the compliant motion controller using the experimental system of Fig. 11.

[0027] Fig. 14 is a graph illustrating measured and theoretical minimum forces required for compliant motion during sensitivity analysis of the compliant motion controller using the experimental system of Fig. 11.

[0028] Fig. 15 is a table providing mean pose error and standard deviation of the nominal kinematics from magnetic sensor measurements during sensitivity analysis of the compliant motion controller using the experimental system of Fig. 11.

[0029] Fig. 16 is a series of images showing compliant insertion of a continuum robot into a cavity of unknown size and shape.

[0030] Fig. 17 is a series of graphs illustrating reaction data for the compliant insertion of Fig. 16.

DETAILED DESCRIPTION

[0031] Before any embodiments of the invention are explained in detail, it is to be understood that the invention is not limited in its application to the details of construction and the arrangement of components set forth in the following description or illustrated in the following drawings. The invention is capable of other embodiments and of being practiced or of being carried out in various ways.

[0032] Fig. 1 illustrates a multiple-segment continuum robot 100 with multiple back-bone structures to control the movement and position of the continuum robot 100. The multiple- segment continuum robot 100 of Fig. 1 includes three independent segments 101, 102, and 103. However, other continuum robots may include more or fewer segments. In operation, a tool or other device would be extended through the continuum robot and would emerge at the distal end of the continuum robot (e.g., the open end of segment 103). The other labels and vectors illustrated in Fig. 1 are referred to below in describing the kinematics of the continuum robot.

[0033] Fig. 2 provides a more detailed view of a single segment 101 of the continuum robot 100. The continuum robot 100 includes a single central back-bone 121 and multiple secondary back-bones 123. Each segment 101 includes a base disc 125 at the proximate end of the segment and an end disc 127 at the distal end of the segment. A plurality of spacer discs 129 are positioned between the base disc 125 and the end disc 127. The secondary back-bones 123 are super elastic NiTi tubes circumferentially distributed around the central back-bone 121. The end disc 127 is attached to all of the back-bones 121, 123. The base disc 125 is connected only to the central backbone 121. The spacer discs 129 area separated by elastomeric spacers (not pictured) and float along the central backbone 121 while maintaining a fixed radial distance between the central backbone 121 and all secondary backbones 123. Using this arrangement, two degrees of freedom (DoF) per segment are controlled by pushing and pulling on the secondary backbones 123 to bend the segment in a circular arc.

[0034] The inset 131 of Fig. 2 illustrates a top view of the discs 125, 127, and 129. A single hole 133 is positioned at the center of each disc. The central backbone 121 extends through the hole 133. Each disc in this example also includes a plurality of holes 135 positioned around the circumference of the disc. The secondary backbones 123 extend through these holes 135. Each disc also includes one or more larger holes 137. Instruments such as, for example, grabbers, cameras, light sources, laser ablation tools, imaging probes (e.g. ultrasound or optical coherence tomography) can be extended through the continuum robot via these larger holes 137 and will emerge from the distal end of the continuum robot 100. As in Fig. 1, the additional labels and vectors illustrated in Fig. 2 are referred to below in describing the kinematics of the continuum robot.

[0035] Fig. 3 illustrates a control system for the continuum robot 101 of Fig. 1. The control system includes a controller 301 with a processor 303 and a memory 305. The memory 305 stores instructions that can be executed by the processor 303 to cause the controller to control the operation of the continuum robot. Multiple actuators 307, 309, 311 are connected to and operated by the controller 301. As described above, the actuators 307, 309, 311 each push and pull on one of the secondary backbones 123 of the continuum robot 100 to adjust the position and pose of the continuum robot 100.

[0036] A continuum robot 100 can be inserted into various cavities to reach a target location. Once the distal end of the continuum robot 100 is positioned at the target location, tools emerging from the distal end of the continuum robot 100 are used to perform operations. Fig. 4 illustrates an example of a continuum robot 100 inserted into the nasal passage of a human head 400. In this example, the target location for the distal end of the continuum robot is the upper sinuses. However, the exact dimensions and size of the nasal passages extending from the nose to the upper sinus may not be known. The compliant insertion methods described herein enable the continuum robot 100 to adapt its shape and position to comply with the shape of the nasal passages as the continuum robot 100 is advanced into the nasal passage. Although the examples described herein refer to a continuum robot being inserted into the nasal passage to reach an upper sinus, the methods and systems described herein also provide compliant motion control for other continuum robot applications such as minimally invasive surgery at other locations of the human body or larger applications such as search and rescue at disaster sites.

[0037] Fig. 5 describes a method of adjusting the position and shape of the continuum robot to adapt to the shape of the cavity in which it has been inserted. The continuum robot 100 is pushed further into the cavity to advance the position of the distal end (step 501). The forces exerted along the length of the continuum robot 100 are measured (step 503). In particular, each determined force indicates the generalized forces applied to the end disc 127 of each segment of the continuum robot. These measured forces include both the forces required to position the continuum robot in its current shape and external forces, or wrenches, applied to the continuum robot 100 by the surfaces of the cavity. The measured generalized forces along the length of the continuum robot are compared to expected generalized forces that would be detected on the continuum robot if it were positioned in its current shape without the presence of external wrenches (step 505). The positions of the continuum robot segments are then adjusted to minimize the difference between the measured generalized forces and the expected generalized forces (step 507). In other words, the shape of the continuum robot is adjusted to minimize the affect of external wrenches on the continuum robot structure along the length of the continuum robot 100.

[0038] Fig. 6 illustrates in further detail how the controller 301 adjusts a single segment of the continuum robot 100. The method illustrated in Fig. 6 is performed concurrently and repeatedly for each segment of the continuum robot 100 as it is inserted into the cavity. As the continuum robot 100 is advanced into the cavity (step 601), the controller measures the forces on each of the controlled secondary backbone structures 123 (step 603). The forces are measured at the actuator units and, again represent the forces required to position the continuum robot in its current position and external wrench applied to the continuum robot structure by the surfaces of the cavity. The controller further determines a generalized force applied to the end disc of each individual segment of the continuum robot (step 605). The determined generalized force is compared with an expected generalized force (step 607) to determine the magnitude and direction of the external wrench applied to the continuum robot at the end disc of each segment. If the difference between the measured generalized force and the expected generalized force (e.g., the magnitude of the wrench) is greater than a threshold, the controller determines the direction of the wrench (step 611) and adjusts the position of the segment accordingly (step 613). If the magnitude of the wrench does not exceed the threshold, the controller continues to monitor forces (step 603) without changing the position of the continuum robot. In some embodiments, the threshold applied in step 609 is adjusted in real-time during insertion of the continuum robot into the cavity to account for elastic cavity surfaces such as internal body tissues.

[0039] Once the continuum robot has been sufficiently inserted into the cavity and the distal end of the continuum robot has reached its target location, the same compliant insertion techniques can be used to brace the continuum robot against the surfaces of the cavity. Bracing the continuum robot provides additional stability and allows for more accurate placement and maneuvering of the tools emerging from the distal end of the continuum robot. To brace the continuum robot, the controller 301 adjusts the position of each segment of the continuum robot to increase the difference between the measured generalized force and the expected generalized force on each end disc 127. To prevent damage to the continuum robot structure, the controller 301 ensures that the difference between the expected generalized force and the measured generalized force (i.e., the magnitude of the wrench) does not exceed a defined threshold. [0040] The following nomenclature is used to describe the kinematics of the continuum robot, the mathematical modeling, and the specific techniques used to map external wrenches to a generalize force to provide for compliant insertion:

A; 1^th row of A

A^[i] 1^th column of A

[A]i_j Entry at the ith row and jth column of the matrix A n Number of segments of the continuum robot m Number of secondary backbones in each continuum segment c_a, s_a Abbreviated form of the cosine and sine respectively where c_a = cos a and s_a = sin a

9_(k) Bending angle for an individual segment

5_(k) Bending plane angle for an individual segment

\|/_(k) Configuration space vector for an individual segment, ψ^₎ = [0₍¾, 5_(k)]^T ψ_α Augmented configuration space vector for the multi-segment continuum robot, ψ_α = [ψ^τ _(ΐ), . . . , ψ^τ _(η)]^{τ A}u, ^Λν, ^Aw Unit basis vectors of an arbitrary coordinate frame such that

[^Au, ^Av, ^Aw] = I e Wl^3X3

(b_(k)} Base frame of the kth segment

(g_(k)} End frame of the kth segment cP_ab Position vector pointing from point a (or the origin of frame {a}) to point b (or the origin of frame {b}) expressed in frame {c} Rotation matrix describing the orientation of frame {b} with respect to frame {a}

Vb/a Linear velocity of frame {b} with respect to frame {a}, expressed in frame

{c}

c

rob/a Angular velocity of frame {b} with respect to frame {a}, expressed in frame {c}

Jq (k) A Jacobian matrix linearly mapping the configuration space velocities to joint velocities such q(k) = J_qv(k) ψ( ) A Jacobian matrix linearly mapping the configuration space velocities to linear velocities such that v = J_VX|/(k) \| (k)

Jco_V(k) A matrix linearly mapping of the configuration space velocities to angular velocities such that ω = J«_v(k) ψ(¾ A linear mapping of the configuration space velocities to the twist such that t = J_tv(k) ψ^) and J_i(

J

E_Y Young's elasticity modulus for the NiTi back-bones

I(k) Cross-sectional second moment of inertia of backbones of the kth segment r Radius of the pitch circle along which the secondary backbones are

circumferentially distributed around the primary backbone

T(k) Backbone actuation forces of an individual segment T^ = [ Ti^), 7^"m,(k)]^T f(k) Generalized force for the kth individual segment f_a Augmented generalized force for the multi-segment continuum robot f_a =

λ, λ Actual and estimated augmented generalized force errors

Κ_{ψί (k)} Configuration space stiffness for the kth individual segment

Κ_ψα Configuration space stiffness for the multi-segment continuum robot x Input feature vector to the Support Vector Regression (SVR)

N The number of training data for the SVR optimization φ(·) High dimensional mapping of the input feature space of the support

vectors in the SVR

[0041] The pose of the k segment continuum robot (as illustrated in Fig. 2) is described in a set of generalized coordinates by a configuration space vector defined as

^ = [ ), S(k)]^T (i)

[0042] where (-)(k) denotes a variable associated with the k* segment. 9(k) defines the bending angle of the segment measured from the plane defined by the base disk, (^AUb(k), ^AVb(k)} , and the direction normal to the end disk ^Aw_g(k). 5(k) defines the orientation of the bending plane of the segment as measured from the plane to the ^AUb(k) about ^AWb(k

[0043] The inverse kinematics of the segment relating the configuration space, \| ₎, to the joint space, q_(k) = [ qi,_(k), q₂,(k), · · · , q_m,(k)]^T, is given by

Lj,(k) = L(k) + ¾,(k) = L(k) + ,(k)B(k> (2)

where L_j^k) is the length of the secondary backbone of the k^th segment for j = 1, . . . , m, L(k₎ is the length of the primary backbone of the k^th segment, A_j^ = r cos σ^; Cj,(k) = 5(k) + (2π/ιη),

[0044] The instantaneous inverse kinematics can be described by differentiating (2) to yield <l (k)- Jqi|/(k) Ψ (k) (3)

where the Jacobian J _k) is given by

[0045] The direct kinematics of the k segment is given by the position b(k)Pb(k)g(k) and π orientation b(k)Rg(k) of the segment end disk with respect to its base disk. For 9_(k)≠— , the kinematics takes the form

[wx] n [vx] _

(k) (k

' (ky " (6)

where V =[0, 1, 0]^T, W=[0, 0, 1]^T and the frames (g(k)} and |b(k) } are shown in Fig. 3. For Θ, 00

— , the formulation singularity, resolves to

(?)

, 3X3

^KS(k) - ¹ (8)

[0046] By differentiating (5) and (6), the instantaneous direct kinematics takes the form

^{{k) t}g(k) ^{l b}(k) - ^Jt _{k) (k) (9) π

where, for 9(k) = — , the Jacobian J_tW(, , is given by5

' ^l {k)

π 1

and for 9_(k) = — , the formulation singularity, is resolved by applying l'Hopital's rule, to

2 %)

yield

Using Euler beam bending analysis and neglecting gravitational potential energy, the total

n

energy of the continuum robot is given by U= ∑ U^) where the potential elastic energy of the

k-l

k^th segment is given by

[0047] The principle of virtual work and the energy expression (12) are used to obtain a statics model for the k^th continuum segment as

T

³ q¥ {k) ^T k) = V¾- / ^x¥(k) e,(k) (13) where represents the actuation forces on the secondary backbones, Vt/_(i) is the gradient of the potential energy, and w_e, _(k) is the external wrench applied to the end disk of the k^th continuum segment. The energy gradient Vt/_(i) is calculated with respect to a virtual displacement in configuration space

[0048] The statics expression (13) projects both the actuation forces and perturbation wrench w_e (k) w into the configuration space of the continuum segment, Thus, after projecting the wrench from three-dimensional Cartesian space, II⁶, into the configuration space, R², the projected generalized force on the segment resides in the vector space controllable within the framework of the single-segment kinematics of equation (1). This projection eliminates the requirements for explicit determination of wrenches required by conventional compliance algorithms and casts the interaction force minimization problem into the configuration space of the continuum segment.

[0049] If the wrench acting on the end disk that is projected into the configuration space of the k segment is defined as the generalized force vector

Applying equation (13) to the generalized force expression, the i row of the generalized force f_(k) can be written as

XT

J T= VU_; J (16) where J ,' denotes the i^th column of J₍ [0050] For small perturbations from an equilibrium configuration, the stiffness of the individual continuum segment can be posed in the configuration space as

δ ΐ=Κ ψ (17) where the stiffness is given by the Jacobian of the generalized force with respect to configuration space perturbation. Thus, the elements of k are given by:

The elements of the stiffness matrix (18) can be expanded as

The first term of the configuration space stiffness, is the Hessian of the elastic energy of the segment given by

d²U d²U

H δθ² δθΰβ

ψ (20)

d²U d²U

δδθ δδ²

The individual elements of the Hessian, Η_ψ , are given by

The second term of equation (19) expands to

Finally, the third term of equation (19) can be expanded by applying a simplifying assumption based on the relative geometry of the backbones in remote actuated continuum system configurations. The stiffness of the continuum robot, as measured from the actuation forces at the proximal end of the actuation lines, is a function of strain throughout the length of the actuation lines. As illustrated in Fig. 7, the contributions to the axial stiffness are divided into the stiffnesses, fa and fa, corresponding to the bending, L_b, and non-bending, L_c, regions of the actuation lines connecting the working distal end to the actuation unit.

[0051] The axial stiffness along the length of a given actuation line can then be expressed as

1 1 1

- =— +— (27)

k k_c k_b where k_c = ^^- and ¾ ₌_5__ and _^4 denotes the cross-sectional area of the backbone.

[0052] For continuum robotic systems with remote actuation that are designed to access deep confined spaces, the lengths of the non-bending regions of the actuation lines far exceed that of the bending regions, L_c » L_b. The stiffness will therefore be dominated by the non-bending regions of the actuation lines. Local perturbations of the backbones at the actuation unit can be expressed as

T_q ^{=K« (28)}

Where

Expanding terms by applying the chain rule and using the instantaneous inverse kinematics of equation (3),— is given by

δψ

The configuration space stiffness therefore reduces to

[0053] Using the generalized force and stiffness as defined in equations (15) and (31), a compliant motion controller can be constructed to minimize the difference between the expected and measured actuation forces in the secondary backbones. The controller provides compliant motion control of each continuum segment subject to a perturbing wrench at the end disk of the segment. This wrench approximates a multitude of small perturbation forces acting along the length of a continuum segment during an insertion through a tortuous path (e.g., a cavity of unknown size, shape, and dimensions). In an analogy to a linear spring model with an equilibrium position, = k(x_eq - x), there exists an equilibrium pose, | ^b^ P_eq ^b^ R_eq j, at which the wrench is minimized in the operation space of the continuum segment. Given this equilibrium position and the projection of the applied wrench to the configuration space, there exists a unique, though unknown, configuration w(k),d = g{^^ ^peq _> ^Req ) ^mat minimizes the wrench applied to the continuum segment. The objective of the compliance controller is to advance to this position using the available sensing information of the actuation forces in the backbones, T_(k) and therefore minimize the interaction wrench. [0054] For multi-segment continuum robots, a pose exists in the overall configuration space that minimizes the augmented generalized force on the system. The augmented generalized force vector for an n-segment continuum robot

is associated with the

[0055] The configuration space stiffness of an individual segment is generalized as the following to result in the augmented configuration space stiffness of the multi-segment contin m robot

[0056] An error function is defined as the distance in configuration space from the current configuration, ψ_& to the desired configuration ψ^, as

(35)

^'Ψα = Ψ ^ad

For a passive wrench condition, the equilibrium pose and corresponding configuration space remain fixed, e.g., ψ_&ά = 0. For external wrenches changing slowly with respect to the compliant control update rate, this condition becomes ψ ^' _a^ ~ 0 and hence negligible when taking the time derivative of β_ψη . The following discussion uses ψ _ad = 0 although it is to be understood that the same derivation follows for ψ _{a d} ~ 0 by changing the equality sign to an approximation sign. The time derivative of the error function under these conditions becomes

(36) [0057] For small perturbations, the distance in configuration space is given as

Δ ψ _a ⁼ Ψ _{a j} ^~ Ψ _a · Thus by applying the configuration space stiffness Κ_ψ , the error in the system can be related to the generalized force as

f_a = K_¥a e_¥a . (37)

Where Af_a = f_a for the small perturbations. This assumption holds for robots that are calibrated such that f_a ~ for unloaded movements throughout the workspace.

[0058] During control of a continuum system the estimated generalized force is calculated using the idealized statics model of f_a based on the estimate of the energy and measured actuation forced as in equation (16). This estimate is contaminated by an error λ due to un- modeled friction and strain along the actuation lines, perturbations from circulate-b ending shape of individual segments, deviations in the cross-section of the backbones during bending, and uncertainties in the elastic properties of the NiTi backbones. Thus without compensation, the generalized force estimate takes the form

^a, uncompensated ^~ ^a ^Jr ^ (38)

where f_a is given by equation (16).

[0059] The generalized force error λ is identified using the definition in equation (38) for movements of the continuum robot through its workspace with no externally applied wrenches. During this unloaded movement, the wrench at the end disk is w_e = 0 and equation (15) predicts f_a = 0. In an ideal continuum robot lacking backlash, friction and other un-modeled effects, the prediction of the generalized force based on the measured values of the actuation forces T(k) in equation (16) for each segment would also return f_a = 0. However, the actual kinetostatic effects cause f_a = 0 and this deviation forms λ that we seek to estimate. In order to compensate for the errors due to the interaction between segments, non-linear regression via Support Vector (SV) machines is investigated and applied.

[0060] These model errors are highly dependent on the pose of the robot and the path in the configuration space to this pose. To compensate for the error, a feed forward term is added to equation (38) to reduce the effects of λ during compliant motion control such that the compensated generalized force estimate is given by

f _a— f _a - — . (39)

An augmented compliant motion controller, taking into account the estimates of the un-modeled error, , takes the form

ψ_Ά = μΜ_Ά (40)

where μ and A correspond respectively to positive definite scalar and matrix gains to be chosen by the operator.

[0061] A compliant motion controller utilizing the error compensation techniques described above, results in a system error that is uniformly bounded for any generalized force error by

¥a

[0062] The dynamics of the error in the configuration space applying equation (40) to equation (36) and accounting for equation (39) is represented as

[0063] A Lyapunov function candidate of the error system for compliant control, represented by equation 42), can be defined as

where P is any symmetric positive definite matrix. Differentiating equation (43) with respect to time and accounting for equation (42) yields

v(« _a ) = e _a Pe_va = - ne _a PA ^ e„_a + λ - λ) (44)

Choosing the control and Lyapunov weighting matrices as A and P = I, the derivative of

the Lyapunov function reduces to

A controller implementing these techniques relies on the invertibility of the configuration space stiffness. The block diagonal matrix is constructed of the sub-matrices, as in equation

(34). Singular configurations of and, therefore, correspond with a straight segment, θ _π . For control purposes, the effect of singular poses can be mitigated using a singularity robust inverse of the configuration space stiffness.

[0064] Noting the expression is positive definite for all and />0 it is sufficient to show to prove the time derivative of the

Lyapunov function candidate is negative definite.

[0065] Assuming the bound on the generalized force error, represented by equation (41), and applying the identity

Applying the Schwarz inequality K^"1 (λ -λ ) e^T K ^{" 1} {λ -λ ϊ to equation (46), the

¥a ¥a ^K '

condition on the generalized force error assumed for stability can be expanded as

.T I -l n \ ₍₄₇₎

¥ ' a K , ¥,, \λ

a

thus proving the stability of the controller given the bounds on the compensation error.

[0066] The techniques described above illustrate controller stability for estimation uncertainties, λ— λ that are small with respect to the system error, . In order to maintain a stable pose in configurations where λ - λ , i.e. at configurations close to the

minimum of the generalized force, the input to the controller is filtered such that estimation error does not drive accidental motion. In the example controller described in further detail below, a dead-band filter is used to ensure that the controller acts for deviation above a reasonable bound for the estimation error. The performance compromise of applying this filter is a reduction in the sensitivity of the controller.

[0067] Bounds for the sensitivity of the compliant motion controller, defined by equation (40), are limited by the error in canceling the generalized force uncertainties,, λ— λ For small load perturbations to the continuum robot, the errors in the generalized force, λ, are well approximated by a function of the robot pose in configuration space lf/_a and the trajectory leading to this pose,. /_a Furthermore, the trajectory required to achieve a pose influences the friction and backlash in the system at the current pose. The performance of the controller depends on canceling deviations from the idealized model. As specified in equation (39), this cancellation is carried out by a feed- forward term, λ that is obtained though off-line training and online estimation of the model error via support vector regression (SVR).

[0068] SV machines provide a method for nonlinear regression through mapping of the input vectors into a higher dimensional feature space. Parameters for the estimation are learned by application of empirical risk minimization in this feature space through convex optimization. A subset of the training data forms the support vectors which define the parameters for regression. The robustness and favorable generalization properties with noisy data, coupled with a compact structure which allows real-time function estimation during motion control motivate the choice of a SV machine.

[0069] To estimate the error in the generalized force based on an initial unloaded exploration of the workspace, the "v" SV Regression (v-SVR) is employed along each dimension of the generalized force. The v-SVR algorithm provides a means for controlling the sparsity of support vectors, therefore providing robustness to over-fitting while simultaneously reducing the number of control parameters required to be defined.

[0070] The training set for the v-SVR contains input pairs |x[j] _y[j] based on the pose and generalized force gathered during an exploration of the continuum robot workspace in the absence of external perturbing forces. For the 1^th component of the generalized force, the input is given as

nitude of the 1^th row of ψ_Ά over the set 1 e [l, N], and ψ_Ά is the configuration space velocity. This expression normalizes each element of the input vector X_{[ j} to the range of [-1 , 1] for all data in the training set.

[0071] Given these training input vector pairs, v-SVR provides a method for estimating the function

where (^',^') is the inner product space, w and b are parameters to be determined by the convex

_R4th _{→ R} n

optimization and is a mapping of the input feature vector X_{[ j} to a higher dimensional feature space.

[0072] As specified in equation (49), the SVR algorithm allows mapping of the input space to a higher dimensional vector space. Uncertainties in predicting the generalized force, including friction and non-linear bending, are modeled by exponential functions. As such, a Gaussian radial basis function (RBF) kernel

is chosen as a suitable candidate for higher dimensional mapping.

[0073] The resulting v-SVR model for function estimation with a new input vector, x*, is given by

It should be noted that only those training points X[₍ ] _y[j] , denoted SV in equation (51), for which

|y [l]— f (x[l])|≥ e form the support vectors. During the computation of new estimates, only these support vectors contribute to the final function estimation of equation (51), thus enforcing sparsity in the regression.

[0074] Software, such as LibSVM, provided by C. C. Chang and C. J. Lin at

http://www.csie.ntu.edu.tw/cjlin/libswm, is employed for solving for the optimal parameters of equation (51) for each coordinate of the generalized force independently. The measured error in the generalized force λ and the output of the regression for each component of the error compensation, equation (51), is presented for a sample data set in Fig. 8. An inset detailing λι at higher resolution is provided in Fig. 9. Data for this sample set, including the robot pose, trajectory and actuation forces, was collected at 125 Hz during an unloaded motion in the configuration space of a two-segment continuum robot. The trajectories in each configuration space variable, #(₂) ^(₂)» ^are uncorrected to provide a robust data set for estimation.

Frames displayed in the Figs. 8 and 9 are given for every tenth collected sample. The v-SVR was trained with the first half of the data set, labeled as "Training" in the figure. The regression function was then applied without further training on the remainder of the samples, labeled "Estimate". The compensation error given by the difference between the predicted λ and actual λ is presented for each direction of the generalized force.

[0075] The v-SVR displays good generalization to the untrained data. The compensation provides a minimum of 1.4 times reduction in the RMS error for λ₄ and a maximum of 2.5 times reduction for λι. While the v-SVR reduces errors on average, the compensation is locally imperfect as can be seen by maxima in the error in the λ directions exceeding the measured errors, Fig. 8. The magnitude of the predicted v-SVR values exceed the measured values, e.g. Fig. 9 for λι, producing local increases in the compensated predicted force given by ^{f >} in equation (39). Filtering is applied to smooth outliers in the prediction values. [0076] Fig. 10 illustrates an example of a real-time controller for the compliant motion algorithm. This multi-rate controller can be implemented utilizing the Matlab xPC computing environment. The main control loop updating the control values at the joint level runs at 1 kHz. Joint forces are measured at 5 kHz and smoothed with a moving average filter and down- sampled to the 1 kHz control loop. The support vector regression (SVR) is run at 100 Hz. A moving average filter smoothes outliers in the estimate provided by the support vector regression functions of equation (51).

[0077] Joint force sensors 701 provide actuation force measurements for individual segments of the continuum robot, 7(_m). The actuation forces are utilized to determine a Jacobian matrix linearly mapping the configuration space velocities to joint velocities (step 703). The Jacobian matrix 703 is multiplied by the energy gradient 705 at step 707. The estimated augmented generalized force errors 709 are filtered by a low pass filter 711. At step 713, the filtered force error is multiplied by the combination of the Jacobian matrix 703 and the energy gradient 705 to provide an estimated augmented force, F_a. In order to reduce aberrant motions of the controller due to modeling and estimation errors, a dead-band filter 715 is applied to the estimated generalized force to reduce joint motion due to uncompensated errors.

[0078] The inverse of the configuration space stiffness 717 is applied to determine difference, Αψ_Ά, in the configuration space between the current configuration of the continuum robot and the desired configuration. The controller then determines the configuration space vector for an equilibrium pose of the continuum robot (step 719). The Jacobian matrix mapping configuration space velocities to joint velocities is applied to the determined equilibrium pose of the continuum robot (step 721) to determine joint positions, q, for the equilibrium pose. An integral 723 is applied and the output provided to a Joint PID controller 725 that adjusts the position of the continuum robot segments to conform to the detected forces.

[0079] In the examples described above, the compliant motion controller uses a feed forward term for compensation of model uncertainties. The sum of the uncertainty estimate and the expected generalized force are filtered through a dead-band to prevent motion by the controller due to errors in the compensation. Thus, generalized forces less than the threshold of the deadband filter will be neglected and the threshold for this filter therefore forms a tradeoff between the sensitivity to external perturbations and insensitivity to errors in the model and compensation.

[0080] In order to quantify the effects of the controller and thresholding, the sensitivity of the controller to external wrenches was quantified on a single-segment continuum robot. Fig. 11 illustrates an experimental system that is used to evaluated the sensitivity of the continuum robot. A wrench was applied at the end disk of the segment 1101 by a Kevlar thread 1105 , attached through a pulley system 1106 to calibrated weights 1107 . The load direction at each pose was quantified relative to the base of the robot by an optical tracking system 1109 with a specified accuracy of 0.20mm RMS. Optical markers 1104 mounted to the Kevlar thread 1105 and to the base of the continuum robot 1103 serve to specify the direction of the applied force. The weight of the thread and markers is less than 1 gram and is therefore negligible with respect to the loads required to initiate the compliant motion.

[0081] The optical tracking system 1109 provides a specification for the linear accuracy of the device while the experiment requires an orientation prediction. The linear accuracy can be used to estimate the orientation accuracy of the optical tracking system 1 109 by noting the orientation of a vector that is calculated based on the position of two marker points. The smallest linear distance between marker points of 48.5 mm used for the orientation estimation occurs at the base marker system 1103. Although the direction marker perpendicular to the thread is significantly shorter, this direction is not used for computing the direction vector of the Kevlar thread 1105. An RMS error of 0.2 mm at a moment of 24.25 mm corresponds to an orientation error of less than 0.5°.

[0082] For the sensitivity measurement at each pose, the segment was guided into position by manually applying an external wrench and allowing the continuum structure to comply to the sampled configuration. The pose estimate was measured via the nominal kinematics and was verified by an embedded magnetic tracker system 1 108 with an RMS orientation accuracy of 0.5°. The load direction was measured via the optical trackers after a 10 gram weight was applied to the Kevlar thread to straighten the thread length between the optical markers.

Calibration weights were added to the load in 1 gram increments until the threshold for motion was exceeded. The dead-band of the controller was set during the sensitivity experiments as < 20 Νπηη . || _ i() ' η. η .

(52)

These thresholds were determined empirically based on the error compensation in the generalized force and the stability of the controller.

[0083] Sensitivity measurements were recorded for 28 poses shown for the odd samples in Fig. 12. The experimental data is presented in Figs. 13 and 14. Poses were selected throughout the workspace to ensure a distribution with load directions varying relative to the pose angle δ. The mean pose error and standard deviation of the nominal kinematics from the measured pose of the magnetic sensors is given in Fig. 15. Note at poses in which Θ = -90°, the accuracy of δ as reported by the magnetic tracking system 1108 reduces significantly due to a configuration singularity in the magnetic tracking system 1108. Poses 7 and 20, corresponding to

configurations Θ = 90° and Θ = 84°, respectively, were therefore excluded from the error calculation for δι. The accuracy verifies the nominal kinematics of the single segment and these values were used for the subsequent analysis. The mean perturbation force required to induce motion under the experimental conditions was 0.99 N with a standard deviation of 0.28 N. The maximal force required to induce motion was 1.5 N.

[0084] The sensitivity measurements provide an opportunity to evaluate the contribution of the model uncertainties and the compensation by v-SVR. Under ideal conditions in which the generalized force is undisturbed by uncertainty, equation (15), the applied force required to induced motion can be estimated as the minimum force required to exceed the motion threshold defined by equation (52). The minimum force required to exceed the threshold in the direction of the applied force measured for each pose based on the idealized model is provided in addition to the measured force in Fig. 14. The average difference between the measured and ideal force was 0.37 N with a standard deviation of 0.14 N. Thus, the data show that model uncertainty contributes significantly to the sensitivity of the controller.

[0085] Noting the experimental setup is sized appropriately for use in exploration and intervention during minimally invasive sinus surgery, the compliant controller for this robot demonstrated adequate sensitivity for this application. Average interaction forces for functional endoscopic sinus surgery have been measured at 2.21 N and forces required to breach the sinus walls ranged between 6.06 N and 17.08 N. The compliant motion controller demonstrated in these experiments obtains a comfortable margin of safety for exploration and interaction.

[0086] To evaluate the controller in the setting of an application, a two segment continuum robot was inserted into an acrylic tube with a three-dimensional shape comprised of multiple out of plane bends. The tube was mounted to an insertion stage that autonomously brought the tube into contact with the robot in a manner analogous to blind insertion into a cavity and subsequent retraction. The controller had no prior knowledge of the geometry of the tube or the path plan of the insertion stage. As illustrated in Fig. 16, the controller successfully complied with the confined complex shape despite a moving contact location unknown to the controller.

[0087] The generalized force estimates during insertion and retraction are presented in Fig. 17. The data display the effect of the threshold on the motion of the system when subject to external perturbations. Generalized force magnitudes below the threshold for motion, denoted by the dashed lines in Fig. 17, are filtered and do not cause motion of the continuum robot. As the generalized force exceeds the threshold, the controller rapidly moves to reduce the force to a level below the threshold. It should be noted that the controller is agnostic to the location of contact and does not therefore require location information for minimization of the environment contact. The results demonstrate the utility of the algorithm for compliant motion control at unknown locations along the length of a continuum robot.

[0088] Thus, the invention provides, among other things, systems and methods for controlling the pose of a multi-segmented continuum robot structure to adapt to the unknown dimensions of a cavity. The shape and position of the individual segments of the continuum robot are continually adjusted as the continuum robot is advanced further into the cavity, thereby providing for compliant insertion of a continuum robot into an unknown cavity. Various features and advantages of the invention are set forth in the following claims.

Claims

CLAIMS What is claimed is:

1. A method for compliant insertion of a continuum robot, the continuum robot including a plurality of independently controlled segments along a length of the continuum robot, the method comprising: inserting a continuum robot into a cavity of unknown dimensions or shape; estimating a plurality of forces acting on the continuum robot, the plurality of forces including a generalized force acting on each of the plurality of segments along the length of the continuum robot, each force of the plurality of forces including a magnitude and a direction; comparing each generalized force of the plurality of forces to a respective expected generalized force for each of the plurality of segments, the expected generalized force being based on the position of the continuum robot; adjusting the position of each segment of the plurality of segments based on a difference between the determined generalized force and the expected generalized force for each of the plurality of segments; and advancing the continuum robot further into the cavity.

2. The method of claim 1, further comprising repeating the acts of determining a plurality of forces, comparing each generalized force of the plurality of forces to a respective expected generalized force, and adjusting the position of each segment as the continuum robot is advanced into the cavity.

3. The method of claim 1, further comprising the acts of determining a position of the continuum robot; and determining an expected generalized force for each of the plurality of segments based on previously stored data corresponding to the position of the continuum robot.

4. The method of claim 1, wherein the continuum robot further includes a plurality of backbone structures extending through the continuum robot and a plurality of actuators positioned at a proximate end of the continuum robot, and wherein the position of each segment is controlled by operating each of the plurality of actuators to advance or retract one of the plurality of backbone structures relative to the continuum robot.

5. The method of claim 4, wherein the act of determining the plurality of forces acting on each of the plurality of segments along the length of the continuum robot includes measuring a force exerted on one of the plurality of back-bone structures by one of the plurality of actuators.

6. The method of claim 5, wherein the each generalized force of the plurality of forces indicates the force required to hold the continuum robot in a current position and external forces applied to the continuum robot by the cavity.

7. The method of claim 6, further comprising compensating each determined generalized force of the plurality of forces for error due to friction between the cavity and the continuum robot.

8. The method of claim 1 , wherein the act of adjusting the position of each segment of the plurality of segments based on the difference between the determined generalized force and the expected generalized force for each of the plurality of segments includes adjusting the position of each segment of the plurality of segments to minimize the difference between the determined for and the expected generalized force for each of the plurality of segments.

9. The method of claim 1, further comprising, after advancing a distal end of the continuum robot to a target location, bracing the continuum robot against surfaces of the cavity to provide stability while a tool at the distal end of the continuum robot performs a function.

10. The method of claim 9, wherein the act of bracing the continuum robot against surfaces of the cavity includes adjusting the position of each segment of the plurality of segments to increase the difference between the determined generalized force and the expected generalized force for each of the plurality of segments until the difference reaches approaches threshold.

11. A controller for compliant insertion of a continuum robot into a cavity of unknown dimensions and shape, the continuum robot including a plurality of independently controlled segments along a length of the continuum robot, the controller comprising: a processor; and a memory storing instructions that, when executed by the processor, cause the controller to determine a plurality of forces acting on the continuum robot, the plurality of forces including a generalized force acting on each of the plurality of segments along the length of the continuum robot, each generalized force of the plurality of forces including a magnitude and a direction; compare each generalized force of the plurality of forces to a respective expected generalized force for each of the plurality of segments, the expected force being based on the position of the continuum robot; and adjust the position of each segment of the plurality of segments based on a difference between the determined generalized force and the expected generalized force for each of the plurality of segments.

12. The controller of claim 11, wherein the instructions, when executed by the processor, further cause the controller to repeating the acts of determining a plurality of forces, comparing each generalized force of the plurality of forces to a respective expected generalized force, and adjusting the position of each segment as the continuum robot is advanced into the cavity.

13. The controller of claim 11, wherein the instructions, when executed by the processor, further cause the controller to determine a position of the continuum robot; and determine an expected generalized force for each of the plurality of segments based on previously stored data corresponding to the position of the continuum robot.

14. The controller of claim 11, further comprising a plurality of actuators each connectable to one of a plurality of back-bone structures extending through the continuum robot at a proximate end of the continuum robot, wherein the controller adjusts the position of the continuum robot by operating each of the plurality of actuators to advance or retract one of the plurality of back-bone structures relative to the continuum robot.

15. The controller of claim 14, wherein the instructions, when executed by the processor, cause the controller to determine the plurality of forces acting on each of the plurality of segments along the length of the continuum robot by measuring a force exerted on one of the plurality of back-bone structures by one of the plurality of actuators.

16. The controller of claim 15, wherein each generalized force of the plurality of forces indicates the force required to hold the continuum robot in a current position and external forces applied to the continuum robot by the cavity.

17. The controller of claim 16, wherein the instructions, when executed by the processor, further cause the controller to compensate each determined generalized force of the plurality of forces for error due to friction between the cavity and the continuum robot.

18. The controller of claim 11 , wherein the instructions, when executed by the processor, cause the controller to adjust the position of each segment of the plurality of segments based on the difference between the determined generalized force and the expected generalized force for each of the plurality of segments by adjusting the position of each segment of the plurality of segments to minimize the difference between the determined for and the expected generalized force for each of the plurality of segments.

19. The controller of claim 11, wherein the instructions, when executed by the processor, further cause the controller to brace the continuum robot against surfaces of the cavity to provide stability while a tool at the distal end of the continuum robot performs a function.

20. The controller of claim 19, wherein the instructions, when executed by the processor, cause the controller to brace the continuum robot against surfaces of the cavity by adjusting the position of each segment of the plurality of segments to increase the difference between the determined generalized force and the expected generalized force for each of the plurality of segments until the difference reaches approaches threshold.

A robotic system comprising: a continuum robot including a plurality of independently controlled segments along a length of the continuum robot and a plurality of back-bone structures extending through the continuum robot; a plurality of actuators each connected to a different one of the plurality of backbone structures, the actuators advancing or retracting each of the plurality of back-bone structures to control the position of the continuum robot; and a controller including a processor and a memory, the memory storing instructions that, when executed by the processor, cause the controller to: determine a plurality of forces acting on the continuum robot based on forces exerted on the plurality of back-bone structures by the plurality of actuators, the plurality of forces including a generalized force acting on each of the plurality of segments along the length of the continuum robot, each generalized force of the plurality of forces including a magnitude and a direction; compare each generalized force of the plurality of forces to a respective expected generalized force for each of the plurality of segments, the expected generalized force being based on the position of the continuum robot; adjust the position of each segment of the plurality of segments to minimize a difference between the determined generalized force and the expected generalized force for each of the plurality of segments; repeat the acts of determining a plurality of forces, comparing each generalized force of the plurality of forces to a respective expected generalized force, and adjusting the position of each segment as the continuum robot is advanced into the cavity.