U.S. patent application number 14/271418 was filed with the patent office on 2016-12-08 for systems and methods for safe compliant insertion and hybrid force/motion telemanipulation of continuum robots.
This patent application is currently assigned to Vanderbilt University. The applicant listed for this patent is Vanderbilt University. Invention is credited to Andrea Bajo, C. Gaelyn Garrett, Roger E. Goldman, James L. Netterville, Nabil Simaan.
Application Number | 20160354924 14/271418 |
Document ID | / |
Family ID | 51841893 |
Filed Date | 2016-12-08 |
United States Patent
Application |
20160354924 |
Kind Code |
A9 |
Simaan; Nabil ; et
al. |
December 8, 2016 |
SYSTEMS AND METHODS FOR SAFE COMPLIANT INSERTION AND HYBRID
FORCE/MOTION TELEMANIPULATION OF CONTINUUM ROBOTS
Abstract
Methods and systems are described for controlling movement of a
continuum robot that includes a plurality of independently
controlled segments along the length of the continuum robot. The
continuum robot is inserted into a cavity of unknown dimensions or
shape. A plurality of forces acting upon the continuum robot by the
surrounding cavity are estimated. A positioning command indicating
a desired movement of the distal end of the continuum robot is
received from a manipulator control. The desired movement is
augmented based, at least in part, on the estimated plurality of
forces acting on the continuum robot such that movement is
restricted to within safe boundaries of the surrounding cavity. The
positioning of the continuum robot is then adjusted based on the
augmented desired movement.
Inventors: |
Simaan; Nabil; (Nashville,
TN) ; Bajo; Andrea; (Fort Lauderdale, FL) ;
Netterville; James L.; (Nashville, TN) ; Garrett; C.
Gaelyn; (Nashville, TN) ; Goldman; Roger E.;
(New York, NY) |
|
Applicant: |
Name |
City |
State |
Country |
Type |
Vanderbilt University |
Nashville |
TN |
US |
|
|
Assignee: |
Vanderbilt University
Nashville
TN
|
Prior
Publication: |
|
Document Identifier |
Publication Date |
|
US 20140330432 A1 |
November 6, 2014 |
|
|
Family ID: |
51841893 |
Appl. No.: |
14/271418 |
Filed: |
May 6, 2014 |
Related U.S. Patent Documents
|
|
|
|
|
|
Application
Number |
Filing Date |
Patent Number |
|
|
PCT/US13/37346 |
Apr 19, 2013 |
|
|
|
14271418 |
|
|
|
|
61819808 |
May 6, 2013 |
|
|
|
61636009 |
Apr 20, 2012 |
|
|
|
Current U.S.
Class: |
1/1 |
Current CPC
Class: |
Y10S 901/30 20130101;
B25J 9/1633 20130101; G05B 2219/40184 20130101; A61B 90/06
20160201; Y10S 901/01 20130101; Y10S 901/50 20130101; A61B 34/30
20160201; A61B 2017/00323 20130101; A61B 2034/301 20160201; A61B
2090/064 20160201; A61B 34/37 20160201; A61B 34/35 20160201; G05B
2219/40299 20130101; A61B 2034/306 20160201; B25J 9/1602 20130101;
A61B 2017/00314 20130101; B25J 9/1625 20130101 |
International
Class: |
B25J 9/16 20060101
B25J009/16 |
Goverment Interests
STATEMENT REGARDING FEDERALLY SPONSORED RESEARCH OR DEVELOPMENT
[0002] 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.
Claims
1. A method of controlling movement 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 the continuum robot into a cavity of unknown
dimensions or shape; estimating a plurality of forces acting on one
or more segments of the continuum robot by the surrounding cavity;
receiving a positioning command from a manipulator control, the
positioning command indicating a desired movement for a distal end
of the continuum robot; augmenting the desired movement for the
distal end of the continuum robot based, at least in part, on the
estimated plurality of forces acting on one or more segments such
that movement of the continuum robot is restricted within safe
boundaries of the surrounding cavity; and adjusting the position of
the continuum robot based on the augmented desired movement.
2. A control system for a continuum robot, the control system being
configured to control the movement of the continuum robot according
to the method of claim 1, the control system comprising: the
manipulator control; a compliant insertion controller configured to
perform the acts of estimating a plurality of forces acting on one
or more segments of the continuum robot by the surrounding cavity;
and a motion controller configured to perform the act of receiving
the positioning command from a manipulator control, the positioning
command indicating a desired movement for a distal end of the
continuum robot.
3. The method of claim 1, wherein the cavity is a nasal passage of
a patient and further comprising advancing the continuum robot into
the nasal cavity until the distal end of the continuum robot
reaches a surgical site in the throat of the patient, wherein the
act of advancing the continuum robot includes advancing the
position of the continuum robot based on the positioning command
from the manipulator control, and where the act of augmenting the
desired movement of the continuum robot includes determining an
updated positioning for each segment along the length of the
continuum robot based on the estimated plurality of forces acting
on the continuum robot by the surrounding cavity.
4. A method of controlling positioning of a continuum robot, the
method comprising: determining a set of allowable motions for the
continuum robot; determining a set of allowable forces for the
continuum robot; projecting the set of allowable motions and the
set of allowable forces as projection matrices into a joint space
corresponding to a manipulator control; translating motion commands
from the manipulator control into one or more task specific
wrenches using an inverse of an inertia matrix; translating the one
or more task specific wrenches into a joint-torque vector command
using a Jacobian matrix; and adjusting the positioning of the
continuum robot based on the joint-torque vector command using a
joint-space PID controller.
Description
RELATED APPLICATIONS
[0001] This application claims priority to U.S. Provisional
Application No. 61/819,808, filed on May 6, 2013 and titled "METHOD
AND SYSTEM FOR COMPLIANT INSERTION 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 INVENTION
[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] International Publication No. WO 2013/158978 to Simaan et
al., the entirety of which is incorporated herein by reference,
describes 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] In one embodiment, the invention provides a method of
controlling movement of a continuum robot that includes a plurality
of independently controlled segments along the length of the
continuum robot. The continuum robot is inserted into a cavity of
unknown dimensions or shape. A plurality of forces acting upon the
continuum robot by the surrounding cavity are estimated. A
positioning command indicating a desired movement of the distal end
of the continuum robot is received from a manipulator control. The
desired movement is augmented based, at least in part, on the
estimated plurality of forces acting on the continuum robot such
that movement is restricted to within safe boundaries of the
surrounding cavity. The positioning of the continuum robot is then
adjusted based on the augmented desired movement.
[0014] In some such embodiments, the desired movement includes an
advancement of the continuum robot through a nasal cavity towards a
surgical site in the throat of a patient. The desired movement is
augmented by determining an appropriate safe pose for the continuum
robot as the distal end is advanced further into the cavity.
[0015] In another embodiment, the invention provides a method of
controlling positioning of a continuum robot. A set of allowable
motions and a set of allowable forces are determines for a
continuum robot based, in some cases, on the current position and
surroundings of the continuum robot. The set of allowable motions
and the set of allowable forces are then projected as projection
matrices into a joint space corresponding to a manipulator control.
Motion commands received through the manipulator control are
translated into one or more task-specific wrenches and then into a
joint-torque vector command. The position of the continuum robot is
adjusted based on the joint-torque vector command using a
joint-space PID controller.
[0016] Other aspects of the invention will become apparent by
consideration of the detailed description and accompanying
drawings.
BRIEF DESCRIPTION OF THE DRAWINGS
[0017] FIG. 1 is a perspective view of a multiple-segment continuum
robot.
[0018] FIG. 2 is a perspective view of a single segment of the
continuum robot of FIG. 1.
[0019] FIG. 3 is a block diagram of a controller for positioning
the continuum robot of FIG. 1.
[0020] FIG. 4 is a perspective view of the continuum robot of FIG.
1 being inserted into the nasal passage of a human head.
[0021] 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.
[0022] 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.
[0023] FIG. 7 is a stiffness model for segment of the continuum
robot of FIG. 1 with multiple back-bone structures.
[0024] FIG. 8 is a graph of sample training and estimation data for
the uncertainty parameters in a multi-segment robot.
[0025] FIG. 9 is a graph of training and estimation data similar to
the data provided in FIG. 8.
[0026] FIG. 10 is a functional block diagram of the operation of a
controller providing for compliant insertion of the continuum robot
of FIG. 1.
[0027] FIG. 11 is a perspective view of an experimental system for
analyzing the sensitivity of a continuum robot utilizing compliant
motion control.
[0028] 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.
[0029] FIG. 13 is a table providing experimental data recording
during sensitivity analysis of the compliant motion controller
using the experimental system of FIG. 11.
[0030] 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.
[0031] 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.
[0032] FIG. 16 is a series of images showing compliant insertion of
a continuum robot into a cavity of unknown size and shape.
[0033] FIG. 17 is a series of graphs illustrating reaction data for
the compliant insertion of FIG. 16.
[0034] FIGS. 18A, 18B, and 18C are cutaway views of a continuum
robot being positioned at a location in the throat of a patient
through trans-nasal access.
[0035] FIG. 19 is an elevation view of an actuation unit for a
continuum robot.
[0036] FIG. 20 is a perspective view of a posed continuum
robot.
[0037] FIG. 21 is a functional block diagram of a hybrid control
system for manipulation of the continuum robot.
[0038] FIG. 22A is a perspective view of a posed continuum
robot.
[0039] FIG. 22B is a perspective view of a posed manipulator
control corresponding to the posed position of the continuum robot
in FIG. 21A.
[0040] FIG. 22C is a view of the distal end of the continuum robot
as seen from the fiberscope.
[0041] FIG. 23 is a functional block diagram of a hybrid
motion/force control system for manipulation of a rigid-link
robotic system.
[0042] FIG. 24 is a functional block diagram of a hybrid
motion/force control system for manipulation of a multi-backbone
continuum robot.
[0043] FIGS. 25A and 25 B are graphs of wrench and twist screw
bases corresponding with a frictionless point contact.
[0044] FIG. 26A is a perspective view of a posed continuum
robot.
[0045] FIG. 26B is a side view of the posed continuum robot of FIG.
26A.
[0046] FIG. 27 is a table of kinematic and static parameters of the
continuum robot of FIG. 26A.
[0047] FIG. 28 is a graph of error between expected and actual
actuation force on a backbone of the continuum robot of FIG. 26A as
a function of bending angle.
[0048] FIG. 29 is a graph of error between expected and actual
actuation force on a second backbone of the continuum robot of FIG.
26A as a function of bending angle.
[0049] FIG. 30 is a graph of error between the expected and actual
actuation force on a third backbone of the continuum robot of FIG.
26A as a function of bending angle.
[0050] FIG. 31 is an elevation view of a continuum robot during
force regulation testing.
[0051] FIG. 32 is a table of experimental results of force
regulation in the x direction for three bend angles and three force
magnitudes.
[0052] FIG. 33 is a table of experimental results of force
regulation in the y direction for three bend angles.
[0053] FIGS. 34A, 34B, and 34C are graphs of force step response on
a silicon block in the x direction at a bend angle of 80.degree.
for three different forces.
[0054] FIGS. 35A, 35B, and 35 C are graphs of force step response
on a silicon block in the x direction at a bend angle of 60.degree.
for three different forces.
[0055] FIGS. 36A, 36B, and 36C are graphs of force step response on
a silicon block in the x direction at a bend angle of 40.degree.
for three different forces.
[0056] FIGS. 37A, 37B, and 37C are graphs of force step response on
a silicon block in the y direction for three different starting
bend angles 80.degree., 60.degree., and 40.degree..
[0057] FIG. 38A is a top view of an continuum robot performing
shape estimation.
[0058] FIG. 38B is an elevation view of the continuum robot of FIG.
38A performing the shape estimation.
[0059] FIG. 39 is an overhead view of a probe used for a manual
scan of phantom tissue.
[0060] FIG. 40 is a series of three graphs illustrating the shape
of a structure as estimated using the continuum robot of FIG. 38A
from a side view, a front view, and a top view.
[0061] FIG. 41 is a series of three graphs illustrating the actual,
ground truth shape of the structure measured by the continuum robot
in FIG. 38A from a side view, a front view, and a top view.
[0062] FIG. 42 is a series of three graphs illustrating the error
between the estimated shape and the actual shape measured by the
continuum robot in FIG. 38A from a side view, a front view, and a
top view.
[0063] FIG. 43 is a top view of a continuum robot being used to
localize a blood vessel.
[0064] FIG. 44 is a graph illustrating the surface as estimated by
the continuum robot in FIG. 43 at a first force.
[0065] FIG. 45 is a graph illustrating the surface as estimated by
the continuum robot in FIG. 43 at a second, greater force.
[0066] FIG. 46 is a stiffness map of the surface as estimated by
the continuum robot in FIG. 43.
DETAILED DESCRIPTION
[0067] 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.
[0068] 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.
[0069] 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.
[0070] 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.
[0071] 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.
[0072] 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.
[0073] 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.
[0074] 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.
[0075] 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.
[0076] 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: [0077] A.sub.i
i.sup.th row of A [0078] A.sup.[i] i.sup.th column of A [0079]
[A].sub.ij Entry at the ith row and jth column of the matrix A
[0080] n Number of segments of the continuum robot [0081] m Number
of secondary backbones in each continuum segment [0082]
c.sub..alpha., s.sub..alpha. Abbreviated form of the cosine and
sine respectively where c.sub..alpha.=cos .alpha. and
s.sub..alpha.=sin .alpha. [0083] .theta..sub.(k) Bending angle for
an individual segment [0084] .delta..sub.(k) Bending plane angle
for an individual segment [0085] .psi..sub.(k) Configuration space
vector for an individual segment, .psi..sub.(k)=[.theta..sub.(k),
.delta..sub.(k)].sup.T [0086] .psi..sub.a Augmented configuration
space vector for the multi-segment continuum robot,
.psi..sub.a=[.psi..sup.T.sub.(1), . . . ,
.psi..sup.T.sub.(n)].sup.T [0087] u, v, w Unit basis vectors of an
arbitrary coordinate frame such that [ u, v,
w]=I.epsilon..sup.3.times.3 [0088] {b.sub.(k)} Base frame of the
kth segment [0089] {g.sub.(k)} End frame of the kth segment [0090]
.sup.cP.sub.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} [0091] .sup.aR.sub.b Rotation matrix describing the
orientation of frame {b} with respect to frame {a} [0092]
.sup.cv.sub.b/a Linear velocity of frame {b} with respect to frame
{a}, expressed in frame {c} [0093] .sup.c.omega..sub.b/a Angular
velocity of frame {b} with respect to frame {a}, expressed in frame
{c} [0094] .sup.ct.sub.b/a Twist of frame {b} with respect to frame
{a}, expressed in frame {c}:
.sup.ct.sub.b/a=[.sup.cv.sup.T.sub.b/a,
.sup.c.omega..sup.T.sub.b/a].sup.T [0095] J.sub.q.psi.(k) A
Jacobian matrix linearly mapping the configuration space velocities
to joint velocities such q.sub.(k)=J.sub.q.psi.(k).psi..sub.(k)
[0096] J.sub.v.psi.(k) A Jacobian matrix linearly mapping the
configuration space velocities to linear velocities such that
v=J.sub.v.psi.(k) .psi..sub.(k) [0097] J.sub..omega..psi.(k) A
matrix linearly mapping of the configuration space velocities to
angular velocities such that .omega.=J.sub..omega..psi.(k)
.psi..sub.(k) [0098] J.sub.t.psi.(k) A linear mapping of the
configuration space velocities to the twist such that
[0098] t = J t .psi. ( k ) .psi. ( k ) and J t .psi. ( k ) = [ J v
.psi. ( k ) J w .psi. ( k ) ] ##EQU00001## [0099] E.sub.Y Young's
elasticity modulus for the NiTi back-bones [0100] I.sub.(k)
Cross-sectional second moment of inertia of backbones of the kth
segment [0101] r Radius of the pitch circle along which the
secondary backbones are circumferentially distributed around the
primary backbone [0102] T.sub.(k) Backbone actuation forces of an
individual segment T.sub.(k)=[T.sub.1,(k), . . . ,
T.sub.m,(k)].sup.T [0103] f.sub.(k) Generalized force for the kth
individual segment [0104] f.sub.a Augmented generalized force for
the multi-segment continuum robot f.sub.a=[f.sup.T.sub.(1)), . . .
, f.sup.T.sub.(n)].sup.T [0105] .lamda., .lamda. Actual and
estimated augmented generalized force errors [0106] K.sub..psi.,(k)
Configuration space stiffness for the kth individual segment [0107]
K.sub..psi.a Configuration space stiffness for the multi-segment
continuum robot [0108] x Input feature vector to the Support Vector
Regression (SVR) [0109] N The number of training data for the SVR
optimization [0110] .phi.() High dimensional mapping of the input
feature space of the support vectors in the SVR
[0111] The pose of the k.sup.th segment continuum robot (as
illustrated in FIG. 2) is described in a set of generalized
coordinates by a configuration space vector defined as
.psi..sub.(k)=[.theta..sub.(k),.delta..sub.(k)].sup.T (1)
[0112] where ().sub.(k) denotes a variable associated with the
k.sup.th segment. .theta..sub.(k) defines the bending angle of the
segment measured from the plane defined by the base disk, {
u.sub.b(k), v.sub.b(k)}, and the direction normal to the end disk
w.sub.g(k). .delta..sub.(k) defines the orientation of the bending
plane of the segment as measured from the plane to the u.sub.b(k)
about w.sub.b(k).
[0113] The inverse kinematics of the segment relating the
configuration space, .psi..sub.(k), to the joint space,
q.sub.1,(k)=[q.sub.2,(k), q.sub.m,(k)].sup.T, is given by
L.sub.j,(k)=L.sub.(k)+q.sub.j,(k)=L.sub.(k)+.DELTA..sub.j,(k).theta..sub-
.(k). (2)
where L.sub.j,(k) is the length of the j.sup.th secondary backbone
of the k.sup.th segment for j=1, . . . , m, L.sub.(k) is the length
of the primary backbone of the k.sup.th segment, .DELTA..sub.j,k=r
cos .delta..sub.j,k; .delta..sub.j,(k)=.delta..sub.(k)+(j-1)
(2.pi./m), and .THETA..sub.(k)=.theta.(k)-.pi./2.
[0114] The instantaneous inverse kinematics can be described by
differentiating (2) to yield
{dot over (q)}.sub.(k)=J.sub.q.psi.(k){dot over (.psi.)}.sub.(k)
(3)
where the Jacobian J.sub.q.psi.(k) is given by
J q .psi. ( k ) = [ rc .sigma. 1 , ( k ) - r .theta. ( k ) s
.sigma. 1 , ( k ) rc .sigma. m , ( k ) - r .theta. ( k ) s .sigma.
m , ( k ) ] ( 4 ) ##EQU00002##
[0115] The direct kinematics of the k.sup.th 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
.theta. ( k ) .noteq. .pi. 2 , ##EQU00003##
the kinematics takes the form
P b ( k ) g ( k ) b ( k ) = L ( k ) .theta. ( k ) [ c .sigma. ( k )
( s .theta. ( k ) - 1 ) - s .sigma. ( k ) ( s .theta. ( k ) - 1 ) -
c .theta. ( k ) ] ( 5 ) R g ( k ) b ( k ) = - .sigma. ( k ) [ w ^ x
] - .theta. ( k ) [ v ^ x ] .sigma. ( k ) [ w ^ x ] ( 6 )
##EQU00004##
where {circumflex over (.nu.)}=[0, 1, 0].sup.T, w=[0, 0, 1].sup.T
and the frames {g(k)} and {b.sub.(k)} are shown in FIG. 3. For
.theta. ( k ) = .pi. 2 , ##EQU00005##
the formulation singularity,
1 .THETA. ( k ) , , ##EQU00006##
resolves to
P b ( k ) g ( k ) b ( k ) = [ 0 0 L ( k ) ] T ( 7 ) R b ( k ) b ( k
) = I .di-elect cons. 3 .times. 3 ( 8 ) ##EQU00007##
By differentiating (5) and (6), the instantaneous direct kinematics
takes the form
t g ( k ) b ( k ) / b ( k ) = J t .psi. ( k ) .psi. . ( k ) ( 9 )
##EQU00008##
where, for
.theta. ( k ) = .pi. 2 , ##EQU00009##
the Jacobian J.sub.r.psi.(k) is given by .delta.
J t .psi. ( k ) = [ Ls .delta. ( k ) .theta. ( k ) c .theta. ( k )
- s .theta. ( k ) + 1 .theta. ( k ) 2 - Ls .delta. ( k ) s .theta.
( k ) .theta. ( k ) - 1 Ls .delta. ( k ) .theta. ( k ) c .theta. (
k ) - s .theta. ( k ) + 1 .theta. ( k ) 2 Lc .delta. ( k ) s
.theta. ( k ) - 1 .theta. ( k ) L .theta. s .theta. ( k ) + c
.theta. ( k ) .theta. ( k ) 2 0 - s .delta. ( k ) c .delta. ( k ) c
.theta. ( k ) - c .delta. ( k ) - s .delta. ( k ) c .theta. ( k ) 0
- 1 + - s .theta. ( k ) ] ( 10 ) ##EQU00010##
and for
.theta. ( k ) = .pi. 2 , ##EQU00011##
the formulation singularity,
1 .theta. ( k ) , ##EQU00012##
is resolved by applying l'Hospital's rule, to yield
J t .psi. ( k ) = [ - L 2 c .delta. ( k ) L 2 s .delta. ( k ) 0 - s
.delta. ( k ) - c .delta. ( k ) 0 0 0 0 0 0 0 ] T ( 11 )
##EQU00013##
Using Euler beam bending analysis and neglecting gravitational
potential energy, the total energy of the continuum robot is given
by
U = k - 1 n U ( k ) ##EQU00014##
where the potential elastic energy of the k.sup.th segment is given
by
U ( k ) = E Y I ( k ) .theta. ( k ) 2 2 ( 1 L ( k ) + j -= 1 m 1 l
j , ( k ) ) ( 12 ) ##EQU00015##
[0116] The principle of virtual work and the energy expression (12)
are used to obtain a statics model for the k.sup.th continuum
segment as
J.sub.q.psi.(k).sup.TT.sub.(k)=.gradient.U.sub.(k)-J.sub.x.psi..sub.(k).-
sup.Tw.sub.e,(k) (13)
where .tau..sub.(k) represents the actuation forces on the
secondary backbones, .gradient.U.sub.(k) is the gradient of the
potential energy, and .psi..sub.e,(k) is the external wrench
applied to the end disk of the k.sup.th continuum segment. The
energy gradient .gradient.U.sub.(k) is calculated with respect to a
virtual displacement in configuration space.
.DELTA..psi..sub.(k)=[.DELTA..theta..sub.(k),
.DELTA..delta..sub.(k)].sup.T and is given by
.gradient. U ( k ) = E Y I ( k ) = [ .theta. ( k ) L ( k ) + j = 1
m .theta. ( k ) L j , ( k ) - .theta. ( k ) 2 2 r j = 1 m c .sigma.
j , ( k ) L j , ( k ) 2 .theta. ( k ) 3 2 r j = 1 m s .sigma. j , (
k ) L j , ( k ) 2 ] ( 14 ) ##EQU00016##
[0117] The statics expression (13) projects both the actuation
forces T.sub.(k) and perturbation wrench w.sub.e,(k)w into the
configuration space of the continuum segment, .psi..sub.(k) Thus,
after projecting the wrench from three-dimensional Cartesian space,
.sup.6, into the configuration space, .sup.2, 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.
[0118] If the wrench acting on the end disk that is projected into
the configuration space of the k.sup.th segment is defined as the
generalized force vector
f(k)J.sub.t,.psi..sub.(k).sup.Tw.sub.e(k) (15)
Applying equation (13) to the generalized force expression, the
i.sup.th row of the generalized force f.sub.(k) can be written
as
f.sub.i=.gradient.U.sub.i-[J.sub.q.psi..sup.T].sub.iT=.gradient.U.sub.i[-
J.sub.q.psi..sup.[i]].sup.TT (16)
where J.sub.q.psi..sup.[i] denotes the i.sup.th column of
J.sub.q.psi..
[0119] For small perturbations from an equilibrium configuration,
the stiffness of the individual continuum segment can be posed in
the configuration space as
.delta.f=K.sub..psi..delta..psi. (17)
where the stiffness is given by the Jacobian of the generalized
force with respect to configuration space perturbation. Thus, the
elements of H.sub..psi. are given by:
.differential. f i .differential. .psi. j = [ K .psi. ] ij =
.differential. .differential. .psi. j [ .gradient. U i - [ J q
.psi. [ I ] ] T T ] ( 18 ) ##EQU00017##
The elements of the stiffness matrix (18) can be expanded as
[ K .psi. ] ij = [ H .psi. ] ij - [ .differential. .differential.
.psi. j ( j q .psi. [ i ] ) ] T T - [ J q .psi. [ i ] ] T
.differential. T .differential. .psi. j ( 19 ) ##EQU00018##
The first term of the configuration space stiffness, H.sub..psi. is
the Hessian of the elastic energy of the segment given by
H .psi. = .differential. 2 U .differential. .theta. 2
.differential. 2 U .differential. .theta. .differential. .beta.
.differential. 2 U .differential. .delta. .differential. .theta.
.differential. 2 U .differential. .delta. 2 ( 20 ) ##EQU00019##
The individual elements of the Hessian, H.sub..psi., are given
by
.differential. 2 U .differential. .theta. 2 = E Y I [ 1 L + I = 1 m
1 L i - 2 r .theta. i - 1 m c .sigma. i L i 2 + .theta. 2 r 2 i = 1
m c .sigma. i 2 L i 3 ] ( 21 ) .differential. 2 U .differential.
.theta. .differential. .delta. = .differential. 2 U .differential.
.delta. .differential. .theta. = E Y I [ 3 .theta. 2 2 r i = 1 m s
.sigma. i L i 2 - .theta. 3 r 2 i = 1 m s .sigma. i c .sigma. i L i
3 ] ( 22 ) .differential. 2 U .differential. .delta. 2 = E Y I [
.theta. 3 2 r i = 1 m c .sigma. i L i 2 + .theta. 4 r 2 i = 1 m s
.sigma. i 2 L i 3 ] ( 23 ) ##EQU00020##
The second term of equation (19) expands to
.differential. .differential. .psi. 1 ( J q .psi. 1 ) = 0 .di-elect
cons. m ( 24 ) .differential. .differential. .psi. 1 ( J q .psi. 2
) = .differential. .differential. .psi. 2 ( J q .psi. 1 ) = - r [ s
.sigma. 1 , , s .sigma. m ] T ( 25 ) .differential. .differential.
.psi. 2 ( J q .psi. 2 ) = - r .theta. [ c .sigma. 1 , , c .sigma. m
] T ( 26 ) ##EQU00021##
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, k.sub.b and k.sub.c, corresponding to
the bending, L.sub.b, and non-bending, L.sub.c, regions of the
actuation lines connecting the working distal end to the actuation
unit.
[0120] The axial stiffness along the length of a given actuation
line can then be expressed as
1 k = 1 k c + 1 k b ( 27 ) ##EQU00022##
where
k c = EyA L c and k b = EyA L b ##EQU00023##
and A denotes the cross-sectional area of the backbone.
[0121] 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.sub.c>>L.sub.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
.differential. T .differential. q = K q Where ( 28 ) K q .apprxeq.
[ E Y A L c 0 0 0 0 0 0 E Y A L c ] .di-elect cons. mxm ( 29 )
##EQU00024##
Expanding terms by applying the chain rule and using the
instantaneous inverse kinematics of equation (3),
.differential. T .differential. .psi. ##EQU00025##
is given by
.differential. T .differential. .psi. = .differential. T
.differential. q .differential. q .differential. .psi. .apprxeq. K
q J q .psi. ( 30 ) ##EQU00026##
The configuration space stiffness therefore reduces to
[ K .psi. ] ij = [ H .psi. ] ij - [ .differential. .differential.
.psi. j ( J q .psi. [ i ] ) ] T T - [ J q .psi. [ i ] ] T K q J q
.psi. [ J ] ( 31 ) ##EQU00027##
[0122] 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, f=k(x.sub.eq-x), there exists an
equilibrium pose, {.sup.b.sup.(k)P.sub.eq.sup.b.sup.(k)R.sub.eq},
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
.psi..sub.(k),d=g(.sup.b.sup.(k)P.sub.eq, .sup.b.sup.(k)R.sub.eq)
that 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.sub.(k) and therefore minimize the
interaction wrench.
[0123] 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
f.sub.a=[f.sub.(1).sup.T . . . f.sub.(n).sup.T].sup.T (32)
is associated with the augmented configuration space
.psi..sub.a=[.psi..sub.(1).sup.T . . . .psi..sub.(n).sup.T].sup.T
(33)
[0124] 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 continuum
robot
K .psi. a = [ K .psi. , ( 1 ) 0 0 0 K .psi. , ( 2 ) 0 0 0 K .psi. ,
( n ) ] ( 34 ) ##EQU00028##
[0125] An error function is defined as the distance in
configuration space from the current configuration, .psi..sub.a to
the desired configuration .psi..sub.ad, as
e.sub..psi..sub.a=.psi..sub.a.sub.d-.psi..sub.a (35)
For a passive wrench condition, the equilibrium pose and
corresponding configuration space remain fixed, e.g.,
.psi..sub.ad=0. For external wrenches changing slowly with respect
to the compliant control update rate, this condition becomes
.psi..sub.ad.apprxeq.0 and hence negligible when taking the time
derivative of e.sub..psi.n. The following discussion uses
.psi..sub.ad=0 although it is to be understood that the same
derivation follows for .psi..sub.ad.apprxeq.0 by changing the
equality sign to an approximation sign.
[0126] The time derivative of the error function under these
conditions becomes
e.sub..psi..sub.a=-{dot over (.psi.)}.sub.a. (36)
[0127] For small perturbations, the distance in configuration space
is given as .DELTA..psi..sub.a=.psi..sub.a.sub.d. Thus by applying
the configuration space stiffness K.sub..psi..sub.a, the error in
the system can be related to the generalized force as
f.sub.a=K.sub..psi..sub.ae.sub..psi..sub.a. (37)
Where .DELTA.f.sub.a.apprxeq.f.sub.a for the small perturbations.
This assumption holds for robots that are calibrated such that
f.sub.a.apprxeq.0 for unloaded movements throughout the
workspace.
[0128] During control of a continuum system the estimated
generalized force {circumflex over (f)}.sub.a is calculated using
the idealized statics model of f.sub.a based on the estimate of the
energy and measured actuation forced as in equation (16). This
estimate is contaminated by an error .lamda. due to un-modeled
friction and strain along the actuation lines, perturbations from
circulate-bending 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
{circumflex over (f)}.sub.a,uncompensated=f.sub.a+.lamda. (38)
where f.sub.a is given by equation (16).
[0129] 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.sub.e=0 and
equation (15) predicts f.sub.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.sub.(k) in equation (16) for each segment
would also return f.sub.a=0. However, the actual kinetostatic
effects cause f.sub.a=0 and this deviation forms .lamda. 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.
[0130] 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 .lamda. during compliant motion
control such that the compensated generalized force estimate is
given by
{circumflex over (f)}.sub.a=f.sub.a+.lamda.-{circumflex over
(.lamda.)}. (39)
An augmented compliant motion controller, taking into account the
estimates of the un-modeled error, {circumflex over (.lamda.)},
takes the form
{dot over (.psi.)}.sub.a=.mu.A{circumflex over (f)}.sub.a (40)
where .mu. and A correspond respectively to positive definite
scalar and matrix gains to be chosen by the operator.
[0131] 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
.parallel.e.sub..psi..sub.a.parallel.>.parallel.K.sub..omega..sub.a.s-
up.-1(.lamda.-{circumflex over (.lamda.)}).parallel.. (41)
[0132] The dynamics of the error in the configuration space
applying equation (40) to equation (36) and accounting for equation
(39) is represented as
.sub..psi..sub.a=.mu.A(K.sub..psi..sub.ae.sub..psi..sub.a+.lamda.-{circ-
umflex over (.lamda.)}) (42)
[0133] A Lyapunov function candidate of the error system for
compliant control, represented by equation (42), can be defined
as
v ( e .psi. a ) = 1 2 e .psi. a T Pe .psi. a ( 43 )
##EQU00029##
where P is any symmetric positive definite matrix. Differentiating
equation (43) with respect to time and accounting for equation (42)
yields
{dot over (V)}(e.sub..psi..sub.a.sup.TP
.sub..psi..sub.a.sup.T=-.mu.e.sub..psi..sub.a.sup.TPA(K.sub..psi..sub.ae.-
sub..psi..sub.a+.lamda.-{circumflex over (.lamda.)}) (44)
Choosing the control and Lyapunov weighting matrices as
A=K.sub..psi.n.sup.-1 and P=I, the derivative of the Lyapunov
function reduces to)
{dot over
(V)}(e.sub..psi..sub.a)=-.mu.(e.sub..psi..sub.a.sup.Te.sub..psi..sub.a+e.-
sub..psi..sub.a.sup.TK.sub..psi..sub.a.sup.-1(.lamda.-{circumflex
over (.lamda.)})) (45)
A controller implementing these techniques relies on the
invertibility of the configuration space stiffness. The block
diagonal matrix is constructed of the sub-matrices, K.sub..psi.(k)
as in equation (34). Singular configurations of K.sub..psi.(k) and,
therefore, K.sub..psi..sub.a correspond with a straight
segment,
.theta. ( k ) = .pi. 2 . ##EQU00030##
For control purposes, the effect of singular poses can be mitigated
using a singularity robust inverse of the configuration space
stiffness.
[0134] Noting the expression
e.sub..psi..sub.a.sup.Te.sub..psi..sub.a is positive definite for
all e.sub..psi..sub.a and .mu.>0 it is sufficient to show
.parallel.e.sub..psi..sub.a.sup.Te.sub..psi..sub.a.parallel.>.parallel-
.e.sub..psi..sub.a.sup.TK.sub..psi..sub.a.sup.-1(.lamda.-{circumflex
over (.lamda.)}).parallel. to prove the time derivative of the
Lyapunov function candidate is negative definite.
[0135] Assuming the bound on the generalized force error,
represented by equation (41), and applying the identity
.parallel.e.sub..psi..sub.a.sup.T.parallel..parallel.e.sub.u.sub.a.parall-
el.=.parallel.e.sub..psi..sub.a.sup.Te.sub..psi..sub.a.parallel.
yields
.parallel.e.sub..psi..sub.a.sup.Te.sub..psi..sub.a.parallel.>.paralle-
l.e.sub..psi..sub.a.sup.T.parallel..parallel.K.sub..psi..sub.a.sup.-1(.lam-
da.-{umlaut over (.lamda.)}).parallel. (46)
Applying the Schwarz inequality
.parallel.e.sub..psi..sub.a.sup.T.parallel..parallel.K.sub..psi..sub.a.su-
p.-1(.lamda.-{umlaut over
(.lamda.)}).parallel..gtoreq..parallel.e.sub..psi..sup.T
K.sub..psi..sup.-1 (.lamda.-{circumflex over (.lamda.)}).parallel.
to equation (46), the condition on the generalized force error
assumed for stability can be expanded as
.parallel.e.sub..psi..sub.a.sup.Te.sub..psi..sub.a.parallel.>.paralle-
l.e.sub..psi..sub.a.sup.TK.sub..psi..sub.a.sup.-1(.lamda.-{circumflex
over (.lamda.)}).parallel. (47)
thus proving the stability of the controller given the bounds on
the compensation error.
[0136] The techniques described above illustrate controller
stability for estimation uncertainties, .lamda.-{circumflex over
(.lamda.)} that are small with respect to the system error,
e.sub..psi..sub.a. In order to maintain a stable pose in
configurations where
.parallel.e.sub..psi..sub.a.sup.Te.sub..psi..sub.a.parallel..apprxeq..par-
allel.(.lamda.-{circumflex over (.lamda.)}).parallel., 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.
[0137] Bounds for the sensitivity of the compliant motion
controller, defined by equation (40), are limited by the error in
canceling the generalized force uncertainties, .lamda.-{circumflex
over (.lamda.)} For small load perturbations to the continuum
robot, the errors in the generalized force, .lamda., are well
approximated by a function of the robot pose in configuration space
.psi..sub.a and the trajectory leading to this pose. .psi..sub.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, {circumflex
over (.lamda.)} that is obtained though off-line training and
online estimation of the model error via support vector regression
(SVR).
[0138] 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.
[0139] To estimate the error in the generalized force based on an
initial unloaded exploration of the workspace, the ".nu." SV
Regression (.nu.-SVR) is employed along each dimension of the
generalized force. The .nu.-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.
[0140] The training set for the .nu.-SVR contains input pairs
{x.sub.[l], y.sub.[l]}.sub.t=1.sup.N 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 i.sup.th component of the generalized force, the input is given
as
x [ I ] = [ .psi. a , 1 N max ( .psi. a , 1 ) , , .psi. a , 2 n N
max ( .psi. a , 2 n ) , .psi. . a , 1 N max ( .psi. . a , 1 ) , ,
.psi. . a , 2 n N max ( .psi. . a , 2 n ) ] T .di-elect cons. R 4 n
y [ I ] = f a , i w e = 0 ( 48 ) ##EQU00031##
where N is the number of training samples,
x [ l ] = [ .psi. a , 1 N max ( .psi. a , 1 ) , , .psi. a , 2 n N
max ( .psi. a , 2 n ) , .psi. . a , 1 N max ( .psi. . a , 1 ) , ,
.psi. . a , 2 n N max ( .psi. . a , 2 n ) ] T .di-elect cons. R 4 n
y ( l ) = f a , i | w e = 0 ( 48 ) ##EQU00032##
is the maximum recorded magnitude of the i.sup.th row of
.psi..sub.a over the set 1.epsilon.[1,N], and .psi..sub.a is the
configuration space velocity. This expression normalizes each
element of the input vector x.sub.[l] to the range of [-1, 1] for
all data in the training set.
[0141] Given these training input vector pairs, .nu.-SVR provides a
method for estimating the function
f(x)=w,.phi.(x)+b (49)
where (,) is the inner product space, w and b are parameters to be
determined by the convex optimization and
.phi.()R.sup.4th.fwdarw.R.sup.n.sup.xh is a mapping of the input
feature vector x.sub.[l] to a higher dimensional feature space.
[0142] 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
k(x.sub.[l],x[.sub.m])=e.sup.-.gamma..parallel.(x.sup.[l].sup.x[.sup.m.s-
up.]).parallel..sup.2, (50)
is chosen as a suitable candidate for higher dimensional
mapping.
[0143] The resulting .nu.-SVR model for function estimation with a
new input vector, x*, is given by
f ( x ) = SV ( a i * - a i ) k ( x [ i ] , x * ) + b ( 51 )
##EQU00033##
It should be noted that only those training points x.sub.[l],
y.sub.[l], denoted SV in equation (51), for which
|y[l]-f(x[l])|.gtoreq..epsilon. 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.
[0144] 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 .lamda. 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 .lamda..sub.1 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, .theta..sub.(1),
.delta..sub.(1), .theta..sub.(2), .delta..sub.(2), are uncorrelated
to provide a robust data set for estimation. Frames displayed in
the FIGS. 8 and 9 are given for every tenth collected sample. The
.nu.-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 {circumflex over (.lamda.)} and actual is
presented for each direction of the generalized force.
[0145] The .nu.-SVR displays good generalization to the untrained
data. The compensation provides a minimum of 1.4 times reduction in
the RMS error for .lamda..sub.4 and a maximum of 2.5 times
reduction for .lamda..sub.1. While the .nu.-SVR reduces errors on
average, the compensation is locally imperfect as can be seen by
maxima in the error in the .lamda. directions exceeding the
measured errors, FIG. 8. The magnitude of the predicted .nu.-SVR
values exceed the measured values, e.g. FIG. 9 for .lamda..sub.1,
producing local increases in the compensated predicted force given
by {circumflex over (f)}.sub..lamda. in equation (39). Filtering is
applied to smooth outliers in the prediction values.
[0146] 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).
[0147] Joint force sensors 701 provide actuation force measurements
for individual segments of the continuum robot, .tau..sub.(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.sub.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.
[0148] The inverse of the configuration space stiffness 717 is
applied to determine difference, .DELTA..sub..psi..sub.a, 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, .sup.-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.
[0149] 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.
[0150] 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.20 mm
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.
[0151] 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
1109 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.degree..
[0152] 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
1108 with an RMS orientation accuracy of 0.5.degree.. 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
.parallel.f.sub.1.parallel..ltoreq.20 Nmm,
.parallel.f.sub.2.parallel..ltoreq.10 Nmm. (52)
These thresholds were determined empirically based on the error
compensation in the generalized force and the stability of the
controller.
[0153] 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 .delta.. 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
.theta.=.about.90.degree., the accuracy of .delta. 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 .theta.=90.degree.
and .theta.=84.degree., respectively, were therefore excluded from
the error calculation for .delta..sub.1. 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.
[0154] The sensitivity measurements provide an opportunity to
evaluate the contribution of the model uncertainties and the
compensation by .nu.-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.
[0155] 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.
[0156] 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.
[0157] 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.
[0158] Trans-Nasal Throat Surgery
[0159] Minimally Invasive Surgery of the throat is predominantly
performed trans-orally. Although trans-oral (TO) access provides a
scarless access into the airways, its outcomes are affected by
complications, high cost, and long setup time. Continuum robotic
systems such as described herein can be used to provide trans-nasal
(TN) access to the throat. Furthermore, as described below, a
hybrid position/compliant motion control of a rapidly deployable
endonasal telerobotic system can also be used to safely manipulate
the robot during trans-nasal access. The system uses a unique 5 mm
surgical slave with force sensing capabilities used to enable
semiautomation of the insertion process. Working channels in the
continuum robot allow the deployment of surgical tools such as a
fiberscope, positioning sensors, grippers, suction tubes, cautery,
and laser fibers. The treatment of vocal fold paralysis is chosen
as a benchmark application and a feasibility study for collagen
injection is conducted. Experiments on a realistic human intubation
trainer demonstrated successful and safe TN deployment of the end
effector and the feasibility of robotic-assisted treatment of vocal
nerve paralysis.
[0160] FIGS. 18A, 18B, and 18C illustrate an example of a robotic
system for trans-nasal (TN) access to the throat. A nasopharyngeal
tube 1801 is inserted into the nasal cavity 1803 through the nose.
The continuum robot is extended through the pharynx 1805 to reach a
surgical site at the larynx 1807. As shown in FIG. 18B, the robotic
stem 1809 is inserted into the nasopharyngeal tube 1801 and the
continuum robot 1811 extends from the distal end of the
nasopharyngeal tube 1801 at the surgical site. In this example as
illustrated in FIG. 18C, the distal end of the continuum robot 1811
includes a needle 1813 for performing vocal fold repair at the
surgical site and a fiberscope 1815. A bronchoscope 1817 is also
inserted with the robot through the nasal cavity 1803. The
end-effector of the continuum robot 1811 is composed of both active
and passive multi-backbone continuum segments. A hybrid
motion/compliance controller assists the surgeon during the
insertion phase by actively complying to the unknown environment
while allowing the operator to control the insertion depth and
speed.
[0161] The robotic system of FIG. 18A is operated using a dexterous
5 DoF continuum manipulator, a Sensable Phantom Omni, an Ascension
3D Guidance trakSTAR 2 with flat metal immune transmitter, a 4 mm
bronchoscope, and a 1.2 mm fiberscope. FIG. 19 illustrates an
example of an actuation unit that drives the movement of the
continuum robot on seven controlled axes, of which six control the
two continuum segments and one controls the insertion depth into
the nasopharyngeal tube. The actuation unit includes a robotic stem
1901, a linear slider 1903 (which controls the extension of the
continuum robot from the robotic stem 1901), one or more load cells
1905 (for monitoring actuation forces), and one or more
potentiometers 1907 (for redundant positioning information).
[0162] The system can operate in both passive and semi-active
modes. The passive mode is based on a standard telemanipulation
architecture where the operator guides the end-effector via a
master manipulator (the Phantom Omni) while observing endoscopic
images provided by the in-hand fiberscope. The semi-active mode is
used to reach the surgical site. During the semi-active mode the
surgeon commands the insertion depth while the continuum
manipulator autonomously complies with the anatomy using a
variation of the compliance motion controller described above.
[0163] FIG. 20 illustrates a surgical slave portion of the robotic
system that is composed of a flexible passive stem and two active
segments. Each 5 mm segment is composed of four super-elastic NiTi
backbones (one central and three secondary ones) that provide two
DoF for bending in any direction. An additional DoF is provided by
the linear stage of the actuator (described in FIG. 19 above),
which controls the insertion of the base of continuum arm. The
robotic arm has three 1.8 min lumens that allow for the deployment
of surgical instruments. In order to compensate for the unknown
shape of the passive segment constrained by the nasopharyngeal
tube, a 0.9 mm 6 DoF electro-magnetic sensor is attached to the
base of the two-segment continuum robot. The metal immune flat
transmitter makes the system robust to surrounding metal and easy
to place under the patient's head.
[0164] FIG. 20 illustrates five coordinate systems that are defined
to describe the direct kinematics of the surgical slave: the world
frame B.sub.w defined by vectors { x.sub.w, y.sub.w, z.sub.w}, the
frame attached to the exit of the nasopharyngeal tube B.sub.0
defined by vectors { x.sub.0, y.sub.0, z.sub.0}, the base frame of
the actively bending continuum robot B.sub.1 defined by vectors {
x.sub.1, y.sub.1, z.sub.1}, the base frame of the second segment
B.sub.2 defined by vectors { x.sub.2, y.sub.2, z.sub.2}, and the
end effector frame B.sub.3 defined by vectors { x.sub.3, y.sub.3,
z.sub.3}.
[0165] The position and orientation of end-effector frame B.sub.3
in B.sub.0 may be described by the following augmented
configuration space vector:
.PSI.=[.theta..sub.1.delta..sub.1q.sub.ins.theta..sub.2.delta..sub.2.the-
ta..sub.3.delta..sub.3].sup.T (53)
where, q.sub.ins is the insertion depth and angles .theta..sub.k
and .delta..sub.k are defined similarly for the passive segment
(k=1) and active segments (k=2, 3) as depicted in FIG. 20. Using
.PSI., the position and orientation of frame B.sub.3 in B.sub.0 is
given by:
p 3 0 = p 1 0 + R 1 0 ( p 2 1 + R 2 1 p 3 2 ) ( 54 ) R 3 0 = R 1 0
R 2 1 R 3 2 where ( 55 ) p k k - 1 = L k .THETA. k [ cos ( .delta.
k ) ( sin .theta. k - 1 ) - sin ( .delta. k ) ( sin .theta. k - 1 )
- cos .theta. k ] ( 56 ) R k k - 1 = Rot ( - .delta. k , z ^ ) Rot
( - .THETA. k , y ^ ) Rot ( .delta. k , z ^ ) ( 57 )
##EQU00034##
with: L.sub.1=q.sub.ins, L.sub.2, and L.sub.3 being the lengths of
the first and second active segments.
.theta..sub.k=.theta..sub.k-.pi./2, and operator Rot(a,b) being the
rotation matrix about axis b by angle a. Note that, since the first
segment is passive, .theta..sub.1 and .delta..sub.1 are not
directly controllable. In the remainder of this example, we assume
that .theta..sub.1 and .delta..sub.1 and the origin of from B.sub.1
are measured by a magnetic tracker (the position and the
orientation frame B.sub.1 is known). Hence, the forward kinematics
is calculated using .theta..sub.1 and .delta..sub.1 and B.sub.1
along with joing values q.sub.i, i=1 . . . 6.
[0166] The configuration space vector .PSI. relates to the joint
space vector as
q(.PSI.)=[q.sub.insq.sub.2(.PSI.).sup.Tq.sub.3(.PSI.).sup.T].sup.T
(58)
where q.sub.2 and q.sub.3 are the amount of pushing-pulling on the
secondary backbones of active segments 2 and 3. In particular
q 2 = r [ .THETA. 1 cos ( .delta. 2 ) - .THETA. 0 cos ( .delta. 0 )
.THETA. 1 cos ( .delta. 2 + .beta. ) - .THETA. 0 cos ( .delta. 0 +
.beta. ) .THETA. 1 cos ( .delta. 2 + 2 .beta. ) - .THETA. 0 cos (
.delta. 0 + 2 .beta. ) ] ( 59 ) q 3 = r .THETA. 2 [ cos ( .delta. 2
) cos ( .delta. 2 + .beta. ) cos ( .delta. 2 + 2 .beta. ) ] ( 60 )
.theta. 0 = .pi. / 2 - atan 2 ( R 13 2 + R 23 2 , R 33 ) ( 61 )
.delta. 0 = - atan 2 ( R 23 , R 13 ) ( 62 ) ##EQU00035##
where .beta.=2.pi./3, r is the radial distance of the secondary
backbone from the center of the base disk,
.theta..sub.0=.theta..sub.0-.pi./2,
.theta..sub.1=.theta..sub.1-.pi./2, and R.sub.ij is element ij of
rotation matrix .sup.wR.sub.1 obtained from the magnetic sensor
placed at the based of the first active segment. Note that angles
.theta..sub.0 and .delta..sub.0 allow us to work in local frame
B.sub.0 decoupling the kinematics of the distal active and passive
segments from shape variations of the passive stem.
[0167] FIG. 21 illustrates the control architecture of the
telesurgical system. Control inputs and electro-magnetic
orientation information are both received at 125 Hz over the Local
Area Network while encoder and actuation force signals are sampled
at 1 kHz and 5 kHz respectively. The conventional hybrid
motion/compliance control is adopted in which two separate
controllers independently provide motion and compliance control
commands. However, the hybrid motion/compliance control approach is
adapted to our continuum robot by decomposing the control task in
configuration space rather than in task space. The projection
matrices .OMEGA. and .sup.-.OMEGA. ensure that the control commands
are projected into subspaces for motion and compliance control.
Joint-level commands to the actuators are then obtained through the
inverse position of the continuum manipulator and the required
model-based actuation compensation. The following details describe
the specific implementation of our approach.
[0168] (1) Compliant Motion Control:
[0169] Compliance Motion Control (CMC), as described above, allows
multi-backbone continuum robots with intrinsic actuation sensing to
autonomously steer away from constraints without a priori knowledge
of the contact nor the external wrench. In the following, the CMC
control mechanism is modified in order to use only information
about actuation forces sensed on the most distal segment. This
control law allows the robot to actively comply to the
circle-shaped nasopharyngeal tube. If actuation force information
on the second active segment is available, the actuation force
error is defined as:
.tau..sub.e.sub.3=.tau..sub.3-.tau..sub.3 (63)
where .tau..sub.3 are the expected actuation forces and
.sup.-.tau..sub.3 are the sensed actuation forces acting on the
third segment respectively. We now project the actuation force
error, .tau..sub.e3, into the configuration space of the distal
segment obtaining the generalized force error:
f.sub.e.sub.3=J.sub.q.psi..sup.T.tau..sub.e.sub.3 (64)
where J.sub.q.psi. is the Jacobian matrix that relates the
configuration space velocities to the joint space velocities.
[0170] Hence, the desired configuration space velocity vector that
steers the robot away from the unknown contact is:
{dot over (.PSI.)}.sub.e=.PHI.K.sub..psi..sub.2.sup.-1f.sub.e.sub.3
(65)
where K.sub..psi.2 is the configuration space stiffness matrix of
the second segment and
.PHI. = [ 0 1 0 1 0 0 0 1 0 1 ] T ( 66 ) ##EQU00036##
Note that Equation (65) is a simplification of the algorithm
presented above for compliant motion control for multiple segments.
In the case of TN insertion, assuming an overall circular shape of
the entire manipulator, both segments need to bend by the same
angle in order pass through the nasopharyngeal tube.
[0171] (2) Motion Control:
[0172] Telemanipulation of the surgical slave is achieved by
defining a mapping between the movement of the master's
end-effector and the desired motion directions in the fiberscope
image space as shown in FIGS. 22A, 22B, and 22C. Because of the
difficulties in accurately controlling the two active segments and
the long unknown-shaped passive stem, the movement of the Phantom
Omni manipulator control (shown in FIG. 22B) is mapped only to 3
DoF of the most distal segment in this example: 1 DoF of insertion
and 2 DoF of bending (as in a conventional bronchoscope).
Therefore, once the robot is inserted (shown in FIG. 22A), the
first segment is bent (.theta..sub.1=60) in the direction of the
vocal folds (.delta..sub.1=0). The user advances the robot in the
direction of z.sub.f (insertion) and tilts about axes x.sub.f and
y.sub.f in fiberscope-attached frame. The fiberscope-attached frame
(shown in FIG. 22C) is related to B.sub.3 by a known fixed
rotation. This movement in image space is first mapped to the
movement of the master stylus { x.sub.ms, y.sub.ms, z.sub.ms} and
then to the movement of the distal segment frame { x.sub.3,
y.sub.3, z.sub.3}.
[0173] The desired position and orientation of distal segment are
given as follows:
.sup.2p.sub.3,des=.sup.2p.sub.3,curr+.sup.2R.sub.3,currp.sub.des,master
(67)
.sup.2R.sub.3,des=.sup.2R.sub.3,currR.sup.TR.sub.des,masterR
(68)
where matrix R defines the transformation between the
camera-attached frame (FIG. 22C) and the master's stylus (FIG.
22B). Once the desired pose is computed, the desired twist
t.sub.des is computed and the desired configuration space
velocities are obtained as follows:
[ q . ins .theta. . 2 .delta. . 2 ] = ( .GAMMA..SIGMA. J 2 ) - 1
.GAMMA..SIGMA. t des . ( 69 ) ##EQU00037##
In Equation (69), J.sub.2 is the Jacobian matrix of the distal
segment:
J 2 = [ 0 L 2 c .delta. 2 .THETA. 2 c .theta. 2 - s .theta. 2 + 1
.THETA. 2 2 - L 2 s .delta. 2 ( s .theta. 2 - 1 ) .THETA. 2 0 - L 2
s .delta. 2 .THETA. 2 c .theta. 2 - s .theta. 2 + 1 .THETA. 2 2 - L
2 c .delta. 2 ( s .theta. 2 - 1 ) .THETA. 2 1 L 2 .THETA. 2 s
.theta. 2 + c .theta. 2 .THETA. 2 2 0 0 - s .delta. 2 c .delta. 2 c
.theta. 2 0 - c .delta. 2 - s .delta. 2 c .theta. 2 0 0 - 1 + s
.theta. 2 ] ( 70 ) ##EQU00038##
matrix .SIGMA. transforms the desired twist into gripper frame
.SIGMA. = [ R 3 , curr T 2 0 0 R 3 , curr T 2 ] ( 71 )
##EQU00039##
And selection matrix .GAMMA. defines a desired subtask
.GAMMA. = [ 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 ] . ( 72 )
##EQU00040##
In this example, matrix .GAMMA. selects the third, fourth, and
fifth rows of the Jacobian matrix allowing to control only 3 DoF:
translation along z.sub.3 and tilting about x.sub.3, y.sub.3. As a
result, we only invert a square 3.times.3 matrix and control only
the desired DoFs.
[0174] Finally, the augmented desired configuration space velocity
vector is given as follows:
{dot over (.PSI.)}.sub.m=[{dot over (q)}.sub.ins00{dot over
(.theta.)}.sub.2{dot over (.delta.)}.sub.2].sup.T (73)
[0175] (3) Projection Matrices:
[0176] Depending on the operational mode (insertion or
telemanipulation) the configuration space .PSI. can be partitioned
in directions in which motion needs to be controlled, T, and
directions in which compliance needs to be controlled, C. Similar
to wrench and twist systems in the hybrid motion/force control
formulation, these two spaces, T and C, are orthogonal and their
sum returns the configuration space .PSI.(T+C=.PSI.). It is,
therefore, possible to construct two projection matrices, .OMEGA.
and .sup.-.OMEGA., that project desired configuration space
velocities into the motion space T and into the compliance space
C:
.OMEGA.=T(T.sup.TT).sup..dagger.T.sup.T (74)
.OMEGA.=I-.OMEGA. (75)
where T is a 5.times.m matrix in which its columns define a base of
T and superscript .sup..dagger. indicate the pseudo-inverse. For
example, in the case of compliant insertion, configuration
variables q.sub.ins, .delta..sub.2, and .delta..sub.3 will be
position-controlled while variables .theta..sub.2 and .theta..sub.3
will be compliance-controlled allowing the segment to adapt to the
shape of the nasal conduit. Hence, matrix T is defined as:
T = [ 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ] T ( 76 ) ##EQU00041##
[0177] (4) Actuation Compensation:
[0178] The desired joint variables are computed using a theoretical
kinematics model and a model-based actuation compensation.
[0179] Motion Force Control of Multi-Backbone Continuum Robots
[0180] This section presents a general framework for hybrid
motion/force control of continuum robots. The examples described
below assume that the kinematic model is known and that the robot
is equipped with a device for measuring or estimating environmental
interaction forces. This information can be provided by a dedicated
miniature multi-axis force sensor placed at the tip of the robot or
by an intrinsic wrench estimator of the types. A multi-backbone
continuum robot with intrinsic wrench estimation capabilities is
used to demonstrate the hybrid motion/force control framework. The
following assumptions specifically apply to multi-backbone
continuum robots with intrinsic force sensing. Different
assumptions are necessary for different types of robots but the
fundamental control framework remain invariant.
[0181] Assumption 1: The continuum robot bends in a circular shape
and gravitational forces are negligible.
[0182] Assumption 2: The continuum robot is able to sense actuation
forces via load cells placed between each actuation line and its
actuator.
[0183] Assumption 3: The geometric constraint is known and the
environment is rigid. This information is used for both the hybrid
motion/force controller and the intrinsic wrench estimation.
However, because of the innate compliance of continuum robots and
as demonstrated by experimental results, an exact knowledge of the
geometric constraint is not necessary.
[0184] FIG. 23 shows an example of a hybrid motion/force controller
for rigid-link robot manipulators. Motion and force controllers
provide two separate control signals that are first projected into
allowable motion and force directions (using projection matrices
.OMEGA. and .OMEGA..sup.-) and then merged into the joint-space of
the manipulator. Motion commands are first transformed into
task-space wrenches using the inverse of the inertia matrix B and
then into joint-torque commands using the transpose of the Jacobian
matrix J. The desired joint-torque vector .GAMMA. is then added to
the feedback linearization term that compensates for
non-linearities (e.g. coriolis/centrifugal and gravitational
effects) and fed to the joint-space PID controller.
[0185] FIG. 24 shows a hybrid motion/force controller for
multi-backbone continuum robots with intrinsic force sensing. As in
the example of FIG. 23, two separate controllers produce motion and
force commands that are then projected into allowable motion/force
directions using projection matrices .OMEGA. and .OMEGA..sup.-.
Hence, the motion and force commands are projected into the
configuration space velocity of the continuum manipulator using
respectively the inverse of the Jacobian matrix J.sub.t.psi. and a
composition of the transpose of the matrix J.sub.t.psi. and the
configuration space compliance matrix K.sup.-1. The configuration
space compliance matrix provides a mapping between the change in
task-space wrenches and the change in configuration space
velocities. Finally, using the closed-form inverse position of the
manipulator, the theoretical desired joint-space positions
q.sub.des are obtained. Similar to the feedback linearization term
in the example of FIG. 23, an additional term is obtained through
the statics of the continuum manipulator and the stiffness of the
actuation lines. This term include both an actuation compensation
term for the applied wrench at the tip of the robot and a
compensation term for extension and friction in the actuation
lines.
[0186] Control Architecture--A First-Order Model of Contact:
[0187] Hybrid motion/force control aims at controlling the
interaction of two rigid bodies by decoupling control inputs into
allowable relative motions and constraining wrenches. The control
inputs are specifically generated to maintain contact between the
two bodies by simultaneously generating motion directions that do
not break the contact state and apply the desired interaction
wrench. For example, in the case of frictionless point contact (as
illustrated in FIGS. 25A and 25B), body 1 can translate along axes
x and y, rotate about axes x, y, and {circumflex over (z)}, and
apply a force along axis {circumflex over (z)}, i.e. the normal
vector between body 1 and body 2.
[0188] Regardless of the type of contact, it is possible to define
two dual vector sub-spaces, one containing wrenches F.sup.6, and
the other containing motion screws M.sup.6. The reciprocal product
between these two spaces is defined as the rate of work done by a
wrench f.sub.i acting on a motion screw m.sub.j. In the case of
rigid bodies, under conservative contact, the bases of the two
spaces must satisfy the reciprocity condition. At any given time,
the type of contact between the two rigid bodies defines two vector
sub-spaces: a n-dimensional space of normal vectors N.OR
right.F.sup.6 and a (6-n)-dimensional space of tangent vectors T.OR
right.M.sup.6, where n is the degree of motion constraint. The
bases of these two spaces can be defined by a 6.times.n matrix N
and a 6.times.(6-n) matrix T. The columns of N and T are
respectively any n linearly independent wrench in N and any 6-n
linearly independent motion screw in T. As a consequence of the
reciprocity condition and the contact constraint, the scalar
product of any column of N with any column of T is zero, i.e.
N.sup.TT=0 (77)
[0189] For example, for the contact constraint depicted in FIGS.
25A and 25B, matrices N and T are given by:
N = [ 1 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 ]
, T = [ 0 0 1 0 0 0 ] ( 78 ) ##EQU00042##
Although in many situations matrices N and T are simply the
composition of canonical vectors in IR.sup.6 and can be obtained by
inspection, this is not the case when dealing with complex
interaction tasks and multi-point contacts.
[0190] The efficacy of hybrid motion/force control stems from its
ability to produce adequate motion and force control inputs that do
not violate the reciprocity condition (Equation 77). The
reciprocity of the control inputs is enforced by projecting each
control input into the correct subspace by pre-multiplication of a
projection matrix. We, therefore, define two projection matrices
.OMEGA..sub.f and .OMEGA..sub.m that project any control input into
consistent wrenches and twists respectively:
.OMEGA..sub.f=N(N.sup.TN).sup.-1N.sup.T
.OMEGA..sub.m=T(T.sup.TT).sup.-1T.sup.T=I-.OMEGA..sub.f. (79)
[0191] According to the classical formulation of hybrid
motion/force control, the frame-work inputs are defined by the
desired twist
t.sub.ref=[v.sub.ref.sup.T.omega..sub.ref.sup.T].sup.T and the
desired interaction wrench at the operational point
q.sub.ref=[f.sub.ref.sup.Tm.sub.ref.sup.T].sup.T.
[0192] At any given time, the wrench error is given by:
w.sub.e=w.sub.ref-w.sub.curr (80)
where w.sub.curr is the sensed wrench.
[0193] The output of the motion controller, t.sub.des, is first
projected onto the allowed motion space M.sup.6 via projection
matrix .OMEGA..sub.m as defined in equation (79) and then onto the
configuration space velocity of the manipulator via the
pseudo-inverse of the Jacobian matrix
J.sup..dagger..sub.t.psi.:
{dot over
(.PSI.)}.sub.m=J.sub.t.psi..sup..dagger..OMEGA..sub.mt.sub.des
(81)
One the other hand, the output of the motion controller w.sub.des
can be obtained with a conventional PI control schemes:
w.sub.des=K.sub.f,pw.sub.e+K.sub.f,i/w.sub.e, (82)
[0194] Similarly to the motion controller, the desired wrench wdes
is first projected into the allowed wrench space F.sup.6 via
projection matrix .OMEGA..sub.f as defined in equation (79), then
into the generalized force space via the transpose of the Jacobian
matrix J.sub.t.psi.. The generalized force is the projection of the
task-space wrench acting at the end effector into the configuration
space of the manipulator. Finally into the configuration velocity
space of the continuum manipulator via the configuration space
compliance matrix K.sub..psi..sup.-1:
{dot over
(.PSI.)}.sub.f=K.sub..psi..sup.-1J.sub.t.psi..sup.T.OMEGA..sub.fw.sub.des-
. (83)
[0195] Using equations (81) and (83) the motion and force control
inputs are merged into a configuration space velocity vector
.PSI.{dot over ( )} that does not violate the contact
constraint:
{dot over (.PSI.)}={dot over (.PSI.)}.sub.m+{dot over
(.PSI.)}.sub.f. (84)
[0196] Equation (84) provides the desired configuration of the
continuum manipulator that accomplishes both the desired motion and
force tasks without violating the contact constraint. The next step
in the control architecture is to generate joint-level commands to
accomplish the desired tasks. Because of the flexibility of the
arm, actuation compensation is required for both achieving the
desired pose and applying the desired wrench at the end-effector.
Using the statics model, the desired actuation forces .tau..sub.des
are obtained as follows:
.tau..sub.des=(J.sub.q.psi..sup.T).sup..dagger.(.gradient.U-J.sub.t.psi.-
.sup.T.OMEGA..sub.fw.sub.ref) (85)
[0197] According to a model-based actuation compensation scheme,
the desired joint-space compensation is given by:
.epsilon.=K.sub.a.sup.-1.tau..sub.des (86)
where K.sub.a.sup.-1 designates the compliance matrix of the
actuation lines.
[0198] By combining equations (84) and (86), the desired
joint-space control input is obtained as follows:
q.sub.des={circumflex over (q)}.sub.des(.PSI.)+.epsilon. (87)
where q.sub.des(.PSI.) is the joint's positions associated with
configuration (.PSI.) according to the theoretical inverse position
analysis.
[0199] The hybrid motion/force control architecture for continuum
robots with intrinsic force sensing is summarized in FIG. 24. Two
independent controllers provide high-level control commands for
both motion and force regulation. These commands are projected into
adequate force-vectors and velocity-vectors that do not violate the
contact constraint and then they are projected into the
configuration space of the continuum manipulator. The motion
commands are projected using the inverse of the Jacobian matrix
J.sub.t.psi., while the force commands are projected using first
the transpose of the Jacobian matrix J.sub.t.psi. and then the
configuration space compliance matrix K.sub..psi..sup.-1. The
configuration space stiffness matrix describes how a change in the
external wrench direction and magnitude is related to a change in
the configuration of the continuum manipulator. Once the desired
configuration space vector is obtained, a model-based actuation
compensation scheme is used to produce the required joint-space
displacements for both configuration compensation and the acting
wrench. Note that, equation (85) includes two terms: the first
compensate for actuation line extension, friction, and model
uncertainties, while the second term is a feedforward term that
produces the desired wrench at the end-effector by actuation the
manipulator in joint-space. Finally, the control input q.sub.des is
fed into the low-level PID controller. In the case of continuum
robots with intrinsic wrench estimation, there are two additional
components: the friction compensation/estimation block, and the
wrench estimator.
[0200] Compensation Uncertainties:
[0201] During control of the real continuum robot there will be a
deviation .lamda. between the desired actuation force vector
.tau..sub.des and the sensed actuation force vector t.sub.curr.
This deviation is due to friction and extension in the actuation
lines, perturbation of the bending shape from the ideal circular
configuration, and geometric and static parameters. Thus, the
sensed actuation force vectors is as follows:
.tau..sub.curr=.tau..sub.des+.lamda.. (88)
[0202] Force deviation .lamda. is a function of the configurations
.PSI. of the manipulator and the joint-space velocities Several
methods have been proposed in order to characterize friction and
uncertainties. A discrete Dahl model for friction compensation in a
master console may be implemented. A Dahl friction model can also
be implemented into the control architecture of steerable
catheters. Friction estimation and compensation can also be used in
the control of concentric tube robots. A non-linear regression via
Support Vector regressors might also be used to compensate for
lumped uncertainties in multi-backbone multi-segment continuum
robots.
[0203] Wrench Estimation:
[0204] Continuum robots with actuation force sensing allow to
estimate external wrenches acting at the tip. Using a statics
model, the external wrench acting at the tip of the continuum robot
is given by:
w curr = ( J t .psi. T ) .dagger. ( .gradient. U - J q .psi. T (
.tau. curr - .lamda. ) ) + ( I - ( J t .psi. T ) .dagger. J t .psi.
T ) .eta. where .eta. = D .dagger. F T S e ( w se - ( J t .psi. T )
.dagger. ( .gradient. U - J q .psi. T ( .tau. curr - .lamda. ) ) )
D = ( I - ( J t .psi. T ) .dagger. J t .psi. T ) T S e ( I - ( J t
.psi. T ) .dagger. J t .psi. T ) S e = [ ( n ^ t .times. n ^ n ) (
n ^ t .times. n ^ n ) T 0 3 .times. 3 0 3 .times. 3 I 3 .times. 3 ]
W se = [ c t n ^ t + n t n ^ n 0 3 .times. 1 ] . ( 89 )
##EQU00043##
S.sub.e and W.sub.se contain a priori knowledge of the contact
constraint and guide the estimation algorithm. The a priori
knowledge is defined by contact normal vector {circumflex over
(n)}.sub.t, and contact tangential vector n.sub.t. These two
vectors define the plane in which the sensible component of the
external wrench lies.
[0205] Experimental Results:
[0206] In order to validate the proposed framework, the control
algorithms were implemented on the one-segment multi-backbone
continuum robot of FIGS. 26A and 26B. The continuum segment is able
to translate along the {circumflex over (z)}.sub.0 axis providing
the end-effector with a total of 3 DoFs: q.sub.ins and angles
.theta. and .delta.. The continuum segment is equipped with three
load cells that monitor the forces acting on the three secondary
backbones. Estimation of the x and y component of a force acting at
the tip of the manipulator is possible. In the remainder of this
section, multiple experiments are presented to validate the
efficacy and usefulness of the proposed framework. While the motion
controller always controls motion in z direction, the force
controller switches between regulating a force in the x or y
directions depending on the task.
[0207] The remainder of this section is organized as follows.
First, the experimental setup is presented. The continuum robot
manipulator and required equipment are described. Second, the
estimation of uncertainties such as friction and un-modeled
actuation forces are presented. Finally, three experiments are
presented: force regulation in the x and y direction, shape
estimation, and stiffness characterization.
[0208] The Continuum Manipulator--Direct Kinematics:
[0209] The surgical slave of FIGS. 26A and 26B is the composition
of a linear stage and a two Degree of Freedom (DoF) continuum
manipulator. The continuum segment has three push-pull backbones
that allows for bending its end disk in space. For the ease of
presentation, in the remainder of this section, the kinematics and
statics of the manipulator is summarized. Six coordinate systems
are defined (Figure V.4): 1) world frame { x.sub.0, y.sub.0,
z.sub.0}, 2) segment's base disk frame { x.sub.1, y.sub.1,
z.sub.1}, 3) segment's bending plane frame { x.sub.2, y.sub.2,
z.sub.2}, 4) segment's end-disk frame { x.sub.3, y.sub.3, z.sub.3},
5) segment's gripper frame { x.sub.4, y.sub.4, z.sub.4}, and 6)
segment's tool frame { x.sub.5, y.sub.5, z.sub.5}. The position,
p.sub.0, and the orientation, R.sup.0.sub.3, of the tool in
reference frame is given by:
p.sub.5.sup.0=p.sub.1.sup.0+R.sub.1.sup.0p.sub.3.sup.1+R.sub.4.sup.0p.su-
b.5.sup.4
R.sub.5.sup.0=R.sub.4.sup.0=Rot(-.delta.,{circumflex over
(z)})Rot(.theta.,{circumflex over (y)})Rot(.delta.,{circumflex over
(z)}) (90)
where R.sub.1.sup.0=I, p.sub.1.sup.0=[0 0 q.sub.ins].sup.T,
p.sub.3.sup.2=[0 0 d].sup.T, d is the distance between frames {4}
and {5} (see FIGS. 26A and 26B), p.sub.3.sup.1 is the position of
the end disk of the continuum segment as in II.2,
.THETA.=.theta.-.theta..sub.0, L is the nominal length of the
continuum segment, .theta..sub.0=.pi./2, and operator Rot(o, w)
returns a rotation of angle o about axis w. The pose of the tool is
uniquely defined by configuration variables .theta., .delta., and
q.sub.ins. We, therefore, define the configuration space vector
.PSI..epsilon.IR.sup.3 as:
.PSI.=[.theta..delta.q.sub.ins].sup.T (91)
[0210] In order to achieve configuration .PSI., the secondary
backbones of the continuum robot (i=1, 2, 3) are shortened or
lengthened as follows:
q.sub.i=.tau. cos(.delta..sub.i)(.theta.-.theta..sub.0) (92)
where .delta..sub.i=.delta.+(i-1).beta., .beta.=2/3.pi., and r is
the radial distance of each secondary back-bone from the
centrally-located backbone. Similarly to the configuration space,
we define the augmented joint-space vector q.xi.IR.sup.4 as:
q=[q.sub.1q.sub.2q.sub.3q.sub.ins].sup.T. (93)
[0211] Differential Kinematics:
[0212] The tool twist t.sup.T=[v.sup.T.omega..sub.jT] is given
by:
t=J.sub.t.psi.{dot over (.PSI.)} (94)
where .PSI.{dot over ( )} is the rate of change of the
configuration space vector .PSI. and
J t .psi. = [ - dc .THETA. c .delta. - Lc .delta. c .theta. .THETA.
- A 2 .THETA. 2 - ds .THETA. s .delta. + A 1 .THETA. 0 dc .THETA. s
.delta. + Ls .delta. c .theta. .THETA. + A 1 .THETA. 2 - ds .THETA.
c .delta. + A 2 .THETA. 0 ds .THETA. + Lc .theta. .THETA. 2 - Ls
.theta. .THETA. 0 1 - s .delta. c .delta. c .theta. 0 - c .delta. -
s .delta. c .theta. 0 0 - 1 + s .theta. 0 ] , ( 95 )
##EQU00044##
where c.sub.y=cos(.gamma.), s.sub.y=sin(.gamma.), A.sub.1=L sin
(.delta.)(sin(.theta.)-1), and A.sub.2=L cos(.delta.)
(sin(.theta.)-1). Similarly, the configuration space velocities are
related to the configuration space velocities as:
q . = J q .PSI. .PSI. . where J q .PSI. = [ r cos ( .delta. 1 ) - r
.THETA.sin ( .delta. 1 ) 0 r cos ( .delta. 2 ) - r .THETA.sin (
.delta. 2 ) 0 r cos ( .delta. 3 ) - r .THETA.sin ( .delta. 3 ) 0 0
0 1 ] . ( 96 ) ##EQU00045##
[0213] Statics:
[0214] Using virtual work arguments we can derived the following
first order linear relationship:
J ~ t .PSI. T w e + J ~ q .PSI. T .tau. = .gradient. U where J ~ t
.PSI. = J t .PSI. [ 1 0 0 1 0 0 ] , J ~ q .PSI. = J q .PSI. [ 1 0 0
1 0 0 ] , and .gradient. U = E Y I [ .THETA. L + i = 1 3 .THETA. L
+ q i - .THETA. 2 2 r i = 1 3 cos ( .delta. i ) ( L + q i ) 2
.THETA. 3 2 r i = 1 3 sin ( .delta. i ) ( L + q i ) 2 ] . ( 97 )
##EQU00046##
[0215] Configuration Space Stiffness:
[0216] The configuration space stiffness is used for compliant
motion control. Let the wrench acting on the end disk that is
projected into the configuration space of the k.sup.th segment be
given as the generalized force vector
f.sub.(k)=J.sub.t.psi..sub.(k).sup.Tw.sub.e,(k). (98)
Applying equation (97) to the generalized force expression, the
i.sup.th row of the generalized force f.sub.(k) can be written
as
f.sub.t=.gradient.U.sub.i-[J.sub.q.psi..sup.T].sub.i.tau.=.gradient.U.su-
b.i-[J.sub.q.psi..sup.[i]].sup.T.tau.. (99)
where J[i] denotes the ith column of Jq.psi..
[0217] For small perturbations from an equilibrium configuration,
the stiffness of the individual continuum segment can be posed in
the configuration space as
.delta.f=K.sub..psi..delta..psi. (100)
where the stiffness is given by the Jacobian of the generalized
force with respect to configuration space perturbation. Thus, the
elements of K.sub..psi. are given by:
.differential. f i .differential. .psi. j = [ K .psi. ] ij =
.differential. .differential. .psi. j [ .gradient. U i - [ J q
.psi. [ i ] ] T .tau. ] . ( 101 ) ##EQU00047##
[0218] The elements of the stiffness matrix (equation (101)) can be
expanded as
[ K .psi. ] ij = [ H .psi. ] ij - [ .differential. .differential.
.psi. j ( J q .psi. [ i ] ) ] T .tau. - [ J q .psi. [ i ] ]
.differential. .tau. .differential. .psi. j . ( 102 )
##EQU00048##
The first term of the configuration space stiffness, H.sub..psi.,
is the Hessian of the elastic energy of the segment given by
H .psi. = [ .differential. 2 U .differential. .theta. 2
.differential. 2 U .differential. .theta. .differential. .delta.
.differential. 2 U .differential. .delta. .differential. .theta.
.differential. 2 U .differential. .delta. 2 ] . ( 103 )
##EQU00049##
The axial stiffness along the length of a given actuation line can
then be expressed as
1 k = 1 k c + 1 k b ( 104 ) ##EQU00050##
where
k c = E Y A L c ##EQU00051## and ##EQU00051.2## k b = E Y A L b ,
##EQU00051.3##
and A denotes the cross-sectional area of the backbone.
[0219] For continuum robotic systems with remote actuation 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.sub.c>>L.sub.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
.differential. .tau. .differential. q = K .alpha. where ( 105 ) K q
.apprxeq. [ E Y A L c 0 0 0 0 0 0 E Y A L c ] .di-elect cons. m
.times. m ( 106 ) ##EQU00052##
[0220] Expanding terms by applying the chain rule and using the
instantaneous inverse .differential..tau. kinematics,
.differential..sub..psi. is given by
.differential. .tau. .differential. .psi. = .differential. .tau.
.differential. q .differential. q .differential. .psi. .apprxeq. K
.alpha. J q .psi. ( 107 ) ##EQU00053##
[0221] The configuration space stiffness therefore reduces to
[ K .psi. ] ij = [ H .psi. ] ij - [ .differential. .differential.
.psi. j ( J q .psi. [ i ] ) ] T .tau. - [ J q .psi. [ i ] ] T K
.alpha. J q .psi. [ j ] ( 108 ) ##EQU00054##
[0222] Kinematic and Static Parameters:
[0223] The numerical value of the kinematic and static parameters
defined in the previous sections are reported in the table of FIG.
27. r is the radial distance from the secondary backbone to the
central backbone, L is the nominal length of the continuum segment,
.beta. is the division angle between secondary backbones, E.sub.Y
is the Young's modulus of the NiTi backbones, od is the outer
diameter of the backbones, and
I = .pi. 64 o d 4 ( 109 ) ##EQU00055##
is the second area moment of each backbone.
[0224] Estimation Uncertainties:
[0225] Uncertainties are due to: 1) deviation of the kinematic and
static parameters from the nominal ones reported in the table of
FIG. 27, 2) unmodeled friction along the actuation lines, 3)
statics and kinematics modeling assumptions, and 4) actuation unit
assembly. The robot used in this work is a 0 5 mm continuum robot
with a cone that re-route the actuation lines and an actuation line
length of more than 300 mm. In this work, we calibrate actuation
uncertainties by populating a look-up table that depends on the
configuration of the continuum manipulator.
[0226] Methods:
[0227] The continuum manipulator of FIGS. 26A and 26B is commanded
to scan its workspace between .delta.=.pi./2 and .delta.=-.pi./2
with intervals of 5.degree..about.=0.098 rad. At each .delta.
configuration the manipulator is bent from .theta.=.pi./2 to
.theta.=0 with constant velocity and accomplishment time of 10 s.
The discrepancy between the expected actuation forces and the
sensed actuation forces was recorded.
[0228] Results: FIGS. 28, 29, and 30 show the actuation force
errors on backbone one, two, and three respectively. The graphs
show a plot of the actuation force error versus the bending angle
.theta. for different values of .delta..
[0229] FIG. 28 shows the actuation force error, .tau..sub.e1,
associated with the first backbone. The graphs shows how the
actuation force error depends on both .theta. and .delta. and,
therefore, on the configuration of the manipulator. The error on
backbone one reaches its minimum at .delta.=.pi./2 and
.delta.=-.pi./2. In these two configurations backbone one is
neither extended nor shortened and the load on it is, therefore,
zero. In all other configurations, the error mainly increases as a
function of .gamma. but also as a function of .delta..
[0230] FIG. 29 shows the actuation force error, .tau..sub.e2,
associated with the second back-bone. In this case, the minimum is
reached for .delta.=-.pi./6. In this configuration the second
backbone is neither extended nor shortened and, therefore, the
resultant load is zero. In all other configurations, .tau..sub.e2,
is non-zero and mainly depends on the bending angle .theta.. In
this case, it is possible to see the effect of friction along the
line. For configurations with a negative .delta., backbone two is
extended while for configurations with positive .delta., backbone
two is shortened. This phenomenon is not seen in backbone one
because between .delta..epsilon.[-.pi./2, .pi./2] the backbone is
only shortened.
[0231] FIG. 29 shows the actuation force error, .tau..sub.e3,
associated with the third backbone. Similarly to backbone
n.degree.2, because of symmetry, the actuation force error
.tau..sub.e is zero at .delta.=.pi./6, negative for
.delta..epsilon.(.pi./6, .pi./2], and positive for
.delta..epsilon.[-.pi./2, .pi./6).
[0232] Several methods can be used to characterize friction and
uncertainties. In this work, we populated a lookup table using the
data shown in FIGS. 28, 29, and 30. The table has a size of
36.times.18 and linearly interpolates over 6 and 0 with increments
of 5.degree.. In the current implementation, the table is only
populated with data associated with negative .theta.{dot over ( )},
i.e. the continuum segment bends from .theta.=.pi./2 (straight
configuration) to .theta.=0. For this reason, the compensation is
not effective when the motion is reversed; thus affecting the force
estimation. Although it is possible to produce velocity-dependent
lookup tables, this is excluded from the scope of this work.
[0233] Force Regulation:
[0234] This section presents force regulation experimental results.
The goal of these experiments is to demonstrate the ability of the
controller to regulate a predetermined force in both x and y
directions at different configurations. As illustrated in FIG. 31,
the experimental setup consists of a 5 mm continuum robot, an
interaction probe with a spherical tip of 0 5 mm radius, an
aluminum block, and an ATI Nano 17 for ground truth.
[0235] Methods:
[0236] In order to test the force estimation and force controller
efficacy in the x direction the continuum robot is bent to the
following three configurations: 1) .theta.=80.degree.,
.delta.=0.degree., 2) .theta.=60.degree., .delta.=0.degree., and 3)
1) .theta.=40.degree., .theta.=0.degree.. At each configuration, a
reference force magnitude of 5 gr, 10 gr, and 15 gr were commanded.
Data from the force estimator and the ATI Nano 17 were compared and
the rise time and steady state error computed.
[0237] In order to test the force estimation and force controller
efficacy in the y direction the continuum robot is bent to the
following configurations: 1) .theta.=80.degree., .delta.=0.degree.,
2) .theta.=60.degree., .delta.=0.degree., and 3) 1)
.theta.=40.degree., .delta.=0.degree.. At each configuration, a
reference force magnitude of 10 gr is tested. Data from the force
estimator and the ATI Nano 17 are compared and the rise time and
steady state error are computed.
[0238] Results: FIGS. 34A, 34B, and 34C; FIGS. 35A, 35B, and 35C;
and FIGS. 36A, 36B, and 36C show the step response for force
magnitudes 5 gr, 10 gr, and 15 gr in the x direction at
.theta.=80.degree., .theta.=60.degree., and .theta.=40.degree.
respectively. Each graph shows the time history of three
quantities: desired force (black), sensed force by the ATI Nano 17
(red), and sensed force by the intrinsic force estimator (blue).
These results are summarized in the table of FIG. 32.
[0239] FIGS. 37A, 37B, and 37C show the step response for a force
magnitude of 10 Gr in y direction at .theta.=80.degree.,
.theta.=60.degree., and .theta.=40.degree.. The results are
summarized in the table of FIG. 33.
[0240] The experimental results demonstrate both that the hybrid
motion/force control is able to regulate forces of different
magnitudes in different directions and that the accuracy depends on
the configuration of the manipulator. Steady state error in the x
direction between the force sensed by the ATI Nano 17 and the
intrinsic force estimator are mainly due to residual un-modeled
uncertainties as the more energy is stored into the system
(.theta.=.pi./2 zero energy, .theta.=0 maximum energy). These
uncertainties are static and dynamic friction, bending shape, and
extension/contraction of the actuation lines. Steady state error in
the y direction is due to uncompensated uncertainties for side
motions .delta..sup../=0 and the much more compliance of the
continuum manipulator in the .delta. direction at this particular
configuration.
[0241] As shown in FIGS. 28, 29, and 30, uncertainties are
symmetric depending on the configuration of the backbones. The
efficacy of the intrinsic force estimator and, therefore, the force
controller is definitely weaker in the 6 direction. However,
depending on such configuration, the x and y directions are
consecutively affected in different ways. It is worth noting that
the experiments were conducted on a non-calibrated robot in which
geometric parameters (nominal length, distance between backbones,
bending shape) and static parameters (Young's modulus, actuation
line stiffness, and second moment area of all backbones) were set
to the nominal value. Furthermore, the intrinsic force estimator
could also undergo proper calibration as in any force/torque
sensor. However, this is beyond the scope of this work in which we
aim to demonstrate force control schemes for continuum robots and
their use in several scenarios.
[0242] Force Regulated Shape Estimation:
[0243] This section presents environment's shape estimation
experimental results. The goal of these experiments is to
demonstrate simultaneous force and motion control. The user is able
to control the end-effector while free of contact and engage the
force control in pre-determined directions when in contact with the
environment. FIGS. 38A and 38B illustrate the experimental setup
which consists of a 5 mm continuum robot, a diamond-shape silicon
phantom, an Ascension Technologies trakSTAR 2 with flat transmitter
placed under the silicone phantom, and a Force Dimension Omega
7.
[0244] The experimental setup of FIGS. 38A and 38B performs shape
estimation of a silicon diamond-shaped extrusion (shown in FIG.
38A). The base plane of the extrusion is placed at approximately 18
mm from the origin along the x direction (shown in FIG. 38B). The
user actively controlled motion of the probe in the y and z
directions. The force controller regulates a force with a magnitude
of 0.1 N in the x direction.
[0245] Methods: The 5 mm continuum robot is equipped with a plastic
probe having a 5 mm radius sphere at the interaction point. The
sphere reduces the impingement of the probe into the soft tissue. A
0.9 mm magnetic coil is delivered through one of the robot's
working channels and secured to the probe. A silicon diamond-shaped
extrusion is placed in a YZ plane at a distance of approximately 18
mm from the robot's reference frame as shown in FIGS. 38A (top
view) and 38B (side view).
[0246] The end-effector's motions in the y and z directions were
commanded via a Force Dimension Omega 7 with 3-axis force feedback.
Position commands were sent over the Local Area Network (LAN) using
User Datagram Protocol (UDP) at 100 Hz. The end-effector's position
was acquired at 125 Hz using the Ascension Technology trakSTAR 2
and sent over the LAN using UDP at 100 Hz. The high-level motion
and force controllers and the wrench estimator run at 200 Hz while
the low-level joint controller run at 1 kHz. Switching between full
motion control and hybrid motion/force control was enabled using
the 7th axis of the Omega 7 (gripper).
[0247] The continuum robot, under full motion telemanipulation
mode, is guided to reach a point on the silicon phantom. Once
hybrid motion/force control is enabled, the continuum robot
autonomously regulates a force of -0.1 N in the x direction.
Position data of the probe were only collected when the sensed
force in the x direction was smaller or equal to -0.05 N and the
hybrid motion/force controller was engaged. These conditions
ensured that each data point was actually on the surface of the
silicon phantom. Switching between full motion/force control and
hybrid motion/force control allowed to cover a workspace of 18
mm.times.30 mm.times.40 mm.
[0248] Ground truth shape data was obtained using a second probe
equipped with another 0 0.9 mm magnetic coil as shown in FIG. 39.
The probe was manually swept over the silicon phantom and data
collected. The use of an identical probe equipped with a similar
sensor ensures comparison of data having the same accuracy,
repeatability, noise, and uncertainties.
[0249] Position data from the electro-magnetic tracker were stored
and linearly interpolated using Matlab function grid data. This
function interpolates the surface at query points on the YZ plane
and returns the interpolated value along the x direction. Multiple
data points are automatically averaged during the
interpolation.
[0250] Results: The estimated shape using the continuum robot is
shown in FIG. 40 while the ground truth data obtained with the
manual scan is shown in FIG. 41. The error between the estimate
shape and the ground truth shape is presented in FIG. 42. As
expected, the maximum deviation is found at the boundaries of the
robot's workspace along the y axis. In fact, in order for the
continuum segment to stretch along the y axis the bending angle
.theta. decreases considerably and the .delta. angle decreases
(positive y ) or increases (negative y ). As shown in the force
regulation experiments reported in the previous section, the
sensitivity of the force estimation in the x direction decreases
considerably while the sensitivity in the y direction increases.
Although the controller is unable to regulate the requested amount
of force due to the force estimator, the continuum manipulator does
not lose contact with the surface. This phenomenon is not seen at
the boundaries of the workspace along the z direction because the
insertion stage provides most of the displacement.
[0251] The very small inertia of the continuum manipulator allows
the force controller to maintain contact with the unknown surface
without perfect knowledge of the tangent and normal vectors at the
contact point. These experiments demonstrate the efficacy of the
proposed hybrid motion/force control scheme to perform independent
motion and force regulation. The intrinsic compliance of the
continuum manipulator also makes the impact phase stable allowing
for safe and smooth transitions between full motion control and
hybrid motion/force control.
[0252] Stiffness Imaging:
[0253] This section presents experimental results on stiffness
characterization of soft tissues using the proposed hybrid
motion/force controller. The goal of the experiment is to build a
stiffness image of the surrounding environment using both position
and force data. One way to build such image is to repetitively scan
the surface applying different force and recording the displacement
of each point between scans. FIG. 43 illustrates the experimental
setup which includes a 5 mm continuum robot, a plastic probe with a
spherical tip of 5 mm radius, a 0.9 mm electro-magnetic sensor, and
a silicone phantom with embedded rubber tubes.
[0254] Methods:
[0255] Five consecutive surface scans were completed under hybrid
motion/force control applying respectively 5 gr, 10 gr, 15 gr, 20
gr, and 25 gr. After selecting the desired force magnitude, the
operator guides the robot to cover the designated area (as shown in
FIG. 43). A rubber tube is embedded at approximately 155 pixels
(px) from the base of the robot. Using the ruler shown in FIG. 43,
it is possible to define a scaling factor between image pixels and
mm. In this case, 39 px=10 mm. Using this scaling ratio, the
position of the rubber tube is identified at approximately 39.7 mm
from the base of the manipulator. The operator was asked to scan a
rectangular area of approximately 20.times.30 mm.
[0256] Position data during each scan were recorded from the
electro-magnetic sensor placed at the tip along with the sensed
force in the x direction and smoothed by spline interpolation.
Because of the position RMS of the Ascension Technologies trakSTAR
2 it was chosen to use the first and the last scan in order to
obtain a better noise/signal ratio.
[0257] Results:
[0258] The estimated surfaces under 5 Gr force control and 25 Gr
force control are shown in FIGS. 44 and 45. FIG. 44 shows a color
map of the estimated surface with points between 10 mm and 13 mm
along the x direction. FIG. 45 shows a color map of the estimated
surface with points between 12 mm and 15 mm along the x
direction.
[0259] In order to characterize the difference in stiffness along
the surface (not the absolute stiffness) a linear spring-model is
used. FIG. 46 shows the estimated stiffness values across the
scanned area. The graph shows a peak of 0.3 N/mm at approximately
40 mm from the base of the robot. The estimated stiffness decreases
to 0.25 N/mm, 0.2 N/mm, and 0.15 N/mm respectively along both
positive z direction and negative z direction. Lower stiffness is
estimated at the boundaries of the scanned surface (z=55 and z=25).
FIG. 43 shows that the rubber tube lays along the y axis at
approximately 155 px=39.7 mm from the base of the robot. The
stiffness map clearly identifies high stiffness along that same
axis as shown in FIG. 46.
[0260] 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.
* * * * *
References