ROBOTIC ASSISTED SUTURING IN MINIMALLY INVASIVE SURGERY By Hyosig Kang A Thesis Submitted to the Graduate Faculty of Rensselaer Polytechnic Institute in Partial Fulfillment of the Requirements for the Degree ofDOCTOR OF PHILOSOPHY Major Subject: Mechanical Engineering Approved by the Examining Committee: Dr. John T. Wen, Thesis Adviser Dr. Stephen J. Derby, Co-Thesis Adviser Dr. Daniel Walczyk, Member Dr. Harry E. Stephanou, Member Rensselaer Polytechnic Institute Troy, New York May 2002 (For Graduation August 2002)
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
7.17 Explicit PI force control with time delay. . . . . . . . . . . . . . . . . . 136
7.18 Root locus of the time delayed system under explicit PI force controlwith K p = 10. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
7.19 Root locus of the time delayed system under explicit PI force controlwith active damping K v = 1000 and K p = 10. . . . . . . . . . . . . . . . 139
7.23 Experimental data from integral control plus active damping with K i =0.5 and K v = 2000. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144
7.24 Experimental data from PI control plus active damping with K p = 0.3,K i = 0.5, and K v = 2000. . . . . . . . . . . . . . . . . . . . . . . . . . . 144
7.25 Experimental data from PI control plus active damping with K p = 0.5,K i = 0.5, and K v = 2000. . . . . . . . . . . . . . . . . . . . . . . . . . . 145
7.26 Experimental data from PI control plus active damping with K p = 0.2,
K i = 0.75, and K v = 2000. . . . . . . . . . . . . . . . . . . . . . . . . . 145
7.27 Experimental data from PI control plus active damping with K p = 0.2,K i = 0.75, and K v = 1000. . . . . . . . . . . . . . . . . . . . . . . . . . 145
7.28 Experimental data from PI control plus active damping with K p = 0.2,K i = 0.75, and K v = 500. . . . . . . . . . . . . . . . . . . . . . . . . . . 146
7.29 Experimental data from PI control plus active damping with K p = 0.2,K i = 0.3, K v = 2000, and the modified force reference trajectory withF des = −(f + 5)N. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146
With the completion of this thesis I am again at a crossroad in my life. The time Ihave spent here has been very fruitful and many people came into my life to inspire
and encourage this effort.
I would first 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 five 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 firmly 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
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 final, and most heartfelt, acknowledgement must go to my wife, Kyunga,who supported my decision to embark in graduate studies despite the significant
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
Open surgery traditionally involves making a large incision to visualize the operativefield 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 fit 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 offers more technical difficulties to surgeons.
Despite the lack of dexterity and perception, all surgeries are moving toward
MIS due to the benefits to patients. However, the demand on surgeons is much
higher during suturing, which is the primary tissue approximation method. Sutur-
ing is known as one of the most difficult tasks in MIS and consumes a significant
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 de-
velopment of the EndoBot controller that is capable of a range of MIS operations
including manual, shared, and supervised autonomous operations. Due to the chal-
lenge 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.
The incision needed to allow surgeons to visualize the field 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 inthe 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 fit into the body through the several pencil-sized holes instead of one
large incision. By eliminating the significant 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 offers 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
benefits to patients at the expense of a more stressful environment to surgeons.
The demands to surgeons for dealing with these technical difficulties are higher
during the suturing task, which is the primary tissue approximation method and
has been known as one of the most difficult tasks in MIS operations and uses a
significant 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 Neu-
romate (ISSS/immi). It is important to note that in the various surgical robots
described above, the surgical procedures are still completely performed by the hu-
man surgeon High skill level procedures such as suturing and precise tissue dissection
continue to depend on the expertise of surgeons, and surgeon’s commands are mim-
icked by the robotic devices through computer control. Despite the important role
and technical challenge of suturing in MIS, there has been little research effort re-
ported 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, whichwe named the EndoBot, for MIS operations. The EndoBot is designed for the col-
laborative 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
surgeon’s manual control, or autonomously where the complete robot system is un-
der computer control and surgeon’s supervision. Furthermore, the robotic tools can
be quickly changed from a robotic docking station, allowing different 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 flexible 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 sur-
geons, conduct certain tasks autonomously to reduce the strain on surgeons, remove
the variability of surgeon’s training levels, and enhance the system efficiency 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 param-eters are identified experimentally.
• Suturing task analysis and implementation. The suturing task is decomposed
into five subtasks and the technical difficulties on each task are analyzed. Knot
placement problem is formulated, and the sliding condition is proposed. The
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 flexible stems was formulated. They performed
the optimization on design of flexible stems by defining 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 free-
dom, was presented in [15], and they carried out an optimal design of the
mechanism.
Laparoscopic suturing Suturing is one of the most difficult tasks and takes asignificant percentage of operating time and involves in complex motion plan-
ning. 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
difference between surgeons lies in the proficiency 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 five 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.
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 Motion and Force Control of Rigid Robot Systems2.3.1 Robot Motion Control
Robot manipulators have complex nonlinear dynamics and there are many con-
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 reflected 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-
edge of positions and velocities. The immediate solution to get the velocity signals
is to numerically differentiate the position signals. Inherently numerical differenti-
ating is very sensitive to the noises because of the improper function characteristics.
When the system dynamics is given, the most effective method to estimate all statescan 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 Robot Force Control
When robot manipulators interact with the environments, force control is es-
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 classified into two categories. The first 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 pro-
posed 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 thisscheme. 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
In laparoscopic surgery, a patient’s abdomen is inflated with carbon dioxide
(CO2) through a needle to lift the abdominal wall away from the organs so as to
expose and access the operating field, and then three pencil-sized small holes are
punctured on abdominal wall for fitting of laparoscopic instruments and camera as
shown in Figure 3.1. Due to the requirement of minimizing the tear of the incisionpoint, 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.
Figure 3.1: Fulcrum effect in laparoscopic surgery.
Fulcrum accommodability Due to the fulcrum constraint, it is required for theMIS surgical robot manipulator, which passes through a small hole, to have
an effective center of rotation at the incision points. The remote center of
rotation can be implemented with several mechanical design such as spherical
joint, spherical link mechanism, double-parallelogram mechanism [37, 20], and
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 designconstraints, 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 mini-
mum 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 semi-
circular arches for yaw and pitch, and a sleeve that can generate rolling motion.
The translational stage carries the specific 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 patient’s abdominal wall. Therefore, motion
of the EndoBot will not cause tearing of the incision point. Each tool can easilyslip through the locking hole in the translational stage and the sleeve and be locked
via a locking pin. Figures 3.3–3.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.
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 spec-
ifies the direction of rotation or translation and q i ∈ 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, E 0, and tool frame, E T , at
Let ω and ν be the angular velocity vector and linear velocity vector of theend-effector. The vector Jacobian of the above manipulator in inertia frame can be
where,( piT )0 = R0i pi,i+1+R0i+1 pi+1,i+2+R04 p4T . Calculating the associated elements
yields the geometric Jacobian:
J =
0 s21c2q 4 + c21c2q 4 0 s2
−c1c2q 4 s1s2q 4 0 −s1c2
−s1c2q 4 −c1s2q 4 0 c1c2
1 0 s2 0
0 c1 −s1c2 0
0 s1 c1c2 0
. (3.19)
3.2.3.2 Analytical Jacobian
The analytical Jacobian can be computed by direct differentiation of the for-
ward 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:
xT = ∂f
∂q q = J a(q )q, (3.20)
where the matrix J a(q ) = ∂f ∂q
is termed analytical Jacobian.
In general, the analytical Jacobian, J a(q ), is different from the geometric Jacobian,J (q ), for orientation part. In case of the EndoBot, since the roll angle can be avail-
able in direct form, the analytical Jacobian and geometric Jacobian are identically
same as follows
J =
0 s21c2q 4 + c21c2q 4 0 s2
−c1c2q 4 s1s2q 4 0 −s1c2
−s1c2q 4 −c1s2q 4 0 c1c2
0 0 1 0
. (3.21)
3.2.4 Singularity Analysis
Since the Jacobian is a function of the configuration q , the singularity can be
occurred at some configurations 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
For many servo applications, the joint friction is the main limitation to preci-
sion and performance. It could lead to stick-slip motions, static positioning errors,
or limit cycle oscillations. Systematic lubrication should be implemented from thedesign stage to reduce frictional disturbance. Stiff (high gain) position control can
reduce the frictional positioning error at the expense of possibly destabilizing ef-
fect. 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.
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 :
θ∗LS = (φT φ)−1φT τ . (3.52)
It is important to note that the matrix (φT φ) is identical to the Hessian of the cost
function. In order to get the minimum, the Hessian must be positive definite and
this is an equivalent condition of the persistent excitation on input signals, which
will be shown in the next section.
3.5.1 Input Signal Design
When the parameter identification comes to real work, the practical questionmay 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 identifiable with invertible
φT φ. Matrix φT φ is often called the persistent matrix or the input correlation
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 con-
stants α1, α2 and δ such that
αaI ≤ t+δt
φT φdτ ≤ α2I. (3.53)
In frequency domain, input signal u(t) is said to be persistently exciting of
order p, if the condition on the power spectrum ,Ψu(w) = 0, is satisfied at at
least p distinct points in the interval −π < ω ≤ π. The strict requirement
on the degree of persistent excitation under the noise signal is not yet fully
investigated. However, since input signal satisfying the persistent exciting
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 filters.In this respect, the input signal swith a user specified design parameters to
allocate the input powers over desired frequency bands will be advisable.
There also exist many efforts seeking to find an optimal input trajectory for iden-
tifying robot dynamic parameters. One of the common criteria is the minimization
of the condition number of the regression matrix. This minimization problem is dif-
ficult to solve and usually is a very time consuming procedure. For this reason, theexisting off-the-shelf input signals are investigated for experimental identification.
Summed multisinusoidal signal
A sum of different sinusoidal signals is effective 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 erroriterations to select the appropriate discrete frequencies and relative amplitudes
of each frequency. The quality of identification 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(2πf (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 specified frequency bands. One shortcoming
may be the trade-off between the duration of excitation at each frequency and
the overall identification time. The overall identification 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 difference from RBS (random binary sequence) is
a periodic and deterministic characteristics and the exact sequence can be re-
generated 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-
Figure 3.12: Generation of PRBS.
ized by two parameters: the switching time (T sw) and the number of registers(nr). The switching time is the minimum time between changes in the binary
level and the maximum length of a sequence is 2nr − 1.
The main usefulness of the PRBS signal lies in the fact that it resembles a
white noise in discrete time and thus excites all frequencies equally well. The
PRBS also uses the maximum power of input signals and consequently has
a high signal-to-noise ratio. The shortcoming with PRBS may be that it is
not possible to design an arbitrary spectrum profile of input signals such as
multiple bands. For the mechanical systems, the exciting with PRBS may
result in tendency of noisy signals in velocity and acceleration signals that can
be obtained from numerically differentiation of measured position signals.
K t [Nm /amp ] 0.01342 0.01342 0.00652 0.00996K g 134 134 72.88 66
3.5.3.1 Identification of the Friction Coefficients in Energy Model
The vectors in the regression matrix associated the Coulomb and viscous co-
efficients are prone to be linearly dependent in the energy model. The functions
|q | and q 2 are correlated and thus, a strong linear dependency between the vectors
exists. This fact gives rise to the difficulty of distinguishing each contribution to
regressor. Consequently, it is very difficult to identify the each coefficient separatelyin energy model. As an experimental verification, the Schroeder-phased input sig-
nals were applied to the first joint of the EndoBot and the regression matrix was
obtained. The comparison on the coefficient of cross correlation between column
vectors in regression matrix gives an indication of correlation:
rXY = cov(X, Y )
σX σY , (3.77)
where rXY is the coefficient 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 stan-
dard deviation. The Table 3.2 shows the comparison on the coefficient of cross cor-
relation 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 first and then compensating the corresponding friction torques
by subtracting them form the generalized torques. The identification of friction pa-
rameters is performed by implementing the velocity control loop as in Figure 3.17.
The Table 3.3 shows the experimental results with friction parameters. E ij stands
Figure 3.17: Block diagram of velocity control loop.
The coefficient of cross correlation between the residual and input is very small (rεu
= 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 existsthe small difference between these two trajectories resulted from wishful assumptions
on model such as the ignored electric dynamics to simplify the analysis, the position
dependent friction coefficients, 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.
on the suture in order for the length to increase. Let P o be the position of the exit
point and P be the current tip position of needle holder. Define ξ = (P − P o). P is
the velocity vector of the needle and L (a scalar) is the rate of change of the suture
length. To keep track of the suture length, we need to find a relationship between L
and P . If P and ξ form an obtuse angle ( P ·ξ < 0), then the suture is not in tension,
therefore L = 0. If P and ξ form an acute angle ( P · ξ > 0), we need to consider
two possibilities. If ξ ≥ L, then the suture would be in tension. Otherwise, the
suture would not be in tension and L = 0. The rate of change for the suture length
is summarized below:
L(t) =
0 if ξ (t) · P (t) ≤ 0 or ξ (t) < L(t)
ξ (t) · P (t) if ξ (t) · P (t) > 0.(4.1)
4.3 Motion Control in the Stitching Task
4.3.1 Dynamic modeling of Stitching
Whenever the robot is in the task of stitching, additional forces are are required
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
From the requirement of desired suture extension, the control objective can be
broken down into the following two sequential steps.
First, the needle must hit on the surface of region of feasible stitching motion whilesatisfying the tension inequality condition:
q (t) → q d as t → ∞, f (t) ≤ f max ∀ t, (4.8)
where q d 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) → q d 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 simplified control problem is given by
Given d and f max, design a feedback control raw, τ , such that (t)
→d
as t → ∞ and f (t) ≤ f max ∀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
positive and certainly the suture extension rate, (t), comes from the motion
of the robot. From this point of view, we can transform the force inequalityconstraint into a motion constraint. Let f max be the maximum tension force
Then, the simplified control problem described above might be written as fol-
lows.
Given d and max, design a feedback control raw, τ , such that
(t) → d as t → ∞ and (t) ≤ max(t) ∀t.
Clearly, the motion control problem with overbounded tension force can be
transformed into the pure motion control problem with a constraint of upper-
bounded velocity. What is the benefit from this transformation?
In case of that the force information is not available, we still can bound the
tension force by f max in order to do a stitch without the problem of tearingoff the tissue.
• Case II.
Suppose we know the maximum tension force, f max, and let f d be less than
f max. Now, the control problem is a regulation problem with a desire position
and a desired force.
Given d and f d, design a feedback control raw, τ , such that (t)
→d
as t → ∞ and f (t) = f d < f max ∀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, eachcan 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 mea-
We will consider three algorithms of automatic ligation in this section. The
first one uses a standard manual stitching tool instrumented for robotic operation.
The second one modifies the grasping tool with a flexible hook to facilitate the knot
tying process. The last one modifies the grasping tool to have an articulated finger.
At the present, the first 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 placedover 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 flexible 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
specified 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
The first 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 modification of the conventional grasping instrument. Thekey observation is that if we can hold on to any part of the suture at a known po-
sition, the rotation of the stitcher would be unnecessary. To achieve this, we added
a reciprocating actuator connected to a flexible hook over the grasper hinge so that
it can be extended or extracted as needed (see Figure 4.6).
Figure 4.6: Flexible hook for catching the suture.
The simple knot algorithm is now modified as described below (shown schemat-
ically in Figure 4.7 and pictures from the experiment in Figure 4.8):
1. Perform Steps 1–2 in Algorithm 1.
2. Extend the flexible hook. Move the stitcher over the hook from the front to
back so that the suture hangs over the hook.
3. Perform Steps 5–6 in Algorithm 1.
4. Retract the flexible hook.
5. Retract the stitcher to tighten the knot.
Again, a mirror image of the first simple knot needs to be performed to ensure a
secured square knot. It is done by replacing the motion over the hook to back to
front (instead of front to back). This algorithm does not have the angle limitation
as in Algorithm 1. The motion over the hook can be designed so the thread is
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), asemi-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 Fig-
ure 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 loopformed 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 off the grasper.
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 2to implement this algorithm.
It should be noted that each step in above knot tying algorithms is performed
autonomously under the surgeon’s supervision on suture entanglement and failure
of suture tail grasping to confirm 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 confirmation.
4.5 Placing the Knot
4.5.1 Problem Formulation
Once developed the knot, the next step would be to place the knot on the
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 figure. Let a post strand represent the suture that the
knot will be tied around and a loop strand represent the suture, which is loopedaround 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. Define 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
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 finding 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)ξ (4.36)
δ (t) = pn0 − pb − pn(t) − pb (4.37)
γ (t) = tan
−1
( pn0
−st
pb ). (4.38)
Due to the sliding condition, (β min ≤ β (t) ≤ β max(t) = f (γ (t), µ)), the trajectory of
where xmin and xmax define the workspace. The autonomous sequencing of suturing
algorithm can be evolving in the autonomous state under the surgeon’s 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 duringthe 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 ei−1. The surgeon also can take the
control in this case and he or she can recover the error in manual state and continue
Figure 5.1: Friciton approximation for compensation.
0 2 4 6 8−15
−10
−5
0
5
10
15(a)
time
p o s i t i o n [ d e g ]
0 2 4 6 8−15
−10
−5
0
5
10
15(b)
time
p o s i t i o n [ d e g ]
0 2 4 6 8−15
−10
−5
0
5
10
15(c)
time
p o s i t i o n [ d e g ]
0 2 4 6 8−15
−10
−5
0
5
10
15(d)
time
p o s i t i o n [ d e g ]
Figure 5.2: Friction compensation.(a) First joint position without friction compensation(b) Second joint position without friction compensation(c) First joint position with friction compensation(d) Second joint position with friction compensation
Friction and velocity estimation are two main factors, which can affect the
performance in motion control applications. The friction can be identified and
compensated as mentioned before. In motion control applications, velocity can bemeasured 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 Differentiating The easiest way to get the velocity information is to
numerically differentiate the position signals. Inherently numerical differenti-
ating is very sensitive to the noise. From the filter perspective, the numerical
differentiating has the following filter characteristics:
F nd(s) = 1 − e−sT S
T s, (5.48)
where T s represents the sampling time. It is an irrational transfer function and
can be approximated by creating a series expansion. With the second order
expansion, it becomes
F nd(s) ≈ 1 − (1 − sT s + 12
s2T 2s ) = sT s(1 − 12
sT s). (5.49)
Clearly, it is an improper function and the Bode plot corresponding to the
second order approximated filter with T s = 0.001 is shown in Figure 5.5. As
can be seed in Figure 5.5, it amplifies the signal in high frequency band where
noise signal mostly lies in. In order to get around this problem, the low-pass
filter can be used with the numerically differentiated signals.
Washout Filter The alternative is to combine the differentiating and smoothing
into one filter, called a washout filter . The washout filter can be represented
as
F wo(s) = ss p
+ 1, (5.50)
where p determines the location of pole and in general can be located 2 ∼ 3
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 different weighting factors on the position error state. Figure 5.11shows the corresponding control input. As seen in Figures 5.10–5.11, the error state
is decreased and the control input is increased by increasing the weighting factor on
q 1 as expected. .
0 5 10 15−40
−30
−20
−10
0
10
20
30
40
time
p o s i t i o n [ d e g ]
desiredwithout friction comp.with friction comp.
Figure 5.8: Experimental results of friction compensation in the LQGcontroller.
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-pitch-
yaw 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 modified to
ensure only the deviation from the specified path is corrected, but not the motion
along the path.
6.1 Constraints Description
In shared control mode, we add artificial constraints to relieve the operatorof some sub task that would be tiring for an operator to concentrate on such as
tool alignments. These artificial constraints are assumed to be holonomic. In or-
der to control robot systems with holonomic constraints, we have to represent the
constraints mathematically. Let the forward kinematics (mapping from the joint
coordinate, q = (q 1, q 2, q 3, q 4) to the end effector coordinate x = (rT , φ)) be denoted
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-
Figure 6.1: Block diagram of task space shared control.
strained space and unconstrained space. It is worth while pointing out that applying
the constrained torques from (6.24) is equivalent to adding virtual mechanisms with
passive springs and dampers normal to constrained space. Suppose that we have a
constrained line parallel to z axis, and then applying shared control has same effecton adding virtual springs and dampers lying on the x-y plane. From this point of
view, intuitively the closed-loop system with control law of (6.24) is always stable
since the robotic system is passive.
These virtual springs and dampers would be very useful for actively guiding
the surgeon. When the surgeon is trying to pull out the suture, the constrained path
will be helpful to guide the surgeon to indicate preferred direction with adjustable
impedance.
6.3.3 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,
Supposed that the end-effector is required to stay along a specific 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 thenumber 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)
These constraints become
C (x) =
x
y
= 0. (6.26)
The constraint Jacobian becomes m × n matrix as follows
J c(x) = ∂C (x)
∂x =
1 0 0 0
0 1 0 0
. (6.27)
6.3.4 Experimental Evaluation of Task Space Shared Controller
This section presents the experimental results that illustrate the performancecharacteristics 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
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 tryingto move the tool in all direction with same impedance for performance evaluation.
Figure 6.3 shows the measured task space position during the motion with shared
0 2 4 6 80
0.02
0.04
0.06
0.08
0.1
time
x [ m ]
0 2 4 6 80
0.02
0.04
0.06
0.08
0.1
time
y [ m ]
0 2 4 6 8−0.2
−0.15
−0.1
−0.05
0
time
z [ m ]
0 2 4 6 8−1.5
−1
−0.5
0
0.5
1
1.5
time
r [ r a d ]
Figure 6.3: Measured task space positions.
controller. As can be seen, the resulting motion was completely satisfactory. For
sliding up and down and rolling, the operator can easily move the robot.
Figure 6.4 shows the constraint force from the shared controller and Figure 6.5
shows the constraint forces mapped into range space R(J T c ). The range space R(J T c )
represents the set of all possible task space forces that make the constraints. Onthe other hand, it represents the force exerted from virtual springs and dampers.
Figure 6.6 shows the corresponding joint torques. The torques in first, second and
fourth joint keep the tip of tool along the constrained path.
Figure 6.7 demonstrates the performance of shared controller in Cartesian
space. The dotted line shows the desired constrained path and the solid line rep-
resents the measured task space positions from the shared controller. The shared
control algorithm brings the tip of tool onto the desired trajectory.
Table 6.2 summarizes the performance of shared control with respect to de-
sired stiffness and actual stiffness getting from maximum deviation and maximumconstraint force in constrained space without considering the dynamic effects.
Table 6.2: Desired and measured stiffness for shared control.
Max(|e|) Max(|F c|) Measured Stiffness, K a Desired Stiffness, K d[m] [N] [N/m] [N/m]
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 different tension. For the first 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 Principle of Direction of Securing a Square Knot
Consider the suturing line locating on the tissue plane in Figure 7.1. Let
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.Define 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. Define ple and p pe 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 e p = ppe− pn ppe− pn
be the unit vectors of the directions along which the
tensions apply. In the current configuration of the Endobots, ple represents the tip
position of stitcher and p pe represents the tip position of grasper.
• Direction of the first throw of a square knot
For the first 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
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 humansoft 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.
Figure 7.5: Strain gauge transducer.
Vision sensors can be used with the pre-modeled and pre-marked sutures. If
the sutures have the marks at predetermined spacing, the tension can be estimated
by measuring the elongation between two marks as shown in Figure 7.6. The diffi-
culty with this method is that the mark may not be visible due to the contamination
of blood or the occlusion of the sutures for the instruments. It is also difficult to
have a high bandwidth and resolution.
The idea to measure the tension in this research is to put a force/torque sensor
in the base of the manipulator, called a base sensor, as shown in Figure 7.7. It has
two main benefits such as
• The sensor does not need to be sterilized,
• Force measurement (and hence control) is independent of the tools.
Beside with these benefits, we can use the sensor information to compensate the
f des, 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 satisfies
f (t)→
f des as t→ ∞
C (x(t)) → 0 ∀ t
where f des is the desired tension, C (x) = 0 represents the holonomic constraint on
the task space variables and defines 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 Hybrid Force/Position Regulating Control
Since artificial constraints are imposed, securing the knot can be considered as
a geometrically constrained problem. When the whole information on environment
geometry is available, an effective strategy to embed the capability of position and
force control would be a hybrid force/motion control scheme, also referred to as a
hybrid control. The basic concept is the partition of the task space into direction of
motion and force based on orthogonality. To this goal, the overall control torques
can be split into two components:
τ = τ m + τ f (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 expressedby C (x) = 0, where x ∈ 2 is the generalized position vector in Cartesian space. To
express the end-effector 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 defined by d(x) = [c(x)T f (x)T ]T and it represents the end-effector position on the
constraint hypersurfaces. Let J c(x) = ∂c(x)/∂x be the Jacobian of the constraint
equation, and J f (x) = ∂f (x)/∂x be the Jacobian of the free motion equation. In
classical dynamic hybrid control literatures, the objective is to design a feedbackcontroller such that it can move the end-effector along the line, J f , while applying
a constraint force in the normal direction to the constraint, J c. In other words, the
Jacobian J c and J f represent the force and position control directions, respectively.
In knot securing case, the objective is to find a feedback control law so that
it can regulate the force along the direction of J f while constraining the motion
in the direction of J c. It can be considered as a problem with planar manipulator
moving in a slot with attached spring. The objective is to regulate the spring force
while applying no force against the side of the slot. The constraint equation can be
Figure 7.10: Constrained motion for securing the knot.
The position regulation to satisfy the constraint is already verified 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 first category is implicit force control, which achieves desired compliant behaviorthrough 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 Explicit Force Control
The general explicit force control scheme used here is
uf = K f f r + K p(f r − f m) + K d( f r − f m) + K i
(f r − f m)dt − K v xm, (7.21)
where f r is the reference force, f m is the measured force, K f is the feedforward force
gain, K p is the proportional force gain, K d is the derivative force gain, K i is the
integral force gain, K v is the velocity gain for the active damping, and xm 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 differentiating 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 filter, 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
effective for soft environment same as a suture model. For soft environment, the
velocity trajectory is considerably smoother than for stiff one. For this reason, active
damping component is added. In addition, to ensure steady state performance and
the tension value, which causes the tissue failure. The free tail of the suture is fixed
Table 7.2: Suture pullout values.
Tissues tension(N)Fat 1.96
Muscle 12.45Skin 17.85
Fascia 36.98
with a fixture 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 filter and then
transformed to the Cartesian velocities, xm = J (q )q . The discrete controller was
implemented at 2ms sampling rate.
7.5.2 Experimental Evaluation of Tension Controller
This section presents the results of the implementation of the explicit force
control strategy described in the previous section. The basic tension control used
in these experiments consisted of first bringing the end-effector 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. Thiscaused 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 off-line analysis. The force reference in these experiments is stepped
to −15N . The performance of each controller was quantified using the following
measures:
RMS force error eRMS = N
k=1(f m(k)−f r)2
N
Maximum force error e∞ = f m(k)∞
The first set of experiments was performed to see the effect of the active damping
and the resulting force trajectories are plotted in Figures 7.21–7.23. As expected
and seen in Figure 7.21, the integral force control gave rise to the zero steady state
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 figures show
the effect of adding the active damping into the system in order to reduce the initialforce spike. With same integral gain, the active damping seems very effective to
achieve the compliant contact transition. The improvement in the performance of
this controller was made by changing the active damping gain, K v, 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 eRMS N e∞ N
K i = 0.5, K v = 0 5.92 71.91K i = 0.5, K v = 1000 4.76 31.45K i = 0.5, K v = 2000 4.70 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 propor-
tional gain, K p achieved the faster response at the expense of slightly increasing the
force spike as shown in Figure 7.24. Figure 7.25 shows the effect of increasing the
proportional gain, K p 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.
Table 7.4: Effect of the proportional gain.
Gains eRMS N e∞ NK p = 0.3, K i = 0.5, K v = 2000 3.68 31.24K p = 0.5, K i = 0.5, K v = 2000 4.84 33.19
The bouncing phenomenon was also observed as decreasing the active damp-
ing gain, K v, and the experimental results can be seen in Figures 7.26–7.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 significant
Figure 7.28: Experimental data from PI control plus active damping withK p = 0.2, K i = 0.75, and K v = 500.
0 5 10 15 20 25 30−80
−70
−60
−50
−40
−30
−20
−10
0
10
time(s)
F o r c e ( N )
Desired Force
Measured Force
Figure 7.29: Experimental data from PI control plus active damping withK p = 0.2, K i = 0.3, K v = 2000, and the modified force referencetrajectory with F des = −(f + 5)N.
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 identification 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 su-
turing 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 op-
eration 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 implementedwith the corresponding state transition diagram for suturing task. For autonomously
evolution of suturing task, an energy based output feedback controller and an opti-
mal state feedback controller with the Kalman filter 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 human’s capability. When a
surgeon and a robot share the different control aspect, shared control proposed in
this thesis can augment the human’s capability by imposing the artificial constraints.
Finally, a base force/torque sensor method is presented for tension measurement and
a hybrid force/position strategy is implemented to effectively regulate the tension
This session describes several possible areas of future research.
Manipulator Enhancement Medical palpation plays an important role in both
diagnosis as well as therapy. The ability to sense the tactile information can
enhance the performance of robustness on the suturing tasks [86, 87]. A
major extension of this research related to palpation can be to develop the
MIS instruments with tactile sensors. With tactile sensors, a closed-loop feed-
back control for regulating the gripping force can be implemented. Clearly, it
can be useful in handling soft tissues and securely grasping the suture. For
performing the cooperative tasks with two the EndoBots, calibration between
two local frames is required. At the present, kinematic calibration is a tediousand time consuming procedure. It is desirable to develop a more automatic
calibration method. One possibility is to equip with supporting arms with
position sensors.
Vision Feedback Vision-based control can enhance the performance of the sutur-
ing task in autonomous operations. The primary advantage of vision sensors
is their ability to provide information on suture trajectory. From the prelim-
inary experiments on the suturing task with the EndoBot system, graspingthe suture tail is a difficult and challenging task. Since it is not possible to
predict the trajectory of the suture tail, integrating the vision sensor in the
feedback loop can enhance the robustness of the suturing algorithms. Calibra-
tion with two cameras to get 3D depth information and visibility due to blood
can be technical challenge issues. Vision sensors can be used to generate the
local map so as to provide an active guidance in manual mode. With active
deformable models generated from vision information, surgeons can teach the
niche for surgical procedures such as knot tying and cutting.
Quantification of Knot Quality Following directly from this research, it will be
important to find the values of the optimal tension for securing the knot.
The optimal tension values can be different to the strength of tissues and the
tensile strength of the sutures, but no publication was reported. Therefore,
[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 hipreplacement 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,“Anintegrated CAD-robotics system for total knee replacement surgery,” inProceedings 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. Zaffagnini, “Computer-assisted knee arthroplastyat 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-enhancedsurgery,” 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 teleoperatedmicrosurgical 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 improvedabsolute 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 forlaparoscopic positioning stands,” in Proceedings of the 1997 IEEE International Conference on Robotics and Automation , pages 1553-1560,Albuquerque, NM, 1997.
[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, pages172-183, 1994.
[11] A.J. Madhani and J.K Salisbury, “Wrist mechanism for surgical instrumentfor performing minimally invasive surgery with enhanced dexterity andsensitivity,” U.S. Patent No. 5,797,900, 1997.
[12] M.C. Cavusoglu, F. Tendick, M. Cohn, and S.S. Sastry, “A laparoscopictelesurgical 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 flexible stems , ERL Technical Report.
[14] A. Faraz, S. Payandeh, and A. Nagy, “Issues and design concepts inendoscopic extenders,” in Proceedings of 1995 IFAC Man/Machine Systems ,pages 231-236,1995.
[15] C. Reboulet and S. Durand-Leguay, “Optimal design of redundant parallelmechanism for endoscopic surgery,” in Proceedings of the 1999 IEEE/RSJ International Conference on Intelligent Robots and Systems , Vol. 2, pages1432-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 , ISISMedical Media, 1995.
[18] C. Bernard, H. Kang, S.K. Singh, and J.T. Wen, “Robotic system forcollaborative 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,” inProceedings 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 endoscopicsurgery,” in ASME IMECE Conference Proceesings: 5th Annual Symposium on Haptic Interfaces for Virtual Environment and Teleoperator Systems ,pages 583-590, Atlanta, GA, 1996.
[22] J. Funda, R. Taylor, K. Gruben, and D. La Rose, “Optimal motion control forteleoperated 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, “Constrainedcartesian 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 cardiacprocedures,” 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 fixedviewpoint/operation-point,” in Proceeding of the 1995 IEEE/RSJ
International Conference on Intelligent Robots and Systems , Vol. 2, pages178-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 minimallyinvasive 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 laparoscopicsurgery,”in Proceedings of the SPIE International Symposium on Biological Optics (BIOS’98), 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 forteleoperators,” U.S. Patent No. 5,086,982, 1992.
[33] A. Douglas and Y. Xu, “Real-time shared control system for spacetelerobotics,” 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 forcecontrol 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 handrobot 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 roboticaugmentation 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 andcompensation,” 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 foran 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 frictioncompensated 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 jointfrcition: 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. Jaffe, and S. Dubowsky, “Achieving fine absolutepositioning accuracy in large powerful manipulators,” in Proceedings of the
1999 IEEE International Conference on Robotics and Automation , pages2819-2824, 1999.
[45] J.T. Wen and D.S. Bayard, “A new class of control laws for roboticmanipulators, 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, pages85-89, 1970.
[49] K. Koztowski, Modeling and identification in robotics , Springer, 1998.
[50] C.H. An, C.G. Atkeson, and J.M. Hollerbach, “Estimation of inertialparameters 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 finding exciting trajectories for identification experimentsinvolving 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 identification of base
inertial parameters of robots,” International Journal of Robotics Research ,Vol.11, pages 362-375, 1992.
[53] M. Gautier, “Optimal motion planning for robot’s inertial parametersidentification,” 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 robotidentification,” in Proceedings of the 1993 IEEE Inernatinal Conference on Robotics and Automation , pages 907-912, 1993.
[55] M. Gautier, “A comparison of filtered models for dynamic identification of
robots,” in Proceedings of the 35th IEEE Conference on Decision and Control ,pages 875-880, 1996.
[56] L.S. Wilfinger, J.T. Wen, and S. Murphy, “Integral force control withrobustness enhancement,” IEEE Control System Magazine , 14(1), pages31-40, 1994.
[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 mechanicalmanipulators,” 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 forindustrial 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 roboticmanipulators,” In Proceeding of IEEE conference on Decision Control , pages409-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 unified perspective on robot: The energy Lyapunov functionapproach,” 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 robotmanipulators: a survey,” International Journal of Control , 72(16), pages1498-1522, 1999.
[70] H. Berghuis, P. Lohnberg, and H. Nijmeijer, “Tracking control of robots usingonly 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, pages1-24, 1985.
[73] M. Spong and M. Vidyasagor, Robot dynamics and control , John Wiley andSon, 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, pages126-133, 1981.
[76] T. Yoshikawa, “Dynamics hybrid position/force control of robotmanipulators- description of hand constraints and calculation of joints drivingforces,” in Proceeding of the IEEE International Conference on Robotics and Automation , pages 1393-1398, 1986.
[77] O. Khatib, “A unified approach for motion and force control of robotmanipulators: 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/positioncontrol of robotic manipulators,” IEEE Transactions on Robotics and Automation , 9(4), pages 361-373, 1993.
[80] L.S. Wilfinger, J.T. Wen, and S. Murphy, “Integral force control withrobustness 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 stiff contact: An experimental study,” in IEEE International Conference on Systems, Man, and Cybernetics , 2 , pages 1285-1290, 1994.
Since the EndoBot has a simple kinematic configuration, 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 Paden-
Kahan 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, q 4) From the forward kinematicsderivation above, a given position vector from the base frame to tool frame
p0T can has the following form:
p0T = eh2q2eh1q1eh3q3 p34 (A.1)
where, p34 = q 4h3. Since the position vector p34 lies on the axis of h3, it yields
p0T = eh2q2eh1q1 p34. (A.2)
Taking the norm, it gives the translational distance, q 4:
p0T =eh2q2eh1q1 p34
= p34 = q 4. (A.3)
Step 2 (solve for first two angles, q 1 and q 2) Since q 4 is known and h1 and h2
intersect, (A.2) can be solved using Subproblem 2 (Rotation about two sub-
sequent axes) and it gives q 1 and q 2 from (A.2). Due to the multiple solutionsproperty 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, q 3) The joint angle of the axis 3, q 3, is simply