Professional Documents
Culture Documents
Approved by the
Examining Committee:
Examining Committee:
Dr. John T. Wen, Thesis Adviser
Dr. Stephen J. Derby, Co-Thesis Adviser
Dr. Daniel Walczyk, Member
Dr. Harry E. Stephanou, Member
c Copyright 2002
by
Hyosig Kang
All Rights Reserved
ii
CONTENTS
LIST OF TABLES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii
LIST OF FIGURES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . viii
ACKNOWLEDGMENT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiv
ABSTRACT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xvi
1. INTRODUCTION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.1
1.2
Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.3
Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.4
Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2. LITERATURE REVIEW . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.1
2.2
2.3
2.3.1
2.3.2
3.2
3.3
EndoBot Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.1.1
3.1.2
Current Prototype . . . . . . . . . . . . . . . . . . . . . . . . 12
Forward Kinematics . . . . . . . . . . . . . . . . . . . . . . . 15
3.2.2
Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . . 17
3.2.3
Jacobian Formulation . . . . . . . . . . . . . . . . . . . . . . . 18
3.2.3.1 Geometric Jacobian . . . . . . . . . . . . . . . . . . 18
3.2.3.2 Analytical Jacobian . . . . . . . . . . . . . . . . . . 19
3.2.4
Singularity Analysis
. . . . . . . . . . . . . . . . . . . . . . . 19
Lagrangian Formulation . . . . . . . . . . . . . . . . . . . . . 20
3.3.2
3.3.3
3.4
3.5
3.5.2
3.5.3
3.5.4
4.2
4.3
4.4
4.5
4.6
4.3.1
4.3.2
Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . 51
4.3.3
4.3.4
Problem Simplication . . . . . . . . . . . . . . . . . . . . . . 52
4.3.5
Problem Transformation . . . . . . . . . . . . . . . . . . . . . 52
Knot Tying . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
4.4.1
Ligation Algorithm 1 . . . . . . . . . . . . . . . . . . . . . . . 54
4.4.2
Ligation Algorithm 2 . . . . . . . . . . . . . . . . . . . . . . . 57
4.4.3
Ligation Algorithm 3 . . . . . . . . . . . . . . . . . . . . . . . 61
Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . 62
4.5.2
4.5.3
4.6.1.2
Hybrid Automaton . . . . . . . . . . . . . . . . . . . 71
4.6.2
4.6.3
4.6.4
Development Environment . . . . . . . . . . . . . . . . . . . . 75
5. MOTION CONTROL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
5.1
5.2
5.2.2
5.2.3
6. SHARED CONTROL
. . . .
System
. . . .
. . . .
. . . .
86
86
87
91
94
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
6.1
6.2
6.3
6.4
6.3.1
Stability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
6.3.2
6.3.3
Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
6.3.4
6.4.2
Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113
6.4.3
7.2
7.3
7.2.2
7.4
7.5
7.3.1
7.3.2
7.3.3
7.4.2
7.5.2
8. CONCLUSIONS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147
8.1
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147
8.2
LITERATURE CITED
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150
APPENDICES
A. INVERSE KINEMATICS USING SUBPROBLEMS . . . . . . . . . . . . . 158
B. LAGRANGIAN FOR A ROBOT MANIPULATOR . . . . . . . . . . . . . 159
B.1 Kinetic Energy of a Robot Manipulator . . . . . . . . . . . . . . . . . 159
B.2 Potential Energy of a Robot Manipulator . . . . . . . . . . . . . . . . 160
C. LEAST SQUARE METHOD . . . . . . . . . . . . . . . . . . . . . . . . . 161
D. PERSISTENT EXCITATION AND OBSERVABILITY GRAMIAN . . . . 163
E. DIFFEOMORPHISM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165
vi
LIST OF TABLES
3.1
3.2
3.3
3.4
5.1
5.2
6.1
6.2
6.3
6.4
7.1
7.2
7.3
7.4
7.5
vii
LIST OF FIGURES
3.1
3.2
3.3
3.4
Translational stage. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.5
3.6
3.7
3.8
3.9
3.10
3.11
3.12
Generation of PRBS. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.13
3.14
3.15
3.16
3.17
3.18
3.19
3.20
4.1
4.2
4.3
4.4
Components of a suture. . . . . . . . . . . . . . . . . . . . . . . . . . . 49
viii
. . . . . . . . . . . . . . 34
4.5
4.6
4.7
4.8
4.9
4.10
4.11
4.12
4.13
4.14
4.15
4.16
4.17
4.18
4.19
4.20
4.21
4.22
4.23
4.24
4.25
4.26
4.27
4.28
4.29
4.30
ix
4.31
4.32
5.1
5.2
Friction compensation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
5.3
5.4
5.5
5.6
5.7
5.8
5.9
5.10
5.11
6.1
6.2
6.3
6.4
6.5
6.6
6.7
Experimental result of task space shared control with constrained line. . 110
6.8
6.9
6.10
6.11
6.12
6.13
6.14
6.15
6.16
6.17
6.18
Actual trajectory of the end-eector in the task space for the circular
constraint . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
7.1
7.2
7.3
7.4
7.5
7.6
7.7
7.8
7.9
7.10
7.11
7.12
7.13
7.14
7.15
7.16
Root locus under explicit PI force control with the active damping Kv =
1000 and Kp = 1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136
7.17
7.18
Root locus of the time delayed system under explicit PI force control
with Kp = 10. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
7.19
Root locus of the time delayed system under explicit PI force control
with active damping Kv = 1000 and Kp = 10. . . . . . . . . . . . . . . . 139
xi
7.20
7.21
7.22
7.23
7.24
7.25
7.26
7.27
7.28
7.29
B.1
C.1
C.2
E.1
E.2
E.3
. . . . . . . . . . . . . . . . . . . . . . . . . . . . 166
xii
To my wife Kyunga
and my daughters Dahyun and Hannah
with all my love...
xiii
ACKNOWLEDGMENT
With the completion of this thesis I am again at a crossroad in my life. The time I
have spent here has been very fruitful and many people came into my life to inspire
and encourage this eort.
I would rst like to express my truly gratitude to my advisor, Dr. John T.
Wen. John is an exceptionally distinguished researcher with enthusiasm, but he
stepped down to my level in order to collaborate with me. He was always there to
answer my questions clearly and helped to make robotics and control fun for me.
Throughout the last ve years, he provided not only the promising research subject
and motivation to challenging problems, but also the way of thinking and the way
of proceeding research. In every sense, non of this work would have been possible
without him. He has been a real partner and I will always keep unforgettable
memories of discussions and friendship we have had together.
I would like to acknowledge Dr. Harry E. Stephanou, director of the Center
for Automation Technologies, who provided me the opportunity of my journey at
Rensselaer Polytechnic Institute and wide latitude to explore interesting research
areas. I would also like to thank the other members of my doctoral committee,
Dr. Stephen J. Derby and Daniel Walczyk, both of whom provided many helpful
comments and suggestions.
My colleagues at the Center for Automation Technologies have created a very
pleasant atmosphere to work in. In particular, I want to thank Wooho Lee and
Jeongsik Sin for being my friend and for making me ponder many things about
my life. I rmly believe that we can work together again. I also want to thank
Ben Potsaid, for our fruitful conversations during many trips to Rio de Janeiro and
Alaska. I would like to thank my long time friend, YoungCheol Park, and my seniors
Youngkee Ryu, Hyunggu Park, Byunghee Won, and Hyungsoo Lee. Our occasional
telephone chats and emails have always provided a welcome relief from the lonely
journey.
I thank my parents for their love, support, and understanding through these
xiv
years. I also thank to my daughters, Dahyun and Hannah, who provided the joy of
life and the ability to make peace with chaos. They prayed for me and brought me
great comfort during times of extreme stress.
My nal, and most heartfelt, acknowledgement must go to my wife, Kyunga,
who supported my decision to embark in graduate studies despite the signicant
changes in our life and provided stability to our family. Her support, encouragement,
and companionship has turned my journey through doctoral studies here into a
pleasure. She is my everlasting love.
Delight yourself in the LORD and he will give you the desires of your heart
(Psalm 37:4)
xv
ABSTRACT
Open surgery traditionally involves making a large incision to visualize the operative
eld and to access human internal tissues. Minimally invasive surgery (MIS) is an
attractive alternative to the open surgery whereby essentially the same operations
are performed using the specialized instruments designed to t into the body through
several pencil-sized holes instead of one large incision. It can minimize trauma and
pain, decrease the recovery time thereby reducing the hospital stay and cost, but
also oers more technical diculties to surgeons.
Despite the lack of dexterity and perception, all surgeries are moving toward
MIS due to the benets to patients. However, the demand on surgeons is much
higher during suturing, which is the primary tissue approximation method. Suturing is known as one of the most dicult tasks in MIS and consumes a signicant
percentage of the operating time. Despite the important role and technical challenge
of suturing in MIS, there is little research on suturing.
Motivated by these observations, a new surgical robotic system, which we have
named EndoBot, is developed in this research. The focus of the research is the development of the EndoBot controller that is capable of a range of MIS operations
including manual, shared, and supervised autonomous operations. Due to the challenge of the suturing task, the particular emphasis is placed on its automation. The
suturing operation is decomposed as knot forming, knot placement, and tension
control. New algorithms are developed and implemented for each subproblem and
integrated for the completely autonomous knot tying. To ensure safe operation,
a discrete event controller allows interruption by the surgeon at any moment and
continuation with autonomous operation after the interruption.
xvi
CHAPTER 1
INTRODUCTION
1.1
can improve precision, lter human motion tremor, extend human reach into the
body, and reduce the risk of infection. Surgical robots can be classied based on
the planning strategy: model-based robotic surgery and nonmodel-based robotic
surgery; or based on the level of surgical invasiveness: open surgery and minimally
invasive surgery.
Model-based surgery uses the geometric models obtained from scanned images
using the computer tomography (CT) or magnetic resonance imaging (MRI) during
the pre-operation stage. Typically, this requires a registration process in which preoperative model is matched with intra-operative model. If the registration process
can be accurately performed, the robotic surgery becomes as o-line robot motion
planning problem as in a CAD-CAM system. This approach is applicable to surgeries of hard tissues such as spine surgery, neurosurgery, hip replacement surgery,
total knee replacement surgery, plastic surgery, and eye surgery. Quality of the
surgery depends to a large extent on accuracy of the model.
Nonmodel-based robotic surgery typically deals soft tissues whose model is
either not available or not useful during operations due to the tendency of tissues
oating in the body or changing in shape. Most surgeries involving a body organ are
nonmodel-based. Surgeons either navigate a hand-held robot directly or manipulate
a input device in the case of teleoperation. The man/machine interface plays a
key role because surgeons are directly responsible for manipulating the surgical
robots. In this surgery, the surgeons expertise is the key factor on the quality of
surgery. Force feedback and cooperative control to guide surgeons can also improve
the surgical results in nonmodel-based robotic surgeries.
Surgery traditionally involves making a large incision to expose the operation
sites and to access human internal tissues. This is referred to as the open surgery.
1
2
The incision needed to allow surgeons to visualize the eld tends to delay patient
recovery and causes most of the pain.
Minimally invasive surgery (MIS), also known as laparoscopic surgery in the
abdominal cavity, arthroscopic surgey in the joints, and thoracoscopic surgery in
the chest, has became increasingly an attractive alternative to open surgery. In
MIS, the same operations are performed with the specialized instruments which are
designed to t into the body through the several pencil-sized holes instead of one
large incision. By eliminating the signicant incision, MIS has many advantages
over conventional open surgery. It can minimize trauma and pain, and decrease the
recovery time thereby reducing the hospital stay and cost for patients.
However, compared to open surgery, MIS oers greater challenges to surgeons.
Instead of looking directly at the part of the body being treated, surgeons monitor
the procedures via a two-dimensional video monitor without the three-dimensional
depth information. Due to the inherent kinematic constraints at the incision points,
the motions of MIS instruments are restricted to four degrees of freedom. Despite of
lack of dexterity and perception, all surgeries are moving toward MIS to give more
benets to patients at the expense of a more stressful environment to surgeons.
The demands to surgeons for dealing with these technical diculties are higher
during the suturing task, which is the primary tissue approximation method and
has been known as one of the most dicult tasks in MIS operations and uses a
signicant percentage of operating time.
1.2
Problem Statement
Several robotic systems have been developed for minimally invasive surgery
[10, 11, 12]. There are also a few commercial systems available on the market such
as AESOP and ZEUS (Computer Motion), daVinci (Intuitive Surgical), and Neuromate (ISSS/immi). It is important to note that in the various surgical robots
described above, the surgical procedures are still completely performed by the human surgeon High skill level procedures such as suturing and precise tissue dissection
continue to depend on the expertise of surgeons, and surgeons commands are mimicked by the robotic devices through computer control. Despite the important role
3
and technical challenge of suturing in MIS, there has been little research eort reported an automated robotic suturing. Most results on robotic surgery focus on the
development of the robotic system and leave the suturing task to the surgeon.
This thesis presents the development of a new surgical robotic system, which
we named the EndoBot, for MIS operations. The EndoBot is designed for the collaborative operations between surgeons and the robotic device. Surgeons can select
the device to operate completely in manual mode, collaboratively where motion of
robotic device in certain directions are under computer control and others under
surgeons manual control, or autonomously where the complete robot system is under computer control and surgeons supervision. Furthermore, the robotic tools can
be quickly changed from a robotic docking station, allowing dierent robotic tools
to be used in operations.
For automated robotic suturing using the EndoBot, the suturing task needs
to be analyzed and the algorithms for knot tying task need to be developed to deal
with a exible suture. In addition, knot placement and knot tension control should
also be considered.
Motivated by the above observations, the goal of this research is to develop a
robotic system that can collaboratively perform laparoscopic procedures with surgeons, conduct certain tasks autonomously to reduce the strain on surgeons, remove
the variability of surgeons training levels, and enhance the system eciency by
decreasing the operation time.
1.3
Contributions
The major contributions of this research are summarized here.
Robotic system for minimally invasive surgery. A new surgical robot system,
called the EndoBot, for MIS operation is developed and the dynamic parameters are identied experimentally.
Suturing task analysis and implementation. The suturing task is decomposed
into ve subtasks and the technical diculties on each task are analyzed. Knot
placement problem is formulated, and the sliding condition is proposed. The
4
knot tying algorithm is presented and implemented with the EndoBot.
Supervisory controller. In order to guarantee the safe operation of suturing task in non-deterministic environments, a supervisory controller is implemented based on the hybrid dynamic system framework.
Shared control. Shared control using articial constraints is proposed and
experimentally veried.
Tension Control. A base sensor method is proposed to measure the tension in
suture. A hybrid force/position control is implemented to eectively regulate
the suture tension while achieving the desired direction.
1.4
Outline
This document is organized in the following manner. In Chapter 2, related
work is reviewed. Chapter 3 provides the kinematics and dynamics analysis of new
surgical robot and discusses the identication method for dynamic parameters. In
Chapter 4, a suturing task analysis is presented with the high level control architecture. Chapter 5 discusses the motion controller design and the experimental results
and the shared control is presented in order to augment the human capability in
Chapter 6. Chapter 7 provides a hybrid force/position control strategy to eectively
regulate the tension with the experimental results. Finally, Chapter 8 summarizes
this document, outlines the future work.
CHAPTER 2
LITERATURE REVIEW
To the best of the authors knowledge, this thesis is unique by being the rst work in
autonomous robotic suturing specically for minimally invasive surgery. However,
there have been many research results related to various aspects of robotic surgery.
This section reviews these results.
2.1
surgical robot system for total hip replacement surgery (RoboDoc, ISS Inc.) was
developed [1]. The robot systems for total knee arthroplasty [2, 3, 4, 5], eyesurgery
[6], and neurosurgery [7, 8] have been developed. In recent years, there have been
many research eorts in minimally invasive surgery.
Manipulator design for laparoscopic surgery Due to the inherent spherical
motion requirement in laparoscopic surgery, several research on mechanisms
has been carried out. Three types of mechanisms for generating the spherical
movements were compared in [9] and they carried out the optimization on
reachability with the concentric multi-link spherical joint mechanism. Funda
and his colleagues [10] compared two manipulator designs (Parallel Linkage
Remote Center of Motion and Hopkins-IBM Assistant Robot) in terms of several criteria such as safety, ergonomics, ease of control, and structural factors.
However, these mechanisms are suitable for telemanipulation, not for direct
navigation. Due to the fulcrum eect at the incision points, the motions of
conventional laparoscopic instruments are restricted to only four degrees of
freedom and hence signicantly the dexterity is reduced [14]. There has been
a research on increasing the dexterity by adding two degrees of freedom to
the end of the laparoscopic tool. This is particularly useful in the cardiac operation where the motion of the tool stem is limited. The laparoscopic wrist
mechanism driven by tendons was invented in [11] and it is now commercially
5
6
available on the market as the da Vinci Surgical System (Intuitive Surgical
Inc.). The six degrees of freedom slave manipulator was developed in [12],
and the parallel mechanism was chosen for gross motion to increase rigidity
and the tendon actuated millirobot was added. In [13], the workspace of
laparoscopic extender with exible stems was formulated. They performed
the optimization on design of exible stems by dening the dexterity measure,
which is the ratio of areas of dexterous workspace and reachable workspace.
The redundant wrist with parallel structure, which has three degrees of freedom, was presented in [15], and they carried out an optimal design of the
mechanism.
Laparoscopic suturing Suturing is one of the most dicult tasks and takes a
signicant percentage of operating time and involves in complex motion planning. The general description on manual laparoscopic suturing can be found
in [16, 17]. As a direct result of constraints in laparoscopic surgery, there is an
extended learning curve that surgeons must go through to gain the required
skill and dexterity. Furthermore, there is a great deal of variability even among
trained surgeons. As demonstrated in [21], time-motion studies of laparoscopic
surgery have indicated that for the operations such as suturing, knot tying,
suture cutting, and tissue dissection, the operation time variation between
surgeons can be as large as 50%. In suturing, it was noted that the major
dierence between surgeons lies in the prociency at grasping the needle and
moving it to a desired position and orientation, without slipping or dropping
it. Cao et al. The motion analysis on the suturing task using conventional
needle holders was performed in [21], and they broke down into ve basic
motions such as reach and orientation, grasp and hold, push, pull, and release.
The teleoperator slave system with a dexterous wrist for minimally invasive
surgery was developed in [20], and they demonstrated the suturing task with
direct vision. More recently, the knot tying simulation for surgical simulation
with a spline of linear spring model was proposed in [85]. The future trends
in laparoscopic suturing can be found in [19]. However, these all publications
did not address the issues on the autonomous robotic suturing.
2.2
robotic applications. Surgical procedures that require the ne motions and take
long time can be good candidates to enhance the surgeons ability. Typically, they
include teleoperation, shared control, collaborative control, supervisory control, and
autonomous control. One class of surgical robotic devices that has been proposed
to assist surgeons in laparoscopic surgeries is based on the concept of teleoperation
where surgeons can perform the operation remotely while robots are operated on the
patient under the surgeons commands. The robot motion is slaved via mechanical
linkages or computer control to the surgeons movements [22, 24, 25, 26]. In [27, 28],
the surgeons view of the operation may be further enhanced by vision system to
create a virtual presence. Funda et al.
to compute the optimal approximation to the desired motion, given the existing
constraints by the extended Jacobian technique.
When robots face the unstructured environments and precise motions are
needed, autonomous planning is not available. Pure teleoperation causes fatigues to
the human operators by demanding more concentration. In this case, higher level
of assistance to human operators is desirable rather than pure telemanipulation.
Shared control, as the name implies, enables the human operators and the computer control the manipulator in parallel. This would be useful in the guidance and
alignment tasks. The shared control schemes have been discussed in [30, 31]. In
[29], exoskeletons to amplify the strength of human operators was developed, and
they performed the analysis of the stability of cooperative human-robot manipulator
control systems. Many research eorts have been carried out on shared control in
the space telerobotics where time-delays can aect the task performance [32, 33],
but little literature has been published in the surgical context. In [35, 34, 36],
the problem of cooperative manipulation to improve positioning and path following
abilities of human was discussed. They compared the assisted mode and the augmented mode experimentally. With well structured environments, augmentation
would be maximized by giving the degree of autonomy to the robotic systems under
the humans supervision. This paradigm is referred to as supervisory control in [30].
8
During the operation, the human supervises each evolution of task with intervening
capability. This framework can be the basis of automating surgical tasks.
2.3
2.3.1
trol schemes in motion control. The selection of the motion controller may depend
on the required task, sensor availability, computational power, and the knowledge
of the values of the dynamic parameters. The basis in motion control of robot
manipulators is the PD (Proportional and Derivative) plus gravity compensation
scheme at the joint level. In [66], it was proved that the PD control with computed
feedforward terms was locally exponentially stable by using an energy-motivated
Lyapunov fuction. In [57], the stability of PD control with gravity compensation
by shaping the potential energy of closed loop system and injecting the required
damping was proved. This controller provides the globally asymptotic stability for
the regulation problem [58] and it is the most widely used in industrial robots. This
controller may be practically used in tracking control applications at the expense of
degrading the tracking performance. For the general trajectory tracking problem,
computed-torque control has been shown to have the globally asymptotic stability
[59, 60, 47, 62]. The basic idea is to transform the nonlinear dynamic system into
a linear one by cancelling the nonlinearities of the robot dynamics with a nonlinear
inner feedback linearization loop [63]. The beauty of this approach is that we can
apply many linear control schemes in designing the outer feedback loop. Linear PD
or PID control is the common choice and linear quadratic optimal control is also
used for designing the outer feedback loop [64, 65]. For robot systems, which do not
admit exact feedback linearization, the outer loop controller must be robust in order
to handle the uncertainties. The robust control problem translated into the optimal
control problem in [67]. They designed an optimal LQ (Linear Quadratic) regulator
for the linearized system with uncertainties reected in the performance index. A
survey on robust control of robots is given in [68, 69]. In many robot systems, only
a part of states can be measured, but the state feedback controller needs the knowl-
9
edge of positions and velocities. The immediate solution to get the velocity signals
is to numerically dierentiate the position signals. Inherently numerical dierentiating is very sensitive to the noises because of the improper function characteristics.
When the system dynamics is given, the most eective method to estimate all states
can be to design a dynamic observer. The computed-torque controller with linear
observer was proposed in [70], and they proved the exponential stability for the
robot system, which has only position measurement. The performance comparison
on linear and nonlinear observers was given in [71] with the linear state feedback
algorithm for a two-link manipulator.
2.3.2
sential for successful execution of tasks. Force control has been a research subject
for many years, and various control schemes have been proposed. Basically, the
existing force control strategies can be classied into two categories. The rst group
aims at performing indirect force control by controlling the relationship between the
manipulator position and the interaction force. Compliance control and impedance
control can be grouped into the indirect force control scheme. In [72], Hogan proposed impedance control to achieve a desired dynamic behavior while compliance
control can achieve the static behavior in [73, 74]. These schemes are suitable for
tasks where accurate force regulation is not required and the force measurement is
not available. In order to regulate the exact force, the parallel position/force control
scheme was introduced in [79]. Parallel position/force control gives the priority to
force errors over position errors by closing an outer force control loop. The second
group aims at controlling the positions and the interacting forces simultaneously by
decomposing the task space into the directions of the admissible motions and the
interacting forces. Hybrid force/motion control, proposed in [75], belongs to this
scheme. This control with consideration on the dynamics of robot manipulators was
extended in [76, 77]. The state of the art of force control for robot manipulators
is surveyed in [78]. For explicit force control, integral force control with robustness
enhancements in order to reduce the initial impact force was proposed in [80]. In
10
[81], experimental results for explicit force control with an active damping term were
presented.
CHAPTER 3
MODELING AND IDENTIFICATION
3.1
3.1.1
EndoBot Design
Manipulator Design Requirements
In laparoscopic surgery, a patients abdomen is inated with carbon dioxide
(CO2 ) through a needle to lift the abdominal wall away from the organs so as to
expose and access the operating eld, and then three pencil-sized small holes are
punctured on abdominal wall for tting of laparoscopic instruments and camera as
shown in Figure 3.1. Due to the requirement of minimizing the tear of the incision
point, a manipulator for MIS has an inherent kinematic constraint (a spherical joint
at the incision point). This constraint is a primary design consideration. This
section describes the mechanical design issues of the EndoBot.
11
12
also can be implemented with tool center position technique with conventional
industrial robot programming.
Backdrivability Backdrivability is needed in the cooperative control mode for the
surgeon to move the manipulator directly. It is also critical for removing the
robot from the patient in case of unanticipated power shutdown or emergency.
Rapid tool changability Surgical operations typically require many surgical tools.
Faster tool change can reduce the overall operation time.
3.1.2
Current Prototype
This section gives a brief overview on the EndoBot manipulator, which was
built by Bernard [18]. By comparing various mechanisms based on the above design
constraints, a simple spherical joint mechanism with semi-circular arches is found
to be a suitable choice because it gives a compact and light design and has minimum number of joints and a mechanical fulcrum constraint, and can be operated in
multiple control mode such as manual mode, shared control mode, and autonomous
mode.
The EndoBot is capable of four degrees of freedom of motion and consists of
two parts as shown in Figure 3.2: rotational stage and translational stage. The
rotational stage creates the spherical motion based on a pair of motor-driven semicircular arches for yaw and pitch, and a sleeve that can generate rolling motion.
The translational stage carries the specic tool, which is actuated pneumatically
and is translated along the tool z-axis. All four actuators are DC servo motors and
linear motion for translational stage is performed by a lead screw. All axes are
back-drivable when the motors are not energized. The center of rotation of the arch
joints is at the incision point of the patients abdominal wall. Therefore, motion
of the EndoBot will not cause tearing of the incision point. Each tool can easily
slip through the locking hole in the translational stage and the sleeve and be locked
via a locking pin. Figures 3.33.4 show the closeups on the semicircular arches and
translational stage carrying grasper tool.
Figure 3.5 shows two manually operated EndoBots and Figure 3.6 shows the
closeup of two EndoBots with grasper and stitching tool.
13
14
15
3.2
3.2.1
Forward Kinematics
The product of exponentials formula [38] is used to derive the kinematics equa-
tion. Consider the following manipulator with spherical joint shown in Figure 3.7.
It consists of four joints - three revolute joints and one prismatic joint, and the base
and tool frame are shown in Figure 3.7. Let hi 3 be a unit vector, which species the direction of rotation or translation and qi be the angle of rotation or
linear displacement and pi,i+1 3 be the position vector between ith and (i + 1)th
link frame. The transformation between base frame, E0 , and tool frame, ET , at
p0T (0) =
I33
0
0 1
I33
(3.1)
0
where
,
0
1 0 0
=
0 1 0 .
0 0 1
(3.2)
16
Then the hi and pi,i+1 can be expressed in base frame as follows:
1
0
0
0
h1 =
0 h2 = 1 h3 = 0 h4 = 0
(3.3)
0
0
0
=
0 p34 = 0 p4E = 0 .
q4
(3.4)
R0 (q) = R04 = R12 R01 R23 R34 = eh2 q2 eh1 q1 eh3 q3 eh4 q4
(3.5)
p0T (q) = p04 = p01 + R01 p12 + R12 R01 p23 + R12 R01 R23 p34 = eh2 q2 eh1 q1 eh3 q3 p34 . (3.6)
The individual exponentials are given by
1 q1
h
=
0
2 q2
h
s1
c1
c1
eh4 q4 = I33 .
(3.8)
c3 s3 0
=
c3 0
s3
(3.7)
0 s2
1 0
s2 0 c2
0 s1
c2
=
0
eh3 q3
(3.9)
1
(3.10)
where, ci and si are abbreviation of cos(qi ) and sin(qi ) respectively. Expanding the
17
terms in the product of exponentials formula results in
c2 s3
c2 c3
R0T (q) =
s1 s2 c3 + c1 s3
s2
s1 s2 s3 + c1 c3
c1 s2 c3 + s1 s3
c1 s2 s3 + s1 c3
s2 q4
p0T (q) =
s1 c2 q4
s1 c2
(3.11)
c1 c2
(3.12)
c 1 c 2 q4
We parameterize the joint vector q as yaw angle (rotation about the inertial x axis),
pitch angle (rotation about the inertial y axis), roll angle (rotation about the body
z axis), and translation (extension of the tool along the body z axis). Since each
EndoBot only has four degrees of freedom, we can dene the conguration of tool
frame as xT = (rT (q), (q3 )) 4 , where rT (q) = p0T (q) is the Cartesian position
of the tool frame and (q3 ) = q3 is the roll angle. Then the forward kinematics can
be viewed as the mapping xT = f (q):
s2 q4
q3
s1 c2 q4
rT
=
xT =
=
z
c 1 c 2 q4
3.2.2
(3.13)
Inverse Kinematics
Due to the spherical joint at the incision point, the inverse kinematics can be
18
z
x
q4 = rT =
y .
z
(3.14)
q2 = tan1 (
xc1
).
z
(3.15)
q1 = tan1 (
y
).
z
(3.16)
Roll angle, q3
q3 = .
3.2.3
(3.17)
Jacobian Formulation
3.2.3.1
Geometric Jacobian
Let and be the angular velocity vector and linear velocity vector of the
end-eector. The vector Jacobian of the above manipulator in inertia frame can be
expressed in as follows:
h1
R01 h2
R02 h3
h1 (p1T )0 R
01 h2 (p2T )0 R02 h3 (p3T )0 R03 h4
(3.18)
19
where,(piT )0 = R0i pi,i+1 +R0i+1 pi+1,i+2 +R04 p4T . Calculating the associated elements
yields the geometric Jacobian:
c c q
1 2 4
s1 c2 q4
J =
s1 s2 q4
c1 s2 q4
s2
c1
s1 c2
s1
c1 c2
0
3.2.3.2
s21 c2 q4 + c21 c2 q4
s2
s1 c2
c1 c2
(3.19)
Analytical Jacobian
The analytical Jacobian can be computed by direct dierentiation of the forward kinematics equation. Let f : n p represent a mapping from joint space
to task space, then the forward kinematics is represented by xT = f (q) and by the
chain rule:
x T =
where the matrix Ja (q) =
f
q
q = Ja (q)q,
q
(3.20)
In general, the analytical Jacobian, Ja (q), is dierent from the geometric Jacobian,
J(q), for orientation part. In case of the EndoBot, since the roll angle can be available in direct form, the analytical Jacobian and geometric Jacobian are identically
same as follows
c1 c2 q4
J =
s1 c2 q4
0
3.2.4
s21 c2 q4 + c21 c2 q4 0
s1 s2 q4
c1 s2 q4
0
s2
0 s1 c2
.
0 c1 c2
(3.21)
Singularity Analysis
Since the Jacobian is a function of the conguration q, the singularity can be
occurred at some congurations with which the rank of J is decreased, i.e., two or
more of the columns of J becomes linearly dependent. When det(J) = 0, the robot
20
is in a singular conguration, implying that there is certain task space motion that
cannot be achieved (and, conversely, small task space motion would result in large
joint space motion). In order to get a reliable operation, it is necessary to identify
singular congurations of a manipulator, if they exist. To analyze the rank of the
Jacobian matrix, consider the determinant given by
det(J) = c2 q42 .
(3.22)
From (3.22), it is easy to nd that the determinant of the Jacobian vanishes when
q2 = ,
2
q4 = 0.
(3.23)
3.3
(3.24)
where L(q, q)
= T (q, q)P
Lagrangian Formulation
The lagrangian function of a robot manipulator with n joints is thus given by
1
L(q, q)
= qT M (q)q P (q).
2
(3.25)
21
Thus, we have
L
= M (q)q
q
d L
( ) = M (q)
q + M (q, q)
dt q
1 T
L
P (q)
=
( q M (q)q)
.
q
q 2
q
(3.26)
(3.27)
(3.28)
The general equations of motion for a rigid robot manipulator can be written as
1
P (q)
M (q)
q + M (q, q)
q ( qT M (q)q)
+
= ,
q 2
q
C(q,q)
q
(3.29)
G(q)
q 2
P (q)
G(q) =
.
q
(3.30)
(3.31)
(3.32)
= 0 for all x Rn .
xT (M (q) 2C(q, q))x
(3.33)
22
Linearity in the dynamic parameters - Through the reparametrization, we can
nd a parameter vector X that is linear in system dynamics. This property
plays an important role on experimental identication of dynamic parameters.
In view of this linearity property, we can write the equation of motion in (3.32)
more compactly
= T (q, q,
q)X,
(3.34)
23
assumed to be diagonal:
Iic
Iix
=
0
0
Iiy
0
.
Iiz
(3.35)
Then, the total kinetic and potential energies of the EndoBot are given by
1 T T
1
q J1 (q)M1 J1 (q)q + qT J2T (q)M2 J2 (q)q
2
2
T
c
T
P = m1 g (p1 )0 + m2 g (p02 + pc2 )0 .
T =
(3.36)
(3.37)
M31
M (q) =
M32 M33
0
(3.38)
M44
where
M11 = (I1x + I2x )c22 + (I1z + I2z )s22 + m2 (l2c + q4 )2 c22
M12 = M21 = (I1z + I2z )s1 s2
M13 = M31 = (I1z + I2z )s2
M22 = (I1x + I2x )c21 + (I1z + I2z )s21 + m2 (l2c + q4 )2 c21
M23 = M32 = (I1z + I2z )s1
M33 = I1z + I2z
M44 = m2 .
(3.39)
24
+ (I1z + I2z )(c1 q2 q3 + c2 q2 q3 s1 c2 q22 )
C2 (q, q)
q = 2[I1z + I2z I1x I2x m2 (l2c + q4 )2 ]c1 s1 q1 q2 + 2m2 (l2c + q4 )c21 q1 q4
(I1z + I2z )(c1 s2 q12 + c1 q1 q3 + c2 q1 q3 + c2 s2 q12 )
+ [I1x + I2x + m2 (l2c + q4 )2 ]c2 s2 q12
C3 (q, q)
q = (I1z + I2z )(c2 c1 )q1 q2
C4 (q, q)
q = m2 (l2c q12 + c21 q22 ).
(3.40)
3.4
(3.41)
sion and performance. It could lead to stick-slip motions, static positioning errors,
or limit cycle oscillations. Systematic lubrication should be implemented from the
design stage to reduce frictional disturbance. Sti (high gain) position control can
reduce the frictional positioning error at the expense of possibly destabilizing effect. Integral action is also a common alternative to reduce the steady-state error
in constant-velocity application. When the friction behavior can be predicted , it
may be compensated by feedforward compensation as in Figure 3.9.
25
The friction compensation requires the knowledge of the friction model and
corresponding parameters. Friction is usually modelled as a map between an instantaneous friction force and velocity. Typical friction models are shown in Figure 3.10:
a) f (q)
= Fc sgn(q)
(3.42)
b) f (q)
= Fc sgn(q)
+ Fv q
(3.43)
=
c) f (q)
Fs sgn(q)
if q = 0
Fc sgn(q)
+ Fv q
otherwise
Fs sgn(q)
if q = 0
qs |s
Fc sgn(q)
+ (Fs Fc )e|q/
+ Fv q
otherwise.
d) f (q)
=
(3.44)
(3.45)
The friction parameters could be estimated by observing the static frictionvelocity curve. In [40, 39], a method of nding the friction parameters, Fc and Fv ,
experimentally was proposed. A series of constant input torques is applied to each
joint and the correlating steady state joint velocities are recorded. Fitting the steady
26
state velocities versus input torques would then give the friction parameters.
3.5
quire the knowledge of the dynamic parameters. In most cases, computing the
numerical value of the dynamic parameters directly from the design data of the
mechanical structure is not feasible due to the complexity. One way to remedy
this problem is to identify the dynamic parameters experimentally. Identication of
robot dynamic parameters is basically grey-box approach requiring the measured input and output signals with a given system structure resulting from analytic (whitebox) modeling. Due to the fact of linearity in the dynamic parameters the equation
of motion can be given with measurements of (t) and (t):
(t) = T (t) + (t),
(3.46)
where n is the vector of unknown parameters and (t) is the unmeasured noise
signal. More compactly with m measurements at given time instants t1 , ..., tm ,
27
+ ,
=
(t1 )
(t )
2
.
.
.
(tm )
(t1 )
(t )
2
.
.
.
(3.47)
(tm )
1 (t1 )
(t )
1 2
2 (t1 ) n (t1 )
2 (t2 ) n (t2 )
where mn is the matrix in which each row represents regressor at each time,
m is a vector with (tk ), and m is a vector with (tk ). Since is unknown,
the exact solution to cannot be obtained. One attractive solution may be to seek
to nd so as to minimize the norm of . Selecting Euclidean norm results in the
standard least squares problem:
.
min
(3.48)
2
2
(3.49)
2J
2
T = 0
= T
(3.50)
= T > 0.
(3.51)
28
Due to convexity of the cost function, the solution is a global optimum and it can
be obtained from the necessary condition with the gradient of J:
1 T .
LS
= (T )
(3.52)
may be how to excite the systems. It is desirable to have systematic guidelines for
designing the input signals. The following factors are considered in this research.
1. Persistent excitation
Let be the regression matrix and unknown parameter vector. The degree
of persistence of excitation should be high enough with respect to the number
of unknown parameters in such that the contribution of each element in
can show up separately and consequently can be identiable with invertible
Matrix T is often called the persistent matrix or the input correlation
T .
matrix. Therefore, persistence of excitation can be checked with either the
input trajectory or the persistent matrix.
A regression matrix is called persistent exciting if there exist positive constants 1 , 2 and such that
a I
t+
t
2 I.
T d
(3.53)
29
condition can make the regressor uncorrelated, persistent exciting also can be
obtained with the nonsingular persistent matrix. Clearly, this invertibility of
T is the requirement of existence of least square solution. The main objective
of the input design is to make regressor vectors as uncorrelated among them
as possible.
2. Condition number of regressor
This property is a quality tag to the input signals to represent the conditioning
of the persistent excitation. The condition number c(A) of the nonsingular
matrix A is dened
c(A) =
1
,
n
(3.54)
where 1 ...n > 0 are the singular values of matrix A. This number also
represents the sensitivity of estimation to noise and unmodeled dynamics.
results in better
The smaller condition number of regression matrix c(T )
identication.
3. Constraints on input amplitudes
Large values of input signals in amplitude are desirable to provide better
signal-to-noise ratio (SNR) in the identication process. However, there exist
the constraints on the input signals in most applications. The input signals
u(k) should be bounded so as to avoid the possibility introducing the nonlinearity from input saturation:
umin u(k) umax ,
(3.55)
where umin and umax represent the minimum and maximum values of input
signals respectively. It is desirable to have a way to impose this constraint
into input design process.
4. Input spectrum
The spectrum of the input signal determines the frequencies where the exciting
energy is allocated. The input signal with the well distributed spectrum over
the frequency bands where the system operates mostly results in high quality
30
of parameters. In this respect a white input signal may be the ideal input
signal since it excites all frequencies with same weights. However, most of real
systems have a limited bandwidth as in Figure 3.11. It makes no sense to put
a lot of energy beyond this bandwidth since the systems act as low-pass lters.
In this respect, the input signal swith a user specied design parameters to
allocate the input powers over desired frequency bands will be advisable.
There also exist many eorts seeking to nd an optimal input trajectory for identifying robot dynamic parameters. One of the common criteria is the minimization
of the condition number of the regression matrix. This minimization problem is difcult to solve and usually is a very time consuming procedure. For this reason, the
existing o-the-shelf input signals are investigated for experimental identication.
Summed multisinusoidal signal
A sum of dierent sinusoidal signals is eective with relatively simple dynamic
model requiring small number of parameters. It has a discrete set of point
spectrum and can provide excitation at certain frequencies. Conceptually it
is simple to generate the signals and has the advantage of long duration of
excitation signal at each frequency. However, it demands many trial and error
iterations to select the appropriate discrete frequencies and relative amplitudes
of each frequency. The quality of identication can vary with the selected
frequencies and amplitudes.
Chirp signal
A chirp signal is a single sinusoid with a time varying frequency:
u(t) = sin(2f (t)t),
(3.56)
where f (t) is the time varying frequency. The common choice of f (t) may be
a linear or logarithmic function of time. The advantage of chirp signal is that
the system can be excited over all specied frequency bands. One shortcoming
may be the trade-o between the duration of excitation at each frequency and
31
the overall identication time. The overall identication time can be reduced
with the fast sweep signal from the lowest to the highest frequency, which
results in relatively short duration of the excitation at each frequency.
PRBS(Pseudo random binary sequence)
A pseudo random binary sequence is a signal that shifts between binary level
in a certain sequence. The dierence from RBS (random binary sequence) is
a periodic and deterministic characteristics and the exact sequence can be regenerated with same starting point. The PRBS can be generated by means of
shift registers and Boolean algebra as in Figure 3.12. The PRBS is character-
32
Consider the periodic and deterministic multisinusoidal signal:
us (k) =
ns
2i cos(i kT + i ),
(3.57)
i=1
Ns
),
2
2i
,
Ns T
Ns is the number
i
ji .
(3.58)
j=1
i = 1,
(3.59)
i=1
where the relative power {i > 0, i = 1, ..., ns } is specied. ns directly determines the order of the persistent excitation and Ns determines the low
frequency end of the Schroeder-phased spectrum. The Schroeder-phased sig-
33
beauty against the other signals may be the capability of designing the arbitrary desired power spectrum proles.
(b) chirp
1.5
0.5
0.5
u(t)
u(t)
(a) multisinusoid
1.5
0.5
0.5
1.5
0.1
0.2
time(s)
0.3
1.5
0.4
1.5
0.5
0.5
0.5
1
0
0.1
0.2
time(s)
0.3
0.4
0.5
1.5
0.2
time(s)
(d) Schroederphased
1.5
u(t)
u(t)
(c) PRBS
0.1
0.3
0.4
1.5
0.1
0.2
time(s)
0.3
0.4
34
(a) multisinusoid
(b) chirp
4000
500
400
3000
2000
300
200
1000
100
1000
2000
w(rad/sec)
3000
4000
(c) PRBS
1000
2000
w(rad/sec)
3000
4000
(d) Schroederphased
1200
120
1000
100
800
80
p
140
1400
600
60
400
40
200
20
500
1000
1500
w(rad/sec)
2000
2500
500
1000
w(rad/sec)
1500
2000
The rst model is the dierential model, which required the measurements of position, velocity, acceleration, and joint torque. In order to carry out a practical
identication using this model, accurate acceleration measurement has been key to
the successful identication, but it is a substantial challenge. Most of robots are
seldom equipped with accelerometers from which one can measure the acceleration
directly due to the cost and installation problem. The alternative is a numerical
reconstruction of acceleration by taking a second derivative of the joint positions at
the expense of amplifying the noise.
The second model is integral model, often referred to as the energy model because of
35
coming from the energy theorem, which only requires the measurement of positions,
velocities, and joint torques. It means that this model does not depend explicitly on
the accelerations. Figure 3.16 shows the principles of two identication methods.
3.5.2.1
The dynamic equation with friction of the robots rigid body model can be
written as
M (q)
q + C(q, q)
q + G(q) + N (q, q)
= ,
(3.60)
where N (q, q)
is friction.
More compactly, it can be written as
= D(q, q,
q)X.
(3.61)
36
X is the unknown dynamic parameter vector and is composed of the inertial parameters and the friction parameters. D(q, q,
q) is the kinematic information matrix,
which describes the motions of the robot. It has been referred to as the regression
matrix or the observation matrix. If measurements are made at given time instants
t1 , ..., tm along a given trajectory, we may write
(t1 )
..
.
(3.62)
(tm )
D(t1 )
..
.
(3.63)
D(tm )
q,
Once we got enough measurements so as to obtain overdetermined Matrix D(q,
q),
the unknown dynamic parameter vector X can be determined by the least-square
estimation method:
T D)
1 D
T ,
X = (D
(3.64)
1 D
T is the left pseudo-inverse matrix of D.
Note that it is important
T D)
where (D
to use the minimum set of (nonredundant) dynamic parameters referred as the base
dynamic parameters by grouping parameters together such that the matrix D is
always full-rank [47].
3.5.2.2
tb
ta
eT qdt
= H(tb ) H(ta ),
(3.65)
where E(ti ) is the Hamiltonian of the system at time instant ti and e does not
include friction torques. Since the total energy(kinetic and potential energy) is
37
linear in the dynamic parameters, it can be written that
tb
ta
eT qdt
= H(tb ) H(ta ) = e(q, q),
(3.66)
where, is the inertial parameter vector without the friction coecients. In order
to take friction torques, we can write
= e + f .
(3.67)
T qdt
= W (tb ) W (ta ) = e(q, q)
+
Then,
tb
T qdt
= W = E(q, q)X,
tb
ta
fT qdt.
(3.68)
(3.69)
ta
W (t1 )
..
=
W
.
(3.70)
W (tm )
E(t1 )
..
E =
.
(3.71)
E(tm )
The energy model in (3.69) has the same form as (3.61) and a left pseudo-inverse of
E can be applied to get the least-square approximation of the dynamic parameters:
1 E T W
.
X = (E T E)
(3.72)
The beauty of this approach is that the dynamic parameters are identied without
the joint accelerations.
38
3.5.3
exciting all links simultaneously with carefully chosen input trajectories. However,
from an implementation viewpoint of inverse dynamic control with assumption of
neglecting the Coriolis term due to the low velocities of surgical robot or PD control
with gravity and friction compensation, only the inertia, gravity, and friction terms
have to be identied. Therefore it was decided to identify the dynamic parameters
of each link independently while the other links remain in their synchronization
positions to simplify identication process and avoid the dimension problems. By
moving each joint, it is equivalent to identication procedure for one link manipulator. The dynamic equation of the one link robot manipulator based on friction
model in (3.43) can be written as
+ Fv (q)
= ,
I q + mgl sin(q) + Fc sgn(q)
(3.73)
where I is the inertia term, Fv is the coecient of viscous friction of the joint, and
Fc is the coecient of Coulomb friction of the joint. With the dierential model,
the regressor vector can be written by
D(q, q,
q) = [
q g sin(q) sgn(q)
q].
(3.74)
Considering the total energy in order to get the energy model, we obtain
tb
ta
1
T qdt
= (q2 (tb ) q2 (ta ))I + (cos(q(tb )) cos(q( ta )))mlg
2
+
tb
ta
|q|
dtFc +
1 2
(q (tb )
2
tb
ta
q2 dtFv
tb
ta
|q|
dt
tb 2
ta q dt
ml
Fc
Fv
39
By moving only each joint, we can get the following inertial parameters and the
friction coecients for each joint expressed by 4 1 vector:
X T = [X1
X2
X3
X4 ] = [I
ml
Fc
Fv ].
(3.75)
With the assumption of neglecting the electrical dynamics for the permanent magnet
brush DC motors, we can get the joint torques from the input control voltages in
current drive mode:
= Ka K t K g v c ,
(3.76)
where Ka is the amplier gain, Kt is the torque constant, which characterizes the
electromechanical conversion of armature currents to the torques, Kg is the gear
ratio, and vc is the input control voltages. Table 3.1 shows these parameters used
in experimental identication.
Table 3.1: Parameters of the electrical system.
Parameters
Joint 1 Joint 2 Joint 3 Joint 4
Ka [amp/volt]
0.375
0.375
0.375
0.375
Kt [Nm /amp ] 0.01342 0.01342 0.00652 0.00996
Kg
134
134
72.88
66
3.5.3.1
The vectors in the regression matrix associated the Coulomb and viscous coecients are prone to be linearly dependent in the energy model. The functions
|q|
and q2 are correlated and thus, a strong linear dependency between the vectors
exists. This fact gives rise to the diculty of distinguishing each contribution to
regressor. Consequently, it is very dicult to identify the each coecient separately
in energy model. As an experimental verication, the Schroeder-phased input signals were applied to the rst joint of the EndoBot and the regression matrix was
obtained. The comparison on the coecient of cross correlation between column
40
1
2
3
4
- -0.8219 0.0004
0.0
0.1651 0.2113
0.9442
-
cov(X, Y )
,
X Y
(3.77)
where rXY is the coecient of cross correlation between X and Y vectors, cov(X, Y ) =
E[(X E(X))(Y E(Y ))] is the covariance , and X and Y are the values of standard deviation. The Table 3.2 shows the comparison on the coecient of cross correlation between vectors. Clearly, the correlation between third and fourth column
is greater than those between others. This problem can be solved by identifying the
friction parameters rst and then compensating the corresponding friction torques
by subtracting them form the generalized torques. The identication of friction parameters is performed by implementing the velocity control loop as in Figure 3.17.
The Table 3.3 shows the experimental results with friction parameters. Eij stands
41
3.5.3.2
Experimental Results
Figure 3.18 shows the input torques and measured positions and velocities for
the rst joint of the EndoBot. The estimation of dynamic parameters is plotted
position[rad]
1
0.5
0
0.5
1
5
time
10
5
time
10
5
time
10
velocity[rad/sec]
4
2
0
2
4
input torque[Nm]
0.5
0.5
42
1
X2
0.5
0
X1
parameters
0.5
1.5
2.5
5
time
10
ml [Kg m] Fc [N m]
0.1273
0.127
0.1122
0.120
0.029
0.069
0.1305
0.149
0.1155
0.125
0.019
0.045
Fv [N m s]
0.044
0.067
0.847
0.742
0.029
0.054
0.795
0.484
the Washout lter. The washout lter is the velocity estimator consisting of a lowpass lter,
s
+1
p
and dierentiating eects in one ltering procedure. The washout lter is the proper
lter as follows
fwashout (s) =
s
p
s
,
+1
(3.78)
43
where p determines where the roll-o occurs. The discrete washout lter was implemented by the backward approximation with the location of pole, p = 100:
fwashout (z) =
p(z 1)
,
(pTs + 1)z + 1
(3.79)
the controller design. In this research, the parameters are validated by comparing the
simulation with the obtained parameters and real output with a given same torque
trajectory. The dierence between the measured outputs y(t) and the estimated
input torque[Nm]
0.5
0.5
10
time
12
14
16
18
20
14
16
18
20
1
simulated trajectory
position (rad)
0.5
0.5
measured trajectory
1
10
time
12
(3.80)
44
The coecient of cross correlation between the residual and input is very small (ru
= 0.0818). The result indicates that there is very small correlation between the
residuals and the inputs and therefore the obtained model can be used. Figure 3.20
shows the comparison on simulated and measured output trajectories. There exists
the small dierence between these two trajectories resulted from wishful assumptions
on model such as the ignored electric dynamics to simplify the analysis, the position
dependent friction coecients, which are assumed to have a position independent
characteristics, and unmodeled nonlinearities. Therefore, it is desirable to design
the robust controller that can withstand these variations.
CHAPTER 4
SUTURING IN MINIMALLY INVASIVE SURGERY
4.1
scopic stitching with absorbable materials, suturing has a long history and is one
of the most dicult tasks and uses a signicant percentage of operating time in
MIS. Although there is ongoing research on alternatives to suturing technique such
as tissue gluing, stapling, and thermal tissue bonding, suturing is still the primary
tissue approximation method. However, the laparoscopic suturing task demands
high level of skills and endurance on surgeons. Despite the needs to be performed
autonomously, no research eorts have been published. It is worth while to decompose the suturing task into the subtasks and gure out what technical diculties is
encountered with each subtask. There are several knot tying techniques in surgery
[17] and two of them are shown in Figure 4.1.
Square Knot - This is the easiest and reliable method for all suture materials.
Basically it consists of two half knots in opposing directions.
Surgeons Knot - This knot requires an additional loop in the rst throw and thus
the surface contact of suture is increase. It results in doubling the friction for
initial half knot and therefore more stable than the square knot. This knot
generally is used in open surgery and sometimes referred to as a friction knot.
45
46
The most common knot technique in surgery is the square knot. It is an ideal knot
for laparoscopic surgery because it requires fewer steps than the surgeons knot [17].
For this reason, the only square knot technique has been considered for analysis
in this thesis. Based on the observations of the manual suturing operations, the
suturing task can be broken down into the following ve subtasks:
1. stitch,
2. create a suture loop,
3. develop a knot,
4. place a knot, and
5. secure a knot.
The following are the technical diculties related to each subtask.
Stitch The stitching subtask involves in entrance and exit bites, extracting the
needle, and pulling out the suture. The task analysis shows that the greatest
diculty for this subtask is to manipulate the curved needle between two
graspers without slipping or dropping the needle.
Create a suture loop In the subtask of loop creating, the wrapping motion is
required to create a suture loop with the loop strand. It is the most dicult
motion to be autonomously implemented because the trajectories of suture
are not predictable due to the exibility. The surgeon creates the loop based
on visual feedback information and his personal experience.
Develop a knot Once created the loop, we need to pass either needle or short tail
through the loop to develop a knot. The diculty will be grasping the short
tail with one of the grasper and pull the suture through the loop.
Place a knot Once developed the knot, the knot needs to be placed into the niche.
Placing of the knot requires sliding of the knot. The diculties will be to determine the sliding condition and obtain the optimal trajectory that minimizes
the tearing trauma on the tissue.
47
Secure a knot Once completed the knot, a sucient amount of tension must be
applied to tighten the knot, but not too much as to damage the tissue. With
the laparoscopic instruments like as long sticks, it is very hard to apply the
precise tension so as not to tear o the soft tissue or loose the knot. The
technical diculty will be to regulate the tension along the desired direction.
From this analysis, the primary diculties are 1) manipulating the curved needle 2)
dealing with a exible suture 3) getting a trajectory for knot placement 4) applying
a proper tension. It should be pointed out that there is an attractive device for
laparoscopic suturing, called a shuttle needle device. The basic idea is to transfer the
needle between two jaws and lock the tapered tip of needle by one of jaws each time.
A disposable shuttle needle device is available on the market by USSC(US Surgical
Corp.). The advantage of this design is eliminating the diculty on manipulating
the curved needle. The usage of this device in MIS is continuously increasing. For
this reason, we adopted this device as our suturing instrument and modied it to
be operated by pneumatic power. Then, the remaining diculties are how to create
the knot with exible sutures, place the knot into the niche, and apply the proper
tension. First two diculties will be discussed in following sessions and the last one
will be covered in the tension control session.
48
4.2
critical for pulling out the suture after exit biting or keeping the boundary of motion
when securing the knot. In teleoperated surgery, the surgeon can track the length of
suture through visual feedback. In autonomous suturing, tracking the suture length
is more challenging. In this section, we are describing our solution to keeping track
of the suture length in real-time without extra sensor information. In this analysis,
we consider the suture attached to the middle of needle as shown in Figure 4.4. The
suture end attached to the needle is referred to as the needle tail or long tail and
similarly the end of suture the free tail or short tail. Then, the length of the suture
is measured from the exit point to the middle point of needle.
The basic concept behind the tracking the length of suture is that we can calculate the eective length of the suture from the exit point by tracking the position
of the needle, which is available from the encoders and robot forward kinematics.
If the suture is pulled continuously by moving the needle holder in one direction,
the rate of increasing suture length would be the same as the rate of the movement
of tip of the needle holder. However, if the direction is changed, the rate would no
longer be the same. For example, if the needle is moving toward the exit point, the
rate of suture length changing would be zero. This is due to the tension requirement
49
L(t)
=
4.3
4.3.1
(4.1)
to pull out the suture from the tissue. Let f denote the force applied to the end of
suture. Then, the dynamic equation of the EndoBot during stitching can be written
as
M (q)
q + C(q, q)
q + G(q) = J T f,
(4.2)
50
where q is the joint displacement vector, M (q) is the inertia matrix, C(q, q)
q is the
vector function characterizing Coriolis and Centrifugal forces, G(q) is the gravitational force, J(q) is the Jacobian matrix, is the applied joint torque, and f is the
suture tension force exerted by the robot at the end-eector.
4.3.1.1
The suture extension is the displacement of the end of the suture from the
stitch position on tissue and certainly it is the function of the joint angle. The
peculiarity is that the suture extension has a directionality and the rate must be
positive. Let the stitch position be in the origin and x(t) represent the current
position in Cartesian coordinate from forward kinematic mapping, then,
xT (t)x(t) if in tension
(t) =
p
max
(4.3)
otherwise,
T (t)x(t)
T J q(t)
2xT (t)x(t)
1
= x x(t)
= K(q(t))
2
K(q(t))
T (t)x(t)
x
=
(t)
4.3.1.2
if in tension
(4.4)
otherwise.
For simplicity, we assume that the suture is massless and hence has no dynamics. In practice, this is a good approximation since the suture has negligible mass.
We further assume that the suture has inelasticity. During pulling out the suture,
the force applied to the suture in tension is given by
f=
x(t)
+ Fv l),
(Fs sgn(l)
x(t)
(4.5)
where, l is the suture extension rate as pulled out by the robot and Fs and Fv respectively denote the static and viscous friction coecients between the suture and
the tissue. The important point here is that the suture tension force f must be positive because suture cannot generate negative tension force due to the positiveness
51
of (t).
Then, the dynamics become
M (q)
q + C(q, q)
q + G(q) = J T
4.3.2
x(t)
+ Fv J q).
(4.6)
Problem Formulation
During the autonomous stitching, the tension applied to the end of suture
must be overbounded with fmax and the joint variables should be regulated about
the desired position independently of the initial conditions. The control objective
here is to choose the controller to solve the following regulation problem.
Given dynamic equation (4.6) and qd , d , and fmax with initial conditions of
= 0, and (0), determine a feedback control law, , so that the closed-loop
q(0), q(0)
system satises
q(t) qd as t
q(t)
0 as t
(t) d as t
f (t) fmax t,
where qd is the desired position, d is the desired suture length after stitching, and
fmax is the force limit above which the tissue will be torn o.
4.3.3
metric constraint from the nite suture length is created. The norm of tip position
of the EndoBot must be radially overbounded with d :
K(q(t)) K(q(0)) d .
(4.7)
It means that the all motions as well as the desired position must be in the domain
of the feasible stitching motion.
52
4.3.4
Problem Simplication
From the requirement of desired suture extension, the control objective can be
(4.8)
where qd is the desired joint position with which the needle lies on the region and
it is equivalent to (t) d .
Then, the robot can move to the desired position:
q(t) qd as t , f (t) = 0 t.
(4.9)
Obviously, the second problem is the purely position control due to the directional
characteristics of the suture tension and the stability is already well proved.
The simplied control problem is given by
Given d and fmax , design a feedback control raw, , such that (t) d
as t and f (t) fmax t.
4.3.5
Problem Transformation
Case I.
It is worth while to take a look at the force applied to the suture in tension in
(4.5). The tension force has a positive value only if the suture extension rate is
(4.10)
53
fmax Fs
max =
.
Fv
(4.11)
Then, the simplied control problem described above might be written as follows.
Given d and max , design a feedback control raw, , such that
max (t) t.
(t) d as t and (t)
Clearly, the motion control problem with overbounded tension force can be
transformed into the pure motion control problem with a constraint of upperbounded velocity. What is the benet from this transformation?
In case of that the force information is not available, we still can bound the
tension force by fmax in order to do a stitch without the problem of tearing
o the tissue.
Case II.
Suppose we know the maximum tension force, fmax , and let fd be less than
fmax . Now, the control problem is a regulation problem with a desire position
and a desired force.
Given d and fd , design a feedback control raw, , such that (t) d
as t and f (t) = fd < fmax t.
4.4
Knot Tying
In this section, we present results on autonomous suturing using a pair of the
EndoBots. One of the EndoBot has a grasping tool and the other a stitching tool.
Both tools are built from disposable tools made by US Surgical Corp. (USSC).
The mechanical handles were cut and mated with pneumatic drives. The grasping
tool can be commanded open or close. The stitching tool contains two jaws, each
can lock in the needle. The tool can also pass the needle between the jaws. The
challenges of suturing operation include,
Only the needle position is known. The suture position is not directly measured.
54
The suture and the tissues are both exible.
The workspace is limited.
We will consider three algorithms of automatic ligation in this section. The
rst one uses a standard manual stitching tool instrumented for robotic operation.
The second one modies the grasping tool with a exible hook to facilitate the knot
tying process. The last one modies the grasping tool to have an articulated nger.
At the present, the rst and second methods have been implemented and the last
method is currently under development.
4.4.1
Ligation Algorithm 1
The key observation for tying a simple knot is that if the suture can be placed
over the jaw carrying the needle, then a loop can be formed by passing the needle
to the other jaw. For a human surgeon, this step is performed by putting the jaws
over the thread and then pass the needle. This is not possible for the EndoBots
since the thread is exible and the position is not directly measured. Instead, we
use the rigidity of the grasper to guarantee that the suture is placing over the jaw.
Automatic tying a simple knot can then be accomplished through the following steps
(shown schematically in Figure 4.5):
1. Make a single stitch near the wound and pull out the suture so that leave
a small suture tail. This may be done manually by the surgeon using the
manual mode or semi-autonomously. In the latter case, the surgeon manually
grasps both sides of the wound with the grasper and uses the foot pedal to
command the stitcher to make a stitch. The stitcher then retracts until a
specied amount of suture has been pulled through the suturing point.
2. Grab the suture tail with the grasper tip. This may be done manually by
the surgeon using the manual mode or semi-autonomously. In the latter case,
the grasper predicts the location of the suture tail based on the location and
direction of the suture performed in the previous step.
3. Move the stitcher so that the open jaw (the jaw without the needle) touches
the front of the grasper stem. This location should be as far up the grasper
55
stem as the workspace would allow. Denote the point of contact between the
stitcher jaw and grasper stem P .
4. Rotate the stitcher 180 about the axis OP where O is the center of the
spherical joint. The angle form between the grasper and the stitcher would
have to be suciently large to guarantee that the tip of the needle does not hit
the stem of the grasper. At the completion of this step, the thread has to lay
over the open jaw. Otherwise the surgeon would have to intervene manually.
5. Move the stitcher towards the grasper until the grasper stem is within the
open jaw of the stitcher. The grasper stem could t through the jaw opening,
but it is too thick for the stitcher jaws to close. The stitcher therefore needs
to move along the grasper stem until it reaches the grasper tool tip.
6. Rotate the grasper so that the narrow side faces the jaw opening. The stitcher
can now close and pass the needle to the other jaw.
7. Retract the stitcher to tighten the knot.
The above procedure would create a simple knot. Two simple knots may be combined to form a square knot. However, the second simple knot must be the mirror
image of the rst. Otherwise, the knot is known as the granny knot, which is not
secure. The only modication is that at Step 3, the stitcher should touch the back
of the step and in Step 4, the rotation should be 180 . The square knot procedure
may then be repeated to form a surgeons knot.
The above procedure works well most of the time in the laboratory, but has
the following drawbacks:
In the Step 4 above, a large angle is required between the two instruments.
For knots near the center of the workspace, this may not be feasible.
Because of errors in positioning, the thin suture thread could fall between the
open jaw and the grasper stem.
During retraction stage, the thread could get tangled.
56
57
4.4.2
Ligation Algorithm 2
The rst algorithm is a step toward automatic laparoscopic suturing, but suf-
fers several drawbacks as to render the procedure less than robust. To address these
issues, we made a small modication of the conventional grasping instrument. The
key observation is that if we can hold on to any part of the suture at a known position, the rotation of the stitcher would be unnecessary. To achieve this, we added
a reciprocating actuator connected to a exible hook over the grasper hinge so that
it can be extended or extracted as needed (see Figure 4.6).
58
always snared by the hook, so the positioning requirement is not as tight as before.
Finally, since the hook is close to the grasper tip, the grasper could retract by a
small amount as to avoid thread tangling.
59
60
61
4.4.3
Ligation Algorithm 3
The two ligation algorithms described above require the use of the special
stitching tool designed for endoscopic surgeries (specially, the EndoStitch by USSC).
For many surgeries, for example, anastomosis (connection between blood vessels), a
semi-circular type of needle is used with two graspers. In such cases, the algorithms
presented so far are not applicable. In this section, we consider another type of
grasper (such as in [11]), which contains an additional universal joint. Then forming
a loop can be accomplished by just wrapping the thread around the bent arm.
The detailed sequence for tying a simple knot is as described below (see Figure 4.9):
1. Hold the needle between needle holder and make a single stitch near the wound.
Pull out the suture so that leave a small suture tail.
2. Move the needle holder around the bent grasper tip to create a loop.
3. Move the two instruments together so that the bent grasper grabs the tail of
the suture while maintaining the loop wrapping around the bent stem.
4. Retract the grasper to tighten the simple knot.
To form a square knot, the second simple knot needs to be done with the loop
formed in the opposite direction. Note that the only reason that a bent tip grasper
is needed is to ensure that the loop does not slip o the grasper.
62
it does not require the use of a special stitching tool (and the associated needles).
This algorithm can also be extended to more general knots. For example, looping
twice around the bent arm would make a friction knot. We are also currently
evaluating the possibility of using the grasper with a hook described in Algorithm 2
to implement this algorithm.
It should be noted that each step in above knot tying algorithms is performed
autonomously under the surgeons supervision on suture entanglement and failure
of suture tail grasping to conrm the advance to the next step. In the future, it
might be anticipated to perform suturing task fully autonomously with additional
sensing capability such as vision system with less intervention or self conrmation.
4.5
4.5.1
tissue. Seating the knot on the tissue involves the magnitude of applying tension
as well as the direction of tension. To investigate the motion of seating the knot,
we can consider the simple knot tying in Figure 4.10. The knot forming procedure
is illustrated in the left gure. Let a post strand represent the suture that the
knot will be tied around and a loop strand represent the suture, which is looped
around the post strand as shown in Figure 4.10. A simple knot can be developed by
wrapping around the post strand with the loop strand. After developing the knot,
applying the tension at both post and loop end results in a simple knot as shown in
Figure 4.10. Dene pa represent the position where the needle enters to the tissue
and pb the position where the needle exits from the tissue. Let pn be the position
of knot and p0 the position at which the knot will be seated.
It was reported that applying tension in nearly horizontal would be desirable in
order not to get stuck in proceeding the placement of the knot. No report, however,
was published about the sliding condition, which guarantees the proceeding the
placement of the knot. The following section will consider the motion of a simple
knot and derive the sliding condition. The problem of the placement of a knot can
be formulated as follows.
63
tively. To investigate the sliding condition, we need to have a model of the knot,
which basically is consisted of two sutures wrapped around each one. The modeling
of a knotted suture is quite dicult and very complicated. Due to the fact that
the proceeding the placement of a knot is highly dominated by the friction between
knotted sutures, the suture local deformation is not considered in this research. A
simplied model of knotted sutures can be obtained from the two point contact
model with assumption of maintaining the half knotted conguration where the
equivalent normal forces and the friction forces pass through each point as shown
in Figure 4.11. Then, the free-body diagram can be drawn in Figure 4.12.
The problem can be stated as follows: Given F1a , F2a , F1b , and
termine the sliding condition. Dene e1a =
e2b =
2b
F
2b
F
e1a +e1a
e1a +e1a
1a
F
1a ,
F
1b
F
e2a =
2a
F
2a ,
F
e1b =
2b
F
2b , deF
1b
F
1b , and
F
apply and et be the unit vector along which the friction forces apply. When the
64
F = 0, so
(4.12)
It has two unknown variables, F1b and F2b , and two component equations as follows:
eu (F1ae1a + F2ae2a + F1be1b + F2be2b ) = 0
(4.13)
(4.14)
(4.15)
(4.16)
It becomes
65
From (4.16),
F1b =
(4.17)
F2b =
F1a (cos 1
sin 1
)
tan 1
F2a (cos 2 +
cos 2
sin 2
tan 1
sin 2
)
tan 1
(4.18)
The condition of occurring the impending motion cab be obtained from the freebody diagram of one strand as shown in Figure 4.13. In equilibrium, we have the
(4.19)
(4.20)
We can obtain the explicit representation of sliding condition, but it is quite a heavy
equation and very dicult to get the physical interpretation. Due to the desired
66
sliding trajectory, which lies in the normal direction to the tissue, henceforth the
symmetric case will be considered as shown in Figure 4.14:
= 1 = 2
(4.21)
= 1 = 2
(4.22)
F1a = F2a .
(4.23)
Then,
(4.24)
(4.25)
sin
F1a .
sin
(4.26)
It means that the tension in the bridge strand can be determined from only the
applying force, Fa , and the geometric conditions, and .
67
Moment equations about p and p give
f1an = F1a sin
(4.27)
(4.28)
(4.29)
1
1
> 2.
tan tan
(4.30)
It becomes
tan
).
2 tan + 1
(4.31)
It means that the sliding begins when the is suciently small to satisfy the inequality condition dependent on the coecient of friction between the strands and
. The sliding condition of (4.31) can be represented geometrically as shown in
Figure 4.15. It is a three dimensional shape, which can be obtained by extracting
a cone from a cylinder shape. It means that the force lying in this shape can slide
down the knot. Due to the constraint, which requires the applied tension to lie on
68
to the length invariant constraints. Let q be the current knot position and p1e and
p2e be the current position of each end of strands. During motion, we assume that
(4.32)
= c2 .
l2 + l2e = l2 + l2e
(4.33)
When the suture ends move to new positions, the knot position q can be obtained
from the following length invariant constraint equations (which is imposed so that
the strands remain in tension):
p1e q + q p10 = c1
(4.34)
p2e q + q p20 = c2 .
(4.35)
69
By choosing the suture end position trajectories, we can shape the knot position
trajectory. We focus on the case that the desired knot trajectory is a straight line
normal to the tissue. This case can be achieved with only symmetric pulling. The
problem becomes nding p1e (t) to move the knot along pn (t) as shown in Figure 4.18.
Let denote the unit vector along the desired direction of knot motion, and s be
the desired sliding rate. The angle (t) and the distance between pb and pn evolve
Figure 4.18: Trajectory of the loop end for placing the knot.
according to
pn (t) = pn0 + (st)
(t) = pn0 pb pn (t) pb
pn0 st
(t) = tan1 (
).
pb
(4.36)
(4.37)
(4.38)
Due to the sliding condition, (min (t) max (t) = f ((t), )), the trajectory of
the suture end to just start knot motion is
p1e (t) = p1e0 + (t)(cos((t))u + sin((t))v).
(4.39)
Excessive tension at the tissue at pa and pb could damage the tissue. The optimal
trajectory in the sense of minimizing the tension force in the bridge strand can be
obtained by using the minimum (t) = min :
p1e (t) = p1e0 + (t)u + (st).
Figure 4.19 compares the trajectories with (t) = max (t) and (t) = 0.
(4.40)
70
trajectories of loop end
4
3.5
v [cm]
2.5
2
loop end
1.5
Beta*
Pn
1
Beta=0
0.5
0
1
0.5
0.5
1
u [cm]
1.5
2.5
4.6
completely predict all dynamics of the system over the entire range of operating
due to the non-deterministic nature of behavior of environments such as exible
sutures and soft tissues. In order to guarantee the safe operation in surgical tasks,
the controller architecture reects that the proceeding of task must evolve under
surgeons supervision. This supervisory controller gives the capability of dealing
with uncertainty and interactive commands from surgeons according to the real
situation without increasing the complexity. In this section, the architecture of the
EndoBots controller will be described.
4.6.1
4.6.1.1
Hybrid State
From the planning perspective, suturing can be seen as sequencing states of the
dynamic system whose states evolve continuously with time or discretely according
to asynchronous events. Inherently it is a hybrid dynamic system in nature. In
order to model the continuous time systems as well as the discrete event systems,
71
the hybrid system framework is introduced with the denition of hybrid state and
the structure of hybrid automaton. The hybrid system can be viewed as a general
framework of a discrete event system (DES), which contains the continuous state
vector and a discrete state vector as
h = (x(t), s),
(4.41)
where h, x(t), and s represents the hybrid state, the continuous state, and discrete
state, respectively. In robotic applications, the continuous state evolves with time
according to motion planning. The discrete state can be updated with the occurrence
of the corresponding events.
4.6.1.2
Hybrid Automaton
(4.42)
where S is a nite set of discrete states (locations), X is the continuous state space
in which x is dened, E is a nite set of events (transitions), is a set of activities
(output functions), and Inv is a set of invariants of each s.
In hybrid automata, the state x(t) evolves continuously according to a control law
within each state until an event occurs. This continuous state must satisfy
x(t) Inv(s)
(4.43)
where is a duration of the continuous evolution. In other words, the Inv can
be viewed as continua where the normal evolution of continuous state can dene a
specied state, s, as shown in Figure 4.20. An unpredictable event can occur when
the continuous state x(t) crosses a boundary during the evolution or when a human
intervention occurs. When the evolution is over, a planned event occurs to transit
72
issues the transition of states and receives events, which indicate either the successful evolution of current state or the occurrence of an error condition. Due to the
fact that all possible events cannot be predicted during the planning phase, the con-
73
troller architecture must reect that the proceeding of task evolve under surgeons
supervision. Figure 4.22 shows the controller architecture implemented in this research. In this research, the human sharing supervisory control is used to supervise
Lock, manual, and autonomous states. The transition to the lock state can be
started from issuing the lock event by either the surgeon or the ADES. In lock
state, the robot manipulator is under a control law, which regulates the current
position, and only two events for transition to the manual or autonomous state can
74
(4.44)
where x(t) = (x1 (t), x2 (t)), x1 (t) represents the position state vector, x2 (t) represents
the velocity state vector, and xcp denotes the current position vector. In manual
state, the surgeon can take all control commands and HDES (Human Discrete Event
System) is active to correspond to the event issued by the surgeon. The manual
state, SM , has substates, SM i , according to values of discrete states as shown in
Figure 4.24. In contrary to the lock state in (4.44), the Inv of the manual substate
can be viewed as a whole workspace:
Inv(SM i ) = {x(t) Inv(SM i) : xmin x(t) xmax },
(4.45)
where xmin and xmax dene the workspace. The autonomous sequencing of suturing
algorithm can be evolving in the autonomous state under the surgeons supervision
and the surgeon can issue the event, which causes the transition to the lock state in
order to gain the control power for dealing with uncertainties.
In Figure 4.25, ei represents the planned event issued by the ADES when the
evolution of the current state ends and ei represents the unpredictable event during
the evolution due to the real situation, which cannot be predicted. The ei makes
the transition to the NOT BE state, si . In this case, it can transit to the previous
state and try it again by generating the event ei1 . The surgeon also can take the
control in this case and he or she can recover the error in manual state and continue
75
Development Environment
In this research, all controllers, the high level discrete event system controller
and the low level motion controller, are implemented in Simulink with ARCSlink
library (ARCS, Inc.), which contains the Simulink device drivers of the DSP Lighting
76
77
78
CHAPTER 5
MOTION CONTROL
5.1
manual move the tip of the tool in roll-pitch-yaw and translation directions. As
early mentioned, most procedures in MIS are time consuming and in manual mode
the surgeon might be tired to deal with operating two EndoBots. Since the manual
operation is performed with low speed and the gravity term will tend to be the key
external static force, the gravity compensation is applied to minimize the surgeons
command input. In the manual mode, the joint torque is applied to compensate
gravitational force and moment:
= g ,
(5.1)
g =
(5.2)
m2 gc1 c2
5.2
79
80
5.2.1
(5.3)
+ Fv q.
N (q)
= Fc sgn(q)
(5.4)
where
(5.5)
where e = q qd represents the error between the desired and the actual position.
The Lyapunov function in (5.5) has an energy based interpretation. The rst term
is the the total kinetic energy and the second term has the interpretation of an
articial potential energy with a shape of global convexity with the desired position
at the global minimum. The control objective here is to design a feedback control
law, , such that e 0 and e 0 as t . Dierentiating (5.5) with respect to
time:
1
V = qT M (q)
q + qT M (q)q + e T Kp e.
2
(5.6)
Substituting M (q)
q in (5.3) yields
V
1
+ e T Kp e
q G(q) F (q)
+ M (q)q)
= qT ( C(q, q)
2
1 T
=
q + qT ( G(q) F (q))
+ e T Kp e
q (M (q) 2C(q, q))
2
= qT ( G(q) F (q))
+ qT Kp e.
(5.7)
(5.8)
(5.9)
By choosing the feedback control law with compensation of gravitational and frictional terms:
= Kp e Kd e + G(q) + F (q).
(5.10)
81
V becomes a negative semi-denite:
V = e T Kd e.
(5.11)
Lyapunovs direct method leads to only stability of the equilibrium point. By invoking the Lasalles invariance principle, it can be shown that the largest invariant
set S is given by
S = {e, e : e = 0, e = 0}.
(5.12)
(5.13)
(5.14)
Since the simplied error dynamics are linear dierential equation, it is possible
to tune the PD controller gains to achieve a desired performance specication.
The gain matrix Kp and Kd are chosen such that the error dynamics becomes
critically damped. Let e be the desired natural frequency of error dynamics,
then for being critically damped error dynamics, this gives
Kp = we2 M
(5.15)
Kd = 2 Kp M .
(5.16)
82
5.2.2
be 7 Hz and approximate the inertia term at the center of the workspace. For being
critically damped error dynamics, this gives
97 0
0 97
0
0
Kp =
1.93
4.4 0
0 4.4
(5.17)
193
Kd =
0.088
.
0
(5.18)
8.8
Friction Compensation
To compensate the friction eect, the estimated amount of Coulomb friction
is added into the input torque as follows
f = Fc sgn(q),
(5.19)
83
(b)
15
10
10
position [deg]
position [deg]
(a)
15
5
0
5
10
15
5
0
5
10
15
time
(d)
15
10
10
position [deg]
position [deg]
(c)
15
5
0
5
10
15
6
time
5
0
5
10
6
time
15
4
time
84
eect with second joint. Figure 5.3 shows the quadrants passing error due to
Circle Tracking
15
10
y [mm]
10
15
15
10
0
x [mm]
10
15
Max.
Max.
Mean
Mean
Cartesian
Joint(1) Joint(2) Joint(1) Joint(2)
Error
Error
Error
Error
Error
(deg)
(deg)
(deg)
(deg)
(mm)
1.060
0.994
0.425
0.314
0.970
On
0.354
0.463
0.108
0.082
0.466
85
performed with three dierence angular velocity of 1 = 0.1Hz(0.62 rad
sec ), 2 =
rad
0.25Hz(1.57 rad
sec ), and 3 = 0.5Hz(3.14 sec ) with radius r1 = 2.5mm, r2 =
5mm, and r3 = 10mm. Gravity and Friction compensation were included in
all experiments.
Circle Tracking
15
10
y [mm]
10
15
15
10
0
x [mm]
10
15
86
5.2.3
dynamics with suitable choice of the input. This approach is attractive in the sense
of availability of rich linear controller design schemes with linear system such as the
optimal or robust controller.
5.2.3.1
Let the rigid robot dynamics be given in general compact matrix form by
M (q)
q + C(q, q)
q + G(q) + N (q)
= .
(5.20)
(q),
=M
q + G(q)
+N
(5.21)
(q), C(q,
q),
(q)
where M
G(q),
and N
denote the approximated matrix of real robot
dynamics. Substituting (5.21) into (5.20) gives
(q) I) u + (M (q)1 H(q, q)
q = u + (M (q)1 M
,
1 (q)
2 (q,q)
(5.22)
q)
(q)
where H(q, q)
= (C(q,
C(q, q))
+ (G(q)
G(q)) + (N
N (q)).
Then it becomes
(5.23)
where (u, q, q)
= 1 (q)u + 2 (q, q)
represents the disturbance of the linearized robot
dynamics due to the lumped model uncertainties. The state equation can be given
by dening a new set of the state variables to be
q
x=
.
q
(5.24)
(5.25)
Then
87
where
0 I
A=
0 0
0
B=
.
I
(5.26)
(5.27)
Clearly, this is globally linearized system. Hence, it can be concluded that the
manipulator can be regarded as a set of fully decoupled second-order time-invariant
linear systems. In practice, it is not possible to have the exact matrices of robot
dynamics, and therefore the linear controller based on this approximated linear
system, (u, q, q)
0, needs to have a robustness against the inevitable uncertainties
in terms of modeling and parameters.
5.2.3.2
The objective of optimal control is to design the control law that minimizes
a cost function while satisfying the equality constraints imposed by the dierential
equation of system dynamics:
min J(x(t), u(t)) = (x(tf )) +
u(t)
tf
= f (x, u, t).
t0
(5.28)
It is, in general, impossible to solve analytically so that we can use the explicit
control law. In the case of the linear time-invariant system with the quadratic cost
functions, the explicit control equation, which is basically the full state feedback
controller, can be obtained. Given the system dynamics:
x(t)
= Ax(t) + Bu(t)
x(t0 ) = x0 ,
(5.29)
(5.30)
88
where S and Q are real symmetric positive semidenite matrices, and R is a real
symmetric positive denite matrix. This is a constrained optimization problem and
it can be converted into an unconstrained optimization problem using Lagrange
multipliers. Then, the augmented cost function is given by
1
x(tf )T Sx(tf ) +
2
1 tf T
(x (t)Qx(t) + u(t)T Ru(t) + (t)T (Ax(t) + Bu(t) x(t)))dt
2 t0
tf
1
1
=
(H(x, u, t) = (t)T x(t))dt,
(5.31)
x(tf )T Sx(tf ) +
2
2 t0
Ja =
H
= Ax(t) + Bu(t)
= Qx(t) AT (t)
(t)
=
x
H
= 0 u(t) = R1 B T (t)
u
x(t)
(5.33)
(5.34)
(5.35)
x(t0 ) = x0
t = tf :
(tf ) =
= Sx(tf ).
x(tf )
(5.36)
(5.37)
Optimal control problem now becomes a TPBVP (Two Point Boundary Value Problem), which has n boundary conditions at the initial time t0 and n conditions at
the nal time tf . The state and costate equations can be given by the Hamiltonian
89
matrix H:
1 T
x(t)
A BR B x(t)
x(t)
H
.
Q
AT
(t)
(t)
(t)
(5.38)
x(t0 )
x(t)
Ht
=e
,
(t)
(t0 )
(5.39)
where (t0 ) is not given. It can be solved, in general, numerically by iterating the
boundary condition, called a shooting method. It basically sequences the state and
f ) with the guessed initial values of (t0 ) and
costate equations in order to get (t
perturbs (t0 ) until the boundary conditions at the nal time are satised to the
f ) = (tf ). In case of LTI system with the quadratic cost
desired accuracy, (t
function, the problem can be solved analytically using the sweep method proposed
by Bryson and Ho [61]. The idea of the sweep method is the assumption of a linear
relationship between the costate and the state, which can be given by
(t) = P (t)x(t).
(5.40)
This transforms the TPBVP with respect to x(t) and (t) into a single point boundary problem in P (t) as
P (t) = AT P (t) + P (t)A + Q P (t)BR1 B T P (t)
(5.41)
with the boundary condition P (tf ) = S. This is called the time varying Riccati
equation. It means that the solution to the Riccati equation gives rise to the optimal
full state feedback control law:
u(t) = K(t)x(t) = R1 B T P (t)x(t),
(5.42)
which minimizes the cost function while satisfying the equality constraints. This
optimal controller is called a linear quadratic regulator (LQR). Note that even for
90
the LTI system, the Kalman gain K(t) = R1 B T P (t) is time-varying for the nite
time case. From the implementation perspective, it is not convenient and very
expensive solution in terms of the memory requirement to store all time-varying
gains. The steady-state optimal control gain can be used in place of the timevarying gain based on the assumption of ignoring the transient gain. It can be
obtained with the following innite horizon cost function:
1
J=
(x(t)T Qx(t) + u(t)T Ru(t))dt Q 0, R > 0.
2 0
(5.43)
(5.44)
(5.45)
The existence of a solution to the ARE and the stability of the closed-loop system are
ensured by the controllability and observability of the plant. Under the assumption
on the control weight matrix, R = I, ( > 0), the ARE can be manipulated to lead
the return dierence inequality condition on the robustness:
min (1 + L(j)) 1
(5.46)
where min denotes the minimum singular value and L(s) = K(sI A)1 B is the
loop transfer function matrix. For SISO systems, it can be given as
|1 + L(j)| = |L(j) (1)| 1.
(5.47)
This means that the Nyquist locus must remain outside the unit circle centered at
(-1,0) in the complex plane. In other words, the LQR control has the guaranteed
stability with innite gain margin, -6 dB gain reduction tolerance, and at least
60 deg phase margin.
91
5.2.3.3
Velocity Estimation
Friction and velocity estimation are two main factors, which can aect the
performance in motion control applications. The friction can be identied and
compensated as mentioned before. In motion control applications, velocity can be
measured directly with velocity sensors at the expense of high cost. In general, only
position signals are available and it is true in our research. In order to implement
the optimal state feedback controller, the state corresponding to the velocity should
be estimated.
Numerical Dierentiating The easiest way to get the velocity information is to
numerically dierentiate the position signals. Inherently numerical dierentiating is very sensitive to the noise. From the lter perspective, the numerical
dierentiating has the following lter characteristics:
Fnd (s) =
1 esTS
,
Ts
(5.48)
(5.49)
s
p
s
,
+1
(5.50)
92
Bode Diagram
40
Magnitude (dB)
30
20
10
0
10
20
Phase (deg)
90
45
0
2
10
10
10
Frequency (rad/sec)
(5.51)
(5.52)
where w(t) represents the process noise and v(t) represents the sensor(measurement)
noise. The observer is constructed as follows
x (t) = A
x(t) + Bu(t) + L(y(t) C x(t)),
(5.53)
93
Bode Diagram
40
39.5
Magnitude (dB)
39
38.5
38
37.5
37
36.5
36
Phase (deg)
60
30
0
2
10
10
10
Frequency (rad/sec)
(5.54)
94
Then, the steady-state optimal gain is computed from
L = C T Ro1 ,
(5.55)
where represents the intensity of the estimation error and it is the solution
of the following the ARE:
A + AT + Qo C T Ro1 C = 0.
(5.56)
LQG
The LQR has guaranteed stability margins, but a major limitation of the LQR is
to require the measurement of all states. In contrary to the LQR, the LQG control
provides the optimal control with partially available states. From the separation
principle, the LQG optimal controller can be obtained:
T he Robot Dynamics : = M (q)
q + C(q, q)
q + G(q) + N (q)
T he F eedback
(q)u + C(q,
q)
(q)
Linearization : = M
q + G(q)
+N
x(t)
T he Linearized P lant :
y(t) = Cx(t) + v(t)
u(t) = K x(t)
T he Optimal
Controller : K = R1 B T P
0 = AT P + P A + Q P BR1 B T P
x (t) = A
x(t) + Bu(t) + L(y(t) C x(t))
T he Optimal
Observer : L = C T Ro1
0 = A + AT + Q C T R1 C.
o
o
95
The LQG controller is basically a regulator, which has a zero command input. In
order to track a reference input, the state error feedback controller is considered via
coordinate translation in this work. Dene the error vector as
x(t) = x(t) xd ,
(5.57)
(5.58)
y(t) = C x(t).
(5.59)
State feedback controller in the second-order system can be viewed as a generalization of proportional-derivative control. To eliminate the steady-state error, we can
augment the system by taking the integral of the error as an additional state:
xi (t) =
(y(t) r)dt =
(Cx(t) r)dt =
C x(t)dt.
(5.60)
(t) B
(t)
x
A 0 x
+
u(t).
C 0
xi (t)
0
x i (t)
It can be written with augmented error state denoted by adding a bar:
x(t) + Bu(t)
x (t) = A
(5.61)
y(t) = C x(t).
(5.62)
Figure 5.7 shows the block diagram of the LQG controller with augmented error
state. Figures 5.85.11 show the experimental results of the LQG controller on the
rst joint. Figure 5.8 compares the tracking performance of LQG controller when
the friction compensation is active and not active. As seen in the gure, friction
leads to no motion in moments of changing of velocity direction without the friction
compensation and the friction compensating improves performance. Figure 5.9 (b)
96
97
compares the measured position error state and the estimated position error state
and (c) shows the estimated velocity error state and the corresponding control input
signal is shown in (d). Figure 5.10 compares the measured position error states
according to dierent weighting factors on the position error state. Figure 5.11
shows the corresponding control input. As seen in Figures 5.105.11, the error state
is decreased and the control input is increased by increasing the weighting factor on
q1 as expected. .
40
desired
without friction comp.
with friction comp.
30
20
position[deg]
10
10
20
30
40
10
15
time
98
(a)
(b)
40
4
actual
estimated
position error[deg]
position[deg]
20
20
40
10
15
time
15
10
15
(d)
15
10
10
control input [Nm]
(c)
15
5
0
5
10
15
10
time
5
0
5
10
10
15
15
time
time
4
q1=1
q1=5
q1=10
3
position error[deg]
10
15
time
99
30
q1=1
q1=5
q1=10
20
control input
10
10
20
30
10
15
time
CHAPTER 6
SHARED CONTROL
Shared control, as the name implies, enables the human operator and the computer
control the manipulator in parallel. It means that the human operator controls some
axes while the computer concurrently controls other axes. This would be useful in
the following scenarios:
Surgeons want to move the tool along the tool axis for drilling. The roll-pitchyaw rotation of the docking station would be computer controlled while the
surgeon manually controls the tool translational motion and operates the tool
(for example, for drilling or cutting).
The surgeon wants to control the tip of the tool along a straight line. For
example, the surgeon may want to perform precision cutting and stitching.
The computer would actively control the tool to stay in a valley along which
the surgeon is free to move the docking station and operate the tool manually.
The surgeon may specify the direction of manual control through a 3D input device.
Our current implementation allows the surgeon to manually operate one of the
EndoBot as the pointing device and use a foot pedal to register the selected points.
In the shared control case, the computer control algorithm needs to be modied to
ensure only the deviation from the specied path is corrected, but not the motion
along the path.
6.1
Constraints Description
In shared control mode, we add articial constraints to relieve the operator
of some sub task that would be tiring for an operator to concentrate on such as
tool alignments. These articial constraints are assumed to be holonomic. In order to control robot systems with holonomic constraints, we have to represent the
constraints mathematically. Let the forward kinematics (mapping from the joint
coordinate, q = (q1 , q2 , q3 , q4 ) to the end eector coordinate x = (rT , )) be denoted
100
101
by x = k(q). Specically, suppose that the surgeon species that the end eector
position rT and roll angle to be constrained as C(rT , ) = 0. The constraints that
the controller needs to enforce are C(k(q)) = 0. Writing them more compactly, the
holonomic constraints can be represented on the task space variables x(t) n as
follows
C(x) = 0.
(6.1)
6.2
C(x)
.
x
(6.2)
Control Objective
In shared control, the control task is to restrict the motion of manipulator
within the constraint space, which can be represented as mapping C(x) = 0. The
control objective here is to choose the controller to solve the following regulation
problem with constraints. Given dynamic equation and constraints, determine a
feedback control law, , so that the closed-loop system satises
C(x(t)) 0 as t .
(6.3)
It means that the motion will be lie in the following Constraint manifold:
S = {x, x : C(x) = 0, Jc (x)x = 0}.
(6.4)
(6.5)
102
6.3
(6.6)
Let xcd be the desired position of constrained variables in task space and xc be the
current position of constrained variables. We apply the following a PD feedback
control law with gravity compensation so that the constrained space error xc xcd
tends asymptotically to zero:
= J T JcT ( Kp xc + Kv Jc x ) + G(q),
(6.7)
where xc = xc xcd .
6.3.1
Stability
Choose the following positive denite quadratic form as a Lyapunov function
candidate:
1
1
V = qT M (q)q + xc T Kp xc .
2
2
(6.8)
(6.9)
Substituting M (q)
q gives
V
1
+ x Tc Kp xc
q G(q) + M (q)q)
= qT ( C(q, q)
2
1 T
=
q + qT ( G(q)) + x Tc Kp xc
q (M (q) 2C(q, q))
2
= qT ( G(q)) + x Tc Kp xc .
(6.12)
= J T F = J T JcT Fc
(6.13)
(6.10)
(6.11)
Since
xc = xc xcd
(6.14)
103
= C(x) xcd
x c = Jc x,
(6.15)
(6.16)
then,
V
= qT J T JcT Fc + (Jc x)
T Kp xc qT G(q)
(6.17)
= (J q)
T JcT Fc + x T JcT Kpxc x T JG(q)
(6.18)
(6.19)
This equation suggests the structure of the controller. By choosing the control law
for constraint force:
Fc = Kp xc Kv Jc x + JcT JG(q),
(6.20)
(6.21)
(6.22)
Hence Lyapunovs direct method leads to only stability of the equilibrium point. By
invoking the Lasalles invariance principle, it can be shown that the largest invariant
set S is given by
S = {x, x : x = 0, xc = 0}.
(6.23)
Consequently the constrained space error, xc = xc xcd , tends to zero asymptotically. The feedback control law becomes
= J T JcT Fc
= J T JcT (Kp xc + Kv Jc x)
+ G(q).
(6.24)
104
6.3.2
Controller Description
The block diagram of the shared control algorithm is shown in Figure 6.1.
Basically, the constraint Jacobian, Jc, decomposes the Cartesian forces into con-
Example
To illustrate the procedures of shared control, we will consider two simple
examples of constrained equations and show how to get the constraint Jacobian,
Jc (x).
105
Move along the Straight Line
Supposed that the end-eector is required to stay along a specic line. In this
case, the surgeon can control the roll as well as translation. Let p(x) 4
represent the tip of the tool and x axis be the preferred line. Let n be the
number of task space variables and m be number of constraints. Then, the
constraint space can be described as the set of all points satisfying the following
constraint equations:
x = 0, y = 0.
(6.25)
C(x) =
x
= 0.
y
(6.26)
Jc (x) =
6.3.4
C(x) 1 0 0 0
=
.
x
0 1 0 0
(6.27)
characteristics of shared controller described in the previous section. For the purpose
of evaluating the shared control algorithm experimentally, a straight line through
the center of the spherical joint was selected as a constrained path. The straight
line can be parameterized as
y
z
x
=
= .
a1
a2
a3
(6.28)
x
y
=0
a1 a2
(6.29)
C2 (xT ) =
x
z
= 0.
a1 a3
(6.30)
106
By combining, we obtain
x
a1
x
a1
y
a2
z
a3
= 0.
(6.31)
JC (xT ) =
1
a1
1
a1
a12
a13
0
.
0
(6.32)
Table 6.1 shows the parameters used to specify the constraint line.
Table 6.1: Parameters for constrained line.
a1
1
a2
1
a3
-2
0
0.02
0.04
0.06
z [m]
0.08
0.1
Desired trajectory
0.12
0.14
0.16
0.18
0.2
0
0.05
0.1
0.05
0.1
0.15
0.2
y [m]
0.15
0.2
x [m]
107
desired trajectory and roll the tool freely. In shared control, the operator can be
interpreted as an actuator in the unconstrained space, but also can be a disturbance
source in the constrained space. It should be noted that the performance of shared
controller depends on the human operator. For this reason, the operator was trying
to move the tool in all direction with same impedance for performance evaluation.
0.1
0.1
0.08
0.08
0.06
0.06
y [m]
x [m]
Figure 6.3 shows the measured task space position during the motion with shared
0.04
0.04
0.02
0.02
time
time
1.5
1
0.05
r [rad]
z [m]
0.5
0.1
0
0.5
0.15
1
0.2
6
time
1.5
4
time
108
10
Fc1 [N]
10
5
time
5
time
10
Fc2 [N]
10
10
10
5
JcFc2 [N]
JcFc1 [N]
10
10
10
10
10
time
JcFc4 [N]
JcFc3 [N]
time
6
time
10
4
time
1
tau2 [Nm]
tau1 [Nm]
109
time
tau4 [Nm]
tau3 [Nm]
time
time
4
time
ec1
ec2
6.4
Desired Stiness, Kd
[N/m]
1000
1000
(6.33)
110
0
0.02
0.04
Actual trajectory
0.06
z [m]
0.08
0.1
Desired trajectory
0.12
0.14
0.16
0.18
0.2
0
0.05
0.1
0.05
0.1
0.15
0.2
0.15
0.2
x [m]
y [m]
Figure 6.7: Experimental result of task space shared control with constrained line.
where c is a constraint function.
Let f () be the complement of c so that
d(xT ) =
c(xT )
f (xT )
(6.34)
is a dieomorphism (i.e., d is one-to-one and onto, and d and d1 are both continuously dierentiable). The basic concept of shared control implemented in this
section is that it is suitable to choose the generalized coordinate vector in order
to describe the constrained motions. Due to the presence of k constraints and the
resulting lack of k degree of freedom, the description of constrained motions in
Cartesian coordinates is not ecient. Clearly, the mapping d(xT ) transforms the
position vector expressed in Cartesian coordinates into generalized coordinate space,
which marches on the constraint surface. With this generalized space, the motions
can be easily decomposed into one in constrained space and the other in free space.
The corresponding vector in Cartesian coordinates can be obtained with the inverse
111
mapping as shown in Figure 6.8 . The assumption of dieomorphism ensures the
bijection mapping of both position and velocity vectors. If we use the same joint
xTd = d1
0
f (k(q))
(6.35)
xTd =
where Jc =
c
x
and Jf =
Jc
Jf
0
Jf J q
(6.36)
f
.
x
Once xTd is found, the same joint space controller can be used:
= Kp (q k 1 (xT d ) Kd (q J 1 (q)xT d ) + G(q) + N (q).
(6.37)
112
6.4.1
Controller Description
113
6.4.2
Examples
(6.38)
c(xT ) =
1
x a12 y = 0
a1
1
x a13 z = 0
a1
1
a1
1
a1
a12
a13
0
xT .
0
(6.39)
a1 a2 a3 0
f (xT ) =
xT .
0 0 0 1
(6.40)
Jc =
1
a1
1
a1
a12
a13
a1
Jf =
(6.41)
a2 a3
0
0
.
1
(6.42)
(6.43)
c2 (xT ) = z z0 .
(6.44)
114
Then, the constraint function is
c(xT ) =
(x x0 ) + (y y0 ) r
= 0.
z z0
(6.45)
2(x x0 ) 2(y y0 ) 0 0
Jc =
.
0
0
1 0
(6.46)
Let denote the angle between the x axis and the vector r:
x = r cos()
(6.47)
y = r sin().
(6.48)
(6.49)
Let the tip of tool be free to rotate about an axis parallel to the tool axis, then
f2 (xT ) = .
Jf becomes
Jf ==
6.4.3
y
r
(6.50)
x
r
0 0
.
0 0 1
(6.51)
Linear Trajectory For the purpose of evaluating the joint space shared controller experimentally and comparing with the task space controller, same
straight line used in evaluating the task space shared control was selected.
Figure 6.10 shows the measured task space positions. The tip of tool is sliding
down and up and the position on x and y axis is increased and decreased corresponding to position of z axis. Figure 6.11 shows the desired and measured
0.1
0.1
0.08
0.08
0.06
0.06
y [m]
x [m]
115
0.04
0.04
0.02
0.02
time
0.8
r [rad]
z [m]
0.05
0.1
0.15
0.2
6
time
0.6
0.4
0.2
time
4
time
(6.52)
(6.53)
(6.54)
116
0.5
0.5
0.5
qd
q
qe
joint 2 [rad]
joint 1 [rad]
qd
q
qe
0.5
time
0.5
qd
q
qe
qd
q
qe
joint 4 [rad]
0.5
joint 3 [rad]
6
time
0.5
0.5
time
time
1
tau2 [Nm]
tau1 [Nm]
time
tau4 [Nm]
tau3 [Nm]
time
6
time
4
time
117
0
0.02
0.04
0.06
Actual trajectory
z [m]
0.08
0.1
Desired trajectory
0.12
0.14
0.16
0.18
0.2
0
0.05
0.1
0.05
0.1
0.15
0.2
0.15
0.2
x [m]
y [m]
ec1
ec2
M ax(|e|)
[m]
0.0013
0.00062
118
Circular Trajectory Figure 6.14 shows the desired circle trajectory, which is
centered at the (0, 0, -0.15 m) with radius of 0.05m. The experimental data
of the joint shared controller with the circular trajectory are illustrated in
Figures 6.156.18. Table 6.4 summarize the performance of joint space shared
controller with the circular motion.
0
0.02
0.04
0.06
Desired trajectory
z [m]
0.08
0.1
0.12
0.14
0.16
0.18
0.2
0
0.05
0.1
0.05
0.1
0.15
0.2
y [m]
0.15
0.2
x [m]
Table 6.4 summarizes the performance of joint space shared control with linear
motion.
Table 6.4: Eective constraint stiness for circular trajectory
ec1
ec2
M ax(|e|)
[m]
0.0032
0.002
0.1
0.1
0.05
0.05
y [m]
x [m]
119
0.05
0.1
0.05
0.1
time
0.8
r [rad]
z [m]
0.05
0.1
0.15
0.2
6
time
0.6
0.4
0.2
time
time
0.5
0.5
0.5
qd
q
qe
joint 2 [rad]
joint 1 [rad]
qd
q
qe
0.5
time
0.5
qd
q
qe
qd
q
qe
joint 4 [m]
0.5
joint 3 [rad]
6
time
0.5
6
time
0.5
time
1
tau2 [Nm]
tau1 [Nm]
120
time
tau4 [Nm]
tau3 [Nm]
time
time
time
0
0.02
0.04
0.06
z [m]
Desired trajectory
Actual trajectory
0.08
0.1
0.12
0.14
0.16
0.18
0.2
0
0.05
0.1
0.05
0.1
0.15
0.2
y [m]
0.15
0.2
x [m]
Figure 6.18: Actual trajectory of the end-eector in the task space for
the circular constraint
CHAPTER 7
TENSION CONTROL
7.1
Securing a Knot
After the knot is formed, securing the knot can be performed by applying the
tension. For the square knot, two throws are required and each throw must be done
in opposite direction and with a dierent tension. For the rst throw, the proper
tension should be applied to place a simple knot on the wound surface such that it
will not contribute on tissue strangulation and also it would have enough traction
force to remain the knot as it is laid down. Applying too much tension might cause
breaking out the tissue. After the second throw, applying the precise tension would
be necessary so as not to break the knot or loose the knot.
7.1.1
the point pa and pb be the entry point and exit point of suture, respectively. For
simplicity without the loss of generality, the base coordinate frame B can be attached
to the point where the suturing line and the line connecting two suturing point pair.
Dene ha and hb to be the unit vectors heading point pa and pb in the frame B.
After the needle is drawn through the tissue, the loop end, which holds the needle,
is at the exit point pb and the post end is at pa . Dene ple and ppe to represent the
position of the loop end and post end and pn to represent the knot position. Let
el =
ple pn
ple pn
and ep =
ppe pn
ppe pn
tensions apply. In the current conguration of the Endobots, ple represents the tip
position of stitcher and ppe represents the tip position of grasper.
Direction of the rst throw of a square knot
For the rst throw of the square knot, the stitcher holding the needle is forced to
move along the direction ha and the grasper holding the suture tail is required to
121
122
(7.1)
(7.2)
(7.3)
123
(7.4)
it yields the following unstable a two half hitch knot as shown in Figure 7.4.
Figure 7.4: An unstable two half hitch knot with reverse-directed tension
during the second throw.
7.2
7.2.1
Tension Measurement
Basic Idea
Measuring a tension in the suture in minimally invasive surgery is very dicult
because of the geometric constraints on mounting sensor and the sterilizing problem.
Strain gauge transducers are the most widely used sensors in the applications of
force measurements. They can be mounted on the laparoscopic instruments and
measure the strains of the instruments according to the external forces as shown
124
in Figure 7.5. These sensors have high sensitivity, accuracy, and bandwidth with
simple electronic circuit. With these sensors, the tension can be measured directly.
The main drawback of using the strain gauge sensors in minimally invasive surgery is
that they are very sensitive to temperature variation. The contact with the human
soft tissues during the surgery may cause drift on the outputs. Another problem is
that the surgical instruments go through in high temperature process in order to be
sterilized for the repeated uses. It forces to have a disposal type strain gauge sensor.
Calibrating with the each tool should be considered.
125
and interacts with the environments as shown in Figure 7.8. The sensor output is
a generalized force vector consisting of force components and moment components.
This generalized force vector can be represented with a wrench:
126
F =
fx
fy
(7.5)
fz
Then, the measured wrenches from the base sensor can be expressed as sum of the
following components:
Fs = Fm + Fg + Fb ,
(7.6)
(7.7)
Then, next step is to calculate the equivalent wrench applied at the end-eector.
The measured base wrench should be transformed from base frame to tool frame.
127
Transformation of the wrench between two coordinate frames can be achieved with
the adjoint transformation, : 6 6 ,
0
.
pR R
(7.8)
Consider B is an inertial frame and E is a frame attached to the rigid body. Let
Fe represent an applied wrench at the origin of E with respect to the E coordinate
frame. Then, the wrench in Bs coordinate frame is given by
Fb = Teb Fe .
Thus, the wrench in the end-eector frame, Fe , is calculated from the measured base
wrench:
Fe = Tbe Fb .
It can be expanded as
Fe =
(7.9)
(7.10)
7.3
7.3.1
ulated about the desired value and the end-eector of robot is forced to lie on the
straight line. In order to maintain the direction we can impose articial constraints
so that the motion can be only occurred along the constraint line. The control objective here is to design a controller to solve the following problem.
Given dynamic model for robot manipulator, environment model and
128
fdes , C(x) = 0 with initial condition of f (0) = 0, q(0), q(0)
= 0, determine
a feedback control law, , so that the closed-loop system satises
f (t) fdes as t
C(x(t)) 0 t
where fdes is the desired tension, C(x) = 0 represents the holonomic constraint on
the task space variables and denes the desired direction along which the tension
would be applied. Clearly, the tension control problem during securing the knot can
be turned into force and motion regulation problem, which will be reviewed in the
following section.
7.3.2
(7.11)
where m is the torque vector applied to motion control and f is the torque vector
for force control. The hybrid controller, which will be addressed here, is similar in
many respect to the dynamic hybrid control approach. Consider a simple problem
of the three-link planer manipulator in Figure 7.9. The constraint can be expressed
by C(x) = 0, where x 2 is the generalized position vector in Cartesian space. To
express the end-eector position on the constraint line, a free motion function, f (x),
can be chosen such that c(x) and f (x) can be mutually independent. Then d(x) can
be dened by d(x) = [c(x)T f (x)T ]T and it represents the end-eector position on the
constraint hypersurfaces. Let Jc (x) = c(x)/x be the Jacobian of the constraint
129
130
written in joint space as
(q) = [1 (q), ..., m (q)]T ,
(q) = C(k(q)) = 0
(7.12)
c(x) k(q)
(q)
=
= JcJ
q
x q
J (q) mn .
(7.13)
Since there is no applied force against the side of the virtual slot, the required force
in the Cartesian space to generate the desired tension can be written as
F n ,
F = JfT ff
(7.14)
where ff nm is the force generating tension in the force controlled subspace.
Then, the joint torque f generating the force, ff , along the Jf direction is given by
f = J T F = J T JfT ff = JT ff .
(7.15)
ff = [0 Inm ]
Jc
Jf
Fe .
(7.16)
(7.17)
m = M (q)um + G(q)
(7.18)
um = Kp (q k 1 (d1 (
0
f (k(q))
Jc
1
))) Kd (q J
Jf
0
(7.19)
J q)
Jf
0
f = J T JcT
.
uf
(7.20)
131
The position regulation to satisfy the constraint is already veried in shared control
section, therefore hereafter only force regulation problem designing the force, uf , will
be considered. The existing force control scheme can be grouped into two categories.
The rst category is implicit force control, which achieves desired compliant behavior
through position error based control and is suitable to avoid excessive force buildup.
Compliance control and impedance control are belonged to this category. The second
category is explicit force control control. As the name implies, explicit force control
has an explicit closure of a force feedback loop such that the output force can be
regulated. Since the tension should be regulated about the desired value and the
measurement of tension is available, the explicit force control is suitable.
7.3.3
uf = Kf fr + Kp (fr fm ) + Kd (fr fm ) + Ki
(fr fm )dt Kv x m ,
(7.21)
where fr is the reference force, fm is the measured force, Kf is the feedforward force
gain, Kp is the proportional force gain, Kd is the derivative force gain, Ki is the
integral force gain, Kv is the velocity gain for the active damping, and x m is the
measured velocity.
In order to achieve the compliant transition during a contact period, damping
needs to be added to the systems. With damping, systems will be more stable and
can reduce the spike of force response. The derivative force gain can be used to
obtain damping with incorporating of numerical dierentiating the force signals.
However, due to presence of noise in the force signals, implementing of derivative
force control essentially is not feasible. Filtered derivative control may be used with
incorporating low-pass lter, which can reduce the noise at the expense of phase
lag. An alternative is to actively add the damping to the systems. Active damping
method is widely used to deal with impact problem and it is much considerably
eective for soft environment same as a suture model. For soft environment, the
velocity trajectory is considerably smoother than for sti one. For this reason, active
damping component is added. In addition, to ensure steady state performance and
132
robustness, it is advisable to endow the controller with an integral action. The
feedforward component usually needs to be added for the case of time-varying desired
force tracking problem, but in this study it is not considered. Finally, the explicit
force control law is given by
uf = Kp (fr fm ) + Ki
proportional
f orce
f eedback
7.4
7.4.1
(fr fm )dt K
x .
v
m
integral
f orce
f eedback
(7.22)
active
damping
f orward
Stability Analysis
Proportional plus Integral Control with Active Damping
(7.23)
(7.24)
133
With PI control, it becomes
m
x(t) + bx(t)
+ kx(t) = Kp (fm (t) fr (t)) Ki
(fm ( ) fr ( ))d,
(7.25)
where fr is the desired reference force. After substituting with fm = kx, taking the
Laplace transformation becomes
{ms2 + bs + k(1 + Kp ) +
kKi
Ki
}X(s) = (Kp +
)Fr (s).
s
s
(7.26)
Kp s + Ki
X(s)
=
.
3
2
Fr (s)
ms + bs + k(1 + Kp )s + kKi
(7.27)
(7.28)
The Routh criterion yields the stability bound on the values of Ki and Kp :
0 < Ki <
b
(1 + Kp ).
m
(7.29)
Note that the stability condition does not contain the environment stiness, k.
The stability can be achieved as long as the gains are bounded. From this stability
condition, since the impedance parameters, m, b, are xed, the way to increase the
range of the integral gain Ki is to either increase the proportional gain Kp or add
134
Root Locus
10
Imag Axis
10
5
Real Axis
(fm fr )dt Kv x.
(7.30)
(7.31)
(7.32)
135
Root Locus
20
15
10
Imag Axis
10
15
20
1
0.8
0.6
0.4
0.2
0.2
Real Axis
Figure 7.14: Root locus under explicit PI force control with Kp = 10.
(b + Kv )
(1 + Kp ).
m
(7.33)
Clearly, the upper bound of Ki increases with active damping as expected and it
can be seen in Figure 7.16. Figure 7.16 shows the root locus of the system with the
active damping under same parameters of Figure 7.13 where the crossover of j-axis
can be observed as Ki is increased without the active damping term. However,
the active damping provides the large upper bound of Ki and thus the stability
condition is satised. Note that another stationary pole is not shown in the plot for
enlargement of root locus.
136
Root Locus
0.4
0.3
0.2
Imag Axis
0.1
0.1
0.2
0.3
0.4
0.2
0.15
0.1
0.05
0.05
0.1
0.15
0.2
Real Axis
Figure 7.16: Root locus under explicit PI force control with the active
damping Kv = 1000 and Kp = 1.
7.4.2
the stability of the system. Due to the signal processing or transportation lag, there
exists a time delay on force measurement. To investigate the inuence of delayed
force measurement, the equation was modied with a time delay in measured force
term. The transfer function of time delay is given by the transform of the delayed
impulse response with T sec:
(7.34)
137
Clearly, the transfer function of a time delay is not a rational function. With a
delayed force measurement, the closed loop system is
m
x(t) + bx(t)
+ kx(t) = Kp (fm (t T ) fr (t)) Ki
(fm ( T ) fr ( ))d.
kKi sT
Ki
e )X(s) = (KP +
)Fd (s).
s
s
(7.35)
The irrational transfer function of a time delay can be approximated with Taylor
series expansion
esT = 1 sT +
s2 T 2 s3 T 3
+ .
2
6
(7.36)
kKi
Ki
]X(s) = (KP +
)Fd (s). (7.37)
s
s
(7.38)
(7.39)
(b kKp T ) > 0
(ii)
kKi > 0
b
kT
Ki > 0
KP <
Ki <
(7.40)
b kKp T
(1 + Kp ).
m + (b kKp T )T
138
Thus, we conclude that the stability bounds of a time delayed system can be obtained
as
Kp <
b
,
kT
0 < Ki < (1 + Kp ).
(7.41)
The eect on stability condition of a time delay can be investigated with the gain .
The upper bound gain of Ki , , depends on the the impedance parameters m, b, k
as well as the time delay T. Let be the reciprocal of , then
1
m
+ T,
=
b kKp T
(7.42)
>0
where b kKp T is always positive due to the rst stability condition of (7.40).
b + Kv
,
kT
0 < Ki < (1 + Kp ),
(7.43)
m
1
=
+ T.
(b + Kv ) kKp T
(7.44)
>0
It is seen that the eect of adding the active damping for the time delayed system
is to increase the range of Ki by providing the wide range of Kp as expected. The
resulting root locus is shown in Figure 7.19. As can be seen in the root locus plot,
139
Root Locus
20
15
10
Imag Axis
10
15
20
1
0.8
0.6
0.4
0.2
0.2
Real Axis
Figure 7.18: Root locus of the time delayed system under explicit PI force
control with Kp = 10.
the active damping make the system more stable by setting the upper bound of Ki
further high.
Root Locus
0.4
0.3
0.2
Imag Axis
0.1
0.1
0.2
0.3
0.4
0.2
0.18
0.16
0.14
0.12
0.1
0.08
0.06
0.04
0.02
0.02
Real Axis
Figure 7.19: Root locus of the time delayed system under explicit PI force
control with active damping Kv = 1000 and Kp = 10.
7.5
7.5.1
Experimental Study
Experimentation Environment
A six axis force/toqrue sensor (ATI Model 15/50) with force range of 65N
and torque range of 5N m is mounted between the base plate and the mounting
140
block as in Figure 7.20. The sensor outputs are interfaced to the PC through
Diameter
min
0.35
0.30
0.20
0.15
0.10
0.07
141
the tension value, which causes the tissue failure. The free tail of the suture is xed
Table 7.2: Suture pullout values.
Tissues
Fat
Muscle
Skin
Fascia
tension(N)
1.96
12.45
17.85
36.98
with a xture and the needle tail is attached to one of the EndoBot as shown in
Figure 7.20. The joint velocities are estimated with the washout lter and then
transformed to the Cartesian velocities, x m = J(q)q.
The discrete controller was
implemented at 2ms sampling rate.
7.5.2
control strategy described in the previous section. The basic tension control used
in these experiments consisted of rst bringing the end-eector close to a certain
position where the suture is not in tension. The hybrid control scheme was then
started and the desired force commanded in the direction of pulling the suture. This
caused the manipulator to come into in tension and exert the desired tension. The
data from each control loop was captured at the sampling rate (100Hz) and stored
by the PC for o-line analysis. The force reference in these experiments is stepped
to 15N . The performance of each controller was quantied using the following
measures:
N
(f (k)fr )2
k=1 m
142
error to the step such that the actual force converged to the desired value of -15
Newtons. However, the initial force spike was observed during the transient contact
period due to the integral action and lack of the damping. Next two gures show
the eect of adding the active damping into the system in order to reduce the initial
force spike. With same integral gain, the active damping seems very eective to
achieve the compliant contact transition. The improvement in the performance of
this controller was made by changing the active damping gain, Kv , as shown in
Figure 7.23. Table 7.3 summarizes the performance of these controller. From these
Table 7.3: Comparison of force controller with active damping.
Gains
eRM S N
Ki = 0.5, Kv = 0
5.92
Ki = 0.5, Kv = 1000
4.76
4.70
Ki = 0.5, Kv = 2000
e N
71.91
31.45
27.21
results, it is clear that the active damping seems promising method to reduce the
initial force spike at the cost of increasing the settling time. Introducing the proportional gain, Kp achieved the faster response at the expense of slightly increasing the
force spike as shown in Figure 7.24. Figure 7.25 shows the eect of increasing the
proportional gain, Kp to the stability bound. As can be seen in plot, the bouncing
phenomenon was observed and the performance was degraded as shown in Table 7.4.
e N
31.24
33.19
The bouncing phenomenon was also observed as decreasing the active damping gain, Kv , and the experimental results can be seen in Figures 7.267.28. As
predicted, reducing the active damping gives rise to lower stability bounds of the
control gain and consequently shows the tendency of being unstable. A signicant
143
improvement in performance of force control was achieved by modifying the force
reference trajectory proposed in [56]. From the notion that small initial force error
prevents the integrator from excessively winding up, the force reference trajectory
was modied such that the desired force value can be increased with a slop according
to the current measure force value. Clearly, the initial force error is small and consequently the initial force spike was reduced considerably as shwon in Figure 7.29. It
is observed that the reduction of the force spike on the force trajectory is apparent
from the plot and the performance in terms of the RMS force error and maximum
force value is in Table 7.5.
Table 7.5: Performance of force controller with a modied force reference
trajectory.
Gains
eRM S N
Kp = 0.2, Ki = 0.3, Kv = 2000
1.93
e N
15.83
10
Desired Force
Measured Force
0
10
Force(N)
20
30
40
50
60
70
80
10
15
time(s)
20
25
30
144
10
Desired Force
Measured Force
0
10
Force(N)
20
30
40
50
60
70
80
10
15
time(s)
20
25
30
Figure 7.22: Experimental data from integral control plus active damping
with Ki = 0.5 and Kv = 1000.
10
Desired Force
Measured Force
0
10
Force(N)
20
30
40
50
60
70
80
10
15
time(s)
20
25
30
Figure 7.23: Experimental data from integral control plus active damping
with Ki = 0.5 and Kv = 2000.
10
Desired Force
Measured Force
0
10
Force(N)
20
30
40
50
60
70
80
10
15
time(s)
20
25
30
Figure 7.24: Experimental data from PI control plus active damping with
Kp = 0.3, Ki = 0.5, and Kv = 2000.
145
10
Desired Force
Measured Force
0
10
Force(N)
20
30
40
50
60
70
80
10
15
time(s)
20
25
30
Figure 7.25: Experimental data from PI control plus active damping with
Kp = 0.5, Ki = 0.5, and Kv = 2000.
10
Desired Force
Measured Force
0
10
Force(N)
20
30
40
50
60
70
80
10
15
time(s)
20
25
30
Figure 7.26: Experimental data from PI control plus active damping with
Kp = 0.2, Ki = 0.75, and Kv = 2000.
10
Desired Force
Measured Force
0
10
Force(N)
20
30
40
50
60
70
80
10
15
time(s)
20
25
30
Figure 7.27: Experimental data from PI control plus active damping with
Kp = 0.2, Ki = 0.75, and Kv = 1000.
146
10
Desired Force
Measured Force
0
10
Force(N)
20
30
40
50
60
70
80
10
15
time(s)
20
25
30
Figure 7.28: Experimental data from PI control plus active damping with
Kp = 0.2, Ki = 0.75, and Kv = 500.
10
Desired Force
Measured Force
0
10
Force(N)
20
30
40
50
60
70
80
10
15
time(s)
20
25
30
Figure 7.29: Experimental data from PI control plus active damping with
Kp = 0.2, Ki = 0.3, Kv = 2000, and the modied force reference
trajectory with Fdes = (f + 5)N.
CHAPTER 8
CONCLUSIONS
The goal of this thesis is to develop a robotic system for laparoscopic suturing task.
This chapter concludes the thesis by providing a summary of results obtained in the
preceding chapters and suggestion of areas for future research.
8.1
Summary
The focus of this thesis is supervisory autonomous robotic system for MIS su-
turing. A new surgical robotic system built in-house for minimally invasive surgery
is presented. Based on the analytical model, parameter identication is carried out
with various input signals. For the robotic suturing using the EndoBot, we carried
out the analysis on the suturing task and developed and implemented robotic suturing algorithms. For knot placement, the sliding condition based on a two-point
contact model of a knotted suture is developed. In order to guarantee the safe operation in the suturing task in which dynamics of the system cannot be completely
predicted over the entire range of operating due to the non-deterministic nature of
behavior of environments, a human sharing supervisory controller is implemented
with the corresponding state transition diagram for suturing task. For autonomously
evolution of suturing task, an energy based output feedback controller and an optimal state feedback controller with the Kalman lter based on the globally linearized
system are implemented. When human operators interact with robot systems, the
augmentation plays a key role in order to enhance the humans capability. When a
surgeon and a robot share the dierent control aspect, shared control proposed in
this thesis can augment the humans capability by imposing the articial constraints.
Finally, a base force/torque sensor method is presented for tension measurement and
a hybrid force/position strategy is implemented to eectively regulate the tension
while achieving the desired motion prole.
147
148
8.2
Future Work
This session describes several possible areas of future research.
149
it is necessary to obtain the optimal tension values through the experiments
with in vivo tissues. Another thrust of the research on suturing will be to
quantitatively evaluate the knot quality. It might be necessary to construct
the evaluation criteria such as the size of the knot and the magnitude of knot
breakage force.
Surgical Simulator The other trust to surgical applications can be to develop a
virtual reality simulator for the suturing tasks in minimally invasive surgery
[88, 89, 90]. A dynamic suture modeling is required to train surgeons for the
suturing tasks [85].
Surgical Robot System for the Beating Heart Beating heart bypass surgery
can eliminate many of the complications associated with stopping the heart
and using a heart-lung machine [92]. Beating heart bypass surgery requires
surgeons to stitch together vessels on the beating heart. Motion synchronization is the key technical diculty [91].
LITERATURE CITED
[1] H. Paul, B.D. Mittelstadt, W.L. Bargar, B.L. Musits, R.H. Taylor, P.
Kazanzides, J.F. Zuhars, and B. Williamson, A Surgical robot for total hip
replacement surgery, in Proceedings of the 1992 IEEE International
Conference on Robotics and Automation, vol. 1, pp 606-611, Nice, France,
1992.
[2] T.C. Kienzle, S.D. Stulberg, M. Peshkin, A. Quaid, and C-H. Wu,An
integrated CAD-robotics system for total knee replacement surgery, in
Proceedings of the 1993 IEEE International Conference on Robotics and
Automation, vol. 1, pages 889-894, Atlanta, GA, 1993.
[3] M. Fadda, T. Wang, M. Marcacci, S. Martelli, G. Marcenaro, M. Nanetti, C.
Paggetti, A. Visani, and S. Zaagnini, Computer-assisted knee arthroplasty
at Rizzoli institutes, In Proceedings of the International Symposium on
Medical Robotics and Computer Assisted Surgery, pages 26-30, 1994.
[4] Ho, S.C., R.D. Hibberd, and B.L. Davies, Robot assisted knee surgery,
IEEE Engineering in Medicine and Biology Magazine Sp. Issue on Robotics in
Surgery, May/June, pages 292-300, 1995.
[5] R.E. Ellis, From scans to sutures: robotics methods for computer-enhanced
surgery, in 9th International Symposium of Robotics Research, pp203-210,
Snowbird, Utah, 1999.
[6] I. W. Hunter, T. D. Doukoglou, S. R. Lafontaine, P. G. Charette, L. A. Jones,
M. A. Sagar, G. D. Mallinson, and P. J. Hunter, A teleoperated
microsurgical robot and associated virtual environment for eye surgery,
Presence, vol. 2, pages 265-280, 1993.
[7] Y. S. Kwoh, J. Hou, E. A. Jonkeere, and S. Hayati, A robot with improved
absolute positioning accuracy for CT guided stereotactic brain surgery, IEEE
Transactions on Biomedical Engineering, 35(2):153-160, 1988.
[8] B.Davies, S.Starkie, S.J. Harris, E. Agterhuis, V.Paul, and L.M. Auer,
Neurobot: a special-purpose robot for neurosurgery, in Proceedings of the
2000 IEEE International Conference on Robotics and Automation, vol. 4,
pages 4104-4109, San Francisco, CA, 2000.
[9] A. Faraz and S. Payandeh, A Robotic Case Study: Optimal design for
laparoscopic positioning stands, in Proceedings of the 1997 IEEE
International Conference on Robotics and Automation, pages 1553-1560,
Albuquerque, NM, 1997.
150
151
[10] J. Funda, B. Eldridge, K. Gruben, S.Gomory, and R. Taylor,Comparison of
two manipulator designs for laparoscopic surgery, IEEE Engineering in
Medicine and Biology Magazine, Vol.14, No.3, pages 289-291, 1994.,
Telemanipulator and Telepresence Technologies, SPIE, Vol.2351, pages
172-183, 1994.
[11] A.J. Madhani and J.K Salisbury, Wrist mechanism for surgical instrument
for performing minimally invasive surgery with enhanced dexterity and
sensitivity, U.S. Patent No. 5,797,900, 1997.
[12] M.C. Cavusoglu, F. Tendick, M. Cohn, and S.S. Sastry, A laparoscopic
telesurgical workstation, IEEE Transactions on Robotics and Automation,
Vol.15, No. 4, pages 728-739, 1999.
[13] A. Faraz and S. Payandeh, Synthesis and workspace study of endoscopic
extenders with exible stems, ERL Technical Report.
[14] A. Faraz, S. Payandeh, and A. Nagy, Issues and design concepts in
endoscopic extenders, in Proceedings of 1995 IFAC Man/Machine Systems,
pages 231-236,1995.
[15] C. Reboulet and S. Durand-Leguay, Optimal design of redundant parallel
mechanism for endoscopic surgery, in Proceedings of the 1999 IEEE/RSJ
International Conference on Intelligent Robots and Systems, Vol. 2, pages
1432-1437, 1999.
[16] C. Scott-Conner, The Sages Manual: Fundamentals of laparoscopy and GI
endoscopy, Springer, 1998.
[17] Z. Szabo and A. Cuschieri, Tissue approximation in endoscopic surgery, ISIS
Medical Media, 1995.
[18] C. Bernard, H. Kang, S.K. Singh, and J.T. Wen, Robotic system for
collaborative control in minimally invasive surgery, Industrial Robot, 26(6),
Nov/Dec, pages 476-484, 1999.
[19] A.Melzer, M.O. Schurr, M. Lirich, B. Klemm, D.Stockel, and G.Buess,
Future trends in endoscopic suturing, End. Surg., pages 78-82, 1994.
[20] A.J. Madhani, G. Niemeyer, and J.K. Salisbury, The black falcon: a
teleoperated surgical instrument for minimally invasive surgery, in
Proceedings of the 1998 IEEE/RSJ International Conference on Intelligent
Robots and Systems, Vol. 2, pages 936-944, 1998.
[21] C. Cao, C. MacKenzie, S. Payandeh,Task and motion analyses in endoscopic
surgery, in ASME IMECE Conference Proceesings: 5th Annual Symposium
on Haptic Interfaces for Virtual Environment and Teleoperator Systems,
pages 583-590, Atlanta, GA, 1996.
152
[22] J. Funda, R. Taylor, K. Gruben, and D. La Rose, Optimal motion control for
teleoperated surgical robots, in SPIE Symposium on Optical Tools for
Manufacturing and Advanced Automation, Vol. 2057, pages 211-222, Boston,
MA, 1993.
[23] J. Funda, R. Taylor, B. Eldridge, S. Gomory, and K. Gruden, Constrained
cartesian motion control for teleoperated surgical robots, IEEE Transactions
on Robotics and Automation, Vol.12, No. 3, pages 453-465, 1996.
[24] Y. Wang, D.R. Uecker, K.P. Laby, J. Wilson, S. Jordan, and J. Wright,
Method and apparatus for performing minimally invasive cardiac
procedures, U.S. Patent No. 5,762,458, 1998.
[25] M. Mitsuishi, T. Watanabe, H. Nakanishi, T. Hori, H. Watanabe, and B.
Kramer, A tele-microsurgery system across the internet with a xed
viewpoint/operation-point, in Proceeding of the 1995 IEEE/RSJ
International Conference on Intelligent Robots and Systems, Vol. 2, pages
178-185, 1995.
[26] S. Charles, H. Das, T. Olm, C. Boswell, G. Rodrigues, R. Steele, and D.
Istrate, Dexterity-enhanced telerobotic microsurgery, in International
Conference on Advanced Robotics, Monterey, CA, 1997.
[27] F. Tendick and M.C. Cavusoglu,Human-machine interfaces for minimally
invasive surgery, in Proceedings of the IEEE International Conference on
Engineering Medical Biology Society, Chicago, IL, 1997.
[28] F. Tendick, M.S. Downes, M.C. Cavusoglu, and L.W. Way, Development of
virtual environments for training skills and reducing errors in laparoscopic
surgery,in Proceedings of the SPIE International Symposium on Biological
Optics (BIOS98), San Jose, CA, 1998.
[29] H. Kazerooni and J. Guo, Human extenders, Journal of Dynamics Systems,
Measurement and Control, Vol. 115, pages 281-290, 1993.
[30] T.B. Sheridan, Telerobotics, automations, and human supervisory control,
Cambridge, MIT Press, 1992.
[31] P.G. Backes and K.S. Tso, UMI: an interactive supervisory and shared
control system for telerobotics, in Proceedings of the 1990 IEEE
International Conference on Robotics and Automation, pages 1096-1101,
Cincinnati, OH, 1990.
[32] S.A. Hayati and S.T. Venkataraman, Bilevel shared control for
teleoperators, U.S. Patent No. 5,086,982, 1992.
153
[33] A. Douglas and Y. Xu, Real-time shared control system for space
telerobotics, Journal of Intelligent and Robotic Systems: Theory and
Applications, vol. 13, pages 247-262, 1995.
[34] R. Kumar, P. Berkelmen, P. Gupta, A. Barnes, P. Jensen, L.L. Whitcomb,
and R.H. Taylor, Preliminary experiments in cooperative human/robot force
control for robot assisted microsurgical manipulation, in Proceedings of the
2000 IEEE International Conference on Robotics and Automation, 2000.
[35] R. Kumar, P. Jensen, and R.H. Taylor, Experiments with a steady hand
robot in constrained compliant motion and path following, In Proceedings of
IEEE RO-MAN99, pages 92-97, Pisa, Italy, 1999.
[36] R. Kumar, T.M. Goradia, A.C. Barnes, P. Jensen, L.L. Whitcomb, D.
Stoianovici, L.M. Auer, and R.H. Taylor, Performance of robotic
augmentation in microsurgery-scale motions, In Proceedings of Medical
Image Computing and Computer Assisted Intervention, pages 1108-115,
Cambridge,UK, 1999.
[37] G.J. Hamlin, and A.C. Sanderson, A novel concentric multilink spherical
joint with parallel robotics applications, in Proceedings of the 1994 IEEE
International Conference on Robotics and Automation, pages 1267-1272, 1994.
[38] R.M. Murray, Z. Li, and S.S. Sastry, A mathematical introduction to roboti
manipulation, CRC Press, Boca Raton, FL, 1994.
[39] B. Armstrong, Friction: Experimental determination, modelling and
compensation, in Proceedings of the 1988 International Conference on
Robotics and Automation, pages 1422-1427, 1988.
[40] B. Armstrong-Helouvry, Control of machine with friction, Kluwer Academic,
1991.
[41] P. Lischinsky, C. Canudas-de-Wit, and G. Morel, Friction compensation for
an industrial hydraulic robot, IEEE Control Systems Magazine, vol. 19, no.
1, pages 25-32, 1999.
[42] S.C.P Gomes and J.P. Chretien, Dynamic modeling and friction
compensated control of a robot manipulator joint, in Proceedings of the 1992
International Conference on Robotics and Automation, pages 1429-1435, 1992.
[43] G. Morel and S. Dubowsky, The precise control of manipulators with joint
frcition: a base force/torque sensor method, in Proceedings of the 1996 IEEE
International Conference on Robotics and Automation, pages 360-365, 1996.
[44] M. Meggiolaro, P. Jae, and S. Dubowsky, Achieving ne absolute
positioning accuracy in large powerful manipulators, in Proceedings of the
154
1999 IEEE International Conference on Robotics and Automation, pages
2819-2824, 1999.
[45] J.T. Wen and D.S. Bayard, A new class of control laws for robotic
manipulators, Part I: Non-adaptive case, International Journal of Control,
47(5), pages 1361-1385, 1988.
[46] J.T. Wen and S.H. Murphy, Position and force control of robot arms, IEEE
Transactions on Automatic Cotnrol, 36(3), pages 365-374, 1991.
[47] C. Canudas-de-Wit, B. Siciliano, and G. Bastin, Theory of robot control,
Springer, 1996.
[48] M.R. Schroeder, Synthesis of low-peak-factor signals and binary sequences of
low autocoorelation, IEEE Transactions on Information Theory, 16, pages
85-89, 1970.
[49] K. Koztowski, Modeling and identication in robotics, Springer, 1998.
[50] C.H. An, C.G. Atkeson, and J.M. Hollerbach, Estimation of inertial
parameters of rigid body links of manipulators, in Proceedings of the 24th
IEEE Conference on Decision and Control, pages 990-995, 1985.
[51] B. Armstrong, On nding exciting trajectories for identication experiments
involving systems with nonlinear dynamics, International Journal of
Robotics Research, Vol.8, pages 28-48, 1989.
[52] M. Gautier and W. Khalil, Exciting trajectories for the identication of base
inertial parameters of robots, International Journal of Robotics Research,
Vol.11, pages 362-375, 1992.
[53] M. Gautier, Optimal motion planning for robots inertial parameters
identication, in Proceedings of the 31th IEEE Conference on Decision and
Control, pages 70-73, 1992.
[54] C. Presse and M. Gautier, New criteria of exciting trajectories for robot
identication, in Proceedings of the 1993 IEEE Inernatinal Conference on
Robotics and Automation, pages 907-912, 1993.
[55] M. Gautier, A comparison of ltered models for dynamic identication of
robots, in Proceedings of the 35th IEEE Conference on Decision and Control,
pages 875-880, 1996.
[56] L.S. Wilnger, J.T. Wen, and S. Murphy, Integral force control with
robustness enhancement, IEEE Control System Magazine, 14(1), pages
31-40, 1994.
155
[57] M. Takegaki and S. Arimoto, A new feedback method for dynamic control of
manipulators, ASME J. Dynamic Systems, Measurement and Control, 103,
pages 119-125, 1981.
[58] S. Arimoto and F. Miyazaki, Stability and robustness of PID feedback conrol
for robot manipulators of sensory capability, In Proceeding of the 1st
International Symposium of Robotics Research, pages 783-799, 1983.
[59] J. Luh, M. Walker, and R. Paul, Resolved acceleration control of mechanical
manipulators, IEEE Transactions on Automatic Control, 25, pages 468-474,
1980.
[60] R. Paul, Robot manipulators: Mathematics, programming, and control,
Cambridge, MIT Press, 1981.
[61] A. Bryson and Y.C. Ho, Applied Optimal Control , Halsted, 1975.
[62] E. Freund, Fast nonlinear control iwth arbitrary pole-placement for
industrial robots and manipulators, International Journal of Robotics
Research, 1, pages 65-78, 1982.
[63] K. Kreutz, On manipulator control by exact linearization, IEEE
Transactions on Automatic Control, 34, pages 763-767, 1989.
[64] G.L. Luo, G.N. Saridis, and C.Z. Wang, A dual-mode control for robotic
manipulators, In Proceeding of IEEE conference on Decision Control, pages
409-414, 1986.
[65] R. Johansson, Quadratic optimization of motion coordination and control,
IEEE Transactions on Automatic Control, 35(11), pages 1197-1208, 1990.
[66] J.T.Wen, A unied perspective on robot: The energy Lyapunov function
approach, International Journal of Adaptive Control and Signal Processing,
4(6), pages 487-500, 1990.
[67] F. Lin and R.D. Brandt, An optimal control approach to robust control of
robot manipulators, IEEE Transactions on Robotics and Automation, 14(1),
pages 69-77, 1998.
[68] C. Abdallah, D. Dawson, P. Dorato, and M. Jamshidi, Survey of robust
control for rigid robots, IEEE Control Systems Magazine, 11(2), pages 24-30,
1991.
[69] H.G. Sage, M.F. De Mathelin, and E. Ostertag, Robust control of robot
manipulators: a survey, International Journal of Control, 72(16), pages
1498-1522, 1999.
156
[70] H. Berghuis, P. Lohnberg, and H. Nijmeijer, Tracking control of robots using
only position measurements, in Proceeding of Conference on IEEE Decision
and Control, pages 1039-1040, 1991.
[71] B. Bona and M. Indri, Analysis and implementation of observers for robotic
manipulators, in Proceeding of the IEEE International Conference on
Robotics and Automation, pages 3006-3011, 1998.
[72] N. Hogan, Impednace control: An approach to manipulation. PartsI-III,
ASME Journal of Dynamic Systems, Measurement, and Control, 107, pages
1-24, 1985.
[73] M. Spong and M. Vidyasagor, Robot dynamics and control, John Wiley and
Son, New York, 1989.
[74] B. Waibel and H. Kazerooni, Theory and experiments on the stability of
robot compliance control, IEEE Transactions on Robotics and Automation,
7(1), pages 95-104, 1991
[75] M. Raibert and J. Craig, Hybrid position/force control of manipulators,
ASME Journal of Dynamic Systems, Measurement, and Control, 103, pages
126-133, 1981.
[76] T. Yoshikawa, Dynamics hybrid position/force control of robot
manipulators- description of hand constraints and calculation of joints driving
forces, in Proceeding of the IEEE International Conference on Robotics and
Automation, pages 1393-1398, 1986.
[77] O. Khatib, A unied approach for motion and force control of robot
manipulators: The operational space formulation, IEEE Journal of Robotics
and Automation, pages 43-53, 1987.
[78] T. Yoshikawa, Force control of robot manipulators, in Proceeding of the
IEEE International Conference on Robotics and Automation, pages 220-226,
2000.
[79] S. Chiaverini and L. Sciavicco, The parallel approach to force/position
control of robotic manipulators, IEEE Transactions on Robotics and
Automation, 9(4), pages 361-373, 1993.
[80] L.S. Wilnger, J.T. Wen, and S. Murphy, Integral force control with
robustness enhancement, in Proceeding of the IEEE International Conference
on Robotics and Automation, pages 100-106, 1994.
[81] N. Mandal and S. Payandeh, Force control strategies for compliant and sti
contact: An experimental study, in IEEE International Conference on
Systems, Man, and Cybernetics, 2 , pages 1285-1290, 1994.
157
[82] U.S. Department of Health and Human Services, Guidance for surgical suture,
2000.
[83] S.Y. Lai, Sutures and needles, eMedicine Journal, 2(9), 2001.
[84] G. Liu, K. Iagnemma, S. Dubowsky, and G. Morel, A base force/torque
sensor approach to robot manipulator inertial parameter estimation, in
Proceeding of the IEEE International Conference on Robotics and
Automation, pages 3316-3321, 1998.
[85] J. Phillips, A. Ladd, and L.E. Kavraki, Simulated knot tying, in Proceeding
of the IEEE International Conference on Robotics and Automation, 2002.
[86] B. Hannaford, J. Trujillo, M. Sinanan, M. Moreyra, J. Rosen, J. Brown, R.
Leuschke, and M. MacFarlane, Computerized endoscopic surgical grasper,
in Proceeding of Medicine Meets Virtual Reality, pages 111-117, 1998.
[87] G. Moy, C. Wagner, and R.S. Fearing, A compliant tactile display for
teletaction, in Proceeding of the IEEE International Conference on Robotics
and Automation, pages 3309-3415, 2000.
[88] J. Rosen, B. Hannaford, C.G. Richards, and M.N. Sinanan, Markov
modeling of minimally invasive surgery based on tool/tissue interaction and
force/torque signatures for evaluating surgical skills, IEEE Transactions on
Biomedical Engineering, 48(4), pages 579-591, 2001.
[89] K. Montgomery, L. Heinrichs, C. Bruyns, S. Wildermuth, C. Hasser, S.
Ozenne, and D. Bailey, Surgical simulator for hysteroscopy: a case study of
visualization in surgical training, in Proceeding of the Visualization,
pages449-452, 2001.
[90] C. Sharma and T. Kesavadas, Investigation of haptic framework for,
quantitative design analysis in a virtual, environment, in Proceeding of the
International Conference on Virtual Systems and Multimedia, pages844-853,
2001.
[91] Y. Nakamura, K. Kishi, and H. Kawakami, Heartbeat synchronization for
robotic cardiac surgery, in Proceeding of the IEEE International Conference
on Robotics and Automation, pages 2014-2019, 2001.
[92] F. Devernay, F. Mourgues, and E. Coste-Maniere, Towards endoscopic
augmented reality for robotically assisted minimally invasive cardiac surgery,
in Proceesings of the International Workshop on Medical Imaging and
Augmented Reality pages 16-20, 2001.
APPENDIX A
INVERSE KINEMATICS USING SUBPROBLEMS
Since the EndoBot has a simple kinematic conguration, the inverse kinematics can
be solved with conventional algebraic or geometric method. But for consistency
purpose, the exponentials formula is introduced to solve the inverse kinematics.
The exponentials formula can solve the inverse kinematics problem with the PadenKahan subproblems whose solutions are known in [38] and has a geometric meaning.
The inverse kinematics can be solved from the following steps:
Step 1 (solve for the translational distance, q4 ) From the forward kinematics
derivation above, a given position vector from the base frame to tool frame
p0T can has the following form:
(A.1)
where, p34 = q4 h3 . Since the position vector p34 lies on the axis of h3 , it yields
(A.2)
(A.3)
Step 2 (solve for rst two angles, q1 and q2 ) Since q4 is known and h1 and h2
intersect, (A.2) can be solved using Subproblem 2 (Rotation about two subsequent axes) and it gives q1 and q2 from (A.2). Due to the multiple solutions
property of Subproblem 2, two possible solution might be existed, but it can
be easily determined from the working range consideration.
Step 3 (solve for the roll angle, q3 ) The joint angle of the axis 3, q3 , is simply
the roll angle .
158
APPENDIX B
LAGRANGIAN FOR A ROBOT MANIPULATOR
B.1
in Figure B.1. Dene the coordinate frame, Oi , attached to the ith joint. Let hi
frame expressed in Oi , Ici be the inertia tensor of the ith link expressed in the frame
attached at the center of mass, and mi be the mass of ith link.
The kinetic energy of the ith link is
mi pci i
1 i Ii
Ti =
,
2 vi
mi pci mi I
vi
(B.1)
Mi
where Ii = Ici +mi (pci (pci )T pci 2 I) denotes the inertia tensor of the ith link expressed
in Oi frame, wi is the instantaneous angular velocity and vi the linear velocity of the
ith link with respect to the instantaneous ith coordinate frame. Mi is the generalized
inertia matrix for the ith link. The total kinetic energy of the manipulator with nth
joints is
T =
n
i=1
159
Ti .
(B.2)
160
To represent the kinetic energy with the generalized coordinates, the instantaneous
angular velocity and the linear velocity can be written by
= Ji (q)q,
vi
(B.3)
h1
hi1
hi 0 0
Ji (q) =
,
i1 pi1,i 0 0 0
i p1i h
h
(B.4)
where all vectors are represented in ith frame. Then, the total kinetic energy of the
manipulator can be given by
T =
=
n
1
qT JiT (q)Mi Ji (q)q
2 i=1
n
1 T
M (q)
(B.5)
It gives
1
T = qT M (q)q,
2
(B.6)
where M (q) nn is the symmetric, positive denite mass inertia matrix.
B.2
(B.7)
where (p0i + pci )0 represents the height of the ith center of mass in the base frame
and g = [0 0
n
i=1
mi g T (p0i + pci )o .
(B.8)
APPENDIX C
LEAST SQUARE METHOD
The equation of motion of a robot manipulator can be given with m measurements
at given time instants t1 , ..., tm
+ ,
=
(C.1)
where mn is the matrix in which each row represents regressor at each time,
m is a vector with (tk ), and m is a vector with (tk ).
The least square solution can be interpreted as a minimum error norm solution
through the orthogonal mapping of vector spaces as shown in Figure C.1. A matrix
161
162
(C.2)
(C.3)
= T .
T
(C.4)
It becomes
1 T .
= (T )
LS
(C.5)
APPENDIX D
PERSISTENT EXCITATION AND OBSERVABILITY
GRAMIAN
A regression matrix is called persistent exciting if there exist positive constants
1 , 2 and such that
a I
t+
t
2 I.
T d
(D.1)
= A(t)x(t),
x(0) = x0
y(t) = c(t)x(t)
(D.2)
(D.3)
(D.4)
(D.5)
(D.6)
163
164
gramian is uniformly bounded and positive denite:
a I
tf
t0
LTo Lo d =
tf
T C T Cd
t0
2 I.
(D.7)
(D.8)
APPENDIX E
DIFFEOMORPHISM
Dieomorphism A map between manifolds is called as a homeomorphism if it is
bijective (one-to-one and onto), continuous, and has a continuous inverse. A
homoemorphism which is dierentialble and has a dierentiable inverse is a
dieomorphism.
Bijective A transformation which is one-to-one and onto is called as bijection. In
other words, it allows every state to be accessed (onto) and has precisely one
pre-image for each state (one-to-one).
A function f from A to B is called one-to-one (or injection) if x, y A
(f (x) = f (y)) x = y.
165
166
A function is called a bijection if it is both an injection and surjection.